#include "xieneng_car_common.h"

extern unsigned char raw_data[MAX_CAN_PORT][MAX_LECU_CNT][FUNC_MAX][MAX_FRAME_NUMBER][CAN_DATA_LEN];

// return -1: error
//         0: 需等待总线返回
//        >0: 立即返回
int modbus_recv(xieneng_car_var_t *var, int port, unsigned char *data, int data_len, unsigned char *rsp)
{
    modbus_down_frame_t modbus;
    unsigned char *p = data;
    int port_idx = can_port_map(port);
    int rsp_len;

    modbus.addr = *p;
    p++;
    modbus.func = *p;
    p++;
    modbus.reg_start = (p[0] << 8) + p[1] ;
    p += 2;
    modbus.reg_size = (p[0] << 8) + p[1] ;
    p += 2;

    dy_syslog_hex(LOG_DEBUG, data, data_len, "modbus slave receive data");
    int crc = modbus_data_check_crc(data, data_len);
    if (crc != 0)
    {
        dy_syslog(LOG_ERR, "modbus check crc error ,calculate crc %04x", crc);
        return -1;
    }

    if (modbus.func == MODBUS_FC_READ_HOLDING_REGISTERS)
    {
        int lecu_addr = modbus.addr - 1; // LECU addr 1 对应modbus地址2，modbus地址1用于模拟总控
        unsigned char *lecu_data = (unsigned char *)&raw_data[port_idx][lecu_addr];
        int offset = modbus.reg_start * 2; // 偏移字节数，一个modbus寄存器两个字节
        int len = modbus.reg_size * 2; //字节长度

        dy_syslog(LOG_DEBUG, "lecu_addr:%d offset:%x len:%d rsp:%p", lecu_addr, offset, len, rsp);

        if (offset + len >= MAX_FRAME_NUMBER * CAN_DATA_LEN * FUNC_MAX)
        {
            dy_syslog(LOG_ERR, "modbus start addr + size too long");
            return -1;
        }

        rsp_len = len + 5; //modbus 头尾空间

        if (rsp == NULL)
        {
            dy_syslog(LOG_ERR, "rsp failed");
            return -1;
        }
        p = rsp;
        *p = modbus.addr;
        p++; // 地址码
        *p = modbus.func;
        p++; // 功能码
        *p = len;
        p++; // 字节数
        memcpy(p, lecu_data + offset, len);
        p += len;

        rsp_len = calculate_crc(rsp, p - rsp);
        dy_syslog_hex(LOG_DEBUG, rsp, rsp_len, "modbus slave: rsp data");
        return rsp_len;
    }
    else if (modbus.func == MODBUS_FC_WRITE_MULTIPLE_REGISTERS)
    {
        // 下行配置接口
        int len = modbus.reg_size * 2; //字节长度
        struct can_frame frame;
        int func;
        int i = 0;

        modbus.data = p;
        func = get_func_code_by_modbus_addr(modbus.reg_start);
        dy_syslog(LOG_DEBUG, "modbus.addr %x  modbus.reg_start:%x modbus.reg_size:%x func:%d", modbus.addr, modbus.reg_start, modbus.reg_size, func);

        if (func == FUNC_CTRL_720)
        {
            frame.can_id = 0x720;
            if (len > 8)frame.can_dlc = 8;
            else frame.can_dlc = len;
            memcpy(frame.data, modbus.data, frame.can_dlc);
            if (frame.data[0] != 0xFF)
            {
                frame.data[0] = modbus.addr - 2;
            }
            dy_syslog_hex(LOG_DEBUG, frame.data, frame.can_dlc, "can send data with ID:%x", frame.can_id);
            can_send(var->can_cfg->can_dev[port_idx].fd, &frame);
            if (frame.data[1] == 0xa)
            {
                // 复位均衡板，直接返回
                if (rsp == NULL)
                {
                    dy_syslog(LOG_ERR, "rsp failed");
                    return -1;
                }
                p = rsp;
                *p = modbus.addr;
                p++; // 地址码
                *p = modbus.func;
                p++; // 功能码
                *p = 2;
                p++; // 字节数
                *p = 0xa;
                p++;
                *p = 0;
                p++;
                rsp_len = calculate_crc(rsp, p - rsp);
                dy_syslog_hex(LOG_DEBUG, rsp, rsp_len, "modbus slave: rsp data");
                return rsp_len;
            }
        }
        else if (func == FUNC_CTRL_7FA)
        {
            dy_syslog(LOG_ERR, "not support now");
        }
        else
        {
            dy_syslog(LOG_ERR, "not support now");
        }
    }
    else if (modbus.func == MODBUS_FC_BALANCE_CONF_SET)
    {
        // 配置均衡板
        struct can_frame frame;
        int len = modbus.reg_size * 2; //字节长度
        int can_id_internal = 0;
        int dev_no = modbus.addr;
        int dev_id = modbus.addr + 2;

        modbus.data = p;

        frame.can_id = 0x7FA;

        can_id_internal |= ((dev_id << 19) | (1 << 16) | (dev_no << 11) | (modbus.reg_start));
        dy_syslog(LOG_DEBUG, "modbus.addr %x  modbus.reg_start:%x modbus.reg_size:%x can_id:%08x", modbus.addr, modbus.reg_start, modbus.reg_size, can_id_internal);
        frame.data[0] = can_id_internal & 0xFF;
        frame.data[1] = (can_id_internal >> 8) & 0xFF;
        frame.data[2] = (can_id_internal >> 16) & 0xFF;
        frame.data[3] = (can_id_internal >> 24) & 0xFF;
        if (len > 4)frame.can_dlc = 8;
        else frame.can_dlc = len + 4;
        memcpy(frame.data + 4, modbus.data, frame.can_dlc - 4);
        dy_syslog_hex(LOG_DEBUG, frame.data, frame.can_dlc, "can send data with ID:%x", frame.can_id);
        can_send(var->can_cfg->can_dev[port_idx].fd, &frame);
        if (modbus.reg_start == 0x146)
        {
            // 复位均衡板EEPROM，直接返回
            if (rsp == NULL)
            {
                dy_syslog(LOG_ERR, "rsp failed");
                return -1;
            }
            p = rsp;
            *p = modbus.addr;
            p++; // 地址码
            *p = modbus.func;
            p++; // 功能码
            *p = 3;
            p++; // 字节数
            *p = 0;
            p++;
            *p = 0;
            p++;
            *p = 0;
            p++;
            rsp_len = calculate_crc(rsp, p - rsp);
            dy_syslog_hex(LOG_DEBUG, rsp, rsp_len, "modbus slave: rsp data");
            return rsp_len;
        }
    }
    else if (modbus.func == MODBUS_FC_BALANCE_CONF_GET)
    {
        // 查询均衡板
        struct can_frame frame;
        int can_id_internal = 0;
        int dev_no = modbus.addr;
        int dev_id = modbus.addr + 2;

        modbus.data = p;

        frame.can_id = 0x7FA;

        can_id_internal |= ((dev_id << 19) | (3 << 16) | (dev_no << 11) | (modbus.reg_start));
        dy_syslog(LOG_DEBUG, "modbus.addr %x  modbus.reg_start:%x modbus.reg_size:%x can_id:%08x", modbus.addr, modbus.reg_start, modbus.reg_size, can_id_internal);
        frame.data[0] = can_id_internal & 0xFF;
        frame.data[1] = (can_id_internal >> 8) & 0xFF;
        frame.data[2] = (can_id_internal >> 16) & 0xFF;
        frame.data[3] = (can_id_internal >> 24) & 0xFF;
        frame.data[4] = (modbus.reg_size >> 8) & 0xFF;
        frame.data[5] = modbus.reg_size & 0xFF;
        frame.can_dlc = 6;
        dy_syslog_hex(LOG_DEBUG, frame.data, frame.can_dlc, "can send data with ID:%x", frame.can_id);
        can_send(var->can_cfg->can_dev[port_idx].fd, &frame);
    }
    else if (modbus.func == MODBUS_FC_SLAVE_CTRL_SET)
    {
        // 配置均衡板
        struct can_frame frame;
        int len = modbus.reg_size * 2; //字节长度
        int can_id_internal = 0;
        int dev_no = modbus.addr;
        int dev_id = modbus.addr + 2;

        modbus.data = p;

        frame.can_id = 0x7FF;

        can_id_internal |= ((dev_id << 19) | (1 << 16) | (dev_no << 11) | (modbus.reg_start));
        dy_syslog(LOG_DEBUG, "modbus.addr %x  modbus.reg_start:%x modbus.reg_size:%x can_id:%08x", modbus.addr, modbus.reg_start, modbus.reg_size, can_id_internal);
        frame.data[0] = can_id_internal & 0xFF;
        frame.data[1] = (can_id_internal >> 8) & 0xFF;
        frame.data[2] = (can_id_internal >> 16) & 0xFF;
        frame.data[3] = (can_id_internal >> 24) & 0xFF;
        if (len > 4)frame.can_dlc = 8;
        else frame.can_dlc = len + 4;
        memcpy(frame.data + 4, modbus.data, frame.can_dlc - 4);
        dy_syslog_hex(LOG_DEBUG, frame.data, frame.can_dlc, "can send data with ID:%x", frame.can_id);
        can_send(var->can_cfg->can_dev[port_idx].fd, &frame);
        if (modbus.reg_start == 0 || modbus.reg_start == 1)
        {
            // 复位均衡板EEPROM，直接返回
            if (rsp == NULL)
            {
                dy_syslog(LOG_ERR, "rsp failed");
                return -1;
            }
            p = rsp;
            *p = modbus.addr;
            p++; // 地址码
            *p = modbus.func;
            p++; // 功能码
            *p = 3;
            p++; // 字节数
            *p = 0;
            p++;
            *p = 0;
            p++;
            *p = 0;
            p++;
            rsp_len = calculate_crc(rsp, p - rsp);
            dy_syslog_hex(LOG_DEBUG, rsp, rsp_len, "modbus slave: rsp data");
            return rsp_len;
        }
    }
    else if (modbus.func == MODBUS_FC_SLAVE_CTRL_GET)
    {
        // 查询均衡板
        struct can_frame frame;
        int can_id_internal = 0;
        int dev_no = modbus.addr;
        int dev_id = modbus.addr + 2;

        modbus.data = p;

        frame.can_id = 0x7FF;

        can_id_internal |= ((dev_id << 19) | (3 << 16) | (dev_no << 11) | (modbus.reg_start));
        dy_syslog(LOG_DEBUG, "modbus.addr %x  modbus.reg_start:%x modbus.reg_size:%x can_id:%08x", modbus.addr, modbus.reg_start, modbus.reg_size, can_id_internal);
        frame.data[0] = can_id_internal & 0xFF;
        frame.data[1] = (can_id_internal >> 8) & 0xFF;
        frame.data[2] = (can_id_internal >> 16) & 0xFF;
        frame.data[3] = (can_id_internal >> 24) & 0xFF;
        frame.data[4] = (modbus.reg_size >> 8) & 0xFF;
        frame.data[5] = modbus.reg_size & 0xFF;
        frame.can_dlc = 6;
        dy_syslog_hex(LOG_DEBUG, frame.data, frame.can_dlc, "can send data with ID:%x", frame.can_id);
        dy_syslog_hex(LOG_DEBUG, &frame, sizeof(frame), "can send data with ID:%x", frame.can_id);

        can_send(var->can_cfg->can_dev[port_idx].fd, &frame);
    }

    return 0;
}