#include "Can.h"
#include "gd32c10x_can.h"
#include "gd32c10x_eval.h"
#include <cstdio>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>

#define DEV_CAN_LOOPBACK_ID 0x0000
#define DEV_CAN0_ID 0xaabb
#define DEV_CAN0_MASK 0x0000

u8 can_gpio_init() {
	/* enable CAN clock */
	rcu_periph_clock_enable(RCU_CAN0);
	rcu_periph_clock_enable(RCU_GPIOA);
	rcu_periph_clock_enable(RCU_AF);
	/* configure CAN0 GPIO */
	gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_MAX, GPIO_PIN_11);
	gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_MAX, GPIO_PIN_12);

	/* enable the EN GPIO clock */
	rcu_periph_clock_enable(RCU_GPIOB);
	gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13);
	gpio_bit_reset(GPIOB, GPIO_PIN_13);
	return 0;
}

/*!
	\brief      初始化CAN0 2.0B 模式
	\param[in]  none
	\param[out] none
	\retval     none
*/
void can_config(void) {
	can_parameter_struct can_parameter;
    can_filter_parameter_struct can_filter;

	can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
	/* initialize CAN register */
	can_deinit(CAN0);

	/* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = DISABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.auto_retrans = DISABLE;
	can_parameter.rec_fifo_overwrite = DISABLE;
	can_parameter.trans_fifo_order = DISABLE;
	can_parameter.working_mode = CAN_NORMAL_MODE;
	/* configure CAN0 baud rate 1MBps, sample point at 80% */
	can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
	can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
	can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
	can_parameter.prescaler = 12;
	/* initialize CAN */
	can_init(CAN0, &can_parameter);

   /* initialize filter */
    /* configure filter mode */
    can_filter.filter_mode = CAN_FILTERMODE_MASK;
    can_filter.filter_bits = CAN_FILTERBITS_32BIT;
    /* configure filter ID */
    can_filter.filter_list_high = (uint16_t)(DEV_CAN_LOOPBACK_ID >> 13U);
    can_filter.filter_list_low = ((uint16_t)(DEV_CAN_LOOPBACK_ID)) << 3U | (1U << 2);
    /* configure filter mask */
    can_filter.filter_mask_high = 0x0000;
    can_filter.filter_mask_low = 0x0000;
    /* select receiver fifo */
    can_filter.filter_fifo_number = CAN_FIFO0;
    can_filter.filter_enable = ENABLE;
    can_filter_init(&can_filter);
	
	/* configure CAN0 NVIC */
	nvic_irq_enable(CAN0_RX0_IRQn, 0, 0);
	/* enable can receive FIFO0 not empty interrupt */
	can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);


}

/*!
	\brief      初始化CAN0 CANFD模式
	\param[in]  none
	\param[out] none
	\retval     none
*/
void canfd_config(void) {
	can_parameter_struct can_parameter;
	can_fdframe_struct can_fd_parameter;
	can_fd_tdc_struct can_fd_tdc_parameter;
    can_filter_parameter_struct can_filter;

    /* enable CAN clock */
    rcu_periph_clock_enable(RCU_CAN0);

    /* initialize CAN register */
    can_deinit(CAN0);

	/* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = DISABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.auto_retrans = ENABLE;
	can_parameter.rec_fifo_overwrite = ENABLE;
	can_parameter.trans_fifo_order = ENABLE;
	can_parameter.working_mode = CAN_NORMAL_MODE;
	/* baudrate 1Mbps, sample piont at 80% */
	can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
	can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
	can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
	can_parameter.prescaler = 12;
	/* initialize CAN */
	can_init(CAN0, &can_parameter);

	can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter);
	can_fd_parameter.fd_frame = ENABLE;
	can_fd_parameter.excp_event_detect = ENABLE;
	can_fd_parameter.delay_compensation = ENABLE;
	can_fd_tdc_parameter.tdc_filter = 0x04;
	can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET;
	can_fd_tdc_parameter.tdc_offset = 0x04;
	can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter;
	can_fd_parameter.iso_bosch = CAN_FDMOD_ISO;
	can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE;
	/* FD mode data baudrate 2Mbps, sample piont at 80% */
	can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ;
	can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ;
	can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ;
	/* CAN-FD data segement prescaler should be the same as non-data segement
	 * prescaler */
	can_fd_parameter.data_prescaler = 12;
	can_fd_init(CAN0, &can_fd_parameter);

    /* initialize filter */
    can1_filter_start_bank(14);
    can_filter_mask_mode_init(DEV_CAN0_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0);

    /* configure CAN0 NVIC */
    nvic_irq_enable(CAN0_RX0_IRQn, 0, 0);
    /* enable can receive FIFO0 not empty interrupt */
    can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
}

/*!
	\brief      初始化CAN0回环测试
	\param[in]  none
	\param[out] none
	\retval     none
*/
void can_loopback_init(void) {
	can_parameter_struct can_parameter;
	can_fdframe_struct can_fd_parameter;
	can_fd_tdc_struct can_fd_tdc_parameter;
    can_filter_parameter_struct can_filter;

    /* enable CAN clock */
    rcu_periph_clock_enable(RCU_CAN0);

    /* initialize CAN register */
    can_deinit(CAN0);

	/* initialize CAN parameters */
	can_parameter.time_triggered = DISABLE;
	can_parameter.auto_bus_off_recovery = DISABLE;
	can_parameter.auto_wake_up = DISABLE;
	can_parameter.auto_retrans = ENABLE;
	can_parameter.rec_fifo_overwrite = ENABLE;
	can_parameter.trans_fifo_order = ENABLE;
	can_parameter.working_mode = CAN_LOOPBACK_MODE;
	/* baudrate 1Mbps, sample piont at 80% */
	can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
	can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
	can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
	can_parameter.prescaler = 12;
	/* initialize CAN */
	can_init(CAN0, &can_parameter);

    can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter);
	can_fd_parameter.fd_frame = ENABLE;
	can_fd_parameter.excp_event_detect = ENABLE;
	can_fd_parameter.delay_compensation = ENABLE;
	can_fd_tdc_parameter.tdc_filter = 0x04;
	can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET;
	can_fd_tdc_parameter.tdc_offset = 0x04;
	can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter;
	can_fd_parameter.iso_bosch = CAN_FDMOD_ISO;
	can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE;
	/* FD mode data baudrate 2Mbps, sample piont at 80% */
	can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ;
	can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ;
	can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ;
	/* CAN-FD data segement prescaler should be the same as non-data segement
	 * prescaler */
	can_fd_parameter.data_prescaler = 12;
	can_fd_init(CAN0, &can_fd_parameter);

    /* initialize filter */
    can1_filter_start_bank(14);
    can_filter_mask_mode_init(DEV_CAN_LOOPBACK_ID, DEV_CAN0_MASK, CAN_EXTENDED_FIFO0, 0);
}

/*!
	\brief      CAN0 loopback test
	\param[in]  none
	\param[out] none
	\retval     ErrStatus
*/
void can_loopback_test(void) {
    can_trasnmit_message_struct transmit_message;
    can_receive_message_struct  receive_message;
    uint32_t timeout = 0xFFFF;
    uint8_t transmit_mailbox = 0;


    can_struct_para_init(CAN_FD_FRAME_STRUCT, &transmit_message);
    transmit_message.tx_sfid = DEV_CAN_LOOPBACK_ID;
    transmit_message.tx_efid = 0x00;
    transmit_message.tx_ft = CAN_FT_DATA;
    transmit_message.tx_ff = CAN_FF_EXTENDED;
    transmit_message.tx_dlen = 12;
    transmit_message.fd_flag = 1;
    transmit_message.fd_brs = 1;
    transmit_message.fd_esi = 0;

	int i = 0;
	while (i++<transmit_message.tx_dlen) {
		transmit_message.tx_data[i] = i;
	}


    /* transmit message */
    transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
    /* waiting for transmit completed */
    while((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) && (0 != timeout)) {
        timeout--;
    }
    timeout = 0xFFFF;
    /* waiting for receive completed */
    while((can_receive_message_length_get(CAN0, CAN_FIFO0) < 1) && (0 != timeout)) {
        timeout--;
    }
    can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
    can_message_receive(CAN0, CAN_FIFO0, &receive_message);

    /* check the receive message */
    if((transmit_message.tx_sfid == receive_message.rx_sfid) && (transmit_message.tx_ff == receive_message.rx_ff)
            && (transmit_message.tx_dlen == receive_message.rx_dlen) ) {
        /* loopback test is success */
		std::printf("CAN LO0PBACK SUCCESS\r\n");
    } else {
        /* loopback test is failed */
		std::printf("CAN LO0PBACK FAIL\r\n");
    }
}

/*!
	\brief CAN_transmiter构造函数
	\note  初始化CAN通信相关参数和硬件配置
*/
CAN_transmiter::CAN_transmiter() {
	// 初始化成员变量
	controller_Mode = 0;
	limit_velocity_union.f = 0;
	limit_voltage = 0;
	mcu_temp = 0;
	motor_temp = 0;
	Power_Voltage = 0;
	rt_angle_union.f = 0;
	Status_code = 0;
	target_angle_union.f = 0;
	Target_Control_Mode = 0;
	target_velocity_union.f = 0;
	u_current_union.f = 0;
	w_current_union.f = 0;
	v_current_union.f = 0;

	// 初始化CAN硬件
	can_gpio_init();
	can_config();

	printf("CAN_transmiter initialized successfully\r\n");

	transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
    timeout = 0xFFFF;	// 默认超时时间 0xFFFF

    // 初始化发送消息结构体
    can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
}

/*!
	\brief CAN 数据更新
*/
void CAN_transmiter::update(void) {
	limit_velocity_union.f = 0;
	limit_voltage = 0;
	mcu_temp = get_mcu_temp();
	motor_temp = get_drv_temp();
	Power_Voltage = get_vbus_voltage();
	rt_angle_union.f = 0;
	rt_velocity_union.f = 0;
	Status_code = (gpio_input_bit_get(GPIOB, GPIO_PIN_2) == SET) ? 1 : 0;
	target_angle_union.f = 0;
	Target_Control_Mode = 0;
	target_velocity_union.f = 0;
	u_current_union.f = get_current('U');
	w_current_union.f = get_current('W');
	v_current_union.f = get_current('V');
}

/*!
	\brief CAN 数据发送
*/
u8 CAN_transmiter::CAN_Send(u32 id, u8 length, u8 *data) {
    // 检查数据长度是否符合CAN规范
    if (length > 8) {
        printf("CAN data length exceeds maximum of 8 bytes\r\n");
        return 0;
    }

    // 检查数据指针是否有效
    if (data == nullptr) {
        printf("Invalid data pointer\r\n");
        return 0;
    }

    /* 配置发送消息参数 */
    transmit_message.tx_ft = CAN_FT_DATA;        // 数据帧
    transmit_message.tx_ff = CAN_FF_EXTENDED;    // 扩展帧格式
    transmit_message.tx_sfid = id;               // 设置帧ID
    transmit_message.tx_dlen = length;           // 设置数据长度

    // 复制数据到发送缓冲区
    for (int i = 0; i < length && i < 8; i++) {
        transmit_message.tx_data[i] = *(data + i);
    }

    // 自动选择空闲mailbox发送消息，不需要人为指定
    transmit_mailbox = can_message_transmit(CAN0, &transmit_message);

    // 检查是否成功获取mailbox
    if (CAN_NOMAILBOX == transmit_mailbox) {
        // 没有可用的mailbox，可能所有mailbox都在使用中
        printf("No available mailbox for CAN transmission\r\n");
        return 0;
    }

    // 等待发送完成，使用自动选择的mailbox进行状态检查
    while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) &&
           (0 != timeout)) {
        timeout--;
    }

    // 超时检查
    if (0 == timeout) {
        printf("CAN transmission timeout for mailbox %d\r\n", transmit_mailbox);
		return 0;
    }
	return 1;
}


/*!
	\brief CANFD 数据发送
*/
u8 CAN_transmiter::CANFD_Send(u32 id, u8 length, u8 *data) {
    // 检查CANFD模式是否启用
    if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
        printf("CANFD mode not enabled\r\n");
        return 0;
    }

	// 检查数据指针是否有效
    if (data == nullptr) {
        printf("Invalid data pointer\r\n");
        return 0;
    }

    // 检查数据长度是否符合CANFD规范
    if (length > 64) {
        printf("CANFD data length exceeds maximum of 64 bytes\r\n");
        return 0;
    }

    /* 配置发送消息参数 */
    transmit_message.tx_ft = CAN_FT_DATA;        // 数据帧
    transmit_message.tx_ff = CAN_FF_EXTENDED;    // 扩展帧格式
    transmit_message.tx_sfid = id;               // 设置帧ID
    transmit_message.tx_dlen = length;           // 设置数据长度

    // 设置CANFD特定参数
    transmit_message.fd_flag = CAN_FDF_FDFRAME;  // CANFD帧格式
    transmit_message.fd_brs = ENABLE;            // 使能比特率切换
    transmit_message.fd_esi = DISABLE;           // 错误状态指示

    // 复制数据到发送缓冲区
    for (int i = 0; i < length && i < 64; i++) {
        transmit_message.tx_data[i] = *(data + i);
    }

    // 自动选择空闲mailbox发送消息，不需要人为指定
    transmit_mailbox = can_message_transmit(CAN0, &transmit_message);

    // 检查是否成功获取mailbox
    if (CAN_NOMAILBOX == transmit_mailbox) {
        // 没有可用的mailbox，可能所有mailbox都在使用中
        printf("No available mailbox for CANFD transmission\r\n");
        return 0;
    }

    // 等待发送完成，使用自动选择的mailbox进行状态检查
    while ((CAN_TRANSMIT_OK != can_transmit_states(CAN0, transmit_mailbox)) &&
           (0 != timeout)) {
        timeout--;
    }

    // 超时检查
    if (0 == timeout) {
        printf("CANFD transmission timeout for mailbox %d\r\n", transmit_mailbox);
		return 0;
    }
	return 1;
}


/**
 * @brief  CAN发送函数
 * @retval 0: 发送失败
 *         1: 发送成功
 */
u8 CAN_transmiter::CAN_all_Send(){
	u32 success_flag = 0;

	constexpr u32 RT_Vel_Ang_FEED = 0x101;
	constexpr u32 SET_Vel_Ang_FEED = 0x110;
	constexpr u32 Current_FEED = 0x117;
	constexpr u32 Status_FEED = 0x118;
	constexpr u32 Limit_Vel_Vol_FEED = 0x119;

	//打包RT_Vel_Ang_FEED
	pack_float_to_bytes(transmit_data, 0, rt_angle_union);
	pack_float_to_bytes(transmit_data, 4, rt_velocity_union);
	//打包SET_Vel_Ang_FEED
	pack_float_to_bytes(transmit_data, 0, target_angle_union);
	pack_float_to_bytes(transmit_data, 4, target_velocity_union);
	//打包Current_FEED
	pack_float_to_bytes(transmit_data, 0, u_current_union);
	pack_float_to_bytes(transmit_data, 4, w_current_union);
	pack_float_to_bytes(transmit_data, 8, v_current_union);
	//打包Status_FEED
	transmit_data[0] = Status_code;
	transmit_data[1] = controller_Mode;
	pack_short_to_bytes(transmit_data, 2, motor_temp);
	pack_short_to_bytes(transmit_data, 4, mcu_temp);
	pack_short_to_bytes(transmit_data, 6, Power_Voltage);
	//打包Limit_Vel_Vol_FEED
	pack_float_to_bytes(transmit_data, 0, limit_velocity_union);
	pack_short_to_bytes(transmit_data, 4, limit_voltage);
	return success_flag;
}

/**
 * @brief  CANFD发送函数
 * @retval 0: 发送失败
 *         1: 发送成功
 */
u8 CAN_transmiter::CANFD_all_Send(){
	u32 success_flag = 0;

	constexpr u32 STATUS_ID = 0x18FF100;
	constexpr u32 CONFIG_ID = 0x18FF101;

	// 打包status数据
	pack_short_to_bytes(transmit_data, 0, mcu_temp);
	pack_short_to_bytes(transmit_data, 2, motor_temp);
	pack_float_to_bytes(transmit_data, 4, u_current_union);
	pack_float_to_bytes(transmit_data, 8, w_current_union);
	pack_float_to_bytes(transmit_data, 12, v_current_union);
	transmit_data[16] = Status_code;
	pack_u16_to_bytes(transmit_data, 17, Power_Voltage);
	pack_float_to_bytes(transmit_data, 19, rt_angle_union);
	pack_float_to_bytes(transmit_data, 23, rt_velocity_union);

	success_flag += CANFD_Send(STATUS_ID, 27, transmit_data);

	// 打包config数据
	transmit_data[0] = controller_Mode;
	pack_u16_to_bytes(transmit_data, 1, limit_voltage);
	pack_float_to_bytes(transmit_data, 3, limit_velocity_union);
	pack_float_to_bytes(transmit_data, 7, target_angle_union);
	pack_float_to_bytes(transmit_data, 11, target_velocity_union);

	success_flag += CANFD_Send(CONFIG_ID, 15, transmit_data);

	return success_flag;
}