#include "bsp/bsp_driver.h"
#include "can_pc_message.h"
#include "libs/logger.h"
#include "os/os_task.h"
#include "foc/motor/motor.h"

bool can_process_iap_message(can_message_t *can_message) {
	uint8_t response[24];
	uint8_t rsplen;
	encoder_can_key(response, can_message->key);
	response[2] = 0;
	rsplen = 3;
	switch(can_message->key) {
		case BUILD_CMD_KEY(0xF0):
			if (PMSM_FOC_GetSpeed() > 10.0f) {
				response[2] = 1;
				iap_send_data(can_message->src, response, rsplen);
				return true;
			}
			fmc_write_magic(0xFFFFFFFF);
			iap_send_data(can_message->src, response, rsplen);
			delay_ms(100);
			system_reboot();
			break;
		case BUILD_CMD_KEY(0xF5):
			memcpy(response + rsplen, can_message->data, can_message->len);
			rsplen += can_message->len;
			break;
		case BUILD_CMD_KEY(0xF9):
			fmc_write_magic(IAP_MAGIC_SUCCESS);
			break;
		case BUILD_CMD_KEY(0xF6):
			if (can_message->len > 0 && can_message->data[0] == 0x01) {
				rsplen += mcu_chip_id(response + 4) + 1;
				response[3] = 0x00;
			}
			break;
		case BUILD_CMD_KEY(0xF8):
			iap_read_string(can_message->src, can_message->data);
			rsplen = 0;
			break;
		default:
			return false;
	}
	if (rsplen > 0) {
		iap_send_data(can_message->src, response, rsplen);
	}
	return true;
}


uint32_t iap_read_magic(void){
	return fmc_read_magic();
}


void iap_read_string(uint8_t can, const uint8_t *args)
{
	#define buff_len 256
	uint8_t buff[buff_len];
	const char *text;
	uint32_t addr;
	uint32_t len;

	addr = ((uint32_t) args[2]) << 16 | ((uint32_t) args[1]) << 8 | args[0];
	text = (const char *) (0x08000000 + addr);

	len = strlen(text);
	if (len > buff_len) {
		len = buff_len;
	}

	buff[0] = 0xF8;
	buff[1] = CAN_MY_ADDRESS;
	buff[2] = 0x00;
	memcpy(buff + 3, args, 3);
	memcpy(buff + 6, text, len);

	iap_send_data(can, buff, len + 6);
}


void iap_send_data(uint8_t can, uint8_t *data, int len){
	can_id_t frame_id;

	frame_id.id=0;
	frame_id.src 	= CAN_MY_ADDRESS;
	frame_id.dest = can;
	frame_id.type 		= ptype_response;
	
	shark_can0_send_message(frame_id.id, data, len);
}