Vehicle-cpp/EgoJC/EgoSystem/message_queue.cpp

#include <Eigen/Dense>
#include <atomic>
#include <string>
#include "PCANBasic.h"
#include "../common/comm.h"
#include "api.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "../common/peer_connection.h"
#include "../common/can_sensor.h"
#include "VideoRenderer.h"
#include <jsoncpp/json/json.h>
#include "protocol.pb.h"
#include "radar_sensor.h"
#include "lidar_sensor.h"
#include "encoder_sensor.h"
#include "car_sensor.h"
#include "message_queue.h"
#include <fstream>
#include <iostream>
#include <cstring>
#include <alloca.h>
#include <sstream>
#include <iomanip>
#include <ctime>
#include <chrono>
#include <time.h>
#include <sys/stat.h> 
#include <sys/types.h>
#include <sys/time.h>
#include "message.h"
#include <sensors/sensors.h>
#include <sensors/error.h>
//#include "auto_sensor.h"
CMessageQueue::CMessageQueue():Head(nullptr),Tail(nullptr)
{
   _peerId=-1;
   _curTick=0;
  
}
CMessageQueue::~CMessageQueue()
{
    _robot->Stop();
}
void renableUSB(const char *file);
void CMessageQueue::Create()
{
  
    Json::Value root;
    Json::Reader jsonReader;
    std::ifstream ifile("./local.json");
    std::string serverip;
   
    
    if(jsonReader.parse(ifile,root))
    {
        std::cout<<"enter config json"<<std::endl;
    
 
        //std::string _lidarip=root["lidar_ip"].asString(); 
       // int32_t _lidar_host=root["lidar_host"].asInt();
       // std::cout<<"lidar ip:"<<_lidarip<<std::endl;
        //_lidar=std::make_unique<SensorSocket<CLidarSensor>>(this,_lidarip,0,_lidar_host);
       // _lidar->Start();
      
             
    

      
       max_left_length=root["max_left_length"].asInt();
       max_right_length=root["max_right_length"].asInt();
       min_left_length=root["min_left_length"].asInt();
       min_right_length=root["min_right_length"].asInt();
       tipin_time=root["tipin_time"].asInt();
       tipout_time=root["tipout_time"].asInt();
     
     
     //  _auto=std::make_unique<SensorSocket<CAutoSensor>>(this,auto_host,32121,32122);
      // _auto->Start();
       
        serverip=root["ip"].asString();
        _name=root["name"].asString();
        _car=static_cast<CarType>(root["car_type"].asInt());
        
        const Json::Value arrayObj=root["camerainfo"];
        int32_t count=arrayObj.size();
        for(int32_t i=0;i<count;i++)
        {
                LocalCameraInfo info;
                const Json::Value& value=arrayObj[i];
                info.index=value["index"].asInt();
                info.label=value["label"].asString();
                _cameraArray.push_back(info);
        }
        for(int i=0;i<_cameraArray.size();i++)
            {
                _peerArray.push_back({nullptr});
        //      _windowArray.push_back({nullptr});
            }
       /*
        const  Json::Value files=root["playlist"];
        count=files.size();
        for(int32_t i=0;i<count;i++)
        {
            const Json::Value& value=files[i];
            int32_t index=value["index"].asInt();
            std::string file=value["file"].asString();
         
            LoadRecord(static_cast<ROUT_LIST>(index),file.c_str());

        }
         */

        int32_t _radarport=root["radar_port"].asInt();
        int32_t _radarhost=root["radar_host"].asInt();
        std::string _radarip=root["radar_ip"].asString();
        std::cout<<"radar ip:"<<_radarip<<std::endl;
        _radar=std::make_unique<SensorSocket<CRadarSensor>>(this,_radarip,_radarport,_radarhost);
        _radar->Start();
         int32_t encodeport=root["encode_port"].asInt();
       int32_t encodehost=root["encode_host"].asInt();
         _encoder=std::make_unique<SensorSocket<CEncoderSensor>>(this,_radarip,encodeport,encodehost);
       _encoder->Start();
        int32_t auto_port=root["auto_port"].asInt();
       std::string auto_host=root["auto_host"].asString();
       _robot=std::make_unique<SensorSocket<CCarSensor>>(this,auto_host,auto_port,23001);
        _robot->Start();
          std::string usb=root["usb_3"].asString();
        renableUSB(usb.c_str());
        usb=root["usb_2"].asString();
        renableUSB(usb.c_str());
       
        _serial=root["serial"].asString();
       
    }
    else{
        std::string error=jsonReader.getFormattedErrorMessages();
        std::cout<<error<<std::endl;
    }
    _client=std::make_unique<SocketClient>(this);
    _client->Start(serverip.c_str());
   
   
  
    //_can=std::make_unique<SocketCan>(this);
    //_can->Start(_canip,_canport,_hostport);
      
    
  
    std::this_thread::yield();
 //    OnNotifyReq(0);
 
}
/*
void CMessageQueue::WriteCanMessage(std::unordered_map<int32_t, cannet_frame>& node,bool islidar)
{
    if(!bDataChannelCreated) return;
     // std::lock_guard<std::mutex> l(_canLock);
      RemoNet::CCCanMesage Req;
      Req.set_islidar(islidar);
      for(auto& p:node)
      {
         int32_t lidar=p.second.canid;
         auto m=Req.add_message();
         m->set_head(p.second.dlc);
         m->set_canid(lidar);
         m->set_data(p.second.data,8);
        
      }
      
      MessageHead Head;
      CIOBuffer pBuffer;
      Head.Command = RemoNet::CC_CAN;
      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;
      _peerArray[0]->SendData(&pBuffer);
}
 
bool CMessageQueue::IsCarId(int32_t value)
{
    return std::find(_carArray.begin(),_carArray.end(),value)!=_carArray.end();
}
*/
void CMessageQueue::EnQueue(CIOBuffer* pBuffer)
{
    bool bNullBuffer=false;
    
    std::unique_lock <std::mutex>  lck(_mlock);
    
    if(Head==nullptr)
    {
        Head=Tail=pBuffer;
        bNullBuffer=true;
    }
    else{
        Tail->NextBuf=pBuffer;
        Tail=Tail->NextBuf;
    } 
    pBuffer->NextBuf=nullptr;
    if(bNullBuffer)
    {
        // std::cout<<"notify "<<_count<<std::endl;
        _cv.notify_all();
    }
    
}  
void CMessageQueue::Process()
{
    CIOBuffer * ptr=nullptr;
    {
        std::unique_lock <std::mutex>  lck(_mlock);
       /*  
        while(Head==nullptr)
        {
            _cv.wait(lck);
        }
         */
         while(Head==nullptr&&_cv.wait_for(lck,std::chrono::seconds(1))==std::cv_status::timeout)
        {
        //  CheckSignal();
          std::cout<<".";
          std::cout.flush();
          
        }
        
        
    }
  
    while(Head!=nullptr)
    {
        ptr=Head;
        Head=Head->NextBuf;
        if(ptr!=nullptr)
        {
            Message* message=reinterpret_cast<Message *>(ptr->Buffer);
            switch (message->cmd)
            {
            case MessageType::ReqVideo:
            OnNotifyReq((int32_t)message->param_l);
            break;
            case MessageType::RepVideo:
            OnNotifyRep((int32_t)message->param_l);
            break;
            case MessageType::Connected:
            OnNotifyConnected((bool)message->param_l);
            break;
            case MessageType::Leave:
            OnNotifyLeave();
            break;
            case MessageType::AsyncMessage:
            OnNotifyMessage();
            break;
            case MessageType::StopSensor:
            OnNotifyStopSensor();
            break;        
            case MessageType::Ping:
            OnNotifyPing(message->param_l);
            break;  
           
            
            }
            ptr->Release(__FILE__,__LINE__);
          
          
        }   
      
    }    
  
}

void CMessageQueue::OnNotifyConnected(bool bRet)
{
    if(bRet)
    {
        
        _client->WriteAddRobot(_serial,_name,std::string(""),static_cast<int32_t>(EgoType::Car),_car);
         _updatethread.start(_client.get());
    }
    else
    {
        if(_updatethread.running())
        {
              _updatethread.stop();
        }
        if(_peerId!=-1)
        {
           
            for (size_t i = 0; i < _peerArray.size(); i++)
            {
                if(_peerArray[i]!=nullptr)
                {
                    _peerArray[i]->Close();
                    _peerArray[i].reset();
                }
                /* code */
            }
         //   for (size_t i = 0; i < _windowArray.size(); i++)
         //   {
                /* code */
         //       if(_windowArray[i]!=nullptr)
         //         _windowArray[i].reset();
         //   }
            _peerId=-1;
//            StopCar();
        }
      
       
    }
}
void CMessageQueue::ChangeState(UserState state)
{
    RemoNet::CSState Req;
    Req.set_state((RemoNet::UserState)state);
    Req.set_uid(_uid);
    MessageHead Head;
    CIOBuffer pBuffer;
    Head.Command = RemoNet::CS_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;
 
    _client->Write(&pBuffer);
}
void CMessageQueue::OnNotifyReq(int32_t index)
{ 
   // std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
   if(_peerId==-1) return;
     if(index==0)
       {
          mrsWebrtcCreateFactory(true);
          
        //  _radar->Start(); 
         // _encoder->Start();
          ChangeState(UserState::Remote);
 
         // _robot->Start();
 
       }
      _curTick=0;
   
//    PeerConnectionWrapper* peer = nullptr;
//    VideoRenderer* window = nullptr;
   // _windowArray[temp]=std::make_unique<VideoRenderer>();// .reset(new VideoRenderer());
    _peerArray[index]=std::make_unique<CPeerConnection>(static_cast<ChannelType>(index),_client.get());
   
    
    _client->WriteVideoRep(_peerId, RemoNet::VideoDesc::OK, index);
}
void CMessageQueue::WriteBuffer(CIOBuffer& buffer)
{
    _client->Write(&buffer);
}
const radar_node* CMessageQueue::GetRadar()
{
     return  _radar->Get()->GetRadarInfo();
}
void CMessageQueue::OnNotifyRep(int32_t index)
{
   // std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
    
     
   // index-=_indexOffset;
//    PeerConnectionWrapper* peer = nullptr;
//    VideoRenderer* window = nullptr;
    //_windowArray[temp]=std::make_unique<VideoRenderer>();
    _peerArray[index]=std::make_unique<CPeerConnection>(static_cast<ChannelType>(index), _client.get());
    
     
    InitPeerConnection(_peerId,index);
    _peerArray[index]->CreateOffer();
    
}
void CMessageQueue::InitPeerConnection(int32_t peer,int32_t index)
{
     
    //bool NeedData=index==RenderPosition::CAR_FRONT;
   
    _peerArray[index]->Initialize(peer,index);
    _peerArray[index]->AddDataChannel(true, false);
      
//    window.reset(new VideoRenderer());
//    window->SetRenderWindow(GetDlgItem(IDC_REMOTE), 1, 1);
    //_peerArray[index]->AddLocalArgb32VideoFrameReady(&VideoRenderer::FrameCallback, _windowArray[index].get());
   // if(_cameraArray[index].type==CaptureType::RTSP)
    //    {
    
   
    _peerArray[index]->AddLocalVideoTrack(static_cast<RenderPosition>(index),_cameraArray[index].index);
    
    if((index+1)==RenderPosition::ALL)
    {
        void * front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        void * back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        assert(front!=nullptr);
        assert(back!=nullptr);
        _peerArray[RenderPosition::FRONT]->SetOtherCtx(back);
        _peerArray[RenderPosition::BACK]->SetOtherCtx(front);
        void * left=_peerArray[RenderPosition::LEFT]->GetCurrentCtx();
        void * right=_peerArray[RenderPosition::RIGHT]->GetCurrentCtx();
        _peerArray[RenderPosition::LEFT]->SetOtherCtx(right);
        _peerArray[RenderPosition::RIGHT]->SetOtherCtx(left);
         void * leftAnchor=_peerArray[RenderPosition::LANCHOR]->GetCurrentCtx();
        void * rightAnchor=_peerArray[RenderPosition::RANCHOR]->GetCurrentCtx();
        
        while(leftAnchor==nullptr||rightAnchor==nullptr)
        {
             std::this_thread::sleep_for(std::chrono::milliseconds(100));	
             leftAnchor=_peerArray[RenderPosition::LANCHOR]->GetCurrentCtx();
              rightAnchor=_peerArray[RenderPosition::RANCHOR]->GetCurrentCtx();
        }
        {
            _peerArray[RenderPosition::LANCHOR]->SetOtherCtx(rightAnchor);
            _peerArray[RenderPosition::RANCHOR]->SetOtherCtx(leftAnchor);
            _camera_inited=true;
        }
      //  _camera_inited=false;
    }
    
    //    }
   // else
    //  {
     //      _peerArray[index]->AddLocalVideoTrack(_cameraArray[index].type, _cameraArray[index].solution, 30);   
    //  } 
    if(index==RenderPosition::FRONT)
        _peerArray[index]->AddLocalAudioTrack();
    //[temp]->StartRender(true);
}
 
void CMessageQueue::OnAdd(int32_t id,bool bRet)
{
   _uid=id;
}
void CMessageQueue::OnConnected(bool bRet)
{
    
    CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
    Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
    message->cmd=MessageType::Connected;
    message->param_l=bRet;
    EnQueue(pBuffer);
   
}

void CMessageQueue::OnVideoLeave(int32_t peer,EgoType type)
{
    
    {
        CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::StopSensor;
        EnQueue(pBuffer);
    }
    {
        CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::Leave;
        EnQueue(pBuffer);
    }
    _curTick=0;
}
#ifdef WIN32
void CMessageQueue::OnVideoRep(int32_t index,RemoNet::VideoDesc desc)
{
   if (desc == RemoNet::VideoDesc::OK)
   {
        assert(_peerId!=-1);
        CIOBuffer * pBuffer=CIOBuffer::Alloc();
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::ReqVideo;
        message->param_l=index;
        
        EnQueue(pBuffer);
   }
 
}
#else
void CMessageQueue::OnVideoReq(int32_t video,int32_t peer)
{
   
    if(video==0&&_peerId!=-1)
    {
         std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
        _client->WriteVideoRep(peer,RemoNet::VideoDesc::Reject,video);
        return;
    }

  
    _peerId=peer;
    CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
    Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
    message->cmd=MessageType::ReqVideo;
    message->param_l=video;
    
    EnQueue(pBuffer);
    
}
void CMessageQueue::OnMoveBegin(WorkArea area, int32_t no)
{
 
         int64_t tick= std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
         if(tick-_robot->Get()->GetLastEncodeTime()>500)
         {
            RemoNet::MoveRet Req;
            Req.set_desc(RemoNet::MoveDesc::Move_Encode_Fail);
            Req.set_peer(_peerId);
             CIOBuffer pBuffer;
             MessageHead Head;
     
            Head.Command = RemoNet::CS_MoveRet;
            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;
            _client->Write(&pBuffer);
            return;
 
         }
         
          
            CIOBuffer pBuffer;
            auto ptr=pBuffer.Buffer;
            MessageHead Head;
            Head.Command=RA::RA_Goal;
            Head.Length=sizeof(ARMoveGoal);
            Head.Serialize(pBuffer.Buffer);
            ptr=pBuffer.Buffer+sizeof(MessageHead);
            ARMoveGoal * node=(ARMoveGoal *)ptr;
            node->area=area;
            node->no=no;
            /*node->forward=true;
            if(area==WorkArea::Area_I)
            {
                 if(no>10&&no<14)
                 {
                    node->forward=false;
                 }
            }
            */
            
    
            Head.Length=sizeof(ARMoveGoal);
            Head.Serialize(pBuffer.Buffer);
            pBuffer.Length=MessageHead::Size()+sizeof(ARMoveGoal);
            _robot->Write(pBuffer.Buffer,pBuffer.Length);
            _robot->Get()->SetAutomous(true,3);
            ChangeState(UserState::Automotive);
         
}
#endif
void CMessageQueue::OnNotifyLeave()
{
  
     SwitchCamera(true);
     std::this_thread::sleep_for(std::chrono::milliseconds(100));   
   
     
     for (size_t i = 0; i < _peerArray.size(); i++)
     {
         if(_peerArray[i]!=nullptr)
         {
             _peerArray[i]->Close();
             _peerArray[i].reset();
         }
          /* code */
     }
     //for (size_t i = 0; i < _windowArray.size(); i++)
    // {
          /* code */
    //     _windowArray[i].reset();
   //  }
      _peerId=-1;
      _robot->Get()->SetChannelReady(false);
      
      //StopCar();
     ChangeState(UserState::Idle);
      
      
}
void CMessageQueue::SetAutomous(bool value,int32_t delay)
{
    _robot->Get()->SetAutomous(value,delay);
}
void CMessageQueue::OnNotifyStopSensor()
{
     // std::cout<<"radar stop"<<std::endl;
    //_radar->Stop();
    //  std::cout<<"robot stop"<<std::endl;
   //  _encoder->Stop();
 
  //  _lidar->Stop();
     _camera_inited=false;
    
     RemoNet::StopAck Rep;
    
     CIOBuffer Buffer;
     MessageHead Head;
     Head.Command = RemoNet::CC_StopACK;
     Head.Length = Rep.ByteSizeLong();
     Head.Serialize(Buffer.Buffer);
     auto ptr = Buffer.Buffer + MessageHead::Size();
     Rep.SerializeToArray(ptr, Head.Length);
     Buffer.Length = Head.Length + MessageHead::Size();
     _peerArray[RenderPosition::FRONT]->SendData(Buffer);
 
 
}
void CMessageQueue::OnVideoOffer(int32_t index,const char* type, const char* sdp)
{ 
 //   std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
    InitPeerConnection(_peerId,index); 
    
    _peerArray[index]->SetRemoteDescription(type,sdp);
    _peerArray[index]->CreateAnswer();
     
}
void CMessageQueue::OnVideoAnswer(int32_t index, const char* type, const char* sdp)
{ 
  //  std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
      
     _peerArray[index]->SetRemoteDescription(type,sdp);
    
}
void CMessageQueue::OnVideoCandidate(int32_t index,const char* candidate,
        int32_t sdp_mline_index,
        const char* sdp_mid)
{ 
      
    _peerArray[index]->AddIceCandidate(candidate,sdp_mline_index,sdp_mid);
     
}
 
void CMessageQueue::OnMessageFrameNotify(ChannelType type,int16_t cmd,int16_t length,const void * data)
{
     
    
     if(cmd==RemoNet::CC_Text)
     {
          RemoNet::TestTextReq Req;
       
          Req.ParseFromArray(data,length);
          std::cout<<Req.text()<<std::endl;
         CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::AsyncMessage;
        EnQueue(pBuffer);
    
     }
      
     else if(cmd==RemoNet::CC_Ping)
     {
 
           _curTick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
           RemoNet::CCPing Req;
           Req.ParseFromArray(data,length);
          
 
           CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
           Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
           message->cmd=MessageType::Ping;
           message->param_l=Req.tick();
           EnQueue(pBuffer);
    
 
     }
     else if(cmd==RemoNet::CC_SensorStop)
     {
 
        CIOBuffer * pBuffer=CIOBuffer::Alloc(__FILE__,__LINE__);
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::StopSensor;
        EnQueue(pBuffer);
     }
     else if(cmd==RemoNet::CC_CANMSG)
     {
         RemoNet::CCCanMsg Req;
         Req.ParseFromArray(data,length);
         cannet_frame* frames=(cannet_frame *)alloca(sizeof(cannet_frame)*Req.frams_size()); 
         for(int32_t i=0;i<Req.frams_size();i++)
         {
             auto& frame =Req.frams(i);
             frames[i].canid=frame.canid();
             frames[i].dlc=frame.dlc();
             memcpy(frames[i].data,frame.data().data(),frame.dlc());
 
         }
        _robot->Get()->OnMessage(frames,Req.frams_size());
	//std::cout << "1111" << std::endl;
        /*
        static auto tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        auto diff=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count()-tick;
        std::cout<<diff<<std::endl;
        tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        */
     }
     else if(cmd==RemoNet::CC_ASKDATACHANNEL)
     {
            switch (type)
            {
            case  ChannelType::CHANNEL_CAR:
               {
                
                    _robot->Get()->SetChannelReady(true);
               }
                break;
 
            case ChannelType::CHANNEL_ENCODE:
             _encoder->Get()->SetChannelReady(true);
            //  _lidar->Get()->SetChannelReady(true);
              break;
   
            case ChannelType::CHANNEL_RADAR:
             _radar->Get()->SetChannelReady(true);
             break; 
            
            }
     }
     /*
     else if(cmd==RemoNet::CC_Switch)
     {
         RemoNet::CCSwitch Req;
         Req.ParseFromArray(data,length);
         bool front=Req.front();
         _peerArray[RenderPosition::FRONT_BACK]->SwitchCapture(front);
 
     }
     */
     
     
}
/*
void CMessageQueue::StopCar()
{
    
    for(auto id:_emergencyArray)
    {
       CIOBuffer Buffer;
      cannet_frame* msg=(cannet_frame *)Buffer.Buffer;
      msg->canid=id;
      msg->dlc=8;
      memset(msg->data,0,sizeof(msg->data));
      Buffer.Length=sizeof(cannet_frame);
      _can->Write(&Buffer);
    }  
    _can->SetStartWrite(false);
}
*/
void CMessageQueue::OnNotifyMessage()
{
         RemoNet::TestTextReq Req;
         Req.set_text("ewqewqewqe");
         CIOBuffer Buffer;
          MessageHead Head;
          Head.Command = RemoNet::CC_Text;
          Head.Length = Req.ByteSizeLong();
          Head.Serialize(Buffer.Buffer);
          auto ptr = Buffer.Buffer + MessageHead::Size();
          Req.SerializeToArray(ptr, Head.Length);
          Buffer.Length = Head.Length + MessageHead::Size();
         _peerArray[RenderPosition::FRONT]->SendData(Buffer);
         
}
void CMessageQueue::SetSteerAngle(int16_t angle)
{
    _steer_angle=angle;
}
void CMessageQueue::SetNdtPos(ARNdtPos* node)
{
    
    std::lock_guard<std::mutex> l(_pose_lock);
     _curpos.x=node->x;
     _curpos.y=node->y;
     _curpos.z=node->z;
     _curpos.rw=node->rw;
     _curpos.rx=node->rx;
     _curpos.ry=node->ry;
     _curpos.rz=node->rz;
     
}
void CMessageQueue::TestPark()
{
     _parking=ParkingState::Turn;
     std::lock_guard<std::mutex> l(_pose_lock);
     _initpos.X=_curpos.x;
     _initpos.Y=_curpos.y;
}
void CMessageQueue::OnTip(int16_t value)
{
    _start_time=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    _parking=value>0?ParkingState::TipIn:ParkingState::TipRise;
}
void CMessageQueue::OnGoalFinish(WorkArea area,int32_t no)
{
    _workarea=area;
    _no=no;
    _parking=ParkingState::Turn;
    Eigen::AngleAxisd axis;
    axis=Eigen::Quaterniond(_curpos.rw,_curpos.rx,_curpos.ry,_curpos.rz);
    double cur_angle=axis.angle();
    auto v=axis.axis();
    if(v[2]==-1)
    {
        cur_angle=EIGEN_PI*2-cur_angle;
    }
    {
        std::lock_guard<std::mutex> l(_pose_lock);
        _initpos.X=_curpos.x;
        _initpos.Y=_curpos.y;
    }
    if(area==WorkArea::Area_I)
    {
            //nothing to do
            _metalArea=Metal_Area::Put_Wait;
             _parking=ParkingState::None;
    }
    else if(area==WorkArea::Area_J)
    {
            if(std::abs(cur_angle-1.5*EIGEN_PI)<EIGEN_PI/4)
            {
                  _side=TurnSide::Left;
		//   std::cout << "left------" << std::endl;
            }
            else{
                _side=TurnSide::Right;
		// std::cout << "right----" << std::endl;
            }
            if(_no == 0 || _no == 1 || _no == 12 || _no ==13)
            {
                _sq_length=430;
                _diff_degree=30;
                _angle_value=10;
                _turn_base=1;
            } else {
                _sq_length=102;
                _diff_degree=30;
                _angle_value=10;
                _turn_base=1000;
            }
    }
    else
    {
            _sq_length=150;
            _diff_degree=30;
            _angle_value=10;
            _turn_base=1000;
           if(cur_angle<EIGEN_PI/4||cur_angle>EIGEN_PI*7/4)  //east
           {
                  switch(area)
                  {
                    case WorkArea::Area_A:
                        _side=TurnSide::Right;
                        _sq_length=140;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                        break;
                    case WorkArea::Area_C:
                    case WorkArea::Area_E:
                    case WorkArea::Area_G:
                        _side=TurnSide::Right;
                        _sq_length=140;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                        break;
                    case WorkArea::Area_B:
                        _side=TurnSide::Left;
                        _sq_length=110;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                    case WorkArea::Area_D:
                    case WorkArea::Area_F:
                    case WorkArea::Area_H:
                        _side=TurnSide::Left;
                        _sq_length=140;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                    break;
                    default:
                    break;
                  }
           }
           else
           {
                  switch(area)
                  {
                    case WorkArea::Area_A:
                        _side=TurnSide::Left;
                        _sq_length=130;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                        break;
                    case WorkArea::Area_C:
                    case WorkArea::Area_E:
                    case WorkArea::Area_G:
                        _side=TurnSide::Left;
                        _sq_length=140;
                        _diff_degree=30;
                        _angle_value=10;
                        _turn_base=1000;
                        break;
                    case WorkArea::Area_B:
                    case WorkArea::Area_D:
                    case WorkArea::Area_F:
                    case WorkArea::Area_H:
                    _side=TurnSide::Right;
                    break;
                    default:
                    break;
                  }
           }
    }
    // std::cout << __FILE__ << " _sq_length: " << _sq_length << std::endl;
   // _robot->Get()->StartParking(true);
 //    _parking=ParkingState::Turn;
}
const int32_t MAX_DGREE= (int32_t)(((4096/360*45)));
void CMessageQueue::GetParkCanMessage(TPCANMsg* msg)
{

   
    switch (_parking)
    {
    case ParkingState::TipIn:
       OnTipIn(msg);
    break;
    case ParkingState::TipOut:
       OnTipOut(msg);
       break;
    case ParkingState::TipDown:
       OnTipDown(msg);
       break;
    case ParkingState::TipRise:
       OnTipRise(msg);
       break;
    case ParkingState::RiseDown:
        OnRiseDown(msg);
    break;
    
  
   
    case ParkingState::Turn:
        OnTurn(msg);
        break;
    case ParkingState::Reverse:
        OnReverse(msg);
        break;
    case ParkingState::Restore:
       OnRestore(msg);
       break;
    
    default:
        break;
    }
}
void CMessageQueue::OnReverse(TPCANMsg* msg)
{
         std::lock_guard<std::mutex> l(_pose_lock);
           double dx=_curpos.x-_initpos.X;
           double dy=_curpos.y-_initpos.Y;
           if((dx*dx+dy*dy)>_sq_length)
           {
                msg[2].ID=0x286; 
                msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;
                msg[2].DATA[0]=0;
                msg[2].DATA[1]=0x21;
                msg[3].ID=0x386;
                msg[3].DATA[0]=0;
                msg[3].DATA[1]=0;
                _parking=ParkingState::Restore;


           }  
           else
            {
                msg[2].ID=0x286; 
                msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;
                msg[2].DATA[0]=0x10;
                msg[2].DATA[1]=0x20;
                msg[3].ID=0x386;    
                msg[3].DATA[0]=40;
                msg[3].DATA[1]=0;
            }
        
       
}
void CMessageQueue::OnRestore(TPCANMsg* msg)
{
    if(std::abs(_steer_angle)>_angle_value)
        {
                int32_t speed=std::max(600,std::abs(_steer_angle)/MAX_DGREE*1000);
                msg[1].ID=0x586;
                msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;
                msg[1].DATA[0]=0xFF;
                msg[1].DATA[1]=0x00;
                msg[1].DATA[2]=hi_byte(speed);
                msg[1].DATA[3]=lo_byte(speed);
                msg[1].DATA[4]=(_side==TurnSide::Left? 0:1)|0x02; 
    			msg[1].DATA[5] = 0;
				msg[1].DATA[6] = 0;
				msg[1].DATA[7] = 0;

         }
         else
         {
            msg[1].ID=0;
            msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;
            msg[1].DATA[0]=0xFF;
            msg[1].DATA[1]=0x00;
            msg[1].DATA[2]=hi_byte(800);
            msg[1].DATA[3]=lo_byte(800);
            msg[1].DATA[4]=(_side==TurnSide::Left? 1:0)|0x02; 
    		msg[1].DATA[5] = 0;
			msg[1].DATA[6] = 0;
			msg[1].DATA[7] = 0;
             msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=120;
            _parking=ParkingState::None;
           
         }
}
void CMessageQueue::OnTurn(TPCANMsg* msg)
{
        msg[2].ID=0x286; 
        msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;
        msg[2].DATA[0]=0;
        msg[2].DATA[1]=0x21;
        if(MAX_DGREE-std::abs(_steer_angle)>_diff_degree)
        {
            int32_t speed=std::max(600,(1-std::abs(_steer_angle)/MAX_DGREE)*_turn_base);
            if(_workarea == WorkArea::Area_J && (_no == 0 || _no == 1 || _no == 12 || _no == 13))
            {
                speed=230;//std::max(300,(1-std::abs(_steer_angle)/MAX_DGREE)*_turn_base);
            }   
            // std::cout << __FILE__ << " speed: " << speed << std::endl;
            msg[1].ID=0x586;

            msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;
            msg[1].LEN=8;
            msg[1].DATA[0]=0xFF;
            msg[1].DATA[1]=0x00;
            msg[1].DATA[2]=hi_byte(speed);
            msg[1].DATA[3]=lo_byte(speed);
            msg[1].DATA[4]=(_side==TurnSide::Left? 1:0)|0x02;
            msg[1].DATA[5] = 0;
            msg[1].DATA[6] = 0;
            msg[1].DATA[7] = 0;
            msg[2].ID=0x286; 
            msg[2].MSGTYPE=PCAN_MESSAGE_STANDARD;
            msg[2].DATA[0]=0x10;
            msg[2].DATA[1]=0x20;
            msg[3].ID=0x386;
            msg[3].DATA[0]=60;
            msg[3].DATA[1]=0;

         }
         else 
         {
                msg[1].ID=0;

                msg[1].MSGTYPE=PCAN_MESSAGE_STANDARD;
                msg[1].LEN=8;
                msg[1].DATA[0]=0xFF;
                msg[1].DATA[1]=0x00;
                msg[1].DATA[2]=hi_byte(1000);
                msg[1].DATA[3]=lo_byte(1000);
                msg[1].DATA[4]=(_side==TurnSide::Left? 1:0)|0x02;
          		msg[1].DATA[5] = 0;
				msg[1].DATA[6] = 0;
				msg[1].DATA[7] = 0;

              _parking=ParkingState::Reverse;
         }
}
/*
void CMessageQueue::OnPlayrecord(TPCANMsg* msg)
{

    auto array=_radar->Get()->GetRadarInfo();
    auto tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
    if(_play_index<frames[_rout_index].size())
     {
        for(int32_t i=0;i<5;i++)
           {
                msg[i].ID=frames[_rout_index][_play_index+i].canid;
                msg[i].LEN=frames[_rout_index][_play_index+i].dlc;
                memcpy(msg[i].DATA,frames[_rout_index][_play_index+i].data,frames[_rout_index][_play_index+i].dlc);
            }
            _play_index+=5;
    }
    else{
         msg[3].ID=0x386;
         msg[3].DATA[0]=0;
         msg[3].DATA[1]=120;
         _parking=ParkingState::None;
    }
}
*/
void CMessageQueue::WriteSensor(radar_node* node,int32_t count)
{
     MessageHead Head;
     CIOBuffer Buffer;
     RemoNet::CCSensor Req;
     for(int32_t i=0;i<count;i++)
     {
         auto n= Req.add_node();
         n->set_id(i);
         n->set_range(node[i].range);

          
     }
      int32_t a=std::abs(_left_length-min_left_length) *400;
      a/=std::abs(max_left_length-min_left_length);

      Req.set_left_encode(a);
      a=std::abs(_right_length-min_right_length)*400;
      a/=std::abs(max_right_length-min_right_length);
      Req.set_right_encode(a);
      Head.Command=RemoNet::CC_Radar;
      Head.Length=Req.ByteSizeLong();
      Head.Serialize(Buffer.Buffer);
      auto ptr = Buffer.Buffer + MessageHead::Size();
      Req.SerializeToArray(ptr, Head.Length);
      Buffer.Length = Head.Length + MessageHead::Size();
      WriteBuffer(ChannelType::CHANNEL_RADAR,Buffer);
}
static double get_value(const sensors_chip_name *name,
			const sensors_subfeature *sub)
{
	double val;
	int err;

	err = sensors_get_value(name, sub->number, &val);
	if (err) {
	 
		val = 0;
	}
	return val;
}
static int get_input_value(const sensors_chip_name *name,
			   const sensors_subfeature *sub,
			   double *val)
{
	int err;

	err = sensors_get_value(name, sub->number, val);
 
	return err;
}
void CMessageQueue::OnNotifyPing(int64_t value)
{
      double temp=0.f;
      
      auto err=sensors_init(NULL);
           if(err==0)
    {
        const sensors_chip_name *chip;
        int chip_nr=0;
        int i=0;
    
        const sensors_feature * feature=nullptr;
        while ((chip = sensors_get_detected_chips(NULL, &chip_nr))) {
            while ((feature = sensors_get_features(chip, &i))) {
            if (feature->type==SENSORS_FEATURE_TEMP) {
            auto sf = sensors_get_subfeature(chip, feature,
                        SENSORS_SUBFEATURE_TEMP_FAULT);
                        
                        if (sf && (temp=get_value( chip , sf))) {
                            break;
                            
                        } 
                        else
                        {
                            sf = sensors_get_subfeature(chip, feature,SENSORS_SUBFEATURE_TEMP_INPUT);
                                if (sf && get_input_value(chip, sf, &temp) == 0) {
                                }
                                else 
                                {
                                    temp=0;
                                }
                        }
                        break;
             }
          }

        }
    }
    //std::cout<<temp<<std::endl;
     sensors_cleanup();
    
     RemoNet::CCPing Rep;
     Rep.set_tick(value);
     Rep.set_temp(temp);
     CIOBuffer Buffer;
     MessageHead Head;
     Head.Command = RemoNet::CC_Ping;
     Head.Length = Rep.ByteSizeLong();
     Head.Serialize(Buffer.Buffer);
     auto ptr = Buffer.Buffer + MessageHead::Size();
     Rep.SerializeToArray(ptr, Head.Length);
     Buffer.Length = Head.Length + MessageHead::Size();
     if(_peerArray[RenderPosition::FRONT]!=nullptr)
        _peerArray[RenderPosition::FRONT]->SendData(Buffer);
}
 
/*
void CMessageQueue::StartCar()
{
    _can->SetStartWrite(true);
}
*/
void CMessageQueue::CheckSignal()
{
    if(_curTick==0) return;
    long long tick=std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    if(tick-_curTick>2000) 
     {
          //_curbrake-=20.f;
          int16_t value=100;//std::max(_curbrake,(int16_t)-127);
          TPCANMsg msg;
          memset(msg.DATA,0,sizeof(msg.DATA));
          msg.ID=0x386;
          msg.LEN=8;
          msg.DATA[0]=hi_byte(value);
          msg.DATA[1]=lo_byte(value);
          _robot->Get()->Write(msg);
          bStopedCar=true;
 
     }  
 
   
}
void CMessageQueue::OnSwitchDriver()
{
     ChangeState(UserState::Remote);
      CIOBuffer pBuffer;
      //auto ptr=pBuffer.Buffer;
      MessageHead Head;
      Head.Command=RA_Stop;
      //   ptr=pBuffer.Buffer+sizeof(MessageHead);
      Head.Length=0;
      Head.Serialize(pBuffer.Buffer);
      pBuffer.Length=MessageHead::Size();
      _robot->Write(pBuffer.Buffer,pBuffer.Length);
}
void CMessageQueue::WriteBuffer(ChannelType type,CIOBuffer& pBufer)
{
      if( _peerArray[type]!=nullptr)
         _peerArray[type]->SendData(pBufer);
}
void CMessageQueue::SetLeftAngle(int16_t angle)
{
    _left_length=angle;
}
void CMessageQueue::SetRightAngle(int16_t angle)
{
    _right_length=angle;
}
/*
void CMessageQueue::WriteRadarData(radar_node* data)
{
    
      RemoNet::CCRadar Req;
      for (size_t i = 0; i < 5; i++)
      {
         auto node=Req.add_array();
         node->set_id(data[i].rid);
         for(auto & n:data[i]._info)
         {
            auto info=node->add_info();
          //  info->set_azimuth(n.second.azimuth);
          //  info->set_count(0);
            info->set_index(n.second.index);
            info->set_range(n.second.range);
         //   info->set_snr(n.second.snr);
          //  info->set_verl(n.second.verl);
            
         }
      }
      
     
      if( _peerArray[ChannelType::CHANNEL_RADAR]!=nullptr)
         _peerArray[ChannelType::CHANNEL_RADAR]->SendData(Buffer);
       
}
*/
 
 /*
#ifdef LIDAR_SENSOR
void CMessageQueue::WriteLidarPoint(const PointCloudMsg<PointXYZI>& msg,ChannelType side)
{
    RemoNet::LidarPoint pt;
    pt.set_is_left(side==ChannelType::CHANNEL_LEFT_LIDAR);
    pt.set_frame_id(msg.frame_id);
    pt.set_height(msg.height);
    pt.set_width(msg.width);
    pt.set_is_dense(msg.is_dense);
    pt.set_seq(msg.seq);
    pt.set_timestamp(msg.timestamp);
    for(int i=0;i<msg.point_cloud_ptr->size();i++)
    {
        pt.add_data((*msg.point_cloud_ptr)[i].x);
        pt.add_data((*msg.point_cloud_ptr)[i].y);
        pt.add_data((*msg.point_cloud_ptr)[i].z);
        pt.add_data((*msg.point_cloud_ptr)[i].intensity);
    }
    MessageHead Head;
    CIOBuffer Buffer;
    Head.Command=RemoNet::CC_LIDARDATA;
    Head.Length=pt.ByteSizeLong();
    Head.Serialize(Buffer.Buffer);
    auto ptr = Buffer.Buffer + MessageHead::Size();
    pt.SerializeToArray(ptr, Head.Length);
    Buffer.Length = Head.Length + MessageHead::Size();
    
    if( _peerArray[side]!=nullptr)
        _peerArray[side]->SendData(&Buffer);
}
#endif
 */
void CMessageQueue::SwitchCamera(bool front)
{
     if(!_camera_inited) return;
     
      if(_peerArray[RenderPosition::FRONT]!=nullptr) _peerArray[RenderPosition::FRONT]->SwitchCapture(front);
      if(_peerArray[RenderPosition::BACK]!=nullptr) _peerArray[RenderPosition::BACK]->SwitchCapture(front);
      if(_peerArray[RenderPosition::LEFT]!=nullptr) _peerArray[RenderPosition::LEFT]->SwitchCapture(front);
      if(_peerArray[RenderPosition::RIGHT]!=nullptr) _peerArray[RenderPosition::RIGHT]->SwitchCapture(front);
      if(_peerArray[RenderPosition::LANCHOR]!=nullptr) _peerArray[RenderPosition::LANCHOR]->SwitchCapture(front);
      if(_peerArray[RenderPosition::RANCHOR]!=nullptr)  _peerArray[RenderPosition::RANCHOR]->SwitchCapture(front);
}
void CMessageQueue::StopAutomous()
{
   SetAutomous(false,0);
   CIOBuffer pBuffer;
      //auto ptr=pBuffer.Buffer;
      MessageHead Head;
      Head.Command=RA_Stop;
      //   ptr=pBuffer.Buffer+sizeof(MessageHead);
      Head.Length=0;
      Head.Serialize(pBuffer.Buffer);
      pBuffer.Length=MessageHead::Size();
      _robot->Write(pBuffer.Buffer,pBuffer.Length);
}
int32_t CMessageQueue::GetPeer()
{
    return _peerId;
}
void CMessageQueue::OnTipRise(TPCANMsg* msg)
{
        msg[3].ID=0x386;
        msg[3].DATA[0]=120;
        msg[3].DATA[1]=120;
        msg[4].ID=0x486;
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
      //   memset(msg[4].DATA,0,sizeof(msg[4].DATA));
       // std::cout<<_robot->Get()->GetRiseDownValue()<<","<<_left_length<<","<<_right_length<<std::endl;
        auto tick= std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        
        if(!_start_check)
        {
              if(tick-_start_time>3)
             {
                _start_check=true;
                _start_time=tick;
             } 
        }
        else
        {
             if(tick-_start_time>1)
            {
                if(pre_left_length==_left_length || pre_right_length==_right_length)
                {
                        msg[3].ID=0x386;
                        msg[3].DATA[0]=0;
                        msg[3].DATA[1]=0;
                        msg[4].DATA[4]=0;
                        msg[4].DATA[5]=0;
                        msg[4].DATA[2]=0;
                        msg[4].DATA[3]=0;
                         pre_left_length=-1;
                        pre_right_length=-1;
                        _parking=ParkingState::TipOut;
                        return;       
                }
                else
                {
                    _start_time=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    pre_left_length=_left_length;
                    pre_right_length=_right_length;
                }
            }
        }

        if(!_start_check)
        {
            BYTE a=lo_byte(_robot->Get()->GetTipValue());
            BYTE b=hi_byte(_robot->Get()->GetTipValue());
            msg[4].DATA[4]=b;
            msg[4].DATA[5]=a;
            msg[4].DATA[2]=b;//hi_byte(_robot->Get()->GetTipValue());
            msg[4].DATA[3]=a;//lo_byte(_robot->Get()->GetTipValue());
        }
        else  if((_left_length>max_left_length&&_left_length<min_left_length)||(_right_length<max_right_length&&_right_length>min_right_length))
        {
           
           BYTE a=lo_byte(_robot->Get()->GetTipValue());
           BYTE b=hi_byte(_robot->Get()->GetTipValue());
            msg[4].DATA[4]=b;
            msg[4].DATA[5]=a;
           msg[4].DATA[2]=b;//hi_byte(_robot->Get()->GetTipValue());
            msg[4].DATA[3]=a;//lo_byte(_robot->Get()->GetTipValue());



        }
            /*
           BYTE a=lo_byte(_robot->Get()->GetTipValue());
           BYTE b=hi_byte(_robot->Get()->GetTipValue());
         if(_left_length>max_left_length&&_left_length<min_left_length)
        {
           
            msg[4].DATA[4]=b;
            msg[4].DATA[5]=a;
        
        }
        if(_right_length<max_right_length&&_right_length>min_right_length)
        {
            msg[4].DATA[2]=b;
            msg[4].DATA[3]=a;
        }*/
       
}
CarType CMessageQueue::GetCarType()
{
    return _car;
}
void CMessageQueue::OnRiseDown(TPCANMsg* msg)
{
    

        msg[3].ID=0x386;
        msg[3].DATA[0]=120;
        msg[3].DATA[1]=120;
        msg[4].ID=0x486;
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
    //    std::cout<<_robot->Get()->GetRiseDownValue()<<","<<_left_length<<","<<pre_left_length<<std::endl;
        auto tick= std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        if(!_start_check)
        {
              if(tick-_start_time>3)
             {
                _start_check=true;
                _start_time=tick;
             } 
        }
        else{
            if(tick-_start_time>1)
            {
                    if(pre_left_length==_left_length && pre_right_length==_right_length)
                    {
                            msg[3].ID=0x386;
                            msg[3].DATA[0]=0;
                            msg[3].DATA[1]=0;
                            msg[4].DATA[4]=0;
                            msg[4].DATA[5]=0;
                            msg[4].DATA[2]=0;
                            msg[4].DATA[3]=0;
                            pre_left_length=-1;
                            pre_right_length=-1;
                            _parking=ParkingState::None;
                            
                            return;       
                    }
                    else
                    {
                        _start_time=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                        pre_left_length=_left_length;
                        pre_right_length=_right_length;
                    }
            }
        }

          if(!_start_check)
          {
               BYTE a=lo_byte(_robot->Get()->GetRiseDownValue());
           BYTE b=hi_byte(_robot->Get()->GetRiseDownValue());
            msg[4].DATA[4]=a;
            msg[4].DATA[5]=b;
           msg[4].DATA[2]=a;//hi_byte(_robot->Get()->GetRiseDownValue());
            msg[4].DATA[3]=b;//lo_byte(_robot->Get()->GetRiseDownValue());
          }
       else  if((_left_length>max_left_length&&_left_length<min_left_length)||(_right_length<max_right_length&&_right_length>min_right_length))
        {
           
           BYTE a=lo_byte(_robot->Get()->GetRiseDownValue());
           BYTE b=hi_byte(_robot->Get()->GetRiseDownValue());
            msg[4].DATA[4]=a;
            msg[4].DATA[5]=b;
           msg[4].DATA[2]=a;//hi_byte(_robot->Get()->GetRiseDownValue());
            msg[4].DATA[3]=b;//lo_byte(_robot->Get()->GetRiseDownValue());



        }
       
        /*
        if((_left_length<=max_left_length||_left_length>=min_left_length)&&(_right_length>=max_right_length||_right_length<=min_right_length)) 
        {
          
          
            msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=0;
             msg[4].DATA[4]=0;
            msg[4].DATA[5]=0;
             msg[4].DATA[2]=0;
            msg[4].DATA[3]=0;
             _parking=ParkingState::None;
         //   _robot->Get()->StartParking(false);
        }
        */

}

/*
void CMessageQueue::OnDown(TPCANMsg* msg)
{
     msg[3].ID=0x386;
        msg[3].DATA[0]=90;
        msg[3].DATA[1]=90;
        msg[4].ID=0x486;
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
     if()
        {
            msg[4].ID=0x486;
            msg[4].LEN=8;
            msg[4].DATA[4]=0x81;
            msg[4].DATA[5]=0x00;
            msg[3].ID=0x386;
            msg[3].DATA[0]=90;
            msg[3].DATA[1]=90;


        }
        else
        {
            msg[4].ID=0x486;
            msg[4].LEN=8;
            msg[4].DATA[4]=0;
            msg[4].DATA[4]=0;
            msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=0;
            _parking=ParkingState::None;
           _robot->Get()->StartParking(false);
        }
}
*/

void CMessageQueue::OnTipOut(TPCANMsg* msg)
{
        msg[3].ID=0x386;
        msg[3].DATA[0]=120;
        msg[3].DATA[1]=120;
        msg[4].ID=0x486;
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
          BYTE a=lo_byte(_robot->Get()->GetTipValue());
           BYTE b=hi_byte(_robot->Get()->GetTipValue());
        auto tick=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
     if(tick-_start_time<tipout_time)
     {
            msg[4].DATA[0]=b;
            msg[4].DATA[1]=a;  
     }
     else
     {
            msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=0;
            msg[4].DATA[0]=0;
            msg[4].DATA[1]=0;  
            _parking=ParkingState::None;
        //      _robot->Get()->StartParking(false);
     }
    
}

void CMessageQueue::OnTipIn(TPCANMsg* msg)
{
         msg[3].ID=0x386;
        msg[3].DATA[0]=120;
        msg[3].DATA[1]=120;
        msg[4].ID=0x486;
          BYTE a=lo_byte(_robot->Get()->GetTipValue());
           BYTE b=hi_byte(_robot->Get()->GetTipValue());
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
         auto tick=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
      if(tick-_start_time<tipin_time)
     {
            msg[4].DATA[0]=b;
            msg[4].DATA[1]=a;  
     }
     else
     {
            msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=0;
            msg[4].DATA[0]=0;
            msg[4].DATA[1]=0;  
            _parking=ParkingState::None;
     }
}
void CMessageQueue::OnTipDown(TPCANMsg* msg)
{
     msg[3].ID=0x386;
        msg[3].DATA[0]=90;
        msg[3].DATA[1]=90;
        msg[4].ID=0x486;
          BYTE a=lo_byte(_robot->Get()->GetTipValue());
           BYTE b=hi_byte(_robot->Get()->GetTipValue());
        memset(msg[4].DATA,0,sizeof(msg[4].DATA));
       if(_left_length>min_left_length)
        {
            msg[4].ID=0x486;
            msg[4].LEN=8;
            msg[4].DATA[4]=b;
            msg[4].DATA[5]=a;
            msg[3].ID=0x386;
            msg[3].DATA[0]=100;
            msg[3].DATA[1]=100;


        }
        else
        {
            msg[4].ID=0x486;
            msg[4].LEN=8;
            msg[4].DATA[4]=0;
            msg[4].DATA[4]=0;
            msg[3].ID=0x386;
            msg[3].DATA[0]=0;
            msg[3].DATA[1]=0;
            _parking=ParkingState::None;
         //  _robot->Get()->StartParking(false);
        }
}
ParkingState CMessageQueue::GetParkingState() 
{
    return _parking;
}
void CMessageQueue::SetParkingState(ParkingState state)
{
    _parking=state;
    if(_parking!=ParkingState::None)
    {
        _start_time=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        _start_check=false;
    }

}
extern std::string getpath();
/*
void CMessageQueue::BegRecord()
{
     if(_start_record==true) return;
         _start_record=true;
    
    auto path=getpath();
    auto now = std::chrono::system_clock::now();
    auto in_time_t = std::chrono::system_clock::to_time_t(now);
    std::stringstream ss;
    ss<<path<<"/../track/";
    std::cout<<ss.str().c_str()<<std::endl;
    if(access(ss.str().c_str(), F_OK) == -1) {
     mkdir(ss.str().c_str(),S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO);
    }
    ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d-%X.track");
    _ofile.open(ss.str().c_str(),std::ios::binary|std::ios::out);
    
    
}
void CMessageQueue::EndRecord()
{
   if(_start_record==true)
  {
      _start_record=false;
      _ofile.close();
  } 
}
void CMessageQueue::WriteRecord(TPCANMsg * msg)
{
    cannet_frame frame[5];
    for(int32_t i=0;i<5;i++)
    {
       frame[i].canid=msg[i].ID;
       frame[i].dlc=msg[i].LEN;
       memcpy(frame[i].data,msg[i].DATA,frame[i].dlc);
    }
   _ofile.write((char *)frame,sizeof(frame));
}
 void CMessageQueue::LoadRecord(ROUT_LIST dir,const char * file)
{
    
   auto path=getpath();
   std::stringstream ss;
  ss<<path<<"/../track/"<<file;
  std::ifstream ifile(ss.str().c_str(),std::ios::in|std::ios::binary);
  if(ifile.is_open())
  {
      long l=ifile.tellg();
      ifile.seekg(0,std::ios::end);

      long m=ifile.tellg();
      //frames[dir]=static_cast<cannet_frame *>(malloc(m-l));
      ifile.seekg(0,std::ios::beg);
      cannet_frame frame;
      int32_t index=0;
      while(ifile.read((char * )&frame,sizeof(frame)))
      {
          frames[dir].push_back(frame);
          index++;
      }
      ifile.close();
     // std::cout<<frames[dir].size()<<std::endl;



  }
    

    
}

void CMessageQueue::PlayRecord(ROUT_LIST index)
{
    if(_parking!=ParkingState::None) return;
    _rout_index=index;
    _play_index=0;
    _parking=ParkingState::PlayRecord;
}
*/