#include <memory>
#include <signal.h>
#include <fstream>
#include <stdio.h>
#include "vehicle_manager/sensor_socket.h"
#include "vehicle_manager/vehicle_manager_node.h"
#include "vehicle_manager/iobuffer.h"
#include "vehicle_manager/message.h"
#include "vehicle_manager/rapidcsv.h"
#include <geometry_msgs/PoseStamped.h>
#include <assert.h>


 

VehicleManager::VehicleManager():nh_(""), pnh_("~")
{
    pub_cmd_ = nh_.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 1);
  //  pub_driver_=nh_.advertise<vehicle_msg::vehicle_state>("/goal_start", 1);
    pub_goal_=nh_.advertise<vehicle_msg::vehicle_state>("/goal_finish", 1);
    sub_goal_ = nh_.subscribe("/goal_finish", 1, &VehicleManager::callbackGoalFinish, this);
    sub_ndt_pos_=nh_.subscribe("/ndt_pose",1,&VehicleManager::callbackNdtPos,this);
    sub_vehiclecmd_= nh_.subscribe("/vehicle_cmd", 1, &VehicleManager::callbackVehicleCmd, this);
    pnh_.param<std::string>("ip", ip_, std::string(""));
    assert(ip_.empty()==false);
    pnh_.param<int32_t>("port", port_, 23001);
    pnh_.param<std::string>("launch_data", file_, std::string("empty"));
    pnh_.param<std::string>("work_area",_areafile,std::string("empty"));
    pnh_.param<bool>("increment",_throttle_increment,false);
    // pnh_.param<int32_t>("throttle_value",max_throttle_value,80);
    pnh_.param<int32_t>("throttle_value",max_throttle_value,60);
    // assert(max_throttle_value<=80);
    pnh_.param<float>("steer_ratio",_steer_ratio,2.5);

}
bool  VehicleManager::Setup()
{
     // std::cout<<ip_.c_str()<<std::endl;
   
    std::ifstream ifile(file_);
    if(ifile.is_open()==false)
    {
          std::cout<<file_<<std::endl;
        return false;
    }
    
    std::string str;
    bool bPreDynamic=true;
    bool bPostDynamic=false;
    bool bStatic=false;
    std::vector<std::string> bootproc;
    while(std::getline(ifile,str))
    {

        //进入
       if(str.find("*****")!=-1)
       {
           
           bPreDynamic=false;
           bPostDynamic=true;   
            continue;
       } 
       else if(str.find("-----")!=-1)
       {
          
           bPreDynamic=false;
           bPostDynamic=false;  
           bStatic=true;
           continue;
       } 
       if(bPreDynamic)
       {
        ROS_INFO(" pre %s",str.c_str());
        proc_info info;
         info.cmd=str;
         info.pid=-1;
         prev_.push_back(info);
       }
       else if(bPostDynamic)
       {
         proc_info info;
         ROS_INFO(" post %s",str.c_str());
         info.cmd=str;
         info.pid=-1;
         post_.push_back(info);
       }
       else{
        ROS_INFO(" boot %s",str.c_str());
        bootproc.push_back(str);
       }
         
    }
     /*  for(int i=0;i<bootproc.size();i++)
       {
          auto& node=bootproc[i];
           int32_t pid=fork();
           if(pid==0)
           {
              
              ROS_INFO("%s",node.c_str());
              system(node.c_str());
              
              exit(0);
           }
       }
*/
    //ROS_INFO("procName size %d",procName_.size());
     
    rapidcsv::Document doc(_areafile.c_str());
	int32_t count= doc.GetRowCount();
	 std::vector < std::string > name= doc.GetColumn<std::string>(0);   //A1 ***  K14
	std::vector<float> x = doc.GetColumn<float>(1);   //x
	std::vector<float> y = doc.GetColumn<float>(2);    //y
	    std::cout << "count="<<count<<std::endl;
	for (int32_t i = 0; i < count; i++)
	{
       // std::cout<<i<<std::endl;
		//std::cout << name[i] << "," << x[i] << "," << y[i] << std::endl;
		const char* str = name[i].c_str() + 1;   
		int32_t index = std::stoi(str)-1;
     //     std::cout<<i<<","<<index<<","<<name[i].c_str()[0] - 'A'<<std::endl;
		AreaPos[name[i].c_str()[0] - 'A'][index].x = x[i];
		AreaPos[name[i].c_str()[0] - 'A'][index].y = y[i];
       // std::cout<<"OK"<<std::endl;
        
	}
    for(auto i=0;i<=WorkArea::Area_H;i++)
    {
        Forward[i].x=AreaPos[i][0].x-AreaPos[i][49].x;
        BackWard[i].x=AreaPos[i][49].x-AreaPos[i][0].x;
        Forward[i].y=AreaPos[i][0].y-AreaPos[i][49].y;
        BackWard[i].y=AreaPos[i][49].y-AreaPos[i][0].y;
        float len=sqrt(Forward[i].x*Forward[i].x+Forward[i].y*Forward[i].y);  
        Forward[i].x/=len;
        Forward[i].y/=len;
        len=sqrt(BackWard[i].x*BackWard[i].x+BackWard[i].y*BackWard[i].y);
        BackWard[i].x/=len;
        BackWard[i].y/=len;
    }

    /*
    xMin=AreaPos[WorkArea::Area_A][49].x;
	for(int32_t i=0;i<WorkArea::Area_H;i++)
    {
 
    }
    */

  
    /*
    while(true)
    {
         std::this_thread::sleep_for(std::chrono::seconds(3));
        OnBootAuto();
        std::this_thread::sleep_for(std::chrono::seconds(10));
        OnStopAuto();
       
    }
    */
    _socket=std::make_unique<SensorSocket>(this,ip_.c_str());
    if(_socket->Start()==false) return false;
     _topic_recved=false;
     _startup=false;
     return true;
}
void VehicleManager::Shutdown()
{
    _topic_recved=false;
    OnStopAuto();
}
inline int8_t hi_byte(int16_t value)
{
	int8_t hi =(int8_t)((value & 0xFF00) >> 8);
	return hi;
}
inline int8_t lo_byte(int16_t value)
{
	int8_t lo = (int8_t)(value & 0xff);
	return lo;
}
void VehicleManager::Notify(int8_t * buffer,int32_t size)
{
    MessageHead Head;
    Head.Deserialize(buffer);
    auto ptr=buffer+MessageHead::Size();
    switch(Head.Command)
    {
      /*  case RA::RA_Boot:
        {
            OnBootAuto();
        }
        break;
        case RA::RA_Stop:
        {
            OnStopAuto();
        }
        break;
        */
        case RA::RA_Goal:
        {
            ARMoveGoal* node=(ARMoveGoal *)ptr;
        
             ROS_INFO("received move to  area %d,%d",node->area,node->no);
            OnAutoMove(node->area,node->no, MoveType::Forward);
        }

        break;
        case RA::RA_Encode:
        {
            cannet_frame * frame=(cannet_frame *)ptr;
            int32_t l0=frame->data[0]&0x000000ff;
            int32_t l1=frame->data[1]&0x000000ff;
    //             for (int i = 0; i < 8; i++) {
    //     printf(" 0x%02X", frame->data[i]);
    // }
            _steer_angle= /*(l3<<24)|(l2<<16)|*/(l1<<8)|l0;
        }
        break;
        case RA::RA_Stop:
        {
            OnStopAuto();
            IsActiveStop = true;
        }
        break;
    }
}
void VehicleManager::OnBootPrev()
{   
     ROS_INFO("proc account %d",prev_.size());
     for(int32_t i=0;i<prev_.size();i++)
     {
        ROS_INFO("%s",prev_[i].cmd);
     }
    for(int i=0;i<prev_.size();i++)
    {
          auto& node=prev_[i];
          if(node.pid==-1)
        {
           int32_t pid=fork();
           if(pid==0)
           {
              
              ROS_INFO("%s",node.cmd.c_str());
              system(node.cmd.c_str());
              
              exit(0);
           }
            else{
            
            node.pid=pid;
           }
            
        }
        //  std::this_thread::sleep_for(std::chrono::seconds(1));
    } 
}
void VehicleManager::OnBootPost()
{
     ROS_INFO("post proc account %d",post_.size());
    for(int i=0;i<post_.size();i++)
    {
          auto& node=post_[i];
          if(node.pid==-1)
        {
           int32_t pid=fork();
           if(pid==0)
           {
              
              ROS_INFO("%s",node.cmd.c_str());
              system(node.cmd.c_str());
              
              exit(0);
           }
            else{
            
            node.pid=pid;
           }
            
        }
         std::this_thread::sleep_for(std::chrono::seconds(2));
    } 
}
/*
void VehicleManager::OnBootAuto()
{
    ROS_INFO("proc account %d",proc_.size());
    for(int i=0;i<proc_.size();i++)
    {
          auto& node=proc_[i];
          if(node.pid==-1)
        {
           int32_t pid=fork();
           if(pid==0)
           {
              
              ROS_INFO("%s",node.cmd.c_str());
              system(node.cmd.c_str());
              
              exit(0);
           }
            else{
            
            node.pid=pid;
           }
            
        }
         std::this_thread::sleep_for(std::chrono::seconds(4));
    } 
    
}
*/
/// @brief     
/// @param x 
/// @param y 
/// @param z 

//#define OFFSET 18.8f C,D
#define GLOBAL_OFFSET 19.0f;
void VehicleManager::OnAutoMove(int32_t area, int32_t no,MoveType type)
{
    if (IsActiveStop)
    {
        OnBootPrev();
        
    }
  
   float OFFSET=GLOBAL_OFFSET;
    std::lock_guard<std::mutex> l(_pos_lock);
      
    // if(_start==true) return;
    // OnStopAuto();
    float cx=_curPos.pose.position.x;
    float cy=_curPos.pose.position.y;
    //float cz=curPos.pose.postion.z;
    float dx=AreaPos[area][no].x;
    float dy=AreaPos[area][no].y;
    float lx=dx-cx;
    float ly=dy-cy;
    Eigen::Vector3d v(lx,ly,0);
    ROS_INFO("%d,%d",cx,cy);
    if(v.norm()<10) return;
    
    _moveType=type;
    area_=area;
    no_=no;
    // OnBootPrev();//临时
   // Direction curDir=GetCarDirection(_curPos.pose.orientation.z,_curPos.pose.orientation.w);
    Quaternion dir;
    geometry_msgs::PoseStamped pose;
   
    
    
    if(area==WorkArea::Area_J)
    {
       Direction d=Direction::South;
       if(no==0||no==1)
       {
         d=Direction::South;
       } 
       else if(no==12||no==13)
       {
          d=Direction::North;
       }
       else{
           d=dy>cy?Direction::North:Direction::South;
       }
       dir=qarray[d];
       if(d==Direction::South)
       {
        dx=AreaPos[area+1][no].x;
        dy=AreaPos[area+1][no].y;
       }
        if ( no == 0 || no ==1 || no == 12 || no == 13)
        {
            dy+=(d==Direction::South?-25:25);
        } else {
            dy+=(d==Direction::South?-16:16);
        }
    }
    else if(area==WorkArea::Area_I)
    {
         dir=qarray[Direction::South];
       
    }
    else
    {
        //v.normalize();
        
        
        Eigen::AngleAxisd axis;
        axis=Eigen::Quaterniond(_curPos.pose.orientation.w,_curPos.pose.orientation.x,_curPos.pose.orientation.y,_curPos.pose.orientation.z);
        //auto e=_curPos.pose.orientation.matrix().eulerAngles(0,1,2);
        double cur_angle=axis.angle();
        auto v=axis.axis();
        if(v[2]==-1)
        {
                 cur_angle=EIGEN_PI*2-cur_angle;
        }
        const auto epilson=(30/160)*EIGEN_PI;
        
        if(dy-cy>20)
        {
           
            if(cx>AreaPos[area_][0].x)
            {
                if(cur_angle>EIGEN_PI*3/4 && cur_angle<EIGEN_PI*5/4)
                {
                       dir=qarray[Direction::East];
                     
                       dx-=OffsetWest(area_);
                }
                else if(cur_angle>EIGEN_PI/4&& cur_angle<EIGEN_PI*5/4)
                 {
                    dir=qarray[Direction::West];
                    
                    dx-=OffsetWest(area_);
                    //dy-=Forward[area].y*10;
                 }
                 else //if(cur_angle>EIGEN_PI*5/4&& cur_angle<EIGEN_PI*7/4)
                 {
                     dir=qarray[Direction::East];
                   
                     dx+=OffsetEast(area_) ;
                     //dy+=Forward[area].y* 10 ;
                 }
            }
            else if(cx<AreaPos[area_][49].x)
            {
                if(cur_angle>0&&cur_angle<EIGEN_PI)
                {
                    dir=qarray[Direction::East];
                    
                    dx+= OffsetEast(area_)  ;
                    //dy+=Forward[area].y* 10 ;
                }
                else
                {
                    dir=qarray[Direction::West];
                    dx-=OffsetWest(area_);
                    //dy-=Forward[area].y*10;
                }
            }
            else
            {
                 if(cur_angle<EIGEN_PI/4||cur_angle>EIGEN_PI*7/4)
                 {
                       dir=qarray[Direction::West];
                     
                       dx-=OffsetWest(area_);
                      //dy-=Forward[area].y*10;
                 }
                 else
                 {
                    dir=qarray[Direction::East];
                     
                    dx+=OffsetEast(area_)  ;
                    //dy+=Forward[area].y* 10 ;
                 }
            }
           
            
        }
        else if(cy-dy>20)
        {
             if(cx>AreaPos[area_][0].x)
            {
                 if(cur_angle>EIGEN_PI/4&& cur_angle<EIGEN_PI*5/4)
                 {
                      dir=qarray[Direction::East];
                   
                     dx+=OffsetEast(area_)  ;
                     //dy+=Forward[area].y* 10 ;
                  
                 }
                 else //if(cur_angle>EIGEN_PI*5/4&& cur_angle<EIGEN_PI*7/4)
                 {
                     dir=qarray[Direction::West];
                    
                    dx-=OffsetWest(area_);
                    //dy-=Forward[area].y*10;
                 }
            }
            else if(cx<AreaPos[area_][49].x)
            {
                if(cur_angle>0&&cur_angle<EIGEN_PI)
                {
                     dir=qarray[Direction::West];
                    
                    dx-=OffsetWest(area_);
                   
                }
                else
                {
                   

                    dir=qarray[Direction::East];
                 
                    dx+=OffsetEast(area_)  ;
                   
                }
            }
            else
            {
                 if(cur_angle<EIGEN_PI/4||cur_angle>EIGEN_PI*7/4)
                 {
                       dir=qarray[Direction::West];
                    
                       dx-=OffsetWest(area_);
                     
                 }
                 else
                 {
                    dir=qarray[Direction::East];
                 
                    dx+=OffsetEast(area_)  ;
                   
                 }
            }
        }
        else{
            if(cx>AreaPos[area_][0].x)
            {
                if(dy>cy)
                {
                     if(cur_angle>EIGEN_PI/4&& cur_angle<EIGEN_PI*3/4)
                        {
                             dir=qarray[Direction::West];
                  
                            dx-=OffsetWest(area_);
                            //dy-=Forward[area].y*10;
                        }
                    else if (cur_angle>EIGEN_PI*3/4&& cur_angle<EIGEN_PI*5/4)
                        {
                             dir=qarray[Direction::West];
                  
                            dx-=OffsetWest(area_);
                            //dy-=Forward[area].y*10;
                        }
                        else //if(cur_angle>EIGEN_PI*5/4&& cur_angle<EIGEN_PI*7/4)
                        {
                          
                            
                            //dy+=Forward[area].y* 10 ;
                             dir=qarray[Direction::East];
                   
                            dx+=OffsetEast(area_)  ;
                        }
                }
                else
                {
                     if(cur_angle>EIGEN_PI/4&& cur_angle<EIGEN_PI*3/4)
                        {
                            
                             dir=qarray[Direction::East];
                            
                            dx+=OffsetEast(area_)  ;
                            //dy-=Forward[area].y*10;
                        }
                        else //if(cur_angle>EIGEN_PI*5/4&& cur_angle<EIGEN_PI*7/4)
                        {
                          
                            //dy+=Forward[area].y* 10 ;
                           
                              dir=qarray[Direction::West];
                             
                              dx-=OffsetWest(area_);
                        }
                }
            }
            else if(cx<AreaPos[area_][49].x)
            {
                if(dy>cy)
                {
                    if(cur_angle>0&&cur_angle<EIGEN_PI)
                    {
                        dir=qarray[Direction::East];
                      
                        dx+=OffsetEast(area_)  ;
                        //dy+=Forward[area].y* 10 ;
                    }
                    else
                    {
                        dir=qarray[Direction::West];
                        
                        dx-=OffsetWest(area_);
                        //dy-=Forward[area].y*10;
                    }
                }
                else
                {
                     if(cur_angle>0&&cur_angle<EIGEN_PI)
                    {
                        dir=qarray[Direction::West];
                        
                        dx-=OffsetWest(area_);
                        //dy+=Forward[area].y* 10 ;
                    }
                    else
                    {
                      
                        //dy-=Forward[area].y*10;
                        dir=qarray[Direction::East];
                   
                        dx+=OffsetEast(area_)  ;
                    }
                }
            }
            else{
             if(cur_angle<EIGEN_PI/4||cur_angle>EIGEN_PI*7/4)
             {
                  dir=qarray[Direction::East];
                   dx+=OffsetEast(area_)  ;
              /*  if(dx<cx)
                {
                     dir=qarray[Direction::East];
                      
                       dx-=OffsetWest(area_);
                    
                }
                else{
                     
                       dir=qarray[Direction::West];
                   
                    dx+=OffsetEast(area_)  ;
                }
                */
             }
             else
             {
                 dir=qarray[Direction::West];
                   
                      dx-=OffsetWest(area_);
               /* if(dx>cx)
                {
                      dir=qarray[Direction::West];
                    
                    dx+=OffsetEast(area_)  ;
                     
                }
                else{
                      dir=qarray[Direction::West];
                   
                      dx-=OffsetWest(area_);
                     
                }
                */
             }
             }

        }
    }
        
        
    ROS_INFO("area %d,no %d, %f,%f,%f,%f",area,no,dx,dy,dir.z,dir.w);
    _destX=dx;
    _destY=dy;
    pose.pose.position.x=dx;
    pose.pose.position.y=dy;
    pose.pose.position.z =0;
    _dir=Eigen::Quaterniond(dir.w,dir.x,dir.y,dir.z);
    
    pose.pose.orientation.x=dir.x;
    pose.pose.orientation.y=dir.y;
    pose.pose.orientation.z=dir.z;
    pose.pose.orientation.w=dir.w;
    std::this_thread::sleep_for(std::chrono::seconds(3));
 
    pub_cmd_.publish(pose);
  //  std::this_thread::sleep_for(std::chrono::seconds(1));
    OnBootPost();
  
    
    
    std::this_thread::sleep_for(std::chrono::seconds(2));
    _topic_recved=true;
    IsActiveStop = false;
  
   /*
    vehicle_msg::vehicle_state state;
    state.state=true;
    
    pub_driver_.publish(state);
    */

}  
float VehicleManager::OffsetEast(int32_t area)
{
      float OFFSET=GLOBAL_OFFSET;
      switch(area)
      {
        case WorkArea::Area_A:
            OFFSET-=0.15f;
            break;
        case WorkArea::Area_B:
            OFFSET-=1.5f;
            if (no_ == 0) 
                OFFSET += 1.0f;
            break;
        case WorkArea::Area_C:
            OFFSET-=0.5f;
            break;
        case WorkArea::Area_D:
            OFFSET-=0.7f;
            break;
        case WorkArea::Area_E:
        case WorkArea::Area_G:
         //OFFSET-=0.5f;
         OFFSET-=0.15f;
           break;
      } 
      return OFFSET;
                     
}
float VehicleManager::OffsetWest(int32_t area)
{
     float OFFSET=GLOBAL_OFFSET;
    switch(area)
     {
        case WorkArea::Area_A:
            OFFSET-=0.2f;
            break;
        case WorkArea::Area_B:
            OFFSET-=0.0f;
            break;
        case WorkArea::Area_C:
            OFFSET-=0.2f;
            break;
        case WorkArea::Area_D:
            OFFSET+=0.2f;
            break;
        case WorkArea::Area_E:
        case WorkArea::Area_G:
         //OFFSET-=0.5f;
         OFFSET-=0.2f;
       break;
    }
    return OFFSET; 
}
bool VehicleManager::IsLarger_X0(float cx,float cy)
{
    float x=AreaPos[area_][0].x;
    float y=AreaPos[area_][0].y;
    return cx>=x+10;
}

bool VehicleManager::IsSmall_X50(float cx,float cy)
{
     float x=AreaPos[area_][49].x;
     return cx<=x;
}
void VehicleManager::OnStopAuto()
{
    std::cout<<"Stop"<<std::endl;
     
    for(int i=0;i<prev_.size();i++)
    {
          auto& node=prev_[i];
            if(node.pid!=-1)
            {
             std::string kill_cmd = "kill -9 ";
            kill_cmd.append(std::to_string(node.pid));
            std::cout<<kill_cmd<<std::endl;
             system(kill_cmd.c_str());
          // kill(node.pid,SIGINT);
            
            }
            node.pid=-1;
    }
      for(int i=0;i<post_.size();i++)
    {
          auto& node=post_[i];
            if(node.pid!=-1)
            {
             std::string kill_cmd = "kill -9 ";
            kill_cmd.append(std::to_string(node.pid));
            std::cout<<kill_cmd<<std::endl;
             system(kill_cmd.c_str());
          // kill(node.pid,SIGINT);
            
            }
            node.pid=-1;
    }
    std::string  p[]={
    "twist_filter",
    "twist_gate",
    "op_global_plann",
    "pure_pursuit",
   
    };

    for(auto& name:p)
    {
        FILE *fp;
        char buf[255];
        char cmd[200] = {'\0'};
         pid_t pid = -1;
         sprintf(cmd, "ps -e | grep \'%s\' | awk \'{print $1}\'", name.c_str());
          
         ROS_INFO("%s",cmd);
          if((fp = popen(cmd, "r")) != NULL)
          {
             while(fgets(buf, 255, fp) != NULL)
             {
              
                pid = atoi(buf);
                  std::string kill_cmd = "kill -9 ";
                     kill_cmd.append(std::to_string(pid));
                std::cout<<kill_cmd<<std::endl;
                system(kill_cmd.c_str());
                printf("pid = %d \n", pid);
 
               
            }
             pclose(fp);
             
          }
          
    }

     
}
/*
Direction VehicleManager::GetCarDirection(float z,float w)
{
    Direction dir;
    float diff=1000000.f;
    for(int i=0;i<Direction::ALL;i++)
    {
        auto ret=(qarray[i].z-z)*(qarray[i].z-z)+(qarray[i].w-w)*(qarray[i].w-w);
        if(diff>ret)
        {
            diff=ret;
            dir=(Direction)i;

        }
    }
    return dir;
}
*/
void VehicleManager::callbackGoalFinish(const vehicle_msg::vehicle_stateConstPtr& ptr)
{
     ROS_INFO("Goal end,%d,%d",area_,no_);
     std::lock_guard<std::mutex> l(_pos_lock);
     if(ptr->state==true)
     {
        
         float dx=_curPos.pose.position.x-_destX;
         float dy=_curPos.pose.position.y-_destY;
         ROS_INFO("%f,%f",dx,dy);
   //      if(dx*dx+dy*dy<1)
         {
            CIOBuffer pBuffer;
            auto ptr=pBuffer.Buffer;
            MessageHead Head;
            Head.Command=AR_Finish;
            ptr=pBuffer.Buffer+sizeof(MessageHead);
            ARFinish * node=(ARFinish *)ptr;
            node->area=area_;
            node->no=no_;
            
            Head.Length=sizeof(ARFinish);
            Head.Serialize(pBuffer.Buffer);
            pBuffer.Length=sizeof(MessageHead)+sizeof(ARFinish);
            _socket->Write(pBuffer.Buffer,pBuffer.Length);
         }
         std::cout<<"Goal end"<<area_<<","<<no_<<std::endl;
          OnStopAuto();
          std::this_thread::sleep_for(std::chrono::seconds(5));
           OnBootPrev();//临时
         _topic_recved=false;
         
     }
}
void VehicleManager::callbackVehicleCmd(const autoware_msgs::VehicleCmdConstPtr vehicle_cmd_ptr)
{
    if(_topic_recved==false) return;
     auto vel=vehicle_cmd_ptr->twist_cmd.twist.linear.x;
     auto accel=vehicle_cmd_ptr->ctrl_cmd.linear_acceleration;
     if(vel < 0.05)
     {
         ROS_INFO("VEL:%f",vel);
     }
     
     if(vel!=0||accel!=0)
     {
        if(_startup==false)
        {
          _startup=true;
        }
        _zero_count=0;

          
     }
     else
     {
        if(_startup==true)
        {
             _zero_count++;
            if(_zero_count>500)
            {
                ROS_INFO("----------------------------------------------------------------------------------------*********");
                vehicle_msg::vehicle_state msg;
                msg.state=true;
                pub_goal_.publish(msg);
                _topic_recved=false;
                _startup=false;
                return;
            }
        }
     }
    
    //  ROS_INFO("vel %f,accel %f",vehicle_cmd_ptr->twist_cmd.twist.linear.x,vehicle_cmd_ptr->ctrl_cmd.linear_acceleration);
     float fs=(vehicle_cmd_ptr->ctrl_cmd.steering_angle*-1.f)*4096/(2*M_PI);
     int16_t steer=fs;
      
     int16_t diff=steer-_steer_angle;
     
     int16_t value=std::min(std::abs(diff*_steer_ratio),3000.f);//ptr->steer>0?1000:-1000; 
    //   ROS_INFO("steer %d,diff %d", steer,value);
     if(accel==0&&vel==0)
     {
        if(_startup==false)
           _brake=80;
        else
        {
			if(area_==WorkArea::Area_J)
			{
				int32_t temp=_brake+40;
				_brake=std::min(temp,100);
			}
			else
            {
				int32_t temp=++_brake;
				_brake=std::min(temp,100);
			}
        }
         _throttle=-30;
        
     }
     else
     {
        //int32_t temp=++_throttle;
        //_throttle=std::min(temp,_moveType==MoveType::Forward?60:40);
        if(_throttle_increment)
        {
            if(value>300)
            {
                if(_throttle>=40)
                {
                   int32_t temp=_throttle;
                  temp--;
                _throttle=std::max(temp,40);
                }
                
              //  _throttle/=2;
            }
            else{
              int32_t temp=++_throttle;
              _throttle=std::min(temp,max_throttle_value);
            } 
           

        }
        else{
             _throttle = max_throttle_value;
        }
       
        _brake=0;
      
     }
    // ROS_INFO("accel %d",_throttle);
  
     
     //int16_t value=std::min(std::abs(diff*5),3000.0);//ptr->steer>0?1000:-1000; 
     diff=_moveType==MoveType::Forward?diff:-diff;
    CIOBuffer pBuffer;
     
    MessageHead Head;
    Head.Command=AR::AR_CTRL;
    Head.Length=sizeof(can_array);
    Head.Serialize(pBuffer.Buffer);
    auto  ptr=pBuffer.Buffer+sizeof(MessageHead);
    double dx=_curPos.pose.position.x-_destX;
    double dy=_curPos.pose.position.y-_destY;
    double d=sqrt(dx*dx+dy*dy);
    if(d<=30.f&&_throttle>0)
    {
       Eigen::Quaterniond q(_curPos.pose.orientation.w,_curPos.pose.orientation.x,_curPos.pose.orientation.y,_curPos.pose.orientation.z);
       if(q.angularDistance(_dir)<(EIGEN_PI/3))
       {
        d/=30.f;
          _throttle=80*d;
       }
        
    } 
    can_array * data=(can_array *)ptr;
    data->count=3;
    data->frames[0].canid=0x586;
     data->frames[0].dlc=8;
     data->frames[0].data[0]=0XFF;
     data->frames[0].data[1]=0;
     data->frames[0].data[2]=hi_byte(value);
     data->frames[0].data[3]=lo_byte(value);
     data->frames[0].data[4]=diff < 0 ? 1 : 0;
     data->frames[0].data[4]|=0x02;
     data->frames[0].data[5]=0;
     data->frames[0].data[6]=0;
     data->frames[0].data[7]=0;
     memset(data->frames[1].data,0,sizeof(data->frames[1].data));
     data->frames[1].canid=0x386;
     data->frames[1].dlc=8;
     data->frames[1].data[0]=_throttle<0?0:_throttle>>1;
     data->frames[1].data[1]=_brake ;


    data->frames[2].canid=0x286;
    data->frames[2].dlc=8;
    data->frames[2].data[0]=0x04;//value>500?0x08:0x04;//_moveType==MoveType::Forward?0x04:0x0;
    data->frames[2].data[1]=0x0;//_moveType==MoveType::Forward?0x0:0x21;
    data->frames[2].data[2]=0x01;
    data->frames[2].data[3]=0x0;
    data->frames[2].data[4]=0x1B;
    data->frames[2].data[5]=0x0;
    data->frames[2].data[6]=0x02;
    data->frames[2].data[7]=0x0;
    pBuffer.Length=sizeof(MessageHead)+sizeof(can_array);
    
    _socket->Write(pBuffer.Buffer,pBuffer.Length);

      
   // ROS_INFO("steer:%f,throttle: %f,brake: %f",output.steer,output.throttle,output.brake);
   


}
void VehicleManager::callbackNdtPos(const geometry_msgs::PoseStampedConstPtr& msg)
{
     std::lock_guard<std::mutex> l(_pos_lock);
   
        _curPos.pose.position.x=msg->pose.position.x;
        _curPos.pose.position.y=msg->pose.position.y;
        _curPos.pose.position.z=msg->pose.position.z;
        _curPos.pose.orientation.x=msg->pose.orientation.x;
        _curPos.pose.orientation.y=msg->pose.orientation.y;
        _curPos.pose.orientation.z=msg->pose.orientation.z;
        _curPos.pose.orientation.w=msg->pose.orientation.w;
       
     static int64_t 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;
     if(diff>500)
     {
       
        
         CIOBuffer pBuffer;
         auto ptr=pBuffer.Buffer;
         MessageHead Head;
         Head.Command=AR_NDTPos;
         Head.Length=sizeof(ARNdtPos);
         Head.Serialize(pBuffer.Buffer);
         ptr=pBuffer.Buffer+sizeof(MessageHead);
         ARNdtPos * node=(ARNdtPos *)ptr;
         //    std::cout<< msg->pose.position.x<<","<<msg->pose.position.y<<","<<node->z<<std::endl;  
         node->rx=_curPos.pose.orientation.x;
         node->ry=_curPos.pose.orientation.y;
         node->rz=_curPos.pose.orientation.z;
         node->rw=_curPos.pose.orientation.w;
         node->x=_curPos.pose.position.x;
         node->y=_curPos.pose.position.y;
         node->z=_curPos.pose.position.z;
        

        
         pBuffer.Length=sizeof(MessageHead)+Head.Length;
         _socket->Write(pBuffer.Buffer,pBuffer.Length);
         
     }
      


}