#include "motor_dev/rs485_driver.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <chrono>
#include <thread>
#include "motor_dev/rs485_driver.hpp"

namespace motor_dev
{

    std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
        std::vector<uint8_t> packet;
        packet.push_back(0x55);  // 同步字节1
        packet.push_back(0xAA);  // 同步字节2
        packet.push_back(0x1E);  // 30字节
        
        packet.push_back(0x01);
        packet.push_back(0x01);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x0a);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x00);

        packet.push_back(cmd);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x00);
        packet.push_back(0x08);
        packet.push_back(0x00);
        packet.push_back(0x00);
        
        for (uint8_t i = 0; i < 8; i++) {
            packet.push_back(data[i]); 
        }
        packet.push_back(0x00);
        bool ret = sendData(packet);
        std::vector<uint8_t> rx_data;
        if(ret){
            rx_data.clear();
            ret=receiveData(rx_data, 40, 1);
            if(ret){
                printf("<---:");
                for(auto d:rx_data)
                    printf("%02x ",d);
                printf("\n");
                return rx_data;
            }
        }
        return rx_data;
    }

    const unsigned char CRC8Table[]={  
        0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
        157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
        35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,  
        190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,  
        70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,  
        219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,  
        101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,  
        248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,  
        140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,  
        17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,  
        175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,  
        50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,  
        202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,  
        87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,  
        233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,  
        116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53  
    }; 
    /**
        *	@name		CRC8_Table
    * @brief  This function is used to Get CRC8 check value.
        * @param  p :Array pointer. counter :Number of array elements.
    * @retval crc8
    */
    unsigned char CRC8_Table(unsigned char *p, int counter)  
    {  
        unsigned char crc8 = 0;  
    
        for( ; counter > 0; counter--)
        {  
        crc8 = CRC8Table[crc8^*p];  
        p++;  
        }  
        return(crc8);  
    }

    uint8_t crc8_maxim_direct(const uint8_t *data, size_t len) {
        uint8_t crc = 0x00; // 初始值
        const uint8_t poly = 0x31;
        
        for (size_t i = 0; i < len; i++) {
            crc ^= data[i];
            for (uint8_t j = 0; j < 8; j++) {
                crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0);
            }
        }
        return crc;
    }
    RS485Driver::RS485Driver()
    {

    }
    
    RS485Driver::~RS485Driver()
    {
        closePort();
    }
    
    speed_t RS485Driver::convertBaudRate(int baud_rate)
    {
        switch (baud_rate)
        {
            case 9600: return B9600;
            case 19200: return B19200;
            case 38400: return B38400;
            case 57600: return B57600;
            case 115200: return B115200;
            case 230400: return B230400;
            case 460800: return B460800;
            case 500000: return B500000;
            case 576000: return B576000;
            case 921600: return B921600;
            default: return B0;
        }
    }
    
    bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
    {
        // 打开串口
        //com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        com_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY);
        if (com_fd_ == -1)
        {
            std::cerr << "无法打开串口: " << port_name << std::endl;
            return false;
        }
        
        // 保存原始配置
        if (tcgetattr(com_fd_, &original_termios_) == -1)
        {
            std::cerr << "无法获取串口属性" << std::endl;
            close(com_fd_);
            com_fd_ = -1;
            return false;
        }
        
        // 配置串口
        struct termios tty;
        memset(&tty, 0, sizeof(tty));
        
        // 设置波特率
        speed_t speed = convertBaudRate(baud_rate);
        if (speed == B0)
        {
            std::cerr << "不支持的波特率: " << baud_rate << std::endl;
            closePort();
            return false;
        }
        
        cfsetospeed(&tty, speed);
        cfsetispeed(&tty, speed);
        
        // 8位数据位，无奇偶校验，1位停止位
        tty.c_cflag &= ~PARENB;  // 无奇偶校验
        tty.c_cflag &= ~CSTOPB;  // 1位停止位
        tty.c_cflag &= ~CSIZE;   // 清除数据位设置
        tty.c_cflag |= CS8;      // 8位数据位
        
        // 禁用硬件流控制
        tty.c_cflag &= ~CRTSCTS;
        
        // 启用接收器，设置本地模式
        tty.c_cflag |= (CLOCAL | CREAD);
        
        // 禁用软件流控制
        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
        
        // 原始输入模式
        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
        
        // 原始输出模式
        tty.c_oflag &= ~OPOST;
        
        // 设置读取超时
        tty.c_cc[VTIME] = 10;  // 1秒超时
        tty.c_cc[VMIN] = 0;
        
        // 应用配置
        if (tcsetattr(com_fd_, TCSANOW, &tty) != 0)
        {
            std::cerr << "无法设置串口属性" << std::endl;
            closePort();
            return false;
        }
        std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
        return true;
    }
    
    void RS485Driver::closePort()
    {
        if (com_fd_>0)
        {
            // 恢复原始配置
            tcsetattr(com_fd_, TCSANOW, &original_termios_);
            close(com_fd_);
            com_fd_ = -1;
            std::cout << "串口已关闭" << std::endl;
        }
    }
    
    bool RS485Driver::sendData(const std::vector<uint8_t> &data)
    {
        static int idx=0;
        idx+=1;
        printf("[%03d]-->",idx);
        for(auto d:data)
            printf("%02x ",d);
        printf("\n");
        ssize_t bytes_written = write(com_fd_, data.data(), data.size());
        if (bytes_written != static_cast<ssize_t>(data.size()))
        {
            std::cerr << "rs485,fd:"<<com_fd_<<",发送ERR:" <<bytes_written<<"/"<<data.size()<<std::endl; 
            return false;
        }
        return true;
    }
    
    bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
    {
        data.clear();
        uint8_t buffer[256];
        ssize_t bytes_read;
        auto start_time = std::chrono::steady_clock::now();
        static int idx=0;
        idx+=1;
        while (data.size() < max_length)
        {
            // 检查超时
            auto current_time = std::chrono::steady_clock::now();
            auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                current_time - start_time).count();
            
            if (elapsed_ms >= timeout_ms)
            {
                break;  // 超时
            }
            
            // 尝试读取数据
            bytes_read = read(com_fd_, buffer, std::min(sizeof(buffer), max_length - data.size()));
            
            if (bytes_read > 0)
            {
                data.insert(data.end(), buffer, buffer + bytes_read);
                start_time = std::chrono::steady_clock::now();  // 重置超时计时器
            }
            else if (bytes_read < 0)
            {
                std::cerr <<idx<< "接收数据错误" << std::endl;
                return false;
            }
            else
            {
                // 没有数据，短暂休眠
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
        }
        
        return !data.empty();
    }

    int RS485Driver::bm_query_id(){
        std::vector<uint8_t> command;
        command.push_back(0);
        command.push_back(0x80);
        command.push_back(0);
        command.push_back(0x65);
        command.push_back(1);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        uint8_t crc8 = CRC8_Table(command.data(), 10);
        //uint8_t crc8=crc8_maxim_direct(command.data(),10);
        command.push_back(crc8);
        bool ret = sendData(command);
        if(ret){
            std::vector<uint8_t> data;
            bool ret=receiveData(data, 20, 1);
            if(ret){
                printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
            }else{
                printf("recv error!\n");
            }
        }
        return 0;
        //00 32 01 00 01 00 01 00 01 00 7f 
        //00 33 01 00 01 00 01 00 01 00 3c 
    }


    int RS485Driver::bm_set_mode(uint8_t motor_id, uint8_t mode){
        std::vector<uint8_t> command;
        command.push_back(0);
        command.push_back(0x36);
        command.push_back(motor_id);
        command.push_back(0x1c);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        command.push_back(2);
        command.push_back(0);
        command.push_back(0);
        //uint8_t crc8 = CRC8_Table(command.data(), 10);
        //command.push_back(crc8);
        bool ret = sendData(command);
        if(ret){
            std::vector<uint8_t> data;
            data.clear();
            ret=receiveData(data, 20, 1);
            if(ret){
                printf("<---:");
                for(auto d:data)
                    printf("%02x ",d);
                printf("\n");
            }
        }
        return 0;
    }
    
    
    int RS485Driver::bm_set_pos(float angle1,float angle2){
        std::vector<uint8_t> command;
        //if(angle>360.0)
        //    angle=360.0;
        int32_t pos=angle1/3.6;
        uint8_t high1 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low1  = pos & 0xFF;         // 低8位
        pos=angle2/3.6;
        uint8_t high2 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low2  = pos & 0xFF;         // 低8位
        //if(motor_id<5){
            command.push_back(0x00);
            command.push_back(0x32);
            command.push_back(high1);
            command.push_back(low1);
            command.push_back(high2);
            command.push_back(low2);
            command.push_back(0);
            command.push_back(0);
            command.push_back(0);
            command.push_back(0);
        //}
        unsigned char crc8 = CRC8_Table(command.data(), 10);
        command.push_back(crc8);
        bool ret = sendData(command);
        if(ret){
            std::vector<uint8_t> data;
            data.clear();
            ret=receiveData(data, 20, 1);
            if(ret){
                printf("<---:");
                for(auto d:data)
                    printf("%02x ",d);
                printf("\n");
            }
        }
        return 0;
    }
    int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
        std::vector<uint8_t> command;
        int32_t pos=speed1*10;
        uint8_t high1 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low1  = pos & 0xFF;         // 低8位
        pos=speed2*10;
        uint8_t high2 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low2  = pos & 0xFF;         // 低8位
        pos=speed3*10;
        uint8_t high3 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low3  = pos & 0xFF;         // 低8位
        pos=speed4*10;
        uint8_t high4 = (pos >> 8) & 0xFF;  // 高8位
        uint8_t low4  = pos & 0xFF;         // 低8位

        command.push_back(high1);
        command.push_back(low1);
        command.push_back(high2);
        command.push_back(low2);
        command.push_back(high3);
        command.push_back(low3);
        command.push_back(high4);
        command.push_back(low4);
        printf("set_speed\n");
        sendCan(0x32,command);
        return 0;
    }
    int RS485Driver::bm_set_enable(uint8_t enable){
        std::vector<uint8_t> command;
        command.push_back(enable);
        command.push_back(enable);
        command.push_back(enable);
        command.push_back(enable);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        printf("set_enable\n");
        sendCan(0x38,command);
        return 0;
    }
     int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
        std::vector<uint8_t> command;
        command.push_back(motor_id);
        command.push_back(param);
        
        command.push_back((uint8_t)(val));
        command.push_back((uint8_t)(val>>8));
        command.push_back((uint8_t)(val>>16));
        command.push_back((uint8_t)(val>>24));
        command.push_back(0);
        command.push_back(0);
        printf("set_param:%d,%d,%d\n",motor_id,param,val);
        sendCan(0x36,command);
        return 0;
    }
    float RS485Driver::bm_get_pos(uint8_t motor_id){
        std::vector<uint8_t> command;
        command.push_back(0);
        command.push_back(0x35);
        command.push_back(motor_id);
        command.push_back(0x0d);
        command.push_back(4);
        command.push_back(0x0b);
        command.push_back(3);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        unsigned char crc8 = CRC8_Table(command.data(), 10);
        command.push_back(crc8);
        bool ret = sendData(command);
        if(ret){
            std::vector<uint8_t> data;
            data.clear();
            ret=receiveData(data, 20, 1);
            if(ret){
                printf("<---:");
                for(auto d:data)
                    printf("%02x ",d);
                printf("\n");
                uint8_t nid=0x70+motor_id;
                if(data[0]==0&&data[1]==nid){
                    //uint16_t pos = (data[2]<<8)+(data[3]&0xff);
                    //float angle=360.0*pos/32768.0;
                    int16_t pos=(data[6]<<8)+(data[7]&0xff);
                    float angle=3.60f*pos;
                    //printf("id:%d,pos:%d\n",motor_id,pos);
                    return angle;
                    ///printf("pos:%d,angle:%.1f\n",pos,angle);
                }
                else{

                }
                //float angle = pos * 360.0 /255.0;
                ///float angle=0;
                
            }
        }
        return -1;
    }
    int RS485Driver::bm_get_speed(){
        std::vector<uint8_t> command;
        command.push_back(1);
        command.push_back(5);
        command.push_back(11);
        command.push_back(13);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        command.push_back(0);
        std::vector<uint8_t> rx=sendCan(0x35,command);
        for(int x=0;x<rx.size();x++)
            if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
                uint8_t motor_id=rx[x+3]-0x70;
                int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
                printf("id:%d,speed:%d\n",motor_id,speed);
            }
        return 0;
    }
    //mt
    void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
        uint16_t crc = 0xFFFF;
        for (uint8_t byte : data) {
            crc ^= byte;
            for (int i = 0; i < 8; i++) {
                if (crc & 0x0001) {
                    crc >>= 1;
                    crc ^= 0xA001;
                } else {
                    crc >>= 1;
                }
            }
        }
        data.push_back(static_cast<uint8_t>(crc & 0xFF));
        data.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF));
    }

    //
    int RS485Driver::mt_set_pos(uint8_t motor_id, float angle){
        std::lock_guard<std::mutex> lock(uart_mutex);
        std::vector<uint8_t> packet;
        packet.push_back(0x3e);
        packet.push_back(motor_id);
        packet.push_back(0x08);
        
        int32_t pos = static_cast<int32_t>(angle * 100);
        packet.push_back(0xa4);
        packet.push_back(0);//方向
        packet.push_back(0x10);
        packet.push_back(0x00);//速度
        packet.push_back((uint8_t)(pos));//位置,pos的低8位
        packet.push_back((uint8_t)(pos>>8));//位置，pos的高8位
        packet.push_back((uint8_t)(pos>>16));
        packet.push_back((uint8_t)(pos>>24));
        add_crc16(packet);
        bool ret=sendData(packet);
        if(!ret)
            printf("send error\n");
        return 0;
    }
    float RS485Driver::mt_get_pos(uint8_t motor_id){
        std::lock_guard<std::mutex> lock(uart_mutex);

        std::vector<uint8_t> packet;
        packet.push_back(0x3e);
        packet.push_back(motor_id);
        packet.push_back(0x08);
        //
        packet.push_back(0x92);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        add_crc16(packet);
        bool ret=sendData(packet);
        if(ret){
            #if 1
            std::vector<uint8_t> data;
            data.clear();
            ret=receiveData(data, 20, 1);
            if(ret){
                if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
                    uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
                    ///printf("pos:%d\n",pos);
                    float angle = pos /100.0;
                    return angle;
                }
                return 9999;
            }
            #else
            static uint8_t com_buf[128];
            memset(com_buf,0,128);
            ret = read(com_fd_, com_buf, 128);
            ///bool ret=receiveData(data, 20, 1000);
            printf("recv:");
            for(int i=0;i<ret;i++)
                printf("%02x ",com_buf[i]);
            printf("\n");
            #endif
        }
        return 9999;
    }
    int RS485Driver::mt_query_id(){
        std::vector<uint8_t> packet;
        packet.push_back(0x3e);
        packet.push_back(0xcd);
        packet.push_back(0x08);
        //
        packet.push_back(0x79);
        packet.push_back(0);
        packet.push_back(1);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        packet.push_back(0);
        add_crc16(packet);
        bool ret=sendData(packet);
        if(ret){
            std::vector<uint8_t> data;
            ret=receiveData(data, 20, 1);
            if(ret){
                uint8_t id= data[10];
                return id;
            }
        }
        return -1;
    }
}
