#include "incp_handler.h"
#include "main.h"

#define INCP_IMPL_CALC
#include "gen_wrapper.h"

#include "i3c.h"
#include "my_i2c.h"

static void handle_msg(uint8_t sender, uint8_t *msg, uint32_t len);
static void handle_datareq(uint8_t sender, uint32_t module, uint8_t *in_bytes, uint32_t in_len, uint8_t *buffer);
static void handle_cmdreq(uint8_t sender, uint32_t module, uint8_t *in_bytes, uint32_t in_len, uint8_t *buffer);

#define I2C_OWN_ADDR 0x01

void handle_incp(msg_buf_t *msgs)
{
	uint8_t *msg;
	uint32_t len;
	err_t err;

	err = msg_buf_peek(msgs, (void**)&msg, &len);
	if (err == ERR_EMPTY)
		return;

	if (err != ERR_OK)
		Error_Handler();

	//FIXME: Fails everytime
	// Check I3C
	if (!i3c_validate(msg, len))
	{
		HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_SET);
		//return; // Ignore failed messages
	}

	uint8_t sender = msg[0];

	handle_msg(sender, msg + I3C_HEADER_SIZE, len - I3C_HEADER_SIZE);
	msg_buf_pop(msgs);
}

#define BUFSIZ 512
static void handle_msg(uint8_t sender, uint8_t *bytes, uint32_t len)
{
	incp_msg_t msg = { 0 };
	uint8_t *content;
	size_t content_len;
	uint8_t buffer[BUFSIZ] = {0};

	if (incp_deserialize_base(&msg, (void**)&content, &content_len, bytes, len) != INCP_OK)
	{
		return; // Ignore failed messages
	}

	switch (msg.kind)
	{
	case incp_ping:
		if (incp_convert_ping_inplace(bytes, len) != INCP_OK)
			return; //Ignore (unlikely to happen)

		bytes[-3] = I2C_OWN_ADDR;
		bytes[-2] = len;
		bytes[-1] = i3c_crc(sender, bytes-I3C_HEADER_SIZE, len+I3C_HEADER_SIZE);
		I2C1_transmit_master(sender, bytes-I3C_HEADER_SIZE, len+I3C_HEADER_SIZE);
		break;

	case incp_manifest_req:
		if (incp_encode_manifest_ret(buffer+3, INCP_LEN_MODULE_MANIFEST, NULL) != INCP_OK)
			return;

		buffer[0] = I2C_OWN_ADDR;
		buffer[1] = INCP_LEN_MODULE_MANIFEST;
		buffer[2] = i3c_crc(sender, buffer, 3+INCP_LEN_MODULE_MANIFEST);
		I2C1_transmit_master(sender, buffer, 3+INCP_LEN_MODULE_MANIFEST);
		break;

	case incp_data_req:
		handle_datareq(sender, msg.module_id, content, content_len, buffer);
		break;


	case incp_cmd_req:
		handle_cmdreq(sender, msg.module_id, content, content_len, buffer);
		break;

	case incp_data_set:
		//TODO:
		return;

	case incp_pong:
	case incp_manifest_ret:
	case incp_log_entry:
	case incp_bulk_xfer:
	case incp_event:
	case incp_data_ret:
	case incp_cmd_ret:
	default:
		// Ignore
		return;
	}
}

static int gb_time_boot = 420;
static int gb_time_real = 69;

static void handle_datareq(uint8_t sender, uint32_t module, uint8_t *in_bytes, uint32_t in_len, uint8_t *buffer)
{
	size_t len;

	//There is only one module (calc)
	if (module != CALC_ID)
	{
		HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_SET);
		return; //Ignore on bad module
	}

	Calc_data_req_t req_msg;
	Calc_data_ret_t ret_msg;
	if (incp_decode_calc_datareq(&req_msg, in_bytes, in_len) != INCP_OK)
		return; //Ignore

	switch (req_msg.data) {
	case req_calc_time_boot:
		ret_msg.kind = ret_calc_time_boot_PRESENT;
		ret_msg.u.ret_calc_time_boot = gb_time_boot++;
		break;

	case req_calc_time_real:
		ret_msg.kind = ret_calc_time_real_PRESENT;
		ret_msg.u.ret_calc_time_real = gb_time_real; gb_time_real += 2;
		break;

	case req_calc_test_str:
		ret_msg.kind = ret_calc_test_str_PRESENT;
		memcpy(&ret_msg.u.ret_calc_test_str, "test", 5);
		break;

	case req_calc_test_enum:
		ret_msg.kind = ret_calc_test_enum_PRESENT;
		ret_msg.u.ret_calc_test_enum = calc_enum_a_a;
		break;

	//TODO: Report
	default:
		return; //Ignore
	}

	incp_encode_calc_dataret(&ret_msg, buffer+3, BUFSIZ-3, &len);
	buffer[0] = I2C_OWN_ADDR;
	buffer[1] = len;
	buffer[2] = i3c_crc(sender, buffer, len+3);
	I2C1_transmit_master(sender, buffer, (uint8_t)(len+3));
}

static void handle_cmdreq(uint8_t sender, uint32_t module, uint8_t *in_bytes, uint32_t in_len, uint8_t *buffer)
{
	size_t len;

	//There is only one module (calc)
	if (module != CALC_ID)
	{
		HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_SET);
		return; //Ignore on bad module
	}

	Calc_cmd_req_t req_msg;
	Calc_cmd_ret_t ret_msg;
	if (incp_decode_calc_cmdreq(&req_msg, in_bytes, in_len) != INCP_OK)
		return; //Ignore

	switch (req_msg.kind)
	{
	case req_calc_add_PRESENT:
		ret_msg.kind = ret_calc_add_PRESENT;
		ret_msg.u.ret_calc_add.val = req_msg.u.req_calc_add.left + req_msg.u.req_calc_add.right;
		break;

	case req_calc_div_PRESENT:
		ret_msg.kind = ret_calc_div_PRESENT;
		if ((ret_msg.u.ret_calc_div.err = (req_msg.u.req_calc_div.right == 0)))
			ret_msg.u.ret_calc_div.val = 0;
		else
			ret_msg.u.ret_calc_div.val = req_msg.u.req_calc_div.left / req_msg.u.req_calc_div.right;
		break;

	default:
		//Ignore
		return;
	}

	incp_encode_calc_cmdret(&ret_msg, buffer+3, BUFSIZ-3, &len);
	buffer[0] = I2C_OWN_ADDR;
	buffer[1] = len;
	buffer[2] = i3c_crc(sender, buffer, len+3);
	I2C1_transmit_master(sender, buffer, (uint8_t)(len+3));
}