#pragma once
#include "../common/comm.h"
#include "../common/notifier.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"

using namespace std;



class CMessageQueue;
class CCanBusSensor
{
public:
     CCanBusSensor(CMessageQueue *q);
     void Notify(struct can_frame *date);
     
     void SetCanBusSensor(SensorCanBus<CCanBusSensor>* can);
     void Start();
     void Stop();

     void OnMessage(cannet_frame* frames, int32_t count = 4);
     void Emergency();
     

/*------------------小车控制参数和函数起------------------*/

     void order(cannet_frame* frame);  // 指令模式
     void toggleLamp(cannet_frame* frame);  // 车灯开关  
     void move_run(cannet_frame* frame); // 车辆前进  
     void move_neutral(cannet_frame* frame); //空挡
     void move_back(cannet_frame* frame); // 车辆后退  
     void move_rot(cannet_frame* frame);  // 车辆右转  
     bool car_stop(cannet_frame* frame);
     int gearselection(cannet_frame* frame); // 1:前进档 2:空挡 3:后退档
     void speedlevel(cannet_frame* frame); //升档降档
     void runSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1); //座舱速度与小车速度线性对应
     void backSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1);
     void leftSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1);
     void rightSpeed(unsigned char input, unsigned char &byte0, unsigned char &byte1);

     bool order_flag ; // 指令模式
     bool lamp_flag ; // 车灯状态 
     bool run_flag ; // 车辆前进后退
     bool rot_flag ; // 车辆转弯
     bool stop_flag ; // 急停状态
     int gear; //前进后退空档位
     int speed_flag = 11; //升档降档档位
     // unsigned int mapped_value;

     unsigned char id_init[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
     unsigned char id_111[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
/*------------------小车控制参数和函数止------------------*/

private:
     void Run();
     void CanSendProc();
     void SendStatusToMSG();
     

private:
     CMessageQueue* _message;
     SensorCanBus<CCanBusSensor> * _canbus;
     bool _run;

     std::thread _can_thread;
     std::mutex _last_can_lock;
     struct can_frame Sendframe[7];
     bool _front_view = true;

     int16_t Direction;//方向
     int16_t Hand_Throttle;//手油门
     int16_t Foot_Throttle;//脚油门
     int16_t Brake;//刹车

     int64_t count;
     FeedData _data;
     int model;//0本地,1远程

     /*-------------------小车控制起--------------------*/

     unsigned char order_on[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};        //id:421 指令模式开
     unsigned char headlamp_on[8] = {0x01,0x01,0x50,0x00,0x00,0x00,0x00,0x00};     //id:121 前车灯常开
     unsigned char headlamp_off[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};    //id:121 前车灯常关
     unsigned char taillamp_on[8] = {0x01,0x00,0x00,0x01,0x50,0x00,0x00,0x00};     //id:121 后车灯常开
     unsigned char taillamp_off[8] = {0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00};    //id:121 后车灯常关
     unsigned char move_forward[8] = {0x00,0x96,0x00,0x00,0x00,0x00,0x00,0x00};    //id:111 以0.15m/s前进
     unsigned char move_backward[8] = {0xFF,0x6A,0x00,0x00,0x00,0x00,0x00,0x00};   //id:111 以0.15m/s后退
     unsigned char turn_left[8] = {0x00,0x00,0x00,0xc8,0x00,0x00,0x00,0x00};       //id:111 以0.2rad/s左转
     unsigned char turn_right[8] = {0x00,0x00,0xFF,0x38,0x00,0x00,0x00,0x00};      //id:111 以0.2rad/s右转
     unsigned char stop_on[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};         //id:111 急停
     /*-------------------小车控制止--------------------*/
     
};