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

#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);
        order_flag = true; 

        if(!order_flag){
        cout<<"order_flag: false"<<endl;
        }
}


void CCanBusSensor::toggleLamp(cannet_frame* frame){

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

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

void CCanBusSensor::car_stop(cannet_frame* frame){

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

        if(carframe.data[2]==0x00){
            memcpy(Sendframe[0].data,stop_on,carframe.dlc); //急停
            
            stop_flag = true; 
            if(stop_flag){
            std::cout<<"stop_flag: true"<<std::endl;
            }
        }
}


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

    {
        auto start = std::chrono::high_resolution_clock::now();
        if(carframe.data[4]>0x02 && carframe.data[4]<=0x7F){
            data2copy = move_forward; //油门前进
            run_flag = true;
        }
        else if(carframe.data[5]>0x02 && carframe.data[5]<=0x7F){
           data2copy = stop_on; //刹车后退
            run_flag = true;
        }

        if(run_flag){
            memcpy(Sendframe[0].data,data2copy,carframe.dlc);
            run_flag = false;
        }
        auto end = std::chrono::high_resolution_clock::now();

        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
        std::cout << "Memcpy of " << carframe.dlc << " bytes took " << duration << " microseconds." << std::endl;
        std::cout << "frame.dlc:"<< carframe.dlc<< std::endl;
    }

}

void CCanBusSensor::move_rot(cannet_frame* frame){

        cannet_frame& carframe = frame[0];     

        Sendframe[1].can_id = 0x111;
        Sendframe[1].can_dlc = carframe.dlc;

        if(carframe.data[0] != 0x01){
            memcpy(Sendframe[0].data,move_rotation,carframe.dlc); //转弯
            rot_flag = true;
        }

}



/*------------------小车控制止-------------------*/


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

    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;
        // CCanBusSensor::order(&frame);
                
        switch (canid)
        {
            case 0x181:
                // Sendframe[0].can_id = 0x421;
                // Sendframe[0].can_dlc = frame.dlc;

                // memcpy(Sendframe[0].data, frame.data, frame.dlc);
                // Hand_Throttle = make_int16(frame.data[1],frame.data[0]);
                // Foot_Throttle = make_int16(frame.data[3],frame.data[2]);
                // Brake = make_int16(frame.data[5],frame.data[4]);
                // cout<<"receive 181: "<<Hand_Throttle<<" "<<Foot_Throttle<<" "<<Brake<<endl;

                // 指令模式
                // memcpy(Sendframe[0].data,order_on,sizeof(order_on)); 
                // Sendframe[0].can_id = 0x121;
                // memcpy(Sendframe[0].data,headlamp_off,sizeof(headlamp_off)); 
                // cout<<"receive 181:"<< headlamp_off<<endl;
                
                // CCanBusSensor::order(&frame);
                CCanBusSensor::toggleLamp(&frame);

                
                // CCanBusSensor::move_run(&frame);     //move_run和car
                // CCanBusSensor::car_stop(&frame);

            break;

            case 0x0CFDD634:  // receive from frame[3]
                


                cout<<"receive 0CFDD634"<<endl;

            break;

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

            //     memcpy(Sendframe[1].data, frame.data, frame.dlc);
            //     //std::cout << "182" << std::endl;
            // break;

            case 0x183:
                Sendframe[2].can_id = 0x183;
                Sendframe[2].can_dlc = frame.dlc;

                Direction = make_int16(frame.data[3],frame.data[2]);
                _message->SendVehicleStatus(Direction,Hand_Throttle,Foot_Throttle,Brake);

                memcpy(Sendframe[2].data, frame.data, frame.dlc);
                //std::cout << "revice 183" << std::endl;
            break;

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

                //模式
                
                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);
                  std::cout << "SendZGJStatus" << std::endl;
                  //_msg[3].DATA[4] &= 0xEF;
                }

                //b_over = (_msg[3].DATA[4] & 0x20) == 0x20 ? 1 : 0;
                if((frame.data[4] & 0x20) == 0x20 ? 1 : 0)//已完成 抓钢机抓钢结束，待无人车离场
                {
                  _message->SendZGJStatus(2);
                  //_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;
            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);
   
    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]);
    }
}