#include "bsp/bsp.h"
#include "os/co_task.h"
#include "libs/logger.h"
#include "libs/utils.h"
#include "can_message.h"
#include "wait_queue.h"
#include "can_pc_message.h"
#include "foc/commands.h"
static void can_process_message(can_message_t *message);
static void free_can_message(can_message_t *message);

#define MAX_CAN_MESSAGE 10
static can_message_t messages[MAX_CAN_MESSAGE];
static wait_queue_t wait_queue;

void can_message_init(void){
	wait_queue_init(&wait_queue, 32);
	shark_can0_init();
}

static can_message_t *get_message_by_id(can_id_t id){
	can_message_t *idle_msg = NULL;
	can_message_t *src_msg = NULL;
	for (int i = 0; i < MAX_CAN_MESSAGE; i++){
		/*first found the same src&dest */
		if ((messages[i].src == id.src) && (messages[i].dest == id.dest)){
			return (messages + i);
		}
		if (messages[i].src == id.src){
			src_msg = messages + i;
		}else if (messages[i].data == NULL){
			idle_msg = messages + i;
		}
	}
	return ((src_msg!=NULL)?src_msg:idle_msg);
}


s32 can_send_message(uint32_t can_id, u8 *data, int len, s32 timeout){
	can_id_t id;
	id.id = can_id;
	if (id.type != ptype_request || id.retry <= 1) {
		return shark_can0_send_message(can_id, data, len) == 0?CAN_SEND_SUCCESS:CAN_SEND_ERROR;
	}
	s32 s_ret = CAN_SEND_NO_WAIT_QUEUE;
	s32 index = wait_queue_add(&wait_queue, decoder_can_key(data));
	if (index >= 0){
		s_ret = CAN_SEND_TIMEOUT;
		s32 retry = id.retry;
		while(retry -- > 0) {
			s_ret = shark_can0_send_message(can_id, data, len) == 0?CAN_SEND_SUCCESS:CAN_SEND_ERROR;
			if (s_ret != CAN_SEND_SUCCESS){
				continue;
			}
			s_ret = wait_queue_wait_key(&wait_queue, index, timeout)?CAN_SEND_SUCCESS:CAN_SEND_TIMEOUT;
			if (s_ret == CAN_SEND_SUCCESS){
				break;
			}
		}
		wait_queue_delete_key(&wait_queue, index);
	}
	return s_ret;

}

void handle_can_frame(can_id_t id, uint8_t *data, int len){
	can_message_t *message = get_message_by_id(id);
	if (message == NULL) {
		return ;
	}
	uint16_t key = 0;

	int total = id.total?id.total:32;
	int idx = id.idx?id.idx:32;

	if((total <= CAN_MESSAGE_MAX_FRAMES) && (idx <= CAN_MESSAGE_MAX_FRAMES)
			&& (idx <= total)){
		if ((idx == 1) && (len >= 2)) { //first frame for message
			key = decoder_can_key(data);
			if (message->data) {
				co_free(message->data);
			}

			message->key = key;
			message->dest = id.dest;
			message->src = id.src;
			message->data = co_malloc(total * CAN_DLC_LENGTH);
			message->len = 0;
			message->idx = idx;
			message->type= id.type;
			message->total_frame = total;
			len = len - 2; //skip key
			data = data + 2;
		}
		if (message->data){
			if ((message->idx == idx) && (message->total_frame == total)){
				memcpy(message->data + message->len, data, len);
				message->len += len;
				if (idx == total) { //last frame
					if (message->type == ptype_response) {
						wait_queue_key_acked(&wait_queue, message->key);
					}
					can_process_message(message);
				}
				message->idx = (idx + 1);
			}else {
				free_can_message(message);
			}
		}
	}
}


static void can_process_message(can_message_t *message){
	sys_debug("can %x [%x -> %x], len = %d\n", message->key, message->src, message->dest, message->len);
	if ((message->key & 0xFF) >= 0xF0) {
		can_process_iap_message(message);
	}else{ //只处理后控的指令
		if ((message->key & 0xFF) < Foc_Cmd_Max){
			foc_cmd_body_t command;
			command.cmd = (foc_cmd_t)(message->key & 0xFF);
			command.can_src = message->src;
			command.data = co_malloc(message->len);
			if (command.data) {
				memcpy(command.data, message->data, message->len);
				foc_send_command(&command);
			}
		}
	}
	free_can_message(message);
}

static void free_can_message(can_message_t *message){
	if (message->data) {
		co_free(message->data);
	}
	message->data = NULL;
	message->src = 0;
	message->len = 0;
	message->idx = 0xFF;
	message->total_frame = 0xFF;
}

void can_send_response(uint8_t can_addr, uint8_t *data, int len){
	can_send_message(get_reponse_can_id(can_addr), data, len, 50);
}

void can_send_ack(uint8_t can_addr, uint16_t key, uint8_t data){
	can_message_t message;
	message.src = can_addr;
	message.key = key;
	can_send_message_ack(&message, data);
}

void can_send_message_ack(can_message_t *msg, uint8_t success){
	u8 response[3];
	encoder_can_key(response, msg->key);
	response[2] = success;
	can_send_message(get_reponse_can_id(msg->src), response, 3, 50);
}


void can_send_indicator(uint8_t can_add, uint8_t *data, int len){
	can_send_message(get_indicator_can_id(can_add), data, len, 50);
}

void can_send_request(uint8_t can_add, uint8_t *data, int len){
	can_send_message(get_request_can_id(can_add,3), data, len, 300);
}