#include "bsp/bsp.h"
#include "libs/os.h"
#include "libs/logger.h"
#include "libs/task.h"
#include "can_message.h"

#define MAX_CAN_MESSAGE 4
static can_message_t messages[MAX_CAN_MESSAGE];
static msg_list_t msg_list[10];

static node_list_t _pending_list = LIST_HEAD_INIT(_pending_list);
static node_list_t _idle_list = LIST_HEAD_INIT(_idle_list);

static void can_process_message(can_message_t *message);
static void free_can_message(can_message_t *message);
static u32 _can_poll_task(void);
static void handle_can_frame(void);

void can_message_init(void){
	for(int i = 0; i < ARRAY_SIZE(msg_list); i++) {
		init_list_node(&msg_list[i].list);
		list_add_tail(&_idle_list, &msg_list[i].list);
	}
	shark_can0_init();
	task_start(_can_poll_task, 0);
}

void put_raw_message(can_id_t id, uint8_t *data, int len) {
	if (list_empty(&_idle_list)){
		return;
	}
	msg_list_t *raw = (msg_list_t *)_idle_list.next;
	list_del(&raw->list);
	raw->msg.id = id;
	raw->msg.len = len;
	memcpy(raw->msg.data, data, len);
	list_add_tail(&_pending_list, &raw->list);
}

static msg_list_t *_get_raw_message(void) {
	if (list_empty(&_pending_list)){
		return NULL;
	}
	msg_list_t *raw = (msg_list_t *)_pending_list.next;
	list_del(&raw->list);
	return raw;
}
static void free_raw_message(msg_list_t *raw) {
	list_add_tail(&_idle_list, &raw->list);
}

static u32 _can_poll_task(void) {
	handle_can_frame();
	return 0;
}


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){
	return shark_can0_send_message(can_id, data, len);
}

static void handle_can_frame(void){
	msg_list_t *raw = _get_raw_message();
	if (raw == NULL) {
		return;
	}
	can_id_t id = raw->msg.id;
	uint8_t *data = raw->msg.data;
	int len = raw->msg.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);

			message->key = key;
			message->dest = id.dest;
			message->src = id.src;
			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
					can_process_message(message);
				}
				message->idx = (idx + 1);
			}else {
				free_can_message(message);
			}
		}
	}
	free_raw_message(raw);
}


static void can_process_message(can_message_t *message){
	if (message->key & 0xFF >= 0xF0) {
		//do iap update
	}else if (message->src == 0x42){ //只处理后控的指令

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


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

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


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

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