// can_comm.cpp - USBCANFD CAN/CANFD 收发封装实现
//
// 代码迁移自 vision-anti-pinch_bak/main.cpp 的 CAN 模块部分。

#include "can_comm.h"

#include <cstdio>
#include <cstring>
#include <pthread.h>
#include <unistd.h>

#include "zcan.h"

/* =====================================================================
 * 类型/宏/全局变量
 * ===================================================================== */

typedef uint32_t UINT;
typedef uint32_t U32;
#define msleep(ms) usleep((ms) * 1000)

#define USBCANFD 33
#define MAX_CHANNELS 2 // 最大通道数量
#define RX_WAIT_TIME 100
#define RX_BUFF_SIZE 1000

// 接收线程上下文
typedef struct
{
    int dev_type; // 设备类型
    int dev_idx;  // 设备索引
    int chn_idx;  // 通道号
    int total;    // 接收总数
    int stop;     // 线程结束标志
} THREAD_CTX;

/* CAN 运行标志, 由信号处理函数(Ctrl+C)或外部置位 */
static volatile bool g_can_running = true;

static int g_receive_count = 0;

/* ===== 仅供 can_start/can_stop 共享的文件内静态变量 ===== */
static int s_DevType = USBCANFD;
static int s_DevIdx = 0;
static THREAD_CTX s_rx_ctx[MAX_CHANNELS];
static pthread_t s_rx_threads[MAX_CHANNELS];
static bool s_can_opened = false;

namespace CanComm {

bool is_open() { return s_can_opened; }

int receive_count() { return g_receive_count; }

/* ===== 报文解析回调 (由 main.cpp 通过 set_msg_callback 注册) ===== */
static MsgCallback s_msg_cb = nullptr;

void set_msg_callback(MsgCallback cb) { s_msg_cb = cb; }

} // namespace CanComm

/* =====================================================================
 * 帧构建与收发
 * ===================================================================== */

namespace CanComm {

// 构建 CAN 帧
static void construct_can_frame(ZCAN_20_MSG *can_msg, UINT id, int chn_idx, int pad)
{
    memset(can_msg, 0, sizeof(ZCAN_20_MSG));
    can_msg->hdr.inf.txm = 0; // 0-正常发送
    can_msg->hdr.inf.fmt = 0; // 0-CAN
    can_msg->hdr.inf.sdf = 0; // 0-数据帧, 1-远程帧
    can_msg->hdr.inf.sef = 0; // 0-标准帧, 1-扩展帧

    can_msg->hdr.id = id;       // ID
    can_msg->hdr.chn = chn_idx; // 通道
    can_msg->hdr.len = 8;       // 数据长度

    if (pad > 0)
    {
        can_msg->hdr.pad = pad;           // 发送后延迟 pad ms
        can_msg->hdr.inf.qsend = 1;       // 队列发送帧,仅判断首帧
        can_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
    }

    for (int i = 0; i < can_msg->hdr.len; i++)
        can_msg->dat[i] = i;
}

// 构建 CANFD 帧
static void construct_canfd_frame(ZCAN_FD_MSG *canfd_msg, UINT id, int chn_idx, int pad)
{
    memset(canfd_msg, 0, sizeof(ZCAN_FD_MSG));
    canfd_msg->hdr.inf.txm = 0; // 0-正常发送
    canfd_msg->hdr.inf.fmt = 1; // 0-CAN, 1-CANFD
    canfd_msg->hdr.inf.sdf = 0; // 0-数据帧 CANFD只有数据帧!
    canfd_msg->hdr.inf.sef = 1; // 0-标准帧, 1-扩展帧
    canfd_msg->hdr.inf.brs = 1; // canfd 加速

    canfd_msg->hdr.id = id;       // ID
    canfd_msg->hdr.chn = chn_idx; // 通道
    canfd_msg->hdr.len = 64;      // 数据长度

    if (pad > 0)
    {
        canfd_msg->hdr.pad = 100;           // 发送后延迟100ms
        canfd_msg->hdr.inf.qsend = 1;       // 队列发送帧,仅判断首帧
        canfd_msg->hdr.inf.qsend_100us = 0; // 队列发送单位,0-ms,1-100us
    }

    for (int i = 0; i < canfd_msg->hdr.len; i++)
        canfd_msg->dat[i] = i;
}

// 分发报文到 main.cpp 注册的解析回调
static inline void dispatch_msg(uint32_t ts, uint32_t msg_id, const uint8_t *data, uint32_t len)
{
    if (s_msg_cb)
        s_msg_cb(ts, msg_id, data, len);
}

// 接收线程
static void *rx_thread(void *data)
{
    THREAD_CTX *ctx = (THREAD_CTX *)data;
    int DevType = ctx->dev_type;
    int DevIdx = ctx->dev_idx;
    int chn_idx = ctx->chn_idx;

    ZCAN_20_MSG can_data[RX_BUFF_SIZE];
    ZCAN_FD_MSG canfd_data[RX_BUFF_SIZE];

    while (!ctx->stop)
    {
        memset(can_data, 0, sizeof(can_data));
        memset(canfd_data, 0, sizeof(canfd_data));

        int rcount = VCI_Receive(DevType, DevIdx, chn_idx, can_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CAN
        for (int i = 0; i < rcount; ++i)
        {
            if (can_data[i].hdr.inf.sdf == 0)
            { // 数据帧
                dispatch_msg(can_data->hdr.ts, can_data[i].hdr.id, can_data[i].dat, can_data[i].hdr.len);
            }
        }
        ctx->total += rcount;

        rcount = VCI_ReceiveFD(DevType, DevIdx, chn_idx, canfd_data, RX_BUFF_SIZE, RX_WAIT_TIME); // CANFD
        for (int i = 0; i < rcount; ++i)
        {
            dispatch_msg(canfd_data->hdr.ts, canfd_data[i].hdr.id, canfd_data[i].dat, canfd_data[i].hdr.len);
        }
        ctx->total += rcount;
        msleep(10);
    }
    g_receive_count = ctx->total;
    printf("chn: %d receive %d\n", chn_idx, ctx->total);
    pthread_exit(0);
}

// 合并接收线程
static void *rx_merge_thread(void *data)
{
    THREAD_CTX *ctx = (THREAD_CTX *)data;
    int DevType = ctx->dev_type;
    int DevIdx = ctx->dev_idx;

    ZCANDataObj obj_data[RX_BUFF_SIZE];

    while (!ctx->stop)
    {
        memset(obj_data, 0, sizeof(obj_data));
        int rcount = VCI_ReceiveData(DevType, DevIdx, 0, obj_data, RX_BUFF_SIZE, RX_WAIT_TIME); // 第三个参数固定为0就好
        for (int i = 0; i < rcount; ++i)
        {
            if (obj_data[i].dataType == ZCAN_DT_ZCAN_CAN_DATA)
            { // CAN
                ZCAN_20_MSG *can_data = &obj_data[i].data.zcanCANData;
                if (can_data->hdr.inf.sdf == 0)
                { // 数据帧
                    dispatch_msg(can_data->hdr.ts, can_data->hdr.id, can_data->dat, can_data->hdr.len);
                }
            }
            else if (obj_data[i].dataType == ZCAN_DT_ZCAN_CANFD_DATA)
            { // CANFD
                ZCAN_FD_MSG *canfd_data = &obj_data[i].data.zcanCANFDData;
                dispatch_msg(canfd_data->hdr.ts, canfd_data->hdr.id, canfd_data->dat, canfd_data->hdr.len);
            }
        }
        ctx->total += rcount;
        msleep(10);
    }
    g_receive_count = ctx->total;
    printf("merge receive %d\n", ctx->total);
    pthread_exit(0);
}

/*
 * 初始化并启动 CAN 收发。
 * 返回 true 成功; false 失败(主流程可继续仅取图)。
 */
bool can_start()
{
    g_can_running = true;
    int IsMerge = 1; // 是否合并接收

    // 打开设备
    if (!VCI_OpenDevice(s_DevType, s_DevIdx, 0))
    {
        printf("Open device fail\n");
        return false;
    }
    s_can_opened = true;
    printf("Open device success\n");

    // 初始化,启动通道
    for (int i = 0; i < MAX_CHANNELS; i++)
    {
        ZCAN_INIT init;      // 波特率结构体,数据根据zcanpro的波特率计算器得出
        init.clk = 60000000; // clock: 60M(V1.01) 80M(V1.03即以上)
        init.mode = 0;       // 0-正常

        init.aset.tseg1 = 14; // 仲裁域 500kbps
        init.aset.tseg2 = 3;
        init.aset.sjw = 2;
        init.aset.smp = 0;
        init.aset.brp = 5;

        init.dset.tseg1 = 10; // 数据域 2000kbps
        init.dset.tseg2 = 2;
        init.dset.sjw = 2;
        init.dset.smp = 0;
        init.dset.brp = 1;

        if (!VCI_InitCAN(s_DevType, s_DevIdx, i, &init)) // 初始化通道
        {
            printf("InitCAN(%d) fail\n", i);
            return false;
        }
        printf("InitCAN(%d) success\n", i);

        U32 on = 1;
        if (!VCI_SetReference(s_DevType, s_DevIdx, i, CMD_CAN_TRES, &on)) // 终端电阻
        {
            printf("CMD_CAN_TRES fail\n");
        }

        if (i == 0)
        {
            if (!VCI_SetReference(s_DevType, s_DevIdx, i, ZCAN_CMD_SET_CHNL_RECV_MERGE, &IsMerge)) // 合并接收
            {
                printf("ZCAN_CMD_SET_CHNL_RECV_MERGE fail\n");
            }
        }

        if (!VCI_StartCAN(s_DevType, s_DevIdx, i)) // 启动通道
        {
            printf("StartCAN(%d) fail\n", i);
            return false;
        }
        printf("StartCAN(%d) success\n", i);

        s_rx_ctx[i].dev_type = s_DevType;
        s_rx_ctx[i].dev_idx = s_DevIdx;
        s_rx_ctx[i].chn_idx = i;
        s_rx_ctx[i].total = 0;
        s_rx_ctx[i].stop = 0;

        if (IsMerge && i == 0)
        {
            pthread_create(&s_rx_threads[i], NULL, rx_merge_thread, &s_rx_ctx[i]); // 合并接收线程
        }
        else
        {
            pthread_create(&s_rx_threads[i], NULL, rx_thread, &s_rx_ctx[i]); // 创建接收线程
        }
    }
    return true;
}

/*
 * 停止 CAN 接收并关闭设备。
 */
void can_stop()
{
    if (!s_can_opened)
        return;

    g_can_running = false;

    for (int i = 0; i < MAX_CHANNELS; i++)
    {
        s_rx_ctx[i].stop = 1;
        pthread_join(s_rx_threads[i], NULL); // 等待接收线程退出

        if (!VCI_ResetCAN(s_DevType, s_DevIdx, i)) // 复位通道
            printf("ResetCAN(%d) fail\n", i);
        else
            printf("ResetCAN(%d) success!\n", i);
    }

    if (!VCI_CloseDevice(s_DevType, s_DevIdx))
        printf("CloseDevice fail\n");
    else
        printf("CloseDevice success\n");

    s_can_opened = false;
}

uint32_t can_send(int chn_idx, uint32_t id, const uint8_t data[8])
{
    if (!s_can_opened)
        return 0;

    ZCAN_20_MSG msg;
    construct_can_frame(&msg, id, chn_idx, 0);
    for (int i = 0; i < 8; ++i)
        msg.dat[i] = data[i];

    return VCI_Transmit(s_DevType, s_DevIdx, chn_idx, &msg, 1);
}

uint32_t canfd_send(int chn_idx, uint32_t id, const uint8_t data[64])
{
    if (!s_can_opened)
        return 0;

    ZCAN_FD_MSG msg;
    construct_canfd_frame(&msg, id, chn_idx, 0);
    for (int i = 0; i < 64; ++i)
        msg.dat[i] = data[i];

    return VCI_TransmitFD(s_DevType, s_DevIdx, chn_idx, &msg, 1);
}

} // namespace CanComm