#include "communication.h"
#include "commands.h"
#include "type_def.h"
#include "computation_graph.h"
#include "util.h"

/*
 * Static variables used to keep track of packet counts
 */
static int n_msg_received = 0;
static size_t total_payload_received = 0;


/* Misc. callbacks */

/**
  * Currently does nothing.
  */
int cb_debug(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
	char buf[255];

	// Get the node ID, parameter ID, parameter value
	u8 node_id = data[0];
	struct computation_graph* graph = structs->parameter_struct.graph;
	float param_val = graph_get_output(graph, node_id, 0);

	int len = snprintf(buf, sizeof buf, "%f", param_val);
	send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
	return 0;
}

/**
  * counts the number of packet logs.
  */
int cb_packetlog(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
	n_msg_received += 1;
	total_payload_received += length;
	return 0;
}

/**
  * Handles a get packet logs request and sends a response
  * with the packet log data.
  */
int cb_getpacketlogs(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
	char buf[255];

	// Message logging number of messages received and size of payload received
	int len = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);

	send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
	return 0;
}

/*
 * Handles receiving new location updates.
 */
int cb_update(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
	//processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));

	quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
	// Packet must come as [NEARPY], 4 bytes each
	int packetId = build_int(data + 0);
//	printf("Packet ID: %d\n", packetId);
	float y_pos = build_float(data + 4);
//	printf("y_pos: %f\n", y_pos);
	float x_pos = build_float(data + 8);
//	printf("x_pos: %f\n", x_pos);
	float alt_pos = build_float(data + 12);
//	printf("alt_pos: %f\n", alt_pos);
	float roll = build_float(data + 16);
//	printf("roll: %f\n", roll);
	float pitch = build_float(data + 20);
//	printf("pitch: %f\n", pitch);
	float yaw = build_float(data + 24);
//	printf("yaw: %f\n", yaw);

	currentQuadPosition->packetId = packetId;
	currentQuadPosition->y_pos = y_pos;
	currentQuadPosition->x_pos = x_pos;
	currentQuadPosition->alt_pos = alt_pos;
	currentQuadPosition->roll = roll;
	currentQuadPosition->pitch = pitch;
	currentQuadPosition->yaw = yaw;

	// Make location as fresh
	structs->user_input_struct.locationFresh = 1;

	return 0;
}

/**
  * This is called on the ground station to begin sending VRPN to the quad.
  */
int cb_beginupdate(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length) {
	structs->user_input_struct.receivedBeginUpdate = 1;
	return 0;
}

/*
 * -----------------------------------------------
 * Callbacks for control network modification/info
 * -----------------------------------------------
*/

// Struct for holding a node ID and either a parameter, output, or input index
struct node_ids {
	int16_t id;
	int16_t sub_id;
};

/*
 * Given a data array, returns a node_ids struct retrieved from the array.
 * Assumes the given array is at least 4 bytes to hold the data.
*/
struct node_ids get_node_ids(u8 *data) {
	return (struct node_ids) {
		.id = build_short(data),
		.sub_id = build_short(data + 2)
	};
}

/**
  * Handles a command to set a node parameter on the quad.
  *
  * NOTE:
  * Expects the uart buff to have data in the following format:
  * |--------------------------------------------------------|
  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
  * |--------------------------------------------------------|
  * |   parameter ||   node ID   | node parmID |  param val  |
  * |--------------------------------------------------------|
  * |       bytes ||      2      |      2      |      4      |
  * |--------------------------------------------------------|
  * 
  * Does not send anything in response.
  */
int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
{
	// Check if the data length is correct
	if (length != 8) {return -1;}
	struct computation_graph* graph = structs->parameter_struct.graph;

	// Get the node ID, parameter ID, parameter value
	struct node_ids ids = get_node_ids(data);
	float param_val = build_float(data + 4);
	// Set the value for that parameter on that node
	graph_set_param_val(graph, ids.id, ids.sub_id, param_val);

	return 0;
}

/**
  * Handles a command to get a controller parameter from the quad.
  *
  * NOTE:
  * Expects the uart buff to have data in the following format:
  * |-------------------------------------------|
  * |  data index ||    0 - 1     |    2 - 3    |
  * |-------------------------------------------|
  * |   parameter ||    node ID   | node parmID |
  * |-------------------------------------------|
  * |       bytes ||       2      |      2      |
  * |-------------------------------------------|
  *
  * Sends a response of type RESPPARAM_ID.
  * The response will have a message ID equal to the one originally received.
  * The data of the response will be in the following format:
  * |--------------------------------------------------------|
  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
  * |--------------------------------------------------------|
  * |   parameter ||   node ID   | node parmID |  param val  |
  * |--------------------------------------------------------|
  * |       bytes ||      2      |      2      |      4      |
  * |--------------------------------------------------------|
  */
int cb_getparam(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
{
	// Check if the data length is correct
	if (length != 4) {return -1;}
	u16 msg_id = meta->msg_id;

	// Get the controller ID, parameter ID
	struct node_ids ids = get_node_ids(data);
	struct computation_graph* graph = structs->parameter_struct.graph;
	float param_val = graph_get_param_val(graph, ids.id, ids.sub_id);

	// Format the response data
	char resp_data[8];
	// Controller ID
	pack_short(ids.id, resp_data);
	// Parameter ID
	pack_short(ids.sub_id, resp_data + 2);
	// Parameter value (4 byte float)
	pack_float(param_val, resp_data + 4);

	// Send the response
	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));

	return 0;
}

/**
  * Handles a command to set a node's input source
  *
  * NOTE:
  * Expects the uart buff to have data in the following format:
  * |---------------------------------------------------------------------------|
  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
  * |---------------------------------------------------------------------------|
  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
  * |---------------------------------------------------------------------------|
  * |       bytes ||       2      |       2       |      2      |       2       |
  * |---------------------------------------------------------------------------|
  * 
  * Does not send anything in response.
  */
int cb_setsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
	if (length != 8) {return -1;}
	int16_t dest_node = build_short(data);
	int16_t dest_input = build_short(data + 2);
	int16_t src_node = build_short(data + 4);
	int16_t src_input = build_short(data + 6);

	struct computation_graph* graph = structs->parameter_struct.graph;
	graph_set_source(graph, dest_node, dest_input, src_node, src_input);

	return 0;
}

/**
  * Handles a command to get the source of a node's input
  *
  * NOTE:
  * Expects the uart buff to have data in the following format:
  * |---------------------------------------------|
  * |  data index ||    0 - 1     |     2 - 3     |
  * |---------------------------------------------|
  * |   parameter ||    node ID   | node input ID |
  * |---------------------------------------------|
  * |       bytes ||       2      |       2       |
  * |---------------------------------------------|
  *
  * Sends a response of type RESPSOURCE_ID.
  * The response will have a message ID equal to the one originally received.
  * The data of the response will be in the following format:
  * |---------------------------------------------------------------------------|
  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
  * |---------------------------------------------------------------------------|
  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
  * |---------------------------------------------------------------------------|
  * |       bytes ||       2      |       2       |      2      |       2       |
  * |---------------------------------------------------------------------------|
  */
int cb_getsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
	if (length != 4) {return -1;}
	u16 msg_id = meta->msg_id;
	// Get requested IDs
	int16_t node_id = build_short(data);
	int16_t input_id = build_short(data + 2);

	u8 resp_data[8];
	pack_short(node_id, resp_data);
	pack_short(input_id, resp_data + 2);

	struct computation_graph* graph = structs->parameter_struct.graph;
	struct node_src source = graph_get_source(graph, node_id, input_id);
	pack_short(source.controller_id, data + 4);
	pack_short(source.controller_output, data + 6);

	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
}

/**
  * Handles a command to get a node output value from the quad.
  * Packet structure is the same as getparam
  */
int cb_getoutput(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
{
	// Check if the data length is correct
	if (length != 8) {return -1;}
	u16 msg_id = meta->msg_id;

	// Get the controller ID, parameter ID
	struct node_ids ids = get_node_ids(data);
	struct computation_graph* graph = structs->parameter_struct.graph;
	float output_val = graph_get_output(graph, ids.id, ids.sub_id);

	// Format the response data
	char resp_data[8];
	// Controller ID
	pack_short(ids.id, resp_data);
	// Output ID
	pack_short(ids.sub_id, resp_data + 2);
	// Output value (4 byte float)
	pack_float(output_val, resp_data + 4);

	// Send the response
	send_data(&structs->hardware_struct.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));

	return 0;
}

/*
 * Handles a request for the list of nodes in the graph
 * For N total nodes, returns data in the following format:
 * The node IDs and type IDs are consecutive shorts
 * The node names are null-separated
 * |---------------------------------------------------------------|
 * |  data index ||   0 - 2*N-1  |  2*N - 4*N-1  | 4*N - (< 4096)  |
 * |---------------------------------------------------------------|
 * |   parameter ||  Node IDs   | Node type IDs  |    Node names   |
 * |---------------------------------------------------------------|
 * |       bytes ||     2*N     |      2*N       |      < 4096     |
 * |---------------------------------------------------------------|
 */
int cb_getnodes(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
	const struct computation_graph* graph = structs->parameter_struct.graph;
	if (graph->n_nodes >= 150) {
		static char* error_msg = "Over 150 nodes. Not responding to cb_getnodes for fear of buffer overflow.";
		send_data(&structs->hardware_struct.uart, DEBUG_ID, 0,
				error_msg, sizeof(error_msg));
		return -1;
	}
	
	// Number of bytes in node ID being sent. Currently short (16 bits)
	size_t id_len = 2;

	char resp_buf[4096];
	int i;
	// Currently ID is always index in array.
	// computation_graph provides no method of accessing ID, since it is implicit
	for (i = 0; i < graph->n_nodes; i++) {
		pack_short(i, resp_buf + (id_len * i));
	}

	// Construct type IDs
	size_t offset = id_len * graph->n_nodes;
	for (i = 0; i < graph->n_nodes; i++) {
		int type_id = graph->nodes[i].type->type_id;
		pack_short(type_id, resp_buf + offset + (id_len * i));
	}

	// Construct list of node names
	offset += id_len * graph->n_nodes;
	for (i = 0; i < graph->n_nodes; i++) {
		size_t remaining_size = sizeof(resp_buf) - offset;
		const char* name = graph->nodes[i].name;
		size_t name_len = strlen(name);
		if (name_len + 1 <= remaining_size) {
			memcpy(resp_buf + offset, name, name_len);
			offset += name_len;
			// Add null-terminator separator
			resp_buf[offset] = 0;
			offset += 1;
		}
	}

	send_data(&structs->hardware_struct.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
	return 0;
}

/*
 * Handles adding a new node with a particular type and name
 * Expects the uart buff to have data in the following format:
 * |---------------------------------------------|
 * |  data index ||    0 - 1     |     2 - ?     |
 * |---------------------------------------------|
 * |   parameter ||    type ID   | New node name |
 * |---------------------------------------------|
 * |       bytes ||       2      |       ?       |
 * |---------------------------------------------|
 *
 * Returns the new node ID in the following format:
 * |-----------------------------|
 * |  data index ||    0 - 1     | 
 * |-----------------------------|
 * |   parameter ||    node ID   |
 * |------------------------------
 * |       bytes ||       2      |
 * |-----------------------------|
 */
int cb_addnode(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
	if (length < 2) {return -1;}
	// Size of name
	size_t name_len = length - 2;

	return 0;
}