#include <stdio.h>
#include "bsp.h"
#include "can.h"
#include "libs/utils.h"
#include "os/queue.h"

#define CAN_RX_MESSAGE_RX_ID 1
#define CAN_SEND_QUEUE_SIZE 32
#define RX_ID_OFFSET 16
#define CAN_SEND_OK 0
#define CAN_SEND_ERROR -1
#define CAN_SEND_WAIT_TIMEOUT -2


static co_queue_t tx_queue;
static co_queue_t rx_queue;
static int shark_send_can0_to_fifo(can_trasnmit_message_struct *P_message);
static int shark_send_can0_data(can_trasnmit_message_struct *P_message);
static void _can_tx_task(void *args);
static void _can_rx_task(void *args);
/* this function can be overide by app, which need recv the can frame */
__weak void handle_can_frame(can_id_t id, uint8_t *data, int len){

}

static void _can_rx_task(void *args){
	can_receive_message_struct message;
	while(1) {
		if (queue_get(rx_queue, &message)) {
			can_id_t can_id;
			can_id.id = message.rx_efid;
			handle_can_frame(can_id, message.rx_data, message.rx_dlen);		
		}
		co_task_yield();
	}
}

static __inline__ void can_fifo_recv(int fifo){
	can_receive_message_struct message;

	can_message_receive(CAN0, fifo, &message);  

	queue_put(rx_queue, &message);
}

void USBD_LP_CAN0_RX0_IRQHandler(void)
{
	can_fifo_recv(CAN_FIFO0);
}

void CAN0_RX1_IRQHandler(void)
{
	can_fifo_recv(CAN_FIFO1);
}

static void shark_can0_txrx_pin_config(void){
    /* enable can clock */
    rcu_periph_clock_enable(RCU_CAN0);
    rcu_periph_clock_enable(RCU_GPIOA);	
    rcu_periph_clock_enable(RCU_AF);

	//gpio_pin_remap_config(GPIO_CAN_FULL_REMAP,DISABLE);
	//gpio_pin_remap_config(GPIO_CAN_PARTIAL_REMAP,ENABLE);
	
    gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12);
	gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_11); 
}


static void shark_can0_config(void)
{
    can_parameter_struct can_parameter;
     
    can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
    /* initialize CAN register */
    can_deinit(CAN0);
    //can_deinit(CAN1);
    
    /* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = ENABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.rec_fifo_overwrite = DISABLE;
	can_parameter.trans_fifo_order = ENABLE;
	can_parameter.no_auto_retrans = ENABLE;
#if CAN0_SPEED==250 //250k bps
	if (SystemCoreClock == 120000000) {
		can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
		can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
		can_parameter.time_segment_2 = CAN_BT_BS2_4TQ;
	} else {
		can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
		can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
		can_parameter.time_segment_2 = CAN_BT_BS2_3TQ;
	}
#endif
	can_parameter.working_mode = CAN_NORMAL_MODE;
	can_parameter.prescaler = 24;
    /* initialize CAN */
    can_init(CAN0, &can_parameter);
    
    /* recv my can ID, use fifo0 */
    can_filter_mask_mode_init(CAN_MY_ADDRESS, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO0, 0);

  	nvic_priority_group_set(NVIC_PRIGROUP_PRE4_SUB0);  
  	nvic_irq_enable(USBD_LP_CAN0_RX0_IRQn,2,0);	
    /* enable can receive FIFO0 not empty interrupt */
    can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
	/* recv broadcast, fifo1 */
	can_filter_mask_mode_init(CAN_NODE_ADDR_FF, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO1, 1);
	/* recv ccu aux fifo2*/
	can_filter_mask_mode_init(CAN_NODE_ADDR_CCU_AUX, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO1, 2);
	/* recv ble fifo3*/
	can_filter_mask_mode_init(CAN_NODE_ADDR_BLE, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO1, 3);
	
	can_filter_mask_mode_init(CAN_NODE_ADDR_ACU, CAN_FILTER_DEST_MASK, CAN_EXTENDED_FIFO1, 4);
	
	nvic_irq_enable(CAN0_RX1_IRQn,10,0);	
    /* enable can receive FIFO1 not empty interrupt */
    can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE1);
}


static int shark_send_can0_data(can_trasnmit_message_struct *P_message){
	if (queue_put(tx_queue, P_message)){
		return CAN_SEND_OK;
	}
	return CAN_SEND_ERROR;
}

static void _can_tx_task(void *args){
	can_trasnmit_message_struct trasnmit_msg;
	while(1) {
		if (queue_get(tx_queue, &trasnmit_msg)) {
			shark_send_can0_to_fifo(&trasnmit_msg);
		}
		co_task_yield();
	}
}

/*when system scheduler is not start or stoped, we MUST not use FreeRTOS 
system api which can cause sleep,context switch */
static int shark_send_can0_to_fifo(can_trasnmit_message_struct *P_message){
	uint8_t mailbox_number = CAN_NOMAILBOX;
	uint32_t retry_count = 20000;
	if (CAN_ERR(CAN0) & CAN_ERR_BOERR){
		shark_can0_reset();
	}
	do {
		mailbox_number = can_message_transmit(CAN0, P_message);
		if (mailbox_number < CAN_NOMAILBOX){
			return CAN_SEND_OK;
		}
	}while(retry_count-- > 0); //wait sender fifo empty

	shark_can0_reset();	
	return CAN_SEND_ERROR;
}

int shark_can0_send_message(uint32_t can_id, const void*buff, int len){
	can_trasnmit_message_struct trasnmit_msg;
	can_id_t can_frame_id;
	u32 total_frames = ((len + 7) >> 3);
	int send_len = len;
	u32 frame_id = 1;
	can_frame_id.id = can_id;
	can_frame_id.total = total_frames;

	while(send_len > 0){
		can_frame_id.idx = frame_id;

		trasnmit_msg.tx_sfid	= 0;
		trasnmit_msg.tx_efid	= can_frame_id.id;				
		trasnmit_msg.tx_ft	= CAN_FT_DATA;
		trasnmit_msg.tx_ff	= CAN_FF_EXTENDED;
		trasnmit_msg.tx_dlen = min(CAN_DATA_SIZE,send_len);
		memcpy((char *)trasnmit_msg.tx_data, (char *)buff + (len - send_len), trasnmit_msg.tx_dlen);
		send_len -= trasnmit_msg.tx_dlen;
		frame_id ++;
		if (shark_send_can0_data(&trasnmit_msg) != CAN_SEND_OK){
			return CAN_SEND_ERROR;
		}
	}

	return CAN_SEND_OK;

}


void shark_can0_reset(void){
	shark_can0_txrx_pin_config();
	shark_can0_config();
}

void shark_can0_deinit(void){
	can_deinit(CAN0);
	rcu_periph_clock_disable(RCU_CAN0);
	gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_2MHZ, GPIO_PIN_9);  
}

void shark_can0_init(void){
	tx_queue = queue_create(64, sizeof(can_trasnmit_message_struct));
	rx_queue = queue_create(32, sizeof(can_trasnmit_message_struct));
	co_task_create(_can_tx_task, NULL, 512);
	co_task_create(_can_rx_task, NULL, 512);
	shark_can0_txrx_pin_config();
	shark_can0_config();
}