/*
 * log_data.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "PID.h"
#include "type_def.h"
#include "sleep.h"
#include "log_data.h"
#include "communication.h"
#include "computation_graph.h"
#include "graph_blocks/node_pid.h"
#include "graph_blocks/node_constant.h"
#include "graph_blocks/node_mixer.h"
 
// Current index of the log array
int arrayIndex = 0;
// Size of the array
int arraySize = LOG_STARTING_SIZE;

struct graph_tuple { // Tuple for
    int block_id;
    int sub_id;
};

struct graph_tuple log_outputs[MAX_LOG_NUM];
struct graph_tuple log_params[MAX_LOG_NUM];
size_t n_outputs;
size_t n_params;

float* logArray;
int row_size;

void addOutputToLog(log_t* log_struct, int controller_id, int output_id) {
	if (n_outputs < MAX_LOG_NUM) {
		log_outputs[n_outputs].block_id = controller_id;
		log_outputs[n_outputs].sub_id = output_id;
		n_outputs++;
	}
}
void addParamToLog(log_t* log_struct, int controller_id, int param_id) {
	if (n_params < MAX_LOG_NUM) {
		log_params[n_params].block_id = controller_id;
		log_params[n_params].sub_id = param_id;
		n_params++;
	}
}

void initialize_logging(log_t* log_struct, parameter_t* ps) {
	addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->pitch_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->roll_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->yaw_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION);
	addOutputToLog(log_struct, ps->cur_pitch, CONST_VAL);
	addOutputToLog(log_struct, ps->cur_roll, CONST_VAL);
	addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL);
	addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL);
	addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL);
	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL);
	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL);
	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL);
	addOutputToLog(log_struct, ps->x_set, CONST_VAL);
	addOutputToLog(log_struct, ps->y_set, CONST_VAL);
	addOutputToLog(log_struct, ps->alt_set, CONST_VAL);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM0);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM1);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM2);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM3);

	// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
	row_size = n_outputs + n_params + 6 + 1;
	size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
	logArray = malloc(needed_memory);
	if (!logArray) { // malloc failed
		arraySize = 0;
	}
}


int log_data(log_t* log_struct, parameter_t* ps)
{
	if(arrayIndex >= arraySize)
	{
		return 1;
	}
	float* thisRow = &logArray[arrayIndex * row_size * sizeof(float)];
	int offset = 0;
	thisRow[offset++] = log_struct->time_stamp;
	thisRow[offset++] = log_struct->gam.accel_x;
	thisRow[offset++] = log_struct->gam.accel_y;
	thisRow[offset++] = log_struct->gam.accel_z;
	thisRow[offset++] = log_struct->gam.gyro_xVel_p;
	thisRow[offset++] = log_struct->gam.gyro_yVel_q;
	thisRow[offset++] = log_struct->gam.gyro_zVel_r;

	int i;
	for (i = 0; i < n_params; i++) {
		thisRow[offset++] = graph_get_param_val(ps->graph, log_params[i].block_id, log_params[i].sub_id);
	}
	for (i = 0; i < n_outputs; i++) {
		thisRow[offset++] = graph_get_output(ps->graph, log_outputs[i].block_id, log_outputs[i].sub_id);
	}

	arrayIndex++;
	return 0;
}


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

void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps){
	if (arrayIndex == 0) {
		// Don't send any logs if nothing was logged
		return;
	}
	int i;//, j;
	char buf[2048] = {};
	buf[0] = 0; // Mark buffer as size 0
	char header1[256] = {};
	char header2[1024] = {};

	// Let user know that allocation failed
	if (arraySize != LOG_STARTING_SIZE) {
		size_t send_len = sprintf(header1, "Failed to allocate enough log memory\n");
		send_data(LOG_ID, 0, header1, send_len);
		return;
	}

	sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z");

	int end = 0;
	// Print all the recorded block parameters
	for (i = 0; i < n_params; i++) {
		const char* block_name = ps->graph->nodes[log_params[i].block_id].name;
		const char* output_name = ps->graph->nodes[log_params[i].block_id].type->param_names[log_params[i].sub_id];
		end += sprintf(&header2[end], ",%s-%s", block_name, output_name);
	}
	// Print all the recorded block outputs
	for (i = 0; i < n_outputs; i++) {
		const char* block_name = ps->graph->nodes[log_outputs[i].block_id].name;
		const char* param_name = ps->graph->nodes[log_outputs[i].block_id].type->output_names[log_outputs[i].sub_id];
		end += sprintf(&header2[end], ",%s-%s", block_name, param_name);
	}
	end += sprintf(&header2[end], "\n");


	strcat(buf,header1);
	strcat(buf,header2);

	send_data(&hardware_struct->uart, LOG_ID, 0, buf, strlen(buf));

	/*************************/
	/* print & send log data */
	for(i = 0; i < arrayIndex; i++){
		int size = format_log(i, log_struct, buf);
		send_data(&hardware_struct->uart, LOG_ID, 0, buf, size);
	}

	char data[1] = {0}; // 1 byte to make compilers happy
	send_data(LOG_END_ID, 0, data, 0);
}

void resetLogging() {
	arrayIndex = 0;
}

int format_log(int idx, log_t* log_struct, char* buf) {
	int i;
	int end = 0;

	float* row = &logArray[idx * row_size  * sizeof(float)];\

	end += sprintf(&buf[end], "%f", row[0]);
	for (i = 1; i < row_size; i++) {
		end += sprintf(&buf[end], ",%f", row[i]);
	}
	end += sprintf(&buf[end], "\n");
	return end;
}