#include "main.h"
#include "Modbussimple.h"
#include "cmsis_os.h"
#include "Callback.h"

uint32_t g_modbusbuffer[40] ; // 当用DMA发送的时候 帧头要四字节对齐
extern float AD1_Vol,AD2_Vol,AD3_Vol,AD4_Vol,AD5_Vol,AD5_I;
extern uint16_t Running_time;			//	运行时长

ModbusRegister_t modbus_map[] = {
    {0x4000, 2, 1, &g_deviceParams.Firmware_Version},    
		{0x4002, 1, 0, &Running_time},
		{0x4010, 1, 0, &g_deviceParams.SlaveDevice_addr},
		{0x4011, 1, 0, &g_deviceParams.Serial_baud_rate},
    {0x4020, 3, 0, &g_deviceParams.Device_SN},    
    
    {0x0001, 1, 0, &Valve_status},
		{0x0002, 1, 0, &Motor_status},
		{0x0003, 2, 1, &Motor_current},
		{0x0005, 2, 1, &Motor_speed},
    {0x0007, 2, 1, &Magnetic_angle},
    {0x0009, 8, 1, hall_data},		
};


// Modbus CRC16（低位在前，高位在后）
uint16_t modbus_crc16(uint8_t *data, uint16_t length) 
{
    uint16_t crc = 0xFFFF;
    for (uint16_t pos = 0; pos < length; pos++) {
        crc ^= data[pos];
        for (int i = 0; i < 8; i++) {
            if (crc & 0x0001)
                crc = (crc >> 1) ^ 0xA001;
            else
                crc >>= 1;
        }
    }
    return crc;
}

// 从映射表中查找寄存器并复制数据
/*
bool read_modbus_registers(uint16_t addr, uint16_t len, uint8_t *dest_buf, uint16_t *out_byte_count) 
{
    uint16_t offset = 0;
    for (int i = 0; i < sizeof(modbus_map) / sizeof(modbus_map[0]); i++) {
        uint16_t reg_start = modbus_map[i].address;
        uint16_t reg_len = modbus_map[i].length;

        if (addr >= reg_start && (addr + len) <= (reg_start + reg_len)) {
            uint16_t relative = addr - reg_start;

            void* ptr = modbus_map[i].data_ptr;

            if (modbus_map[i].is_uint32_big_endian) {
                copy_uint32_to_modbus_bytes_be((uint32_t*)ptr, relative / 2, len, dest_buf, &offset);
                *out_byte_count = offset;
                return true;
            }

            // 默认处理为 uint16_t 方式
            uint16_t *data_ptr = (uint16_t *)ptr;
            for (uint16_t j = 0; j < len; j++) {
                uint16_t val = data_ptr[relative + j];
                dest_buf[offset++] = (val >> 8) & 0xFF;
                dest_buf[offset++] = val & 0xFF;
            }

            *out_byte_count = offset;
            return true;
        }
    }
    return false;
}
*/
bool read_modbus_registers(uint16_t addr, uint16_t len, uint8_t *dest_buf, uint16_t *out_byte_count)
{
    uint16_t offset = 0;
    uint16_t remaining = len;      // 还有多少个寄存器要读取
    uint16_t curr_addr = addr;     // 当前要读取的地址

    while (remaining > 0) {
        bool matched = false;

        for (int i = 0; i < sizeof(modbus_map) / sizeof(modbus_map[0]); i++) {
            uint16_t map_start = modbus_map[i].address;
            uint16_t map_end = map_start + modbus_map[i].length;

            // 当前地址在该 map 区块范围内
            if (curr_addr >= map_start && curr_addr < map_end) {
                matched = true;

                uint16_t index_in_map = curr_addr - map_start;
                uint16_t can_read = map_end - curr_addr;
                uint16_t read_count = (remaining < can_read) ? remaining : can_read;

                void *ptr = modbus_map[i].data_ptr;

                if (modbus_map[i].is_uint32_big_endian) {
                    // 每个 uint32_t 映射为 2 个寄存器（4 字节）
//                    uint8_t *raw = (uint8_t *)ptr;

                    for (uint16_t j = 0; j < read_count; j++) {
                        uint16_t reg_idx = index_in_map + j;
                        uint32_t word_idx = reg_idx / 2;
                        uint8_t high = (reg_idx % 2 == 0);  // 寄存器高位 or 低位

                        uint32_t val = ((uint32_t *)ptr)[word_idx];

                        uint16_t word = high ? (val >> 16) : (val & 0xFFFF);
                        dest_buf[offset++] = (word >> 8) & 0xFF;
                        dest_buf[offset++] = word & 0xFF;
                    }
                } else {
                    uint16_t *data_ptr = (uint16_t *)ptr;

                    for (uint16_t j = 0; j < read_count; j++) {
                        uint16_t val = data_ptr[index_in_map + j];
                        dest_buf[offset++] = (val >> 8) & 0xFF;
                        dest_buf[offset++] = val & 0xFF;
                    }
                }

                curr_addr += read_count;
                remaining -= read_count;
                break;
            }
        }

        if (!matched) {
            return false;  // 地址不在任何 map 内，非法访问
        }
    }

    *out_byte_count = offset;
    return true;
}

// 将 uint32_t 数组按高字节优先方式拷贝为 Modbus 输出格式（每个 uint32_t 占 4 字节）
void copy_uint32_to_modbus_bytes_be(uint32_t *src, uint16_t start_index, uint16_t word_count, uint8_t *dest_buf, uint16_t *offset) 
{
    for (uint16_t i = 0; i < word_count / 2; i++) {
        uint32_t val = src[start_index + i];
        uint16_t high = (val >> 16) & 0xFFFF;
        uint16_t low  = val & 0xFFFF;

        dest_buf[(*offset)++] = (high >> 8) & 0xFF;
        dest_buf[(*offset)++] = high & 0xFF;
        dest_buf[(*offset)++] = (low >> 8) & 0xFF;
        dest_buf[(*offset)++] = low & 0xFF;
    }
}


// 功能码 0x03 响应帧（角度浮点数值float）
uint16_t modbus_03response_float(uint8_t device_addr, uint8_t num, float value, uint8_t *out_buf) 
{
    uint8_t i = 0;

    out_buf[i++] = device_addr;    // 设备地址
    out_buf[i++] = 0x03;           // 功能码 0x03
    out_buf[i++] = num;           // 字节数（float = 4字节）

    // 将 float 转为 4 字节 IEEE 754 格式
    uint8_t float_bytes[4];
    memcpy(float_bytes, &value, 4);

    // 注意字节序是否和主机匹配
    out_buf[i++] = float_bytes[3];
    out_buf[i++] = float_bytes[2];
	  out_buf[i++] = float_bytes[1];
    out_buf[i++] = float_bytes[0];

    // 计算 CRC
    uint16_t crc = modbus_crc16(out_buf, i);
    out_buf[i++] = crc & 0xFF;      // CRC低位
    out_buf[i++] = crc >> 8;        // CRC高位

    return i; // 返回总长度
}





// 返回错误响应帧，例如非法功能码、非法地址等错误
uint16_t modbus_error_response(uint8_t addr, uint8_t func_code, uint8_t err_code, uint8_t *buf) 
{
    buf[0] = addr;
    buf[1] = func_code | 0x80; // 错误码 = 功能码 + 0x80
    buf[2] = err_code;

    uint16_t crc = modbus_crc16(buf, 3);
    buf[3] = crc & 0xFF;
    buf[4] = crc >> 8;

    return 5;
}

// 修改485串口1的波特率
void Set_485_Baudrate(uint16_t baudrate_id)
{
    uint32_t baudrate = 9600; // 默认值

    // 根据 ID 映射波特率
    switch (baudrate_id)
    {
        case 0x0001: baudrate = 4800; break;
        case 0x0002: baudrate = 9600; break;
        case 0x0003: baudrate = 19200; break;
        case 0x0004: baudrate = 38400; break;
        case 0x0005: baudrate = 57600; break;
        case 0x0006: baudrate = 115200; break;
        case 0x0007: baudrate = 230400; break;
        case 0x0008: baudrate = 256000; break;
        default:   baudrate = 9600; break; // 非法 ID，默认 9600
    }

    // 重新设置 huart1 配置并初始化
    huart1.Instance = USART1;
    huart1.Init.BaudRate = baudrate;
    huart1.Init.WordLength = UART_WORDLENGTH_8B;
    huart1.Init.StopBits = UART_STOPBITS_1;
    huart1.Init.Parity = UART_PARITY_NONE;
    huart1.Init.Mode = UART_MODE_TX_RX;
    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart1.Init.OverSampling = UART_OVERSAMPLING_16;
    huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
    huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;

    if (HAL_UART_DeInit(&huart1) != HAL_OK)
    {
        Error_Handler();
    }

    if (HAL_UART_Init(&huart1) != HAL_OK)
    {
        Error_Handler();
    }
}


/**
	* 做主机时使用
	*
	*
**/		
//  Modbus 读保持寄存器命令帧（功能码 0x03）
uint16_t build_modbus_read_cmd(uint8_t device_addr, uint16_t reg_addr, uint16_t reg_num, uint8_t *out_buf) 
{
    uint16_t i = 0;

    out_buf[i++] = device_addr;             // 设备地址
    out_buf[i++] = 0x03;                    // 功能码 0x03 读保持寄存器
    out_buf[i++] = (reg_addr >> 8) & 0xFF;  // 起始寄存器地址高位
    out_buf[i++] = reg_addr & 0xFF;         // 起始寄存器地址低位
    out_buf[i++] = (reg_num >> 8) & 0xFF;   // 读取寄存器数量高位
    out_buf[i++] = reg_num & 0xFF;          // 读取寄存器数量低位

    uint16_t crc = modbus_crc16(out_buf, i);
    out_buf[i++] = crc & 0xFF;              // CRC低位
    out_buf[i++] = crc >> 8;                // CRC高位

    return i;  // 命令帧总长度
}

// Modbus 功能码0x10的写寄存器命令
// data: uint16_t 数组，每个元素是一个寄存器的数据（2字节）
// 返回帧长度
uint16_t build_modbus_write_cmd(uint8_t device_addr, uint8_t func_code,
																uint16_t reg_addr, uint16_t reg_count,
                                uint16_t *data, uint8_t *out_buf) 
{
    uint8_t byte_count = reg_count * 2;
    uint16_t i = 0;

    out_buf[i++] = device_addr;                    // 设备地址
    out_buf[i++] = func_code;                      // 功能码（0x10）
    out_buf[i++] = (reg_addr >> 8) & 0xFF;         // 起始寄存器地址高8位
    out_buf[i++] = reg_addr & 0xFF;                // 起始寄存器地址低8位
    out_buf[i++] = (reg_count >> 8) & 0xFF;        // 寄存器数量高8位
    out_buf[i++] = reg_count & 0xFF;               // 寄存器数量低8位
    out_buf[i++] = byte_count;                     // 数据字节数

    // 写入数据（每个寄存器2字节）
    for (uint16_t j = 0; j < reg_count; j++) {
        out_buf[i++] = (data[j] >> 8) & 0xFF;
        out_buf[i++] = data[j] & 0xFF;
    }

    // 计算 CRC
    uint16_t crc = modbus_crc16(out_buf, i);
    out_buf[i++] = crc & 0xFF;    // CRC低位
    out_buf[i++] = crc >> 8;      // CRC高位

    return i; // 返回命令总长度
}


