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

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

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;
    _front_view = true;

    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;
    halfautomatic = true;
    _message->_Feeddata.right_lock = 1;
}

void CCanBusSensor::CanSendProc()
{
    while (_run)
    {
        /*
        volatile int64_t tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
         if(tick-_last_can_uptime < 400)
         {
            
             std::lock_guard<std::mutex> lg(_last_can_lock); 
             for(int32_t i=0;i<sizeof(Sendframe)/sizeof(can_frame);i++)
             {
                if(Sendframe[i].can_id != 0)
                {
		    if(_message->_Feeddata.left_lock)
                    	_canbus->Write(&Sendframe[i]);
                    Sendframe[i].can_id  = 0;
                }
             }
         }
        std::this_thread::sleep_for(std::chrono::milliseconds(20)); 
        */
       //std::lock_guard<std::mutex> lg(_last_can_lock);
       for(int32_t i=0;i<sizeof(Sendframe)/sizeof(can_frame);i++)
        {
            if(Sendframe[i].can_id != 0)
            {
                if(!_message->_Feeddata.right_lock)
                {
                    _canbus->Write(&Sendframe[i]);
                    //printf("%x\r\n",Sendframe[i].can_id);
                }
                Sendframe[i].can_id  = 0;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(5)); 

    }
    
   
}

void CCanBusSensor::Start()
{
    /*
    struct can_frame Sendframe_Test;
    Sendframe_Test.data[0] = 0xB1;
    Sendframe_Test.data[1] = 0x10;
    Sendframe_Test.data[2] = 0xFF;
    Sendframe_Test.can_dlc = 3;
    Sendframe_Test.can_id = 0x601;

    int ret = write(_canbus->sockfd, &Sendframe_Test, sizeof(Sendframe_Test)); 

    if(sizeof(Sendframe_Test) != ret) 
        std::cout << "CCanBusSensor write error" << std::endl;
    */
    _run = true;

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

    Gear_position_Set = 0;
    Gear_position_now = 1;

    count = 0;
    _message->_Feeddata.right_lock = 1;
}


void CCanBusSensor::Run()
{
    
}
  
void CCanBusSensor::Stop()
{
    /*
    struct can_frame Sendframe_Test;
    Sendframe_Test.data[0] = 0xB1;
    Sendframe_Test.data[1] = 0x10;
    Sendframe_Test.data[2] = 0x00;
    Sendframe_Test.can_dlc = 3;
    Sendframe_Test.can_id = 0x601;

    int ret = write(_canbus->sockfd, &Sendframe_Test, sizeof(Sendframe_Test)); 
    
    if(sizeof(Sendframe) != ret) 
        std::cout << "CCanBusSensor write error" << std::endl;
    */
    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));

    halfautomatic = false;
    Gear_position_Set = 0;
    Gear_position_now = 1;
    count = 0;
    _message->_Feeddata.right_lock = 1;
}

void CCanBusSensor::GetGearCurrent(uint8_t data)
{
    /*
    if((GET_BIT(data,4) == 1) && (GET_BIT(data,5) == 0) && (GET_BIT(data,6) == 0))
        Gear_position_now = 1;
    else if((GET_BIT(data,4) == 0) && (GET_BIT(data,5) == 1) && (GET_BIT(data,6) == 0))
        Gear_position_now = 2;
    else if((GET_BIT(data,4) == 1) && (GET_BIT(data,5) == 1) && (GET_BIT(data,6) == 0))
        Gear_position_now = 3;
    else if((GET_BIT(data,4) == 0) && (GET_BIT(data,5) == 0) && (GET_BIT(data,6) == 1))
        Gear_position_now = 4;
    else
        Gear_position_now = 0;
    */
   if((GET_BIT(data,0) == 1) && (GET_BIT(data,1) == 0))
   {
        Gear_direction = 'F';
        _message->_Feeddata.f_gear = 1;
   }
   else if((GET_BIT(data,0) == 0) && (GET_BIT(data,1) == 1))
   {
        Gear_direction = 'R';
        _message->_Feeddata.f_gear = 2;
   }
   else
   {
        Gear_direction = 'N';
        _message->_Feeddata.f_gear = 3;
   }
        

    if((GET_BIT(data,4) == 0) && (GET_BIT(data,5) == 1) && (GET_BIT(data,6) == 0))
    {
        Gear_position_now = 2;
        _message->_Feeddata.gear_1 = 2;
    }
    else if((GET_BIT(data,4) == 1) && (GET_BIT(data,5) == 1) && (GET_BIT(data,6) == 0))
    {
        Gear_position_now = 3;
        _message->_Feeddata.gear_1 = 3;
    }
    else if((GET_BIT(data,4) == 0) && (GET_BIT(data,5) == 0) && (GET_BIT(data,6) == 1))
    {
        Gear_position_now = 4;
        _message->_Feeddata.gear_1 = 4;
    }
    else
    {
        Gear_position_now = 1;
        _message->_Feeddata.gear_1 = 1;
    }
}

void CCanBusSensor::SetGear_FR_Current(uint8_t data,uint8_t type)//0，F，1，R
{
    if(!type)//F
    {
        if((GET_BIT(data,3) == 1))//1
            Gear_position_Set = 1;
        if((GET_BIT(data,2) == 1))//2
            Gear_position_Set = 2;
        if((GET_BIT(data,1) == 1))//3
            Gear_position_Set = 3;
        if((GET_BIT(data,0) == 1))//4
            Gear_position_Set = 4;
        
    }
    else//R
    {
        if((GET_BIT(data,4) == 1))//1
            Gear_position_Set = 1;
        if((GET_BIT(data,5) == 1))//2
            Gear_position_Set = 2;
        if((GET_BIT(data,6) == 1))//3
            Gear_position_Set = 3;
        if((GET_BIT(data,7) == 1))//4
            Gear_position_Set = 4;
    }
}

void CCanBusSensor::Notify(struct can_frame *date)
{
    //std::cout << std::hex << date->can_id << std::endl;
    //memcpy(&Sendframe[ii++],date,sizeof(can_frame));
    //_canbus->Write(&Sendframe[0]);
    if(_run)
    {
        
        switch (date->can_id)
        {
        case 0x8CFF2303:
            GetGearCurrent(date->data[0]);//变速箱挡位和FRN
            _message->_Feeddata.gearbox_oil_temp = (int32_t)(date->data[4] - 50);//变速箱油温
            //std::cout << _message->_Feeddata.gearbox_oil_temp << std::endl;
            break;

        case 0x98FEEE00:
            _message->_Feeddata.cold_water = (int32_t)(date->data[0] -40);//发动机水温
            //std::cout << _message->_Feeddata.cold_water << std::endl;
            break;

        case 0x98FEEF00:
            _message->_Feeddata.engine_pressure = (int32_t)(date->data[3] * 4);//发动机压力
            //std::cout << _message->_Feeddata.engine_pressure << std::endl;
            break;

        case 0x8CF00400:
            _message->_Feeddata.engine_rpm = make_int16(date->data[4],date->data[3])  / 8 ;//发动机转速
            //std::cout << _message->_Feeddata.engine_rpm << std::endl;

            count++;
            if( _message->btimeStopedCar && _message->_Feeddata.engine_rpm > 700)
                printf("----%d-----\r\n",count);
             /*count++;
           
            if((count > 10) && _message->btimeStopedCar && _message->_Feeddata.engine_rpm > 700)
            {
                NoEmergency();
                count = 0;
            }
            else if(count == 1000)
                count = 0;
            */
            break;

        case 0x98FEE500:
            {
                int16_t dh = make_int16(date->data[3],date->data[2]);
                int16_t dl = make_int16(date->data[1],date->data[0]);
                int32_t ddd = (((dh & 0x0000ffff) << 16) | (dl & 0x0000ffff)); 
                
                _message->_Feeddata.engine_time = ddd / 20;//发动机运行小时
                //std::cout << _message->_Feeddata.engine_time << std::endl;
            }
            break;

        case 0x180 + 0x71:
            _message->_Feeddata.brake_pressure = make_int16(date->data[5],date->data[4]);//制动压力（Kpa）
            //std::cout << _message->_Feeddata.brake_pressure << std::endl;
            break;

        case 0x180 + 0x74:
            _message->_Feeddata.gearbox_oil_pressure = (int32_t)(date->data[2]);//变速箱油压
            //std::cout << _message->_Feeddata.gearbox_oil_pressure << std::endl;
            break;

        case 0x180 + 0x76://报警信息页面
            memcpy(_message->_Feeddata.Error_Buff,date->data,date->can_dlc);
            //SendStatusToMSG();
            _message->SendVehicleStatue();
            break;

        case 0x280 + 0x72:
            _message->_Feeddata.fuel_level = make_int16(date->data[1],date->data[0]);//燃油油量（%）
            //std::cout << _message->_Feeddata.fuel_level << std::endl;
            break;

        case 0x380 + 0x72:
            _message->_Feeddata.lock_switch_l = ((date->data[3] & 0x02) == 0x02) ? 1 : 0;//锁钩到位开关左
            _message->_Feeddata.lock_switch_r = ((date->data[3] & 0x04) == 0x04) ? 1 : 0;//锁钩到位开关右
            _message->_Feeddata.tip_limit = ((date->data[2] & 0x20) == 0x20) ? 1 : 0;//倾翻限位

            //std::cout << _message->_Feeddata.lock_switch_l  << " "<< _message->_Feeddata.lock_switch_r << " " << _message->_Feeddata.tip_limit <<std::endl;
            break;

        case 0x480 + 0x72:
            _message->_Feeddata.parking = ((date->data[0] & 0x02) == 0x02) ? 1 : 0;//驻车
            _message->_Feeddata.outgrigger_l = ((date->data[0] & 0x04) == 0x04) ? 1 : 0;//左支腿伸到位开关
            _message->_Feeddata.outgrigger_r = ((date->data[0] & 0x08) == 0x08) ? 1 : 0;//右支腿伸到位开关
            _message->_Feeddata.tip_pressure = ((date->data[0] & 0x20) == 0x20) ? 1 : 0;//倾翻压力开关

            

            _message->_Feeddata.turn_left = ((date->data[4] & 0x01) == 0x01) ? 1 : 0;//左转
            _message->_Feeddata.turn_right = ((date->data[4] & 0x02) == 0x02) ? 1 : 0;//右转
            //_message->_Feeddata.low_beam = ((date->data[4] & 0x04) == 0x04) ? 1 : 0;//近光灯
            //_message->_Feeddata.hight_beam = ((date->data[4] & 0x08) == 0x08) ? 1 : 0;//远光灯

            _message->_Feeddata.low_beam = ((date->data[4] & 0x08) == 0x08) ? 1 : 0;//近光灯
            _message->_Feeddata.hight_beam = ((date->data[4] & 0x04) == 0x04) ? 1 : 0;//远光灯

            _message->_Feeddata.front_work_lamp = ((date->data[4] & 0x10) == 0x10) ? 1 : 0;//前工作灯
            _message->_Feeddata.rear_work_lamp = ((date->data[4] & 0x20) == 0x20) ? 1 : 0;//后工作灯
            _message->_Feeddata.brake = ((date->data[6] & 0x01) == 0x01) ? 1 : 0;//刹车
            _message->_Feeddata.back_car = ((date->data[6] & 0x02) == 0x02) ? 1 : 0;//倒车

            _message->_Feeddata.left_lock = ((date->data[7] & 0x01) == 0x01) ? 1 : 0;//遥操模式
            _message->_Feeddata.right_lock = ((date->data[7] & 0x02) == 0x02) ? 1 : 0;//人工模式
            
            //printf("%d--%d\r\n",_message->_Feeddata.turn_left ,_message->_Feeddata.turn_right);
            //std::cout << _message->_Feeddata.left_lock << " " << _message->_Feeddata.right_lock << std::endl;
            break;

        case 0x480 + 0x74:
            _message->_Feeddata.system_vol = make_int16(date->data[1],date->data[0]);//系统电压
            _message->_Feeddata.cargo_weight = make_int16(date->data[3],date->data[2]);//渣包重量
            break;
        
        

        default:
            break;
        }
    }
}

void CCanBusSensor::OnMessage(cannet_busframe* frames, int32_t count)
{

    //std::lock_guard<std::mutex> lg(_last_can_lock);
    _last_can_uptime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    
    for (int32_t i = 0; i < count; i++)
    {
        cannet_busframe& frame = frames[i];

        int32_t canid = frame.canid;
        
        //printf("%x\r\n",canid);
        switch (canid)
        {
            case 0x181:
                Sendframe[0].can_id = 0x186;
                Sendframe[0].can_dlc = frame.dlc;

                //切换前后摄像头
                //_front_view = (Sendframe[0].data[1] & 0x01) == 0x01 ?  false: true;
                _front_view = (frame.data[1] & 0x01) == 0x01 ?  false: true;
                //printf("%x\r\n",frame.data[1]);
                _message->SwitchCamera(_front_view);

                memcpy(Sendframe[0].data, frame.data, frame.dlc);

                //_canbus->Write(&Sendframe[0]);
            break;

            case 0x281:
                Sendframe[1].can_id = 0x286;
                Sendframe[1].can_dlc = frame.dlc;
                
                halfautomatic = (frame.data[1] & 0x20) == 0x20 ?  false: true;//0手动，1自动


                    if((frame.data[1] & 0x0f) != 0)//N档
                    {
                        if(halfautomatic)
                        {
                            Sendframe[1].data[0] = 0x00;
                            Gear_position_Set = 1;
                        }
                        else
                            Sendframe[1].data[0] = 0x10;
                        
                    }
                    else
                    {
                        if((frame.data[0] & 0x0f) != 0)//F档
                        {
                            
                            SetGear_FR_Current(frame.data[0],0);
                            if(Gear_position_now != Gear_position_Set)
                            {
                                if(Gear_position_now < Gear_position_Set)//up
                                {
                                    if(halfautomatic)
                                        Sendframe[1].data[0] = 0x05;
                                    else
                                        Sendframe[1].data[0] = 0x15;
                                }
                                else//down
                                {
                                    if(halfautomatic)
                                        Sendframe[1].data[0] = 0x09;
                                    else
                                        Sendframe[1].data[0] = 0x19;
                                }
                            }
                            else
                            {
                                if(halfautomatic)
                                        Sendframe[1].data[0] = 0x01;
                                    else
                                        Sendframe[1].data[0] = 0x11;
                            }
                        }
                        if(((frame.data[0] & 0xf0) >> 4) != 0)//R档
                        {
                            SetGear_FR_Current(frame.data[0],1);
                            if(Gear_position_now != Gear_position_Set)
                            {
                                if(Gear_position_now < Gear_position_Set)//up
                                {
                                    if(halfautomatic)
                                        Sendframe[1].data[0] = 0x06;
                                    else
                                        Sendframe[1].data[0] = 0x16;
                                }
                                else//down
                                {
                                    if(halfautomatic)
                                        Sendframe[1].data[0] = 0x0A;
                                    else
                                        Sendframe[1].data[0] = 0x1A;
                                }
                            }
                            else
                            {
                                if(halfautomatic)
                                        Sendframe[1].data[0] = 0x02;
                                    else
                                        Sendframe[1].data[0] = 0x12;
                            }
                        }
                    }
                
                if(((unsigned char)frame.data[7] == (unsigned char)0xff) && !_message->btimeStopedCar)
                    {
                        Emergency();
                        std::cout<< "现在是急停"<<((unsigned char)frame.data[7] == (unsigned char)0xff)<<std::endl;
                    }
                else if((frame.data[7] == 0x00) && _message->btimeStopedCar)
                {
                    NoEmergency();
                    std::cout<< "没有急停"<<((frame.data[7] == 0x00))<<std::endl;
                }
                    
                
                /*
                if(!_message->btimeStopedCar && halfautomatic)
                { 
                    memcpy(Sendframe[1].data + 1, frame.data + 1, frame.dlc - 1);
                } 
                else if(!_message->btimeStopedCar && !halfautomatic)
                { 
                    memcpy(Sendframe[1].data, frame.data, frame.dlc);
                } 
                */
               if(!_message->btimeStopedCar)
               {
                    memcpy(Sendframe[1].data + 1, frame.data + 1, frame.dlc - 1);
               }
               else
               {
                   /*
                   long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    if(tick-_message->_curStopTick > 100) 
                    {
                        _message->btimeStopedCar = false;
                        NoEmergency();
                    }
                    else
                        Emergency();*/
               }

                //_canbus->Write(&Sendframe[1]);
            break;

            case 0x381:
                Sendframe[2].can_id = 0x386;
                Sendframe[2].can_dlc = frame.dlc;
                memcpy(Sendframe[2].data, frame.data, frame.dlc);

                //_canbus->Write(&Sendframe[2]);
            break;

            case 0x481:
            
                Sendframe[3].can_id = 0x486;
                Sendframe[3].can_dlc = frame.dlc;
                memcpy(Sendframe[3].data, frame.data, frame.dlc);
                //_canbus->Write(&Sendframe[3]);

            break;

            case 0x1A0:
                //Sendframe[4].can_id = 0x306;
                //Sendframe[4].can_dlc = frame.dlc;
                //memcpy(Sendframe[4].data, frame.data, frame.dlc);
                _message->Senddirection(make_int16(frame.data[2],frame.data[3]));
                /*
                    GL_CANTxData.Data[0]:=16#FF;
                    GL_CANTxData.Data[1]:=16#00;
                    GL_CANTxData.Data[2]:=FB_EXTRACT_B(HZ_remote, 1);
                    GL_CANTxData.Data[3]:=FB_EXTRACT_B(HZ_remote, 0);
                    GL_CANTxData.Data[4]:=FB_PACK(Turn_left_Remote,Emoto_TX_Remote,0,0,0,0,0,0);
                    GL_CANTxData.Data[5]:=16#00;
                    GL_CANTxData.Data[6]:=16#67;
                    GL_CANTxData.Data[7]:=16#18;
                */

            break;
        }
    }
}

void CCanBusSensor::SendStatusToMSG()
{
    RemoNet::State req;
    
    req.set_work_pressure(_message->_Feeddata.work_pressure);
    req.set_brake_pressure(_message->_Feeddata.brake_pressure);
    req.set_gearbox_oil_temp(_message->_Feeddata.gearbox_oil_temp);
    req.set_gearbox_oil_pressure(_message->_Feeddata.gearbox_oil_pressure);
    req.set_engine_rpm(_message->_Feeddata.engine_rpm);//发动机转速                    
	req.set_speed(_message->_Feeddata.speed);//不知道？---可能是后加的传感器---驱动轮增量编码器
	req.set_gear(_message->_Feeddata.gear);
	req.set_engine_pressure(_message->_Feeddata.engine_pressure);//发动机压力               
	req.set_cold_water(_message->_Feeddata.cold_water);//冷却液温度-发动机水温          
	req.set_steer_angle(_message->_Feeddata.steer_angle);//转向角度（原来金川是轮子的现在大冶用铰链位置）          铰接转向姿态编码器
	req.set_left_lock(_message->_Feeddata.left_lock);//遥操模式
	req.set_right_lock(_message->_Feeddata.right_lock);//人工模式
    req.set_engine_time(_message->_Feeddata.engine_time);//发动机运行小时
	req.set_wire_sensor_l(_message->_Feeddata.wire_sensor_l);//拉线传感器左侧
	req.set_wire_sensor_r(_message->_Feeddata.wire_sensor_r);//拉线传感器右侧
	req.set_fuel_level(_message->_Feeddata.fuel_level);//燃油油位
	req.set_lock_switch_l(_message->_Feeddata.lock_switch_l);//锁钩到位开关左
	req.set_lock_switch_r(_message->_Feeddata.lock_switch_r); //锁钩到位开关右
	req.set_tip_limit(_message->_Feeddata.tip_limit);//倾翻限位
	req.set_turn_left(_message->_Feeddata.turn_left);//左转
	req.set_turn_right(_message->_Feeddata.turn_right);//右转
	req.set_hight_beam(_message->_Feeddata.hight_beam);//远光灯
	req.set_low_beam(_message->_Feeddata.low_beam);//近光灯 
	req.set_parking(_message->_Feeddata.parking);//驻车
	req.set_brake(_message->_Feeddata.brake);//刹车
	req.set_back_car(_message->_Feeddata.back_car);//倒车
	req.set_front_work_lamp(_message->_Feeddata.front_work_lamp);//前工作灯
	req.set_rear_work_lamp(_message->_Feeddata.rear_work_lamp);//后工作灯
	req.set_cargo_weight(_message->_Feeddata.cargo_weight);//渣包重量
	req.set_system_vol(_message->_Feeddata.system_vol);//系统电压
	req.set_error_buff((char *)_message->_Feeddata.Error_Buff,8);//报警信息
	req.set_f_gear(_message->_Feeddata.f_gear);//1F，2R，3N
	req.set_gear_1(_message->_Feeddata.gear_1);//1，2，3，4
    req.set_outgrigger_l(_message->_Feeddata.outgrigger_l);//左支腿伸到位开关
    req.set_outgrigger_r(_message->_Feeddata.outgrigger_r);//右支腿伸到位开关
    req.set_tip_pressure(_message->_Feeddata.tip_pressure);//倾翻压力开关
   
    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()
{
    /*
    Sendframe[1].can_id = 0x286;
    Sendframe[1].can_dlc = 8;
    Sendframe[1].data[7] = 0xFF;
    //_canbus->Write(&Sendframe[3]);
    */
   //std::cout << "stop" << std::endl;
   _message->btimeStopedCar = true;
   //_message->_curStopTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
   _message->SendEmergency();
}

void CCanBusSensor::NoEmergency()
{
    /*
    Sendframe[1].can_id = 0x286;
    Sendframe[1].can_dlc = 8;
    Sendframe[1].data[7] = 0xFF;
    //_canbus->Write(&Sendframe[3]);
    */
   //std::cout << "start" << std::endl;
   _message->btimeStopedCar = false;
   _message->SendNoEmergency();
}