#include "can_bus.h"
#include <thread>
#include "message_queue.h"
#include <math.h>


#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);
  int16_t low= (int16_t)(l&0x00FF);

   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)
            {
                // //接收到的frame
                // for (int i = 0; i < Sendframe[i]->can_dlc; i++) {
                //     printf("%02x ", Sendframe[i]->data[i]);
                // }
                // printf("\n");
                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;
}

//_data 
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:
            travelSpeed = make_int16(date->data[1],date->data[0]);   //行驶速度
            _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;
            }

            _data.engine_fmi = (static_cast<uint16_t>(date->data[3]) << 8 | static_cast<uint16_t>(date->data[2]));
            _data.engine_spn = (static_cast<uint16_t>(date->data[5]) << 8 | static_cast<uint16_t>(date->data[4]));
            _data.engine_num = (static_cast<uint16_t>(date->data[7]) << 8 | static_cast<uint16_t>(date->data[6]));
            
            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:

            fGear = (date->data[0] & 0x40) == 0x40 ? 1 : 0;        //前进档
            bGear = (date->data[0] & 0x80) == 0x80 ? 1 : 0;        //后退档
            gearStatus = (date->data[1] & 0x01) == 0x10 ? 1 : 0;   //实际档位

            _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;//左回转先导比例阀异常cab_start
            _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;//安全开关阀

            //增加
            memcpy(_data.all_Buff,date->data,date->can_dlc);

            _message -> SendStatusToServer( travelSpeed,fGear,bGear,gearStatus);

            break;

        case 0x280:
            memcpy(_data.Error_Buff,date->data,date->can_dlc);
            SendStatusToMSG();;
            break;
        
        case 0x384:
            // _data.function_code= make_int16(date->data[1],date->data[0])  ;
            // _data.main_add = make_int16(date->data[3],date->data[2]);
            // _data.sub_add = make_int16(date->data[5],date->data[4]);
            // _data.paramter = make_int16(date->data[7],date->data[6]);

            _data.function_code = (static_cast<uint16_t>(date->data[0]) << 8 | static_cast<uint16_t>(date->data[1]));
            _data.main_add = (static_cast<uint16_t>(date->data[2]) << 8 | static_cast<uint16_t>(date->data[3]));
            _data.sub_add = (static_cast<uint16_t>(date->data[4]) << 8 | static_cast<uint16_t>(date->data[5]));
            _data.paramter = (static_cast<uint16_t>(date->data[6]) << 8 | static_cast<uint16_t>(date->data[7]));
            break;

        default:
            break;
        }
    }
}



//sendframe 
void CCanBusSensor::OnMessage(cannet_frame* frames, int32_t count)
{
    // vehicle_status_t vehicle_status;
    for (int32_t i = 0; i < count; i++)
    {
        cannet_frame& frame = frames[i];

        int32_t canid = frame.canid;
        
        //std::cout << std::hex << canid << std::endl;
        switch (canid)
        {
            case 0x181:
                Sendframe[0].can_id = 0x181;
                Sendframe[0].can_dlc = frame.dlc;
                memcpy(Sendframe[0].data, frame.data, frame.dlc);
                control_can.accPedalH = make_int16(frame.data[1],frame.data[0]);   //手油门
                control_can.accPedalF = make_int16(frame.data[3],frame.data[2]);  //脚油门
                control_can.brakePedal =  make_int16(frame.data[5],frame.data[4]);  //刹车
                control_can.baseLegControl = make_int16(frame.data[7],frame.data[6]);  //支腿动作控制

                //std::cout << "revice 181" << std::endl;
            break;

            case 0x182:
                Sendframe[1].can_id = 0x182;
                Sendframe[1].can_dlc = frame.dlc;

                memcpy(Sendframe[1].data, frame.data, frame.dlc);
                control_can.taskJoint_1 = make_int16(frame.data[1], frame.data[0]);//第一个作业关节控制  
                control_can.taskJoint_2 = make_int16(frame.data[7], frame.data[6]);//第2个作业关节控制
                control_can.taskJoint_3 = make_int16(frame.data[3], frame.data[2]);//第3个作业关节控制
                control_can.toolControl = make_int16(frame.data[5],frame.data[4]);  //末端控制工具
                //std::cout << "182" << std::endl;
            break;

            case 0x183:
                Sendframe[2].can_id = 0x183;
                Sendframe[2].can_dlc = frame.dlc;
                memcpy(Sendframe[2].data, frame.data, frame.dlc);
                control_can.steeringWheel = make_int16(frame.data[3],frame.data[2]);   //方向盘转速
                control_can.endJoint = make_int16(frame.data[1],frame.data[0]);    //抓具旋转
            break;

            case 0x184:
            
                Sendframe[3].can_id = 0x184;
                Sendframe[3].can_dlc = frame.dlc;

                control_can.keyStatus = (frame.data[3] & 0x01 ? 1:0 ) + (frame.data[2] & 0x80 ? 1:0)*2;  //钥匙状态  0-熄火   1-上电   2-启动

                control_can.parkControl = frame.data[1] & 0x01 ? 1:0 ;   //驻车控制   1-1
                control_can.travelMode = frame.data[3] & 0x04? 1:0 ;   //驾驶模式   3-3
                control_can.eStop = frame.data[3] & 0x02 ? 1:0 ;         //急停开关   3-2
                control_can.directSwitch = frame.data[4] & 0x04 ? 1:0 ;  //前后切换   4-3

                control_can.gearControl = (frame.data[0] & 0x20 ? 1:0)  + (frame.data[0] & 0x40 ? 1:0) * 2 ;  //车辆档位控制  1-前进   2-后退

                control_can.hazardLight = frame.data[0] & 0x04 ? 1:0 ;   //双闪灯0-3

                control_can.travelLight = (frame.data[0] & 0x02 ? 1:0)  +(frame.data[0] & 0x01 ? 1:0)*2;   //行驶灯光  0-全关闭  1-近光   2-远光
                control_can.vehicleHorn = frame.data[1] & 0x02 ? 1:0 ;   //喇叭1-2
                control_can.silencedAlarm  = frame.data[2] & 0x40 ? 1:0; //消报警音按钮2-7100000data[2] & 0x20 ? 1:0) *2 ;  //2- 4*5  工作灯 0-全关闭  1-前大灯  2工作灯  3 全开

                control_can.turnSignal = (frame.data[0] & 0x08 ? 1:0) + (frame.data[0] & 0x10 ? 1:0) * 2 ;  //转向灯   0-关   1-近光灯   2-远光灯
                control_can.turnMode = (frame.data[3] & 0x08 ? 1:0) + (frame.data[3] & 0x10 ? 1:0) * 2 + (frame.data[3] & 0x20 ? 1:0) * 3 +(frame.data[3] & 0x40 ? 1:0) * 4;   //转向模式
                control_can.enableHydraulic = frame.data[2] & 0x01 ? 1:0;              //液压使能开关
                control_can.workLight = (frame.data[2] & 0x20 ? 1:0)  + (frame.data[2] & 0x40 ? 1:0)*2 ;                   //工作灯  0-关  1-前大灯  2-工作灯  3-全开
                control_can.bypassSwitch = frame.data[1] & 0x02 ? 1:0 ;                  //2- 1  旁通开关
                control_can.baseLegSwitch =(frame.data[1] & 0x04 ? 1:0)*1 +(frame.data[1] & 0x08  ? 1:0)*2 +(frame.data[1] & 0x10  ? 1:0)*4+(frame.data[1] & 0x20  ? 1:0)*8; //四个支腿选择开关
                control_can.cabLift = (frame.data[1] &0x40 ? 1:0) * 1 + (frame.data[1] & 0x80 ? 1:0) * 2;   //驾驶室升降  0 无   1-升  2-降
                control_can.esCabLift = frame.data[2] & 0x08 ? 1:0;    //驾驶室应急下降
                control_can.suckerSelect = frame.data[2] & 0x04 ? 1:0;          //吸盘选择开关
                control_can.errBasOperation = 0;    //基本操作故障
                control_can.errAccPedal = (frame.data[5] & 0x40 ? 1:0) + (frame.data[5] & 0x80 ? 1:0)*2;        //油门信号故障 0-无故障  1-油门旋扭故障 2-脚油门故障  3-都有故障
                control_can.errBrakePedal = frame.data[6] & 0x01 ? 1:0;   //刹车信号故障
                control_can.errSteeringWheel = frame.data[6] & 0x02 ? 1:0 ; //转向故障
                control_can.errHandle = (frame.data[5] & 0x01 ? 1:0) + (frame.data[5] & 0x02 ? 1:0)*2 +(frame.data[5] & 0x04 ? 1:0)*4 +( frame.data[5] & 0x08 ? 1:0) *8 +(frame.data[5] & 0x10 ? 1:0)*16    ; //手柄信号故障
                control_can.errEndTool = frame.data[5] & 0x20 ?1:0;//末端工具控制故障
                control_can.errOther = 0;//其他故障   
                //模式
                
                model = (frame.data[3] & 0x04) == 0x04 ? 1 : 0;

                //切换前后摄像头
                _front_view = (frame.data[4] & 0x04) == 0x04 ?  false: true;
                _message->SwitchCamera(_front_view);

                //b_ready = (_msg[3].DATA[4] & 0x10) == 0x10 ? 1 : 0;
                if((frame.data[4] & 0x10) == 0x10 ? 1 : 0)//已就绪 抓钢机就绪状态，待无人车入场
                {
                  _message->SendZGJStatus(1);
                  control_can.coopSignal = 1;      //就位
                  std::cout << "SendZGJStatus" << std::endl;
                  //_msg[3].DATA[4] &= 0xEF;
                }

                if((frame.data[4] & 0x20) == 0x20 ? 1 : 0)//已完成 抓钢机抓钢结束，待无人车离场
                {
                  _message->SendZGJStatus(2);
                  control_can.coopSignal = 0;      //结束
                  //_msg[3].DATA[4] &= 0xD7;
                }

                if(!_message->btimeStopedCar)
                {
                    memcpy(Sendframe[3].data, frame.data, frame.dlc);  
                } 
                else
                {
                    long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    /*
                    if(tick-_message->_curStopTick > 500) 
                    {
                        Emergency();
                        _message->btimeStopedCar = false;
                    }
                    */
                    if(tick-_message->_curStopTick < 500) 
                        Emergency();
                    else
                        _message->btimeStopedCar = false;
                }
                //_message->_dst  = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                //    printf("data delay:%ld\r\n",_message->_dst - _message->_source);
                //std::cout << "revice 184" << std::endl;

                // _message -> SendVehicleStatus( vehicle_status);
            break;


            case 0x185:
                Sendframe[4].can_id = 0x185;
                Sendframe[4].can_dlc = frame.dlc;

                memcpy(Sendframe[4].data, frame.data, frame.dlc);
                _message -> SendVehicleStatus( control_can);
            break;
        }
    } 
}

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);

    //增加
    req.set_function_code(_data.function_code);
    req.set_main_add(_data.main_add);
    req.set_sub_add(_data.sub_add);
    req.set_paramter(_data.paramter);


    //增加0x28b  
	req.set_engine_spn(_data.engine_spn);        //发动机实时故障SPN
	req.set_engine_fmi(_data.engine_fmi);       //发动机实时故障FMI
	req.set_engine_num(_data.engine_num);        //发动机故障次数

    //增加0X28F
    req.set_all_buff((char *)_data.all_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]);
    }
}