#include "can_bus.h"
#include <thread>
#include "message_queue.h"
#include <iomanip>
#include <chrono>   // 用于时间相关的功能
#include <thread>   // 用于线程操作

#define	GET_BIT(x, bit)	((x & (1 << bit)) >> bit)



float Canb2f(uint8_t m0, uint8_t m1, uint8_t m2, uint8_t m3)
{
    //求符号位
    float sig = 1.;
    if (m0 >=128.)
        sig = -1.;
  
    //求阶码
    float jie = 0.;
     if (m0 >=128.)
    {
        jie = m0-128.  ;
    }
    else
    {
        jie = m0;
    }
    jie = jie * 2.;
    if (m1 >=128.)
        jie += 1.;

    jie -= 127.;
 
    //求尾码
 
    float tail = 0.;
    if (m1 >=128.)
        m1 -= 128.;
    tail =  m3 + (m2 + m1 * 256.) * 256.;
    tail  = (tail)/8388608;   //   8388608 = 2^23
 
    float f = sig * pow(2., jie) * (1+tail);
 
    return f;
}

int16_t make_int16(unsigned char h,unsigned char l) //解析
{
  int16_t hi=(int16_t)(h&0x00FF);        // 高8位
  int16_t low= (int16_t)(l&0x00FF);      // 低8位

   return (hi<<8)|low;
}

CCanBusSensor::CCanBusSensor(CMessageQueue* q) :_message(q)
{
    _run = false;

    for (int32_t i = 0; i < sizeof(Sendframe)/sizeof(can_frame); i++)
        memset(&Sendframe[i], 0, sizeof(can_frame));
}

void CCanBusSensor::SetCanBusSensor(SensorCanBus<CCanBusSensor>* can)
{
    _canbus = can;
}

void CCanBusSensor::CanSendProc()
{
    while (_run)
    {
       for(int32_t i=0;i<sizeof(Sendframe)/sizeof(can_frame);i++)
        {
            if(Sendframe[i].can_id != 0)
            {
                if(Sendframe[i].can_id == 0x184)
                {
                    _message->_dst  = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    //printf("data delay:%lld\r\n",_message->_dst - _message->_source);
                }
                _canbus->Write(&Sendframe[i]);
                Sendframe[i].can_id  = 0;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(5)); 

    }
    
   
}

void CCanBusSensor::Start()
{
    _run = true;

    _can_thread = std::thread(&CCanBusSensor::CanSendProc, this);

    Direction = 0;//方向
    Hand_Throttle = 0;//手油门
    Foot_Throttle = 0;//脚油门
    Brake = 0;//刹车
    _message->DataMqtt_SendDate = false;
    _message->DataMqtt_curTick = 0;
}


void CCanBusSensor::Run()
{
    
}
  
void CCanBusSensor::Stop()
{
    if (_run)
    {
        _run = false;
        _can_thread.join();
    }

    for (int32_t i = 0; i < sizeof(Sendframe)/sizeof(can_frame); i++)
        memset(&Sendframe[i], 0, sizeof(can_frame));

    Direction = 0;//方向
    Hand_Throttle = 0;//手油门
    Foot_Throttle = 0;//脚油门
    Brake = 0;//刹车

    _message->DataMqtt_SendDate = false;
    _message->DataMqtt_curTick = 0;
}


void CCanBusSensor::Notify(struct can_frame *date) //上传状态信息
{
    //std::cout << std::hex << date->can_id << std::endl;
    if(_run)
    {
        
        switch (date->can_id)
        {
        case 0x283:
            _data.Main_pump_1_pressure = make_int16(date->data[1],date->data[0]);//主泵1压力
            _data.Main_pump_2_pressure = make_int16(date->data[3],date->data[2]);//主泵2压力
            break;
        
        case 0x285:
            _data.Stick_angle = make_int16(date->data[1],date->data[0]);//斗杆角度
            _data.Boom_angle = make_int16(date->data[3],date->data[2]);//动臂角度
            break;

        case 0x286:
            _data.Hydraulic_oil_temperature = make_int16(date->data[1],date->data[0]);//液压油油温
            _data.Fuel_level = (int32_t)date->data[6];//燃油油位
            break;
        
        case 0x28A:
            _data.Travel_speed = make_int16(date->data[1],date->data[0]);//行驶速度
            _data.Engine_temperature = make_int16(date->data[3],date->data[2]);//发动机水温
            _data.Hand_gear = make_int16(date->data[5],date->data[4]);//水箱水位
            _data.Actual_gear = make_int16(date->data[7],date->data[6]);//发动机机油压力
            break;

        case 0x28B:
            _data.Engine_speed = make_int16(date->data[1],date->data[0]);//发动机转速
            {
                long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

                if(_data.Engine_speed > 700 && _message->bStopedCar && tick-_message->_curStopTick > 500)
                    _message->bStopedCar = false;
            }
            
            break;

        case 0x28D:
            _data.Gripper_height = Canb2f(date->data[3], date->data[2], date->data[1], 
                date->data[0]);//抓具高度
            _data.amplitude = Canb2f(date->data[7], date->data[6], date->data[5], 
                date->data[4]);//幅度
            break;

        case 0x28F:
            _data.Idle_protection = (date->data[3] & 0x80) == 0x80 ? 1 : 0;//怠速保护

            _data.Front_toggle = (date->data[1] & 0x08) == 0x08 ? 1 : 0;//前轮对中
            _data.Back_toggle = (date->data[1] & 0x10) == 0x10 ? 1 : 0;//后轮对中


            _data.interlock = (date->data[5] & 0x08) == 0x08 ? 1 : 0;//启动联锁

            _data.safety_switch = (date->data[6] & 0x80) == 0x80 ? 1 : 0;//安全开关阀异常

            _data.arm_lift_pilot = (date->data[7] & 0x01) == 0x01 ? 1 : 0;//大臂提升先导比例阀异常
            _data.arm_lowering_pilot = (date->data[7] & 0x02) == 0x02 ? 1 : 0;//大臂下降先导比例阀异常
            _data.lever_lifting_pilot = (date->data[7] & 0x04) == 0x04 ? 1 : 0;//斗杆提升先导比例阀异常
            _data.rod_lowering_pilot = (date->data[7] & 0x08) == 0x08 ? 1 : 0;//斗杆下降先导比例阀异常
            _data.left_rotary_pilot = (date->data[7] & 0x10) == 0x10 ? 1 : 0;//左回转先导比例阀异常
            _data.right_rotary_pilot = (date->data[7] & 0x20) == 0x20 ? 1 : 0;//右回转先导比例阀异常
            _data.grab_open_pilot = (date->data[7] & 0x40) == 0x40 ? 1 : 0;//抓斗开启先导比例阀异常
            _data.grab_close_pilot = (date->data[7] & 0x80) == 0x80 ? 1 : 0;//抓斗闭合先导比例阀异常

            _data.safety_valves = (date->data[4] & 0x10) == 0x10 ? 1 : 0;//安全开关阀

            break;

        case 0x280:
            memcpy(_data.Error_Buff,date->data,date->can_dlc);
            SendStatusToMSG();
            break;
        
        
        

        default:
            break;
        }
    }
}


/*------------------小车控制起-------------------*/

void CCanBusSensor::order(cannet_frame* frame){
    
        cannet_frame& carframe = frame[0];          
        Sendframe[0].can_id = 0x421;
        Sendframe[0].can_dlc = carframe.dlc;

        memcpy(Sendframe[0].data,order_on,carframe.dlc);
}

bool CCanBusSensor::car_stop(cannet_frame* frame){

        cannet_frame& carframe = frame[0];                

        switch (carframe.data[2])
        {
            case 0x00:
                std::copy(id_init,id_init+8,id_111);
                stop_flag = true;
                // std::cout<<"car stop!"<<std::endl;
            break;
   
            default:
                stop_flag = false;
                // std::cout<<"car ends stop..."<<std::endl;
            break;
        }
        // memset(Sendframe[0].data, 0, sizeof(Sendframe[0].data));
    return stop_flag;
}

void CCanBusSensor::toggleLamp(cannet_frame* frame){

        cannet_frame& carframe = frame[0];             
        Sendframe[2].can_id = 0x121;
        Sendframe[2].can_dlc = carframe.dlc;

        switch (carframe.data[0])
                {
                    case 0x00:
                        memcpy(Sendframe[2].data,headlamp_off,carframe.dlc); //前后车灯关
                        // std::cout<<"前车灯关,后车灯关 "<<std::endl;
                    break;
                
                    case 0x10:
                        memcpy(Sendframe[2].data,headlamp_on,carframe.dlc); //前车灯开
                        // std::cout<<"前车灯开,后车灯关 "<<std::endl;
                    break;
                    
                    case 0x08:
                        memcpy(Sendframe[2].data,taillamp_on,carframe.dlc); //后车灯开
                        // std::cout<<"前车灯关,后车灯开 "<<std::endl;
                    break;
                }
}


int CCanBusSensor::gearselection(cannet_frame* frame){
    
    cannet_frame& carframe = frame[0];
    
    CCanBusSensor::speedlevel(&carframe);
    switch (carframe.data[5])
    {
        case 0x10:              //前进档
            return 1;

        case 0x00:              //空档
            return 2;

        case 0x40:              //后退档
            return 3;

        default:
        return -1;
        
    }
}


void CCanBusSensor::speedlevel(cannet_frame* frame){

    cannet_frame& gearframe = frame[0];

    switch (gearframe.data[5])
    {
        case 0x14:              //前进档升档
            if(speed_flag < 14)
               speed_flag++;
            std::this_thread::sleep_for(std::chrono::seconds(2));
        break;

        case 0x11:              //前进档降档
            if(speed_flag > 11)
                speed_flag--;
            std::this_thread::sleep_for(std::chrono::seconds(2));
        break;

        case 0x44:              //后退档升档
            if(speed_flag < 14)
                speed_flag++;
            std::this_thread::sleep_for(std::chrono::seconds(2));
        break;

        case 0x41:              //后退档升档
            if(speed_flag > 11)
                speed_flag--;
            std::this_thread::sleep_for(std::chrono::seconds(2));
        break;

        default:
        break;
    }

}


void CCanBusSensor::runSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1) {

    // unsigned int mapped_value;
    // cout<<"speed_flag before change: "<<speed_flag<<endl;
    // switch (speed_flag)
    //     {
    //         case 11:
    //             mapped_value = static_cast<unsigned int>(input-2) * 375 / 0x7F;
    //             cout<<"run speed: 11"<<mapped_value<<endl;
    //         break;

    //         case 12:
    //             mapped_value = static_cast<unsigned int>(input-2) * 750 / 0x7F;
    //             cout<<"run speed: 12"<<mapped_value<<endl;
    //         break;

    //         case 13:
    //             mapped_value = static_cast<unsigned int>(input-2) * 1125 / 0x7F;
    //             cout<<"run speed: 13"<<mapped_value<<endl;
    //         break;

    //         case 14:
    //             mapped_value = static_cast<unsigned int>(input-2) * 1500 / 0x7F;
    //             cout<<"run speed: 14"<<mapped_value<<endl;
    //         break;

    //         default:
    //         break;
    //     }
    // cout<<"speed_flag after change: "<<speed_flag<<endl;

    // 将输入值从 0x00 - 0xFF 映射到 0x0000 - 0x05DC (0 - 1500)
    // unsigned int mapped_value = static_cast<unsigned int>(input-2) * 1500 / 0x7F;
    // cout<<"run speed: "<<mapped_value<<endl;
    // 提取高八位和低八位
    byte0 = static_cast<unsigned char>((mapped_value >> 8) & 0xFF); // 高八位
    byte1 = static_cast<unsigned char>(mapped_value & 0xFF);        // 低八位
}

void CCanBusSensor::backSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1) {
    
    // unsigned int mapped_value;
    // switch (speed_flag)
    //     {
    //         case 11:
    //             mapped_value = static_cast<unsigned int>(input-2) * -375 / 0x7F;
    //             cout<<"back speed: 11"<<mapped_value<<endl;
    //         break;

    //         case 12:
    //             mapped_value = static_cast<unsigned int>(input-2) * -750 / 0x7F;
    //             cout<<"back speed: 12"<<mapped_value<<endl;
    //         break;

    //         case 13:
    //             mapped_value = static_cast<unsigned int>(input-2) * -1125 / 0x7F;
    //             cout<<"back speed: 13"<<mapped_value<<endl;
    //         break;

    //         case 14:
    //             mapped_value = static_cast<unsigned int>(input-2) * -1500 / 0x7F;
    //             cout<<"back speed: 14"<<mapped_value<<endl;
    //         break;

    //         default:
    //         break;
    //     }
    
    // 将输入值从 0x00 - 0xFF 映射到 -1500 到 0 的范围
    int mapped_value = static_cast<int>(input-2) * -1500 / 0x7F;
    cout<<"back speed: "<<mapped_value<<endl;

    // 将负数调整为 0xFA24 ( -1500) 到 0x0000 的正数范围
    unsigned short adjusted_value = static_cast<unsigned short>(mapped_value + 0x10000);

    // 提取高八位和低八位
    byte0 = static_cast<unsigned char>((adjusted_value >> 8) & 0xFF); // 高八位
    byte1 = static_cast<unsigned char>(adjusted_value & 0xFF);        // 低八位
}


void CCanBusSensor::move_run(cannet_frame* frame){
    
    cannet_frame& carframe = frame[0];     
        
    if(!(carframe.data[5]>0x02 && carframe.data[5]<=0x7F))
    {
        if(carframe.data[4]>0x02 && carframe.data[4]<=0x7F)
        {
            CCanBusSensor::runSpeed(carframe.data[4],id_111[0],id_111[1]);
            
            run_flag = true;
            // std::cout<<"油门前进"<<std::endl;
        }
        else
        {
            id_111[0] = 0x00;
            id_111[1] = 0x00;
            run_flag = true;
            // std::cout<<"前进油门松开"<<std::endl;
        }
    }
    else  
    {   
        std::copy(id_init,id_init+8,id_111);
        run_flag = false;

        // std::cout<<"前进档：刹车"<<std::endl;
    }

}


void CCanBusSensor::move_neutral(cannet_frame* frame){
    
    cannet_frame& carframe = frame[0];
    if(!(carframe.data[5]>0x02 && carframe.data[5]<=0x7F))
    {
        id_111[0] = 0x00;
        id_111[1] = 0x00;
    }
    else
    {
        std::copy(id_init,id_init+8,id_111);
    }

}


void CCanBusSensor::move_back(cannet_frame* frame){

    cannet_frame& carframe = frame[0];

    if(!(carframe.data[5]>0x02 && carframe.data[5]<=0x7F))
    {
        if(carframe.data[4]>0x02 && carframe.data[4]<=0x7F)
        {
            CCanBusSensor::backSpeed(carframe.data[4],id_111[0],id_111[1]);
            run_flag = true;

            // std::cout<<"油门后退"<<std::endl;
        }
        else
        {
            id_111[0] = 0x00;
            id_111[1] = 0x00;
            run_flag = true;
            
            // std::cout<<"后退油门松开"<<std::endl;
        }
    }
    else  
    {   
        std::copy(id_init,id_init+8,id_111);
        run_flag = false;

        // std::cout<<"后退档：刹车"<<std::endl;
    }
}

void CCanBusSensor::leftSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1){

    // 将输入值从 0x00 - 0xFA 映射到 0x0000 - 0x020B (0 - 523)
    unsigned int mapped_value = static_cast<unsigned int>(input) * 523 / 0xFA;
    // cout<<"run speed: "<<mapped_value<<endl;
    // 提取高八位和低八位
    byte0 = static_cast<unsigned char>((mapped_value >> 8) & 0xFF); // 高八位
    byte1 = static_cast<unsigned char>(mapped_value & 0xFF);        // 低八位
}

void CCanBusSensor::rightSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1){

    // 将输入值从 0x00 - 0xFA 映射到 -523 到 0 的范围
    int mapped_value = static_cast<int>(input) * -523 / 0xFA;
    // cout<<"back speed: "<<mapped_value<<endl;

    // 将负数调整为 0xFA24 ( -1500) 到 0x0000 的正数范围
    unsigned short adjusted_value = static_cast<unsigned short>(mapped_value + 0x10000);

    // 提取高八位和低八位
    byte0 = static_cast<unsigned char>((adjusted_value >> 8) & 0xFF); // 高八位
    byte1 = static_cast<unsigned char>(adjusted_value & 0xFF);        // 低八位
}

void CCanBusSensor::move_rot(cannet_frame* frame){

        cannet_frame& carframe = frame[0];

        switch(carframe.data[0])
        {
            case 0x01:
                    
                id_111[2] = 0x00;
                id_111[3] = 0x00;
                
                // cout<<"No rotation"<<endl;
            break;

            case 0x04:

                // id_111[2] = 0x00;
                // id_111[3] = 0xc8;
                CCanBusSensor::leftSpeed(carframe.data[1], id_111[2], id_111[3]);
                // cout<<"turn left"<<endl;
            break;

            case 0x10:

                // id_111[2] = 0xFF;
                // id_111[3] = 0x38;
                CCanBusSensor::rightSpeed(carframe.data[1], id_111[2], id_111[3]);
                // cout<<"turn right"<<endl;
            break;
        }
}



/*------------------小车控制止-------------------*/
/* ****Sendframe[3]和Sendframe[4]不能用***** */
/* ****不需要order_on,就能直接用指令控制***** */

void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)  //下发控制指令
{

    for (int32_t i = 3; i >= 0; i--) //count=4
    {
        cannet_frame& frame = frames[i];

        int32_t canid = frame.canid;
        Sendframe[3].can_dlc = frame.dlc;
        //std::cout << std::hex << canid << std::endl;
        CCanBusSensor::order(&frame);
        switch (canid)
        {
            case 0x181:       // receive from frame[3]

                // Sendframe[0].can_dlc = frame.dlc;
                // Sendframe[1].can_dlc = frame.dlc;
                // cout<<"receive 181: "<<endl;

                // Sendframe[0].can_id = 0x421;
                // memcpy(Sendframe[0].data,order_on,frame.dlc); //前后车灯关
                // Sendframe[1].can_id = 0x121;
                // memcpy(Sendframe[1].data,headlamp_off,frame.dlc); //前后车灯关
                // cout<<"前后车灯关"<<endl;
                // memcpy(Sendframe[2].data,move_forward,frame.dlc); //前进
               
                
                CCanBusSensor::toggleLamp(&frame);
                if(!CCanBusSensor::car_stop(&frame))
                {
                    // cout<<"car ends stopping!"<<endl;    
                    switch (gear)
                    {
                        case 1:
                            CCanBusSensor::move_run(&frame);
                            // cout<<"case 1:前进档"<<endl;
                        break;

                        case 2:
                            CCanBusSensor::move_neutral(&frame);
                            // cout<<"case 2:空档"<<endl;
                        break;

                        case 3:
                            CCanBusSensor::move_back(&frame);
                            // cout<<"case 3:后退档"<<endl;
                        break;

                        default:
                            // cout<<"gear error"<<endl;
                            cout<<"升档降档"<<endl;
                        break;
                    }
                }
                
            break;

            case 0x0CFDD633:  // receive from frame[3]
            
                if(!(CCanBusSensor::car_stop(&frame)))
                {
                    // cout<<"car ends stopping!"<<endl;  
                    gear = CCanBusSensor::gearselection(&frame);
                    // cout<<"档位："<<gear<<endl;
                    CCanBusSensor::move_rot(&frame);
                }
                

            break;

            default:
            break;
        }
    } 
    Sendframe[3].can_id = 0x111;
    std::copy(id_111,id_111+8,Sendframe[3].data);
    
    // std::cout << "Array contents: ";
    // for (unsigned char value : id_111) {
    //     std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(value) << " ";
    // }
    // std::cout << std::endl;
}

void CCanBusSensor::SendStatusToMSG()
{
    RemoNet::State req;
    req.set_engine_speed(_data.Engine_speed);
    req.set_travel_speed(_data.Travel_speed);
    req.set_fuel_level(_data.Fuel_level);
    req.set_engine_temperature(_data.Engine_temperature);
    req.set_hydraulic_oil_temperature(_data.Hydraulic_oil_temperature);
    req.set_main_pump_1_pressure(_data.Main_pump_1_pressure);
    req.set_main_pump_2_pressure(_data.Main_pump_2_pressure);
    req.set_hand_gear(_data.Hand_gear);
    req.set_actual_gear(_data.Actual_gear);
    req.set_gripper_height(_data.Gripper_height);
    req.set_amplitude(_data.amplitude);
    req.set_boom_angle(_data.Boom_angle);
    req.set_stick_angle(_data.Stick_angle);

    req.set_idle_protection(_data.Idle_protection);
    req.set_front_toggle(_data.Front_toggle);
    req.set_back_toggle(_data.Back_toggle);


    req.set_error_buff((char *)_data.Error_Buff,8);
   
    MessageHead Head;
    CIOBuffer pBuffer;
    Head.Command = RemoNet::CC_STATE;
    Head.Length = req.ByteSizeLong();
    Head.Serialize(pBuffer.Buffer);
    auto ptr = pBuffer.Buffer + MessageHead::Size();
    req.SerializeToArray(ptr, Head.Length);
    pBuffer.Length = MessageHead::Size() + Head.Length;

    _message->WritePacket(ChannelType::CHANNEL_CAR, pBuffer);
}


void CCanBusSensor::Emergency()
{
    if(model)
    {
        Sendframe[3].can_id = 0x184;
        Sendframe[3].can_dlc = 8;

        Sendframe[3].data[3] &= 0xFD;
        //_canbus->Write(&Sendframe[3]);
    }
}