#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);

#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);
}

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;

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

	switch (msg.kind)
	{
	case Incp_header_t_kind_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_header_t_kind_manifest_req:
	case Incp_header_t_kind_data_req:
	case Incp_header_t_kind_data_set:
	case Incp_header_t_kind_cmd_req:
		//TODO:
		return;

	case Incp_header_t_kind_pong:
	case Incp_header_t_kind_manifest_ret:
	case Incp_header_t_kind_log_entry:
	case Incp_header_t_kind_bulk_xfer:
	case Incp_header_t_kind_event:
	case Incp_header_t_kind_data_ret:
	case Incp_header_t_kind_cmd_ret:
	default:
		// Ignore
		return;
	}
}