/*
 * log_data.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "PID.h"
#include "type_def.h"
#include "log_data.h"
#include "communication.h"
#include "computation_graph.h"
#include "graph_blocks.h"

// Current index of the log array
static int arrayIndex = 0;
// Size of the array
static int arraySize = 0;

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

// Holds a statically allocated string, as well as info about its size and capacity
// Used by safe_sprintf_cat
struct str {
	char* str;
	size_t size;
	size_t capacity;
};


 /**
  * Forward declarations
  */
 void format_log(int idx, log_t* log_struct, struct str* buf);

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

static float* logArray = NULL;
static int row_size;

static char units_output_str[512] = {};
static char units_param_str[512] = {};
static struct str units_output = { .str = units_output_str, .size = 0, .capacity = sizeof(units_output_str)};
static struct str units_param = { .str = units_param_str, .size = 0, .capacity = sizeof(units_output_str)};

/*
 * Does an sprintf and concatenation. Used just like sprintf, but pass in a pointer to a struct str instead.
 * Returns the number of bytes that would have been written (just like snprintf)
 * Will not write more than is available in the given string
*/
int safe_sprintf_cat(struct str *str, const char *fmt, ...) {
	size_t available = str->capacity - str->size;
	va_list args;
	va_start(args, fmt);
	// Print to offset not more than remaining capacity characters
	size_t full_size = vsnprintf(str->str + str->size, available, fmt, args);
	va_end(args);
	if (full_size >= available) { // Check for truncation
		str->size = str->capacity;
	} else {
		str->size = str->size + full_size;
	}
	return full_size;
}

void addOutputToLog(log_t* log_struct, int controller_id, int output_id, char* units) {
	if (n_outputs < MAX_LOG_NUM) {
		log_outputs[n_outputs].block_id = controller_id;
		log_outputs[n_outputs].sub_id = output_id;
		n_outputs++;
		safe_sprintf_cat(&units_output, "\t%s", units);
	}
}
void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* units) {
	if (n_params < MAX_LOG_NUM) {
		log_params[n_params].block_id = controller_id;
		log_params[n_params].sub_id = param_id;
		n_params++;
		safe_sprintf_cat(&units_param, "\t%s", units);
	}
}

void initialize_logging(log_t* log_struct, parameter_t* ps) {
	char* rad = "rad";
	char* rad_s = "rad/s";
	char* pwm_val = "10ns_dutycycle";
	char* m = "m";
	char* m_s = "m/s";
	addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION, rad);
	addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION, rad);
	addOutputToLog(log_struct, ps->pitch_pid, PID_CORRECTION, rad_s);
	addOutputToLog(log_struct, ps->roll_pid, PID_CORRECTION, rad_s);
	addOutputToLog(log_struct, ps->yaw_pid, PID_CORRECTION, rad_s);
	addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->pitch_trim_add, CONST_VAL, rad);
	addOutputToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
	addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
	addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL, m);
	addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL, m);
	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
	addOutputToLog(log_struct, ps->lidar, CONST_VAL, m);
	addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
	addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
	addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM0, pwm_val);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
	addOutputToLog(log_struct, ps->mixer, MIXER_PWM3, pwm_val);
	addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
	addOutputToLog(log_struct, ps->x_vel_pid, PID_CORRECTION, rad);
	addOutputToLog(log_struct, ps->y_vel_pid, PID_CORRECTION, rad);
	addOutputToLog(log_struct, ps->x_vel, PID_CORRECTION, m_s);
	addOutputToLog(log_struct, ps->y_vel, PID_CORRECTION, m_s);

	// 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;
	} else {
		arraySize = LOG_STARTING_SIZE;
	}
}


int log_data(log_t* log_struct, parameter_t* ps)
{
	if(arrayIndex >= arraySize)
	{
		return 1;
	}
	float* thisRow = &logArray[arrayIndex * row_size];
	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;
	}
	char buf_arr[2560] = {};
	struct str buf = {.str = buf_arr, .size = 0, .capacity = sizeof(buf_arr)};

	// Let user know that allocation failed
	if (arraySize != LOG_STARTING_SIZE) {
		safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
		send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);
		return;
	}

	int i;
	// Comment header
	safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);

	// List Pid Constants
	for (i = 0; i < ps->graph->n_nodes; ++i) {
		struct graph_node* node = &ps->graph->nodes[i];
		if (node->type->type_id == BLOCK_PID) {
			double kp, ki, kd;
			kp = graph_get_param_val(ps->graph, i, 0);
			ki = graph_get_param_val(ps->graph, i, 1);
			kd = graph_get_param_val(ps->graph, i, 2);
			safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf\n", node->name, kp, ki, kd);

		}
	}

	// Header names for the pre-defined values
	safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");

	// 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];
		safe_sprintf_cat(&buf, "\t%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];
		safe_sprintf_cat(&buf, "\t%s_%s", block_name, param_name);
	}
	safe_sprintf_cat(&buf, "\n");

	// Send header names
	send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);

	// Send units header
	buf.size = 0;
	safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
	safe_sprintf_cat(&buf, units_output.str);
	safe_sprintf_cat(&buf, units_param.str);
	safe_sprintf_cat(&buf, "\n");
	send_data(&hardware_struct->uart, LOG_ID, 0, buf.str, buf.size);

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

	// Empty message of type LOG_END to indicate end of log
	send_data(&hardware_struct->uart, LOG_END_ID, 0, buf.str, 0);
}

void resetLogging() {
	arrayIndex = 0;
}

/*
  * Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
*/
void format_log(int idx, log_t* log_struct, struct str* buf) {
	int i;
	buf->size = 0;
	
	float* row = &logArray[idx * row_size];\

	safe_sprintf_cat(buf, "%f", row[0]);
	for (i = 1; i < row_size; i++) {
		safe_sprintf_cat(buf, "\t%f", row[i]);
	}
	safe_sprintf_cat(buf, "\n");
}