#include "dev_can.h"
#include "core_cmFunc.h"
#include "dev_iap.h"
#include "hal_can.h"
#include "hal_conf.h"
#include "queue.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

double pow_branch(double x, long long N)
{
    double ans = 1.0;
    // 贡献的初始值为 x
    double x_contribute = x;
    // 在对 N 进行二进制拆分的同时计算答案
    while (N > 0)
    {
        if (N % 2 == 1)
        {
            // 如果 N 二进制表示的最低位为 1，那么需要计入贡献
            ans *= x_contribute;
        }
        // 将贡献不断地平方
        x_contribute *= x_contribute;
        // 舍弃 N 二进制表示的最低位，这样我们每次只要判断最低位即可
        N = N >> 1;
    }
    return ans;
}
double pow(double x, double N)
{
    return N >= 0 ? pow_branch(x, N) : 1.0 / pow_branch(x, -N);
}

uint8_t recv_can_id = 0;

static uint64_t base_data;

void data_bit_move(uint8_t start_bit, uint8_t bit_len, uint64_t data)
{
    uint64_t mask        = 0;
    uint64_t source_data = (uint64_t)data;
    mask                 = pow(2, bit_len) - 1;
    base_data |= (mask & source_data) << start_bit;
}

void product_array(uint8_t send_array[8])
{
    send_array[0] = (uint8_t)base_data;
    send_array[1] = (uint8_t)(base_data >> 8);
    send_array[2] = (uint8_t)(base_data >> 16);
    send_array[3] = (uint8_t)(base_data >> 24);
    send_array[4] = (uint8_t)(base_data >> 32);
    send_array[5] = (uint8_t)(base_data >> 40);
    send_array[6] = (uint8_t)(base_data >> 48);
    send_array[7] = (uint8_t)(base_data >> 56);
    base_data     = 0;
}

uint8_t push_can_message_to_queue(uint32_t id, uint8_t len, uint8_t *p_data)
{
    pdu_tag      response_msg;
    QUEUE_STATUS result_status;
    response_msg.id.r    = id;
    response_msg.reg.dlc = len;
    memcpy(&response_msg.data.u8_buf[0], p_data, len);

    if (id > 0x7FF)
    {
        response_msg.reg.ide = CAN_Id_Extended;
    }
    else
    {
        response_msg.reg.ide = CAN_Id_Standard;
    }

#if 1
    __disable_irq();
    result_status = en_queue(&can_tx_queue, response_msg);
    __enable_irq();

    if (result_status != Q_OK)
    {
        return 0;
    }
#else
    en_queue(can_tx, &response_msg, 1);
#endif
    return 1;
}

static uint8_t can_tx_frame(pdu_tag can_message)
{
    uint8_t result = CAN_TxStatus_NoMailBox;
    result         = hal_can_msg_tx(can_message.id.r, can_message.reg.ide, can_message.data.u8_buf, can_message.reg.dlc);
    return result;
}

// void can_tx_stop(CAN_TypeDef *can_perpiph, u8 mailbox_number)
// {
//     can_tx_mail_box_stop(can_perpiph, mailbox_number);
// }

// QUEUE_STATUS can_tx_process(p_can_queue_tag p_queue)
// {
//     u8 mailbox_number = 0;
//     u8 query_result   = CAN_TxStatus_NoMailBox;

//     if (queue_empty(p_queue))
//         return Q_OK;
//     mailbox_number = can_tx_frame(p_queue->can_message[p_queue->tail]);
//     if (mailbox_number == CAN_TxStatus_NoMailBox)
//         return Q_OK;
//     query_result = can_tx_mail_box_state(CAN1, mailbox_number);
//     if (query_result == CAN_TxStatus_Ok || query_result == CAN_TxStatus_Pending)
//     {
//         p_queue->tail = (p_queue->tail + 1) % MAX_QSIZE;
//         p_queue->count--;
//         return Q_OK;
//     }
//     return Q_ERR;
// }

void can_tx_callback(void)
{
    pdu_tag tx_data;
    // de_queue(can_tx, &tx_data);
    if (de_queue(&can_tx_queue, tx_data) == Q_OK) // 返回值为1代表读取成功
    {
        can_tx_frame(tx_data);
        CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE);
    }
    else
    {
        CAN_ITConfig(CAN1, CAN_IT_TME, DISABLE);
    }
}

void can_rx_callback(CanRxMsg rx_message)
{
    pdu_tag data;
    uint8_t ps;
    uint8_t pf;
    switch (rx_message.ExtId)
    {
    case 0x18011801:
        data.id.r    = 0x18011801;
        data.reg.dlc = rx_message.DLC;
        memcpy(&data.data.u8_buf[0], rx_message.Data, rx_message.DLC);
        en_queue(&can_rx_queue, data);
    case 0x18DFF4E1:
        data.id.r    = 0x18DFF4E1;
        data.reg.dlc = rx_message.DLC;
        memcpy(&data.data.u8_buf[0], rx_message.Data, rx_message.DLC);
        en_queue(&can_rx_queue, data);
    case 0x18011802:
        data.id.r    = 0x18011802;
        data.reg.dlc = rx_message.DLC;
        memcpy(&data.data.u8_buf[0], rx_message.Data, rx_message.DLC);
#if 1
        en_queue(&can_rx_queue, data);
#else
        en_queue(can_rx, &data, 1);
#endif
        break;
    default:
        pf = (rx_message.ExtId & CAN_PGN_PF) >> 16;
        if (pf >= 0xF0)
        {
            data.id.r   = rx_message.ExtId;
            recv_can_id = data.id.b.sa;
            memcpy(&data.data.u8_buf[0], rx_message.Data, rx_message.DLC);
#if 1
            en_queue(&can_rx_queue, data);
#else
            en_queue(can_rx, &data, 1);
#endif
        }
        else
        {
            // ps = (rx_message.ExtId & CAN_PGN_PS) >> 8;
            // if (ps == 0xF4)
            // {
            // }
#if 1
            en_queue(&can_rx_queue, data);
#else
            en_queue(can_rx, &data, 1);
#endif
        }
    }
}
uint8_t tx_flag = 0;

void can_rx_ctl(pdu_tag rec_msg)
{
    // rec_msg.data.u8_buf[0];
    // LED2    = !LED2;
    tx_flag = 1;
}
uint8_t tx_data[8] = {0};
uint8_t flag       = 0;
void    can_rx_param(pdu_tag rec_msg)
{
    // LED2 = !LED2;

    tx_data[0] = rec_msg.data.u8_buf[0];
    tx_data[1] = rec_msg.data.u8_buf[1];
    tx_data[2] = rec_msg.data.u8_buf[2];
    tx_data[3] = rec_msg.data.u8_buf[3];
    if (tx_data[1] = 0xFF)
    {
        tx_flag = 1;
    }
    else
    {
        flag = 1;
    }

    // push_can_message_to_queue(rec_msg.id.r, rec_msg.reg.dlc, rec_msg.data.u8_buf);
}

void can_rx_update(pdu_tag rec_msg)
{
    if ((rec_msg.data.u8_buf[0] == 'A') && (rec_msg.data.u8_buf[1] == 'P') && (rec_msg.data.u8_buf[2] == 'P') && (rec_msg.data.u8_buf[3] == 'L') && (rec_msg.data.u8_buf[4] == 'O') && (rec_msg.data.u8_buf[5] == 'A') && (rec_msg.data.u8_buf[6] == 'D') && (rec_msg.data.u8_buf[7] == 0x01))
    {
        __set_FAULTMASK(1); //  关闭所有中断
        NVIC_SystemReset(); //  复位
    }
}

can_rx_tab can_tab[] = {
    0x18011802,
    can_rx_param,
    0x18011801,
    can_rx_ctl,
    0x18DFF4E1,
    iap_rec_handler,

};

#define REG32(addr)     (*(volatile uint32_t *)(uint32_t)(addr))
#define CAN_INTEN(canx) REG32((canx) + 0x14U)

void can_start_send(void)
{
    pdu_tag tx_msg;

    if (0 == ((CAN1->IER) & 0x1))
    {
#if 0
        de_queue(can_tx, &tx_msg);
        if (can_tx.ret == 1) // 返回值为1代表读取成功
        {
            can_tx_frame(tx_msg);
            CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE);
        }
#else
        if (de_queue(&can_tx_queue, tx_msg) == Q_OK) // 返回值为1代表读取成功
        {
            can_tx_frame(tx_msg);
            CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE);
        }

#endif
    }
}

static uint8_t can_rx_process(void)
{
    uint8_t      i;
    uint8_t      flg = 0;
    pdu_tag      rec_msg;
    QUEUE_STATUS result;

    for (uint8_t j = 0; j < 15; j++)
    {
#if 1
        __disable_irq();
        result = de_queue(&can_rx_queue, rec_msg);
        __enable_irq();
        if (Q_OK == result)
        {
            for (i = 0; i < ARR_SIZE(can_tab); i++)
            {
                if (can_tab[i].id == rec_msg.id.r)
                {
                    can_tab[i].p_func(rec_msg);
                    flg = 1;
                    break;
                }
                else if (can_tab[i].id == (rec_msg.id.b.pf << 8 | rec_msg.id.b.ps))
                {
                    can_tab[i].p_func(rec_msg);
                    flg = 1;
                    break;
                }
                else if (can_tab[i].id == rec_msg.id.b.pf)
                {
                    can_tab[i].p_func(rec_msg);
                    flg = 1;
                    break;
                }
            }

            if (flg == 0)
            {
                // j1939_msg_push_queue(rec_msg);
            }
            flg = 0;
        }
#else
        de_queue(can_rx, &rec_msg);
        if (can_rx.ret == 1) // 返回值为1代表读取成功
        {
            for (i = 0; i < ARR_SIZE(can_tab); i++)
            {
                if (can_tab[i].id == rec_msg.id.r)
                {
                    can_tab[i].p_func(rec_msg);
                    break;
                }
            }
        }
#endif
    }
}

void can_tx_0x18010000_message(void)
{
    uint8_t data[8] = {0};
    data[2]         = 0xFF;
    push_can_message_to_queue(0x18010000, 8, data);
}

void can_tx_0x18010001_message(void)
{
    uint8_t data[8] = {0};
    push_can_message_to_queue(0x18010001, 8, data);
}

void can_tx_0x18030001_message(void)
{
    uint8_t data[8] = {0};
    data[1]         = 8;
    push_can_message_to_queue(0x18030001, 8, data);
}

void can_tx_0x18010002_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 0x02;
    push_can_message_to_queue(0x18010002, 8, data);
}

void can_tx_0x18030003_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 0x02;
    push_can_message_to_queue(0x18030003, 8, data);
}

void can_tx_0x18010004_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010004, 8, data);
}

void can_tx_0x18010005_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010005, 8, data);
}

void can_tx_0x18010006_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010006, 8, data);
}

void can_tx_0x18010007_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010007, 8, data);
}

void can_tx_0x18010008_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010008, 8, data);
}

void can_tx_0x18010009_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x18010009, 8, data);
}

void can_tx_0x1801000F_message(void)
{
    uint8_t data[8] = {0};

    data[0] = 1;
    data[1] = 5;
    data[2] = 3000 & 0xFF;
    data[3] = 3000 >> 8;
    data[4] = 3000 & 0xFF;
    data[5] = 3000 >> 8;
    push_can_message_to_queue(0x1801000F, 8, data);
}

void can_tx_0x18010010_message(void)
{
    uint8_t data[8] = {0};
    // data_bit_move(0, 8, 1);
    // data_bit_move(8, 8, 1);
    // data_bit_move(32, 8, 0);
    // data_bit_move(40, 8, 0);
    // product_array(data);
    data[0] = 1;
    data[1] = 1;
    data[4] = 0;
    data[5] = 0;
    // push_can_message_to_queue(0x18010010, 8, data);
    // data[1] = 5;
    // push_can_message_to_queue(0x18010010, 8, data);
    // data[1] = 10;
    push_can_message_to_queue(0x18010010, 8, data);
}

void can_tx_0x1801000A_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x1801000A, 8, data);
}

void can_tx_0x1801000B_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x1801000B, 8, data);
}

void can_tx_0x1801000C_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x1801000C, 8, data);
}

void can_tx_0x1801000D_message(void)
{
    uint8_t data[8] = {0};
    data[4]         = 33;
    data[5]         = 22;
    push_can_message_to_queue(0x1801000D, 8, data);
}

void can_tx_0x18010012_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 24;
    data[1]         = 1;
    data[2]         = 3;
    data[3]         = 11;
    data[4]         = 12;
    data[5]         = 13;
    data[6]         = 12;
    data[7]         = 13;
    push_can_message_to_queue(0x18010012, 8, data);
}

void can_tx_0x18010017_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 24;
    data[1]         = 1;
    data[0]         = 3;
    data[1]         = 11;
    data[0]         = 12;
    data[1]         = 13;
    push_can_message_to_queue(0x18010017, 8, data);
}

void can_tx_0x18010018_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 2;
    data[1]         = 0xFF;
    push_can_message_to_queue(0x18010018, 8, data);
}

void can_tx_0x18010019_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 2;
    data[1]         = 0xFF;
    push_can_message_to_queue(0x18010019, 8, data);
}

void can_tx_0x1801001A_message(void)
{
    volatile static int i       = 0;
    uint8_t             data[8] = {0};

    data[0] = 2 + i;
    data[1] = 0x12 >> i;
    i++;
    if (i > 7)
    {
        i = 0;
    }
    push_can_message_to_queue(0x1801001A, 8, data);
}

void can_tx_0x1801001B_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 2;
    data[1]         = 0xFF;
    push_can_message_to_queue(0x1801001B, 8, data);
}

void can_tx_0x1801001C_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 2;
    data[4]         = 0xFF;
    push_can_message_to_queue(0x1801001C, 8, data);
}

void can_tx_0x1801001D_message(void)
{
    uint8_t data[8] = {0};
    data[0]         = 3;
    data[2]         = 0xFF;
    push_can_message_to_queue(0x1801001D, 8, data);
}

void can_tx_0x18010023_message(void)
{
    uint8_t send_data[8] = {0};
    // data[0]    = 'A';
    // data[1]    = 'A';
    // data[2]    = 'A';
    // data[3]    = 'A';
    // data[4]    = 'A';
    // data[5]    = 'A';
    // data[6]    = 'A';
    // data[7]    = 'A';
    data_bit_move(0, 8, 'A');
    data_bit_move(8, 8, 'A');
    data_bit_move(16, 8, 'A');
    data_bit_move(24, 8, 'C');
    data_bit_move(32, 8, 'A');
    data_bit_move(40, 8, '-');
    data_bit_move(48, 8, 'A');
    data_bit_move(56, 8, 'B');
    product_array(send_data);
    push_can_message_to_queue(0x18010023, 8, send_data);
}

void can_tx_0x18011802_message(void)
{
    static uint8_t fdf = 0;
    if (flag == 1)
    {
        push_can_message_to_queue(0x18011802, 8, tx_data);
        flag = 0;
    }
    uint8_t  data[8] = {0};
    uint16_t addr    = 0xA000;
    if (tx_flag == 1)
    {
        data[0] = (addr + fdf) & 0xFF;
        data[1] = (addr + fdf) >> 8;
        data[2] = rand() & 0xFF;
        data[3] = rand() >> 8;
        fdf++;
        if (fdf == 80)
        {
            tx_flag = 0;
        }
        push_can_message_to_queue(0x18011802, 8, data);
    }
}

void can_tx_0x18011803_message(void)
{
    uint8_t        data[8] = {0};
    uint16_t       addr    = 0xB000;
    static uint8_t fdf     = 0;
    static int16_t a       = -263;
    data[0]                = (addr + fdf) & 0xFF;
    data[1]                = (addr + fdf) >> 8;
    data[2]                = a & 0xFF;
    data[3]                = a >> 8;
    fdf++;
    a++;
    if (fdf > 6)
    {
        fdf = 0;
    }
    push_can_message_to_queue(0x18011803, 8, data);
}

void can_tx_0x18011803_2_message(void)
{
    uint8_t        data[8] = {0};
    static uint8_t fdf_2   = 0;

    uint16_t addr_vol = 0xB110;
    data[0]           = (addr_vol + fdf_2) & 0xFF;
    data[1]           = (addr_vol + fdf_2) >> 8;
    data[2]           = 1;
    data[3]           = -2 & 0xFF;
    data[4]           = -2 >> 8;
    fdf_2++;
    if (fdf_2 > 80)
    {
        fdf_2 = 0;
    }
    push_can_message_to_queue(0x18011803, 8, data);
}

void send_message(void)
{
    static volatile uint8_t times = 0;

    if (times >= 100)
    {
        times = 0;
    }
    if (times % 100 == 0)
    {
        can_tx_0x18010001_message();
        can_tx_0x18010002_message();
        can_tx_0x18010004_message();
        can_tx_0x18010005_message();
    }
    else if (times % 100 == 5)
    {
        can_tx_0x18010006_message();
        can_tx_0x18010007_message();
        can_tx_0x18010008_message();
        can_tx_0x18010009_message();
        can_tx_0x1801000A_message();
    }
    else if (times % 100 == 10)
    {
        can_tx_0x1801000B_message();
        can_tx_0x1801000C_message();
        can_tx_0x1801000D_message();
        can_tx_0x18010023_message();
        can_tx_0x18010010_message();
    }
    else if (times % 100 == 15)
    {
        can_tx_0x18010018_message();
        can_tx_0x18010019_message();
        can_tx_0x1801001A_message();
        can_tx_0x1801001B_message();
        can_tx_0x1801001C_message();
    }
    else if (times % 100 == 20)
    {
        can_tx_0x1801001D_message();
        can_tx_0x1801000F_message();
        can_tx_0x18030003_message();
        can_tx_0x18030001_message();
        can_tx_0x18010000_message();
    }
    else if (times % 100 == 25)
    {
        can_tx_0x18010017_message();
        can_tx_0x18010012_message();
        can_tx_0x18011802_message();
        can_tx_0x18011803_message();
        // can_tx_0x18011803_2_message();
    }
    else
    {
        // return;
    }
    times++;
}

void dev_can_network_init(void)
{
    hal_can_init();
    hal_can_rx_back_init(can_rx_callback);
    hal_can_tx_back_init(can_tx_callback);
}

void can_process(void)
{
    can_rx_process();
    send_message();
    can_start_send();
}