/*
 * log_data.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
/*
 #include "log_data.h"
 
// Current index of the log array
int arrayIndex = 0;
// Size of the array
int arraySize = LOG_STARTING_SIZE;
int resized = 0;

// The number of times we resized the array
int resizeCount = 0;

// Pointer to point to the array with all the logging information
// for now its not dynamic
log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log

int log_data(log_t* log_struct)
{
	updateLog(*log_struct);
    return 0;
}

*
 * Fills up an xbox hueg amount of memory with log data

void updateLog(log_t log_struct){
	// If the first iteration, allocate enough memory for "arraySize" elements of logging
//	if(logArray == NULL){
//		// size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
//		// up to 20 seconds of log before resizing
//		logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
//		uart0_sendStr("initialized log array.\n");
//		sleep(1);
//	}

	// semi dynamic log
//	if((arrayIndex >= arraySize - 1) && (!resized)){
//		realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
//		resized = 1;
//		arraySize = LOG_STARTING_SIZE * 3;
//		uart0_sendStr("resized log array.\n");
//		sleep(1);
//	}

	if(arrayIndex >= arraySize - 1)
	{
		return;
	}

	// Add log to the array
	logArray[arrayIndex++] = log_struct;

	// If the index is too big, reallocate memory to double the size as before
//	if(arrayIndex == arraySize){
//		arraySize *= 2;
//		logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
//		++resizeCount;
//	}
//	else if(arrayIndex > arraySize){
//		// Something fishy has occured
//		xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
//	}
}


*
 * Prints all the log information.
 *
 * TODO: This should probably be transmitting in binary instead of ascii


void printLogging(){
	int i, numBytes;
	char buf[2304] = {};
	char comments[256] = {};
	char header[1024] = {};
	char units [1024] = {};

	char tempLog[4096*2] = {};

	sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
	sprintf(header, "%%Time\t" "LoopPeriod\t"

			//current points (measurements)
			"X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
			"Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
			"Quad_Meas_Roll\tQuad_Meas_Pitch\t"
			"roll_velocity\tpitch_velocity\tyaw_velocity\t"

			//setpoints
			"X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
			"Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
			"Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"

			//corrections
			"PID_x\t"
			"PID_y\t"
			"PID_z\t"
			"PID_roll\t"
			"PID_pitch\t"
			"PID_yaw\t"
			"PID_roll_vel\t"
			"PID_pitch_vel\t"
			"PID_yaw_vel\t"

			//trims
			"Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"

			//motor commands
			"Motor_0\tMotor_1\tMotor_2\tMotor_3\n"

					);


	sprintf(units,	"&sec\tsec\t"

			//current points
			"meters\tmeters\tmeters\t"
			"radians\tradians\tradians\t"
			"radians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"

			//setpoints
			"meters\tmeters\tmeters\t"
			"radians\tradians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"

			//corrections
			"radians\tradians\tradians\t"
			"radians//sec\tradians//sec\tradians//sec\t"
			"none\tnone\tnone\t"

			//trims
			"none\tnone\tnone\tnone\t"

			//motors
			"none\tnone\tnone\tnone\n"

					);

	strcat(buf,comments);
	strcat(buf,header);
	strcat(buf,units);


	numBytes = logData(buf, tempLog);
	uart0_sendBytes(tempLog, strlen(tempLog));
	usleep(100000);

	***********************
	 print & send log data
	for(i = 0; i < arrayIndex; i++){
			char* logLine = format(logArray[i]);
			numBytes = logData(logLine, tempLog);
			uart0_sendBytes(tempLog, numBytes);
			usleep(10000);
			//free(logLine);
			//break;
	}
}


char* format(log_t log){
	char *retString = malloc(4096*2);

			sprintf(retString, 	"%.3f\t%.4f\t" //Time and TimeSlice

								// current points
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//setpoints
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//corrections
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"
								"%.3f\t%.3f\t%.3f\t"

								//trims
								"%d\t%d\t%d\t%d\t"

								//motors
								"%d\t%d\t%d\t%d\n"

								,log.time_stamp, log.time_slice,

								// current points
								log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
								log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
								log.roll_angle_filtered, log.pitch_angle_filtered,
								log.phi_dot, log.theta_dot, log.psi_dot,

								//setpoints
								log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
								log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
								log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,

								//corrections
								log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
								log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
								log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,

								//trims
								log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,

								//motors
								log.motors[0], log.motors[1], log.motors[2], log.motors[3]
			);


		return retString;
}
*/