#include <control/can.h>
#include <control/control_status.h>
#include <common/constants.h>
#include <common/logout.h>
#include <perception/sensor_radar.h>

#include <QTime>

#define VCI_USBCAN1		3
#define VCI_USBCAN2		4
#define VCI_USBCAN2A		4

#define VCI_USBCAN_E_U 		20
#define VCI_USBCAN_2E_U 	21

#define VCI_ACCCODE_DEFAULT		0x00000000
#define VCI_ACCMASK_DEFAULT		0xFFFFFFFF
#define VCI_TIMER0_DEFAULT		0x00
#define VCI_TIMER1_DEFAULT		0x1C
#define VCI_FILTER_DEFAULT		1
#define VCI_MODE_DEFAULT		0

//函数调用返回状态值
#define	STATUS_OK					1
#define STATUS_ERR					0

#define LOG_ENABLE
#ifndef M_PI
#define M_PI (3.1415926535897932384626433832795)
#endif

iv::control::CanUtil::CanUtil() {
	obs_radar1 = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
	obs_radar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
	obs_radar_ui = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
#ifdef WIN32
    m_hDLL = LoadLibrary(TEXT("ControlCAN.dll"));

        VCI_OpenDevice = (LPVCI_OpenDevice)GetProcAddress(m_hDLL, "VCI_OpenDevice");//取得函数地址
        VCI_CloseDevice = (LPVCI_CloseDevice)GetProcAddress(m_hDLL, "VCI_CloseDevice");
        VCI_InitCAN = (LPVCI_InitCan)GetProcAddress(m_hDLL, "VCI_InitCAN");
        VCI_ReadBoardInfo = (LPVCI_ReadBoardInfo)GetProcAddress(m_hDLL, "VCI_ReadBoardInfo");
        VCI_ReadErrInfo = (LPVCI_ReadErrInfo)GetProcAddress(m_hDLL, "VCI_ReadErrInfo");
        VCI_ReadCanStatus = (LPVCI_ReadCanStatus)GetProcAddress(m_hDLL, "VCI_ReadCANStatus");
        VCI_GetReference = (LPVCI_GetReference)GetProcAddress(m_hDLL, "VCI_GetReference");
        VCI_SetReference = (LPVCI_SetReference)GetProcAddress(m_hDLL, "VCI_SetReference");
        VCI_GetReceiveNum = (LPVCI_GetReceiveNum)GetProcAddress(m_hDLL, "VCI_GetReceiveNum");
        VCI_ClearBuffer = (LPVCI_ClearBuffer)GetProcAddress(m_hDLL, "VCI_ClearBuffer");
        VCI_StartCAN = (LPVCI_StartCAN)GetProcAddress(m_hDLL, "VCI_StartCAN");
        VCI_ResetCAN = (LPVCI_ResetCAN)GetProcAddress(m_hDLL, "VCI_ResetCAN");
        VCI_Transmit = (LPVCI_Transmit)GetProcAddress(m_hDLL, "VCI_Transmit");
        VCI_Receive = (LPVCI_Receive)GetProcAddress(m_hDLL, "VCI_Receive");
        VCI_GetReference2 = (LPVCI_GetReference2)GetProcAddress(m_hDLL, "VCI_GetReference2");
        VCI_SetReference2 = (LPVCI_SetReference2)GetProcAddress(m_hDLL, "VCI_SetReference2");
        VCI_ResumeConfig = (LPVCI_ResumeConfig)GetProcAddress(m_hDLL, "VCI_ResumeConfig");
        VCI_ConnectDevice = (LPVCI_ConnectDevice)GetProcAddress(m_hDLL, "VCI_ConnectDevice");
        VCI_UsbDeviceReset = (LPVCI_UsbDeviceReset)GetProcAddress(m_hDLL, "VCI_UsbDeviceReset");
#endif
}
iv::control::CanUtil::~CanUtil() {

}

void iv::control::CanUtil::openCanCard() {
	VCI_INIT_CONFIG initconfig;
	initconfig.AccCode = VCI_ACCCODE_DEFAULT;
	initconfig.AccMask = VCI_ACCMASK_DEFAULT;
	initconfig.Timing0 = VCI_TIMER0_DEFAULT;
	initconfig.Timing1 = VCI_TIMER1_DEFAULT;
	initconfig.Filter = VCI_FILTER_DEFAULT;
	initconfig.Mode = VCI_MODE_DEFAULT;

	int status = VCI_OpenDevice(m_devtype, m_devind, 0);		//打开设备
    m_connect = -1;
	if (status == 1)
	{
		int init = VCI_InitCAN(m_devtype, m_devind, m_cannum, &initconfig);	//初始化设备
		if (init == 1)
		{
            if (VCI_StartCAN(m_devtype, m_devind, m_cannum) == 1) {
                send_connect = true;
                m_connect = 1;
                thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));
                thread_receive =  boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive, this)));
            }
            else {
                throw 0;
			}
		}
        else
        {
            throw 1;
        }
		//	打开2通道，供毫米波雷达读取数据用
		init = VCI_InitCAN(m_devtype, m_devind, m_cannum_radar, &initconfig);	//初始化设备
		if (init == 1)
		{
			if (VCI_StartCAN(m_devtype, m_devind, m_cannum_radar) == 1) {
                m_connect = 1;
                thread_radar_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_radar, this)));	//启动毫米波雷达接收线程5

            }
			else {
                throw 2;
			}
			
		}
		else
		{
            throw 3;
		}
	}
	else if(status == 0) {
        throw 4;
	}
	else if (status == -1) {
        throw 5;
	}

#ifdef USE_MOBILEYE
    status = VCI_OpenDevice(m_devtype, m_devindmob, 0);		//打开设备
    if (status == 1)
    {
        int init = VCI_InitCAN(m_devtype, m_devindmob, m_cannum, &initconfig);	//初始化设备
        if (init == 1)
        {
            if (VCI_StartCAN(m_devtype, m_devindmob, m_cannum) == 1) {
                m_connect = 1;
                thread_mobileye_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_mobileye, this)));
             }
            else {
                throw 0;
            }
        }
        else
        {
            throw 1;
        }
    }
    else if(status == 0) {
        throw 4;
    }
    else if (status == -1) {
        throw 5;
    }

#endif
}

void iv::control::CanUtil::closeCanCard() {
    if(m_connect != 0)
    {
        thread_receive->interrupt();
        thread_receive->join();
        thread_send->interrupt();
        thread_send->join();
        thread_receive->interrupt();
        thread_receive->join();
        unsigned int rec = VCI_CloseDevice(m_devtype, m_devind);
#ifdef USE_MOBILEYE
        rec = VCI_CloseDevice(m_devtype, m_devindmob);
#endif
         std::cout << "close can:" << rec << std::endl;
    }
}

void iv::control::CanUtil::receive() {
    while (m_connect)
    {
        VCI_CAN_OBJ receivedata[2500];
        int res = VCI_Receive(m_devtype, m_devind, m_cannum, receivedata, 2500, 10);
        if (res > 0)	//未能从can卡中读到数
        {
   //         std::cout<<"reee"<<std::endl;
            int i;
            for(i=0;i<res;i++)
            {
                if(receivedata[i].ID == 0x282)
                {
                    int i;
                    unsigned int xvalue[8];
                    float fvalue[8];
                    unsigned int x;
                    for(i=0;i<8;i++)
                    {
                        memcpy(&x,&receivedata[i].Data[0],4);
                        x = (x<<((7-i)*4))>>28;
                        xvalue[i] = x;
                        fvalue[i] = x;
                        if(x == 0)fvalue[i] = -1;
                        else fvalue[i] = (fvalue[i] - 1.0)*0.2;
                    }
                    ServiceCarStatus.multrasonic_obs.mfront_leftmid = fvalue[0];
                    ServiceCarStatus.multrasonic_obs.mfront_left = fvalue[1];
                    ServiceCarStatus.multrasonic_obs.mfront_right = fvalue[2];
                    ServiceCarStatus.multrasonic_obs.mfront_rightmid = fvalue[3];
                    ServiceCarStatus.multrasonic_obs.mrear_leftmid = fvalue[4];
                    ServiceCarStatus.multrasonic_obs.mrear_left = fvalue[5];
                    ServiceCarStatus.multrasonic_obs.mrear_right = fvalue[6];
                    ServiceCarStatus.multrasonic_obs.mrear_rightmid = fvalue[7];

//                    std::cout<<"rec oooo value is back "<<(int)(receivedata[i].Data[3])<<" head "<<(int)receivedata[i].Data[1]<<std::endl;
                }
            }
        }
        else
        {
            usleep(1000);
        }
    }
}

void iv::control::CanUtil::initial()
{
    ServiceControlStatus.command.byte[0] = 0xEB;

    ServiceControlStatus.command.byte[1] = 0;

    ServiceControlStatus.command.byte[2] = 0;

    ServiceControlStatus.command.byte[3] = 0;

    ServiceControlStatus.command.byte[4] = 0;

    ServiceControlStatus.command.byte[5] = 0;

    ServiceControlStatus.command.byte[6] = 0;

    ServiceControlStatus.command.byte[7] = 0;
}

void iv::control::CanUtil::pack_command()
{
    ServiceControlStatus.command.bit.heartbreak++;

    if (ServiceControlStatus.command.bit.heartbreak >= 256)
        ServiceControlStatus.command.bit.heartbreak = 0;

    ServiceControlStatus.command.byte[7] = (ServiceControlStatus.command.byte[1] + ServiceControlStatus.command.byte[2]
        + ServiceControlStatus.command.byte[3] + ServiceControlStatus.command.byte[4]
        + ServiceControlStatus.command.byte[5] + ServiceControlStatus.command.byte[6]) % 256;
}

void iv::control::CanUtil::send()
{
    VCI_CAN_OBJ Command_data;

        QTime time1;

        int LastTimeSend1;

        time1.start();

        LastTimeSend1 = time1.elapsed();

        int Send1Int = 10;  //100ms

        Command_data.SendType = m_sendtype;
        Command_data.ExternFlag = m_frameflag;
        Command_data.RemoteFlag = m_frameformat;

        Command_data.ID = ServiceControlStatus.command_ID;
        Command_data.DataLen = 8;

        initial();

        while (true)
        {
            int print_time = time1.elapsed() - LastTimeSend1;
            if (print_time >= Send1Int)
            {
                LastTimeSend1 = time1.elapsed();

                pack_command();

                memcpy(Command_data.Data, ServiceControlStatus.command.byte, 8);

                VCI_Transmit(m_devtype, m_devind, m_cannum, &Command_data, 1);

                /*for (int c = 0; c < 8; c++)
                ODS("%x  ", Command_data.Data[c]);

                ODS("\n");*/

                //ODS("    %d  \n", print_time);
            }
        }
}

/******************************************************mobileye AWS display*****************************************************
 *  message ID:0x700
 *  member:                 type            physical unit       range                   note
 *  dusk_time:              double                /              0,1
 *  night_time              double                /              0,1
 *******************************************************************************************************************************/
void iv::control::CanUtil::impl_0x700(VCI_CAN_OBJ receivedata)
{
    ServiceCarStatus.aws_display.dusk_time = (receivedata.Data[0] >> 3) & 0x1;
    ServiceCarStatus.aws_display.night_time = (receivedata.Data[0] >> 4) & 0x1;
    ServiceCarStatus.aws_display.lanes_on = receivedata.Data[4] & 0x1;
    //std::cout << "lanes_on:" << ServiceCarStatus.aws_display.lanes_on << std::endl;
}

/******************************************************mobileye lane***********************************************************
 *  message ID:0x737
 *  member:                 type            physical unit       range                   note
 *  curvature:              double             1/meter      [-0.12, 0.12]       "a" in the equation:y = ax^2+bx+c
 *  heading:                double                /          [-1.0, 1.0]        "b" in the equation:y = ax^2+bx+c
 *  construction area:      bool                  /               /                       /
 *  pitch angle:            double             radians      [-0.05, 0.05]      pitch of the host vehicle(derived from lanes analysis)
 *  yaw angle:              double             radians            /            yaw of the host vehicle(derived from lanes analysis)
 *  right_LDW:              bit/bool              /              0,1           0 stands for unavailable, 1 for available
 *  left_LDW:               bit/bool              /              0,1           0 stands for unavailable, 1 for available
 *******************************************************************************************************************************/
void iv::control::CanUtil::impl_0x737(VCI_CAN_OBJ receivedata)
{
    //lane curvature
    int32_t tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    tmp <<= 16;
    tmp >>= 16;
    ServiceCarStatus.Lane.curvature = tmp * 3.81 * 1e-6;
    //std::cout<<"curvature:"<<ServiceCarStatus.Lane.curvature<<std::endl;

    tmp = 0;

    //lane heading
    tmp = ((receivedata.Data[3] & 0xf) << 8) | receivedata.Data[2];
    tmp <<= 20;
    tmp >>= 20;
    ServiceCarStatus.Lane.heading = tmp * 0.0005;

    tmp = 0;

    //construction area
    ServiceCarStatus.Lane.is_ca = (receivedata.Data[3] >> 4) & 0x1;

    //left_LDW
    ServiceCarStatus.Lane.is_left_LDW_available = (receivedata.Data[3] >> 5) & 0x1;

    //right_LDW
    ServiceCarStatus.Lane.is_right_LDW_available = (receivedata.Data[3] >> 6) & 0x1;

    //yaw
    tmp = (receivedata.Data[5] >> 8) | receivedata.Data[4];
    tmp <<= 16;
    tmp >>= 16;
    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
    ServiceCarStatus.Lane.yaw = tmp * 0.000977;
    std::cout << "yaw:"<< ServiceCarStatus.Lane.yaw<< std::endl;

    tmp = 0;

    //pitch
    tmp = (receivedata.Data[7] >> 8) | receivedata.Data[6];
    tmp <<= 16;
    tmp >>= 16;
    //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0 / 512;
    ServiceCarStatus.Lane.yaw = tmp * 0.000002;
    std::cout << "pitch:" << ServiceCarStatus.Lane.pitch << std::endl;
}

/****************************************************  mobileye obstacle status  ****************************************************************************
 *  message ID:0x738
 *  member:                  type           physical unit       range               note
 *  num_obstacles            int                   /           [0:255]                /
 *  timestamp                int                   ms          [0:255]          only the lowest 8 bits of the timestamp is given
 *  app_version:             int                   /           [0:255]          vesion info consists of X.Y.Z.W
 *  active_version           int/2bits             /            [0:3]           index of the active section of app_version signal
 *  left_close_rang_cut_in   bool                  /             0,1            0 false, 1 true
 *  right_close_rang_cut_in  bool                  /             0,1            0 false, 1 true
 *  go                       enum                  /           0,1...15         0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
 *  protocol version         uchar/8bit            /          0x00...0xff             /
 *  is_close_car             bool                  /             0,1            0 no close, 1 close car exists
 *  failsafe                 int/4bit              /            [0:7]           failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x738(VCI_CAN_OBJ receivedata)
{
    //num_obstacles
    ServiceCarStatus.obstacleStatus.num_obstacles = receivedata.Data[0];
    //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;


    //timestamp
    ServiceCarStatus.obstacleStatus.timestamp = receivedata.Data[1];

    //app version
    ServiceCarStatus.obstacleStatus.app_version = receivedata.Data[2];

    //active version
    ServiceCarStatus.obstacleStatus.active_version = receivedata.Data[3] & 0x3;

    //is left close rang cut in
    ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (receivedata.Data[3] >> 2) & 0x1;

    //is right close rang cut in
    ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (receivedata.Data[3] >> 3) & 0x1;

    //go
    ServiceCarStatus.obstacleStatus.go = receivedata.Data[3] >> 4;

    //protocol version
    ServiceCarStatus.obstacleStatus.protocol_version = receivedata.Data[4];

    //close car
    ServiceCarStatus.obstacleStatus.is_close_car = receivedata.Data[5] & 0x1;

    //failsafe
    ServiceCarStatus.obstacleStatus.failsafe = (receivedata.Data[5] >> 1) & 0xf;
}

/****************************************************  mobileye obstacle data a  ****************************************************************************
 *  message ID:0x739
 *  member:                  type           physical unit       range               note
 *  obstacle_ID              int                   /           [0:63]           new obstacles are given the last used free ID
 *  obstacle_pos_x           double                m           [0,250]          longtitude position of the obstacle relative to the ref point
 *  obstacle_pos_y           double                m       [-31.93,31.93]       lateral position of the obstacle, error relative to pos_x  below 10% or 2meters
 *  blinker_info             int/enum              /            [0:4]           0 unavailable, 1 off, 2 left, 3 right, 4 both   indicated blinkers status
 *  cut_in_and_out           int                   /            [0:4]           0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
 *                                                                              cut_in: target entering the host lane, cut_out: exiting  but not distinguish between sides
 *  obstacle_rel_vel_x       double               m/s      [-127.93,127.93]     relative longtitude velocity of the obstacle
 *  obstacle_status          int/enum              /            [0:6]           0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
 *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
 *  is_obstacle_brake_lights bool                  /             0,1            0 object's brake light off or not identified, 1 object's brake light on
 *  obstacle_valid           int/enum              /            [1:2]           1 new valid(detected this frame), 2 older valid
 *************************************************************************************************************************************************************/
obstacle_data_a iv::control::CanUtil::impl_0x739(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;
    obstacle_data_a obs_tmp;

    //obstacle id
    obs_tmp.obstacle_ID = receivedata.Data[0];
    //ServiceCarStatus.obstacleStatusA.obstacle_ID = receivedata.Data[0];

    //obstacle pos x
    tmp = ((receivedata.Data[2] & 0xf) << 8) | receivedata.Data[1];
    obs_tmp.obstacle_pos_x = tmp * 0.0625;
    //ServiceCarStatus.obstacleStatusA.obstacle_pos_x = tmp * 0.0625;

    tmp = 0;

    //obstacle pos y
    tmp = ((receivedata.Data[4] & 0x3) << 8) | receivedata.Data[3];
    obs_tmp.obstacle_pos_y = tmp * 0.0625;
    //ServiceCarStatus.obstacleStatusA.obstacle_pos_y = tmp * 0.0625;

    tmp = 0;
    //blinker info
    obs_tmp.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
    //ServiceCarStatus.obstacleStatusA.blinker_info = (receivedata.Data[4] >> 2) & 0x7;

    //cut in and out
    obs_tmp.cut_in_and_out = receivedata.Data[4] >> 5;
    //ServiceCarStatus.obstacleStatusA.cut_in_and_out = receivedata.Data[4] >> 5;

    //obstacle rel vel x
    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
    obs_tmp.obstacle_rel_vel_x = tmp * 0.0625;
    //ServiceCarStatus.obstacleStatusA.obstacle_rel_vel_x = tmp * 0.0625;

    tmp = 0;

    //obstacle type
    obs_tmp.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
    //ServiceCarStatus.obstacleStatusA.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;

    //obstacle status
    obs_tmp.obstacle_status = receivedata.Data[7] & 0x7;
    //ServiceCarStatus.obstacleStatusA.obstacle_status = receivedata.Data[7] & 0x7;

    //obstacle brake lights
    obs_tmp.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
    //ServiceCarStatus.obstacleStatusA.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;

    //obstacle valid
    obs_tmp.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
    //ServiceCarStatus.obstacleStatusA.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
    return obs_tmp;
}

/****************************************************  mobileye obstacle data b  ****************************************************************************
 *  message ID:0x73a
 *  member:                  type           physical unit       range               note
 *  obstacle_length          double                m           [0,31]           length of the obstacle(longitude axis)
 *  obstacle_width           double                m           [0,12.5]         width of  the obstacle(lateral axis)
 *  obstacle_age             int                   /           [0:255]          value starts at 1 when it is first detected, increaments in i each frame
 *  obstacle_lane            int/enum              /           [0:3]            0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
 *                                                                              3 invalid signal
 *  is_cipv_flag             int/enum              /            [0:1]           0 not cipv, 1 cipv(closest in path vehicle)
 *  radar_pos_x              double                m           [0,250]          longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
 *                                                                              if no radar target is matched, value = 0xfff
 *  radar_vel_x              double               m/s       [-127.93,127.93]    longitude velocity of the radar target matched to the vision targets, if no radar target is matched value=0xfff
 *                                                                              4 oncoming,  5 parked(never moved, back lights off), 6 unused
 *  radar_match_confidence   int                   /            [0:5]           0 no match, 1 multi match: radar doesn't describe well, 2-4 vision-radar match:with bounded error between
 *                                                                              vision and radar measurements higher->smaller error, 5 high confidence match
 *  matched_radar_id         int                   /           [0:127]          ID of Primary radar target matched to the vision target if applicable
 *************************************************************************************************************************************************************/
obstacle_data_b iv::control::CanUtil::impl_0x73a(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;
    obstacle_data_b obs_tmp;

    //obstacle length
    tmp = receivedata.Data[0];
    obs_tmp.obstacle_length = tmp * 0.5;
    //ServiceCarStatus.obstacleStatusB.obstacle_length = tmp * 0.5;

    tmp = 0;

    //obstacle width
    tmp = receivedata.Data[1];
    obs_tmp.obstacle_width = tmp * 0.05;
    //ServiceCarStatus.obstacleStatusB.obstacle_width = tmp * 0.05;

    tmp = 0;

    //obstacle age
    obs_tmp.obstacle_age = receivedata.Data[2];
    //ServiceCarStatus.obstacleStatusB.obstacle_age = receivedata.Data[2];

    //obstacle lane
    obs_tmp.obstacle_lane = receivedata.Data[3] & 0x3;
    //ServiceCarStatus.obstacleStatusB.obstacle_lane = receivedata.Data[3] & 0x3;

    //is cipv flag
    obs_tmp.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
    //ServiceCarStatus.obstacleStatusB.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;

    //radar pos x
    tmp = (receivedata.Data[4] << 4) | (receivedata.Data[3] >> 4);
    obs_tmp.radar_pos_x = tmp * 0.0625;
    //ServiceCarStatus.obstacleStatusB.radar_pos_x = tmp * 0.0625;

    tmp = 0;

    //radar vel x
    tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
    tmp <<= 20;
    tmp >>= 20;
    obs_tmp.radar_vel_x = tmp * 0.0625;
    //ServiceCarStatus.obstacleStatusB.radar_vel_x = tmp * 0.0625;

    tmp = 0;

    //radar match confidence
    obs_tmp.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
    //ServiceCarStatus.obstacleStatusB.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;

    //matched radar ID
    obs_tmp.matched_radar_id = receivedata.Data[7] & 0x7f;
    //ServiceCarStatus.obstacleStatusB.matched_radar_id = receivedata.Data[7] & 0x7f;

    return obs_tmp;
}

/****************************************************  mobileye obstacle data c  ****************************************************************************
 *  message ID:0x73b
 *  member:                  type           physical unit       range               note
 *  obstacle_angle_rate      double           degree/s     [-327.68, 327.68]    Angle rate of Center of Obstacle, negative->moved to left
 *  obstcale_scale_change    double            pix/s       [-6.5532, 6.5532]    /
 *  object_accel_x           double            m/s2         [-14.97, 14.97]     longtitude acceleration of the object
 *  obstacle_replaced        bool                /               0,1            0 not replaced in this frame, 1 replace in this frame
 *  obstacle_angle           double           degree        [-327.68,327.68]    Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
 *                                                                              exactly in front of us, a positive angle indicates that theobstacle is to the right
 *************************************************************************************************************************************************************/
obstacle_data_c iv::control::CanUtil::impl_0x73b(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;
    obstacle_data_c obs_tmp;

    //obstacle angle rate
    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    tmp <<= 16;
    tmp >>= 16;
    obs_tmp.obstacle_angle_rate = tmp * 0.01;
    //ServiceCarStatus.obstacleStatusC.obstacle_angle_rate = tmp * 0.01;

    tmp = 0;

    //obstacle scale change
    tmp = (receivedata.Data[3] << 8) | receivedata.Data[2];
    tmp <<= 16;
    tmp >>= 16;
    obs_tmp.obstacle_scale_change = tmp * 0.0002;
    //ServiceCarStatus.obstacleStatusC.obstacle_scale_change = tmp * 0.0002;

    tmp = 0;

    //object accel x
    tmp = ((receivedata.Data[5] & 0x3) << 8) | receivedata.Data[4];
    tmp <<= 22;
    tmp >>= 22;
    obs_tmp.object_accel_x = tmp * 0.03;
    //ServiceCarStatus.obstacleStatusC.object_accel_x = tmp * 0.03;

    tmp = 0;

    //obstacle replaced
    obs_tmp.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
    //ServiceCarStatus.obstacleStatusC.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;

    //obstacle angle
    tmp = (receivedata.Data[7] << 8) | receivedata.Data[6];
    tmp <<= 16;
    tmp >>= 16;
    obs_tmp.obstacle_angle = tmp * 0.01;
    //ServiceCarStatus.obstacleStatusC.obstacle_angle = tmp * 0.01;

    return obs_tmp;
}

/****************************************************  mobileye aftermarket lane  ****************************************************************************
 *  message ID:0x669
 *  member:                  type           physical unit       range               note
 *  lane_conf_left           int/2bit              /            [0:3]            0 lowest, 3 highest
 *  is_ldw_availablity_left  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
 *  lane_type_left           int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
 *  dist_to_lane_l           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
 *  lane_conf_right          int/2bit              /            [0:3]            0 lowest, 3 highest
 *  is_ldw_availablity_right  bool                  /             0,1             lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
 *  lane_type_right          int/enum              /            [0:6]            0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
 *  dist_to_lane_r           double                m           [-40:40]          "c" in the equation:y = ax^2+bx+c
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x669(VCI_CAN_OBJ receivedata)
{
    int32_t tmp;
    //lane confidence left
    tmp = receivedata.Data[0] & 0x3;
    tmp <<= 30;
    tmp >>= 30;
    ServiceCarStatus.aftermarketLane.lane_conf_left = tmp;
    //std::cout << "left confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_left << std::endl;

    tmp = 0;

    //is LDW availability left
    ServiceCarStatus.aftermarketLane.is_ldw_availablility_left = (receivedata.Data[0] >> 2) & 0x1;

    //lane type left
    tmp = (receivedata.Data[0] >> 4) & 0xf;
    tmp <<= 28;
    tmp >>= 28;
    ServiceCarStatus.aftermarketLane.lane_type_left = tmp;
    std::cout << "lane_type_left: " << ServiceCarStatus.aftermarketLane.lane_type_left << std::endl;

    tmp = 0;

    //dist to left lane

    tmp = (receivedata.Data[2] << 4) | ((receivedata.Data[1] >> 4) & 0xf);
    tmp <<= 20;
    tmp >>= 20;
    ServiceCarStatus.aftermarketLane.dist_to_lane_l = tmp * 0.02;
    std::cout << "dist_to_lane_l:" << ServiceCarStatus.aftermarketLane.dist_to_lane_l << std::endl;

    tmp = 0;

    //lane confidence right
    tmp = receivedata.Data[5] & 0x3;
    tmp <<= 30;
    tmp >>= 30;
    ServiceCarStatus.aftermarketLane.lane_conf_right = tmp;
    //std::cout << "right confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_right << std::endl;

    tmp = 0;

    //is LDW availability right
    ServiceCarStatus.aftermarketLane.is_ldw_availablility_right = (receivedata.Data[5] >> 2) & 0x1;

    //lane type right
    tmp = (receivedata.Data[5] >> 4) & 0xf;
    tmp <<= 28;
    tmp >>= 28;
    ServiceCarStatus.aftermarketLane.lane_type_right = tmp;
    std::cout<<"lane_type_right: " << ServiceCarStatus.aftermarketLane.lane_type_right << std::endl;

    tmp = 0;

    //dist to right lane
    tmp = (receivedata.Data[7] << 4) | ((receivedata.Data[6] >> 4) & 0xf);
    tmp <<= 20;
    tmp >>= 20;
    ServiceCarStatus.aftermarketLane.dist_to_lane_r = tmp * 0.02;
    std::cout << "dist_to_lane_r:" << ServiceCarStatus.aftermarketLane.dist_to_lane_r << std::endl;

    tmp = 0;
}

/****************************************************  mobileye LKA left lane a ****************************************************************************
 *  message ID:0x766
 *  member:                  type           physical unit       range               note
 *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
 *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
 *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
 *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
 *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
 *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
 *  width_left_marking       double                m           [0, 2.5]          left lane marking width
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x766(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    //lane type
    ServiceCarStatus.LKAleftLaneA.lane_type = receivedata.Data[0] & 0xf;

    //quality
    ServiceCarStatus.LKAleftLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;

    //model degree
    ServiceCarStatus.LKAleftLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;

    //position
    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
    tmp <<= 16;
    tmp >>= 16;
    ServiceCarStatus.LKAleftLaneA.position = tmp / 256.0;
    //ServiceCarStatus.LKAleftLaneA.position = tmp * 0.003906;

    tmp = 0;

    //curvature
    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
    ServiceCarStatus.LKAleftLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
    //ServiceCarStatus.LKAleftLaneA.curvature = tmp * 0.000001 + -0.031999;

    tmp = 0;

    //curvature derivative
    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
    ServiceCarStatus.LKAleftLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
    //ServiceCarStatus.LKAleftLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;

    tmp = 0;

    //width left marking
    tmp = receivedata.Data[7];
    ServiceCarStatus.LKAleftLaneA.width_left_marking = tmp * 0.01;

}

/****************************************************  mobileye LKA left lane b ****************************************************************************
 *  message ID:0x767
 *  member:                  type           physical unit       range               note
 *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
 *  view_range               double                m         [0, 127.996]        physical view range of lane mark
 *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x767(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    //heading angle
    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    ServiceCarStatus.LKAleftLaneB.heading_angle = (tmp - 0x7fff) / 1024;
    //ServiceCarStatus.LKAleftLaneB.heading_angle = tmp * 0.000977 + -31.999023;

    tmp = 0;

    //view range
    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
    ServiceCarStatus.LKAleftLaneB.view_range = tmp / 256.0;
    //ServiceCarStatus.LKAleftLaneB.view_range = tmp * 0.003906;

    tmp = 0;

    //view range availability
    ServiceCarStatus.LKAleftLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;

}

/****************************************************  mobileye LKA right lane a ****************************************************************************
 *  message ID:0x768
 *  member:                  type           physical unit       range               note
 *  lane_type                enum                  /            [0:6]            0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
 *  quality                  enum                  /            [0:3]            0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
 *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
 *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
 *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
 *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
 *  width_right_marking      double                m           [0, 2.5]          right lane marking width
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x768(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    //lane type
    ServiceCarStatus.LKArightLaneA.lane_type = receivedata.Data[0] & 0xf;

    //quality
    ServiceCarStatus.LKArightLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;

    //model degree
    ServiceCarStatus.LKArightLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;

    //position
    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
    tmp <<= 16;
    tmp >>= 16;
    ServiceCarStatus.LKArightLaneA.position = tmp / 256.0;
    //ServiceCarStatus.LKArightLaneA.position = tmp * 0.003906;

    tmp = 0;

    //curvature
    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
    ServiceCarStatus.LKArightLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
    //ServiceCarStatus.LKArightLaneA.curvature = tmp * 0.000001 + -0.031999;

    tmp = 0;

    //curvature derivative
    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
    ServiceCarStatus.LKArightLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
    //ServiceCarStatus.LKArightLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;

    tmp = 0;

    //width left marking
    tmp = receivedata.Data[7];
    ServiceCarStatus.LKArightLaneA.width_left_marking = tmp * 0.01;
}

/****************************************************  mobileye LKA right lane b ****************************************************************************
 *  message ID:0x767
 *  member:                  type           physical unit       range               note
 *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
 *  view_range               double                m         [0, 127.996]        physical view range of lane mark
 *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x769(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    //heading angle
    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    ServiceCarStatus.LKArightLaneB.heading_angle = (tmp - 0x7fff) / 1024;
    //ServiceCarStatus.LKArightLaneB.heading_angle = tmp * 0.000977 + -31.999023;

    tmp = 0;

    //view range
    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
    ServiceCarStatus.LKArightLaneB.view_range = tmp / 256.0;
    //ServiceCarStatus.LKArightLaneB.view_range = tmp * 0.003906;

    tmp = 0;

    //view range availability
    ServiceCarStatus.LKArightLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;

}

/****************************************************  mobileye next lane a **********************************************************************************
 *  message ID:0x76c
 *  member:                  type           physical unit       range               note
 *  lane_type                enum                  /            [1:2]            1 solid, 2 undecided
 *  quality                    /                   /              /              not yet implemented for next lanes
 *  model_degree             enum                  /            [1:3]            1 linear model, 2 parabolic model, 3 3rd-degree model
 *  position                 double                m          [-127,128]         physical distance between lane mark and camera on the lateral position
 *  curvature                double                /         [-0.02, 0.02]       given a very low curvature derivative, positive curvature indicates a right hand side curve
 *  curvature_derivative     double                /      [-0.00012, 0.00012]    /
 *  lane_mark_widt           double                m           [0, 2.5]          lane marking width
 *************************************************************************************************************************************************************/
next_lane_a iv::control::CanUtil::impl_0x76c(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;
    next_lane_a nextLane_tmp;

    //lane type
    nextLane_tmp.lane_type = receivedata.Data[0] & 0xf;
    //ServiceCarStatus.nextLaneA.lane_type = receivedata.Data[0] & 0xf;

    //quality
    nextLane_tmp.quality = (receivedata.Data[0] >> 4) & 0x3;
    //ServiceCarStatus.nextLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;

    //model degree
    nextLane_tmp.model_degree = (receivedata.Data[0] >> 6) & 0x3;
    //ServiceCarStatus.nextLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;

    //position
    tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
    tmp <<= 16;
    tmp >>= 16;
    nextLane_tmp.position = tmp * 0.003906;
    //ServiceCarStatus.nextLaneA.position = tmp / 256.0;
    //ServiceCarStatus.nextLaneA.position = tmp * 0.003906;

    tmp = 0;

    //curvature
    tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
    nextLane_tmp.curvature = tmp * 0.000001 + -0.031999;
    //ServiceCarStatus.nextLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
    //ServiceCarStatus.nextLaneA.curvature = tmp * 0.000001 + -0.031999;

    tmp = 0;

    //curvature derivative
    tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
    nextLane_tmp.curvature_derivative = tmp * 0.000000 + -0.000122;
    //ServiceCarStatus.nextLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
    //ServiceCarStatus.nextLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;

    tmp = 0;

    //width left marking
    tmp = receivedata.Data[7];
    nextLane_tmp.lane_mark_width = tmp * 0.01;
    //ServiceCarStatus.nextLaneA.lane_mark_width = tmp * 0.01;

    return nextLane_tmp;
}

/****************************************************  mobileye LKA right lane b ****************************************************************************
 *  message ID:0x76d
 *  member:                  type           physical unit       range               note
 *  heading_angle            double            radians     [-0.357, 0.357]       physical slope of the lane mark   positive->steering towards the right
 *  view_range               double                m         [0, 127.996]        physical view range of lane mark
 *  view_range_availability  enum                  /            0,1              0 not valid, 1 valid
 *************************************************************************************************************************************************************/
next_lane_b iv::control::CanUtil::impl_0x76d(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    next_lane_b nextLane_tmp;

    //heading angle
    tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    nextLane_tmp.heading_angle = tmp * 0.000977 + -31.999023;
    //ServiceCarStatus.nextLaneB.heading_angle = (tmp - 0x7fff) / 1024;
    //ServiceCarStatus.nextLaneB.heading_angle = tmp * 0.000977 + -31.999023;

    tmp = 0;

    //view range
    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
    nextLane_tmp.view_range = tmp * 0.003906;
    //ServiceCarStatus.nextLaneB.view_range = tmp / 256.0;
    //ServiceCarStatus.nextLaneB.view_range = tmp * 0.003906;

    tmp = 0;

    //view range availability
    nextLane_tmp.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
    //ServiceCarStatus.nextLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;

    return nextLane_tmp;
}

/****************************************************  mobileye reference points ******************************************************************************
 *  message ID:0x76a
 *  member:                  type           physical unit       range               note
 *  ref_point_1_position     double               m      [-127.996, 127.996]     physical distance between camera and reference point 1 on lateral axis.
 *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
 *                                                                               ref Point 1 is this measurement at 1 second headway.
 *  ref_point_1_dist         double               m       [0, 127.99609376]      physical distance between reference point and camera
 *  ref_point_1_validity     enum                 /              0,1             0 invalid, 1 valid
 *  ref_point_2_position     double               m      [-127.996, 127.996]     empty! physical distance between camera and reference point 2 on lateral axis.
 *                                                                               The reference point defines the lateral location of the lane center at ref-Point distance.
 *                                                                               ref Point 2 is this measurement at 1 second headway.
 *  ref_point_2_dist         double               m       [0, 127.99609376]      empty! physical distance between reference point and camera
 *  ref_point_2_validity     enum                 /              0,1             empty! 0 invalid, 1 valid
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x76a(VCI_CAN_OBJ receivedata)
{
    int32_t tmp = 0;

    //ref_point_1_position
    //tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
    ServiceCarStatus.refPoints.ref_point_1_position = (tmp - 0x7fff) / 256.0;
    ServiceCarStatus.refPoints.ref_point_1_position = tmp * 0.003906 + -127.996094;

    tmp = 0;

    //ref_point_1_dist
    tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
    //ServiceCarStatus.refPoints.ref_point_1_dist = tmp / 256.0;
    ServiceCarStatus.refPoints.ref_point_1_dist = tmp * 0.003906;

    tmp = 0;

    //is ref point 1 validity
    ServiceCarStatus.refPoints.is_ref_point_1_validity = (receivedata.Data[3] >> 7) & 0x1;

    //ref_point_2_position
    tmp = (receivedata.Data[5] << 8) | receivedata.Data[4];
    //ServiceCarStatus.refPoints.ref_point_2_position = (tmp - 0x7fff) / 256.0;
    ServiceCarStatus.refPoints.ref_point_2_position = tmp * 0.003906 + -127.996094;

    tmp = 0;

    //ref_point_2_dist
    tmp = ((receivedata.Data[7] & 0x7f) << 8) | receivedata.Data[6];
    //ServiceCarStatus.refPoints.ref_point_2_dist = tmp / 256.0;
    ServiceCarStatus.refPoints.ref_point_2_dist = tmp * 0.003906;

    tmp = 0;

    //is ref point 2 validity
    ServiceCarStatus.refPoints.is_ref_point_2_validity = (receivedata.Data[7] >> 7) & 0x1;
}

/**********************************************  mobileye number of next lane markers reported **************************************************************
 *  message ID:0x76b
 *  member:                             type           physical unit       range               note
 *  num_of_next_lane_mark_reported      enum                 /              1,2    indicates how many extra lane markers are also reported, on top of left
 *                                                                                 and right lane. Currently two lane marks are always reported one for left and one for
 *                                                                                 right, and they are valid only if the lane type is Solid.
 *************************************************************************************************************************************************************/
void iv::control::CanUtil::impl_0x76b(VCI_CAN_OBJ receivedata)
{
    //num_of_next_lane_mark_reported
    ServiceCarStatus.numOfNextLaneMarkReported.num_of_next_lane_mark_reported = receivedata.Data[0];
}

void iv::control::CanUtil::receive_radar() {
    int32_t range, rate, angle;
	while (m_connect)
	{
		VCI_CAN_OBJ receivedata[2500];
		int res = VCI_Receive(m_devtype, m_devind, m_cannum_radar, receivedata, 2500, 10);
		int radar_ID_index = -1;
		if (res <= 0)	//未能从can卡中读到数
		{
#ifdef linux
            usleep(1000);
#endif
#ifdef WIN32
            Sleep(1);
#endif
            continue;
		}
        for (int i = 0; i < res; i++) {
			//对应毫米波雷达数据解析
			radar_ID_index = receivedata[i].ID - 0x500;
			if (radar_ID_index >= 0x00 && radar_ID_index <= 0x3f) {
                ServiceCarStatus.mRadarS = 10;
				angle = ((receivedata[i].Data[1] & 0x1F) << 5) + ((receivedata[i].Data[2] & 0xF8) / 8);
				range = ((receivedata[i].Data[2] & 0x07) << 8) + receivedata[i].Data[3];
				rate = ((receivedata[i].Data[6] & 0x3F) << 8) | receivedata[i].Data[7];
				if (angle & 0x200) {
					angle = angle | 0xFFFFFC00;
				}
				if (rate & 0x2000) {
					rate = rate | 0xFFFFC000;
				}
				//If angle and range both are 0, it is an invalid data.
				if (angle != 0 || range != 0) {
                    obs_find_flag = true;
                    ServiceCarStatus.obs_radar[radar_ID_index].valid = true;
                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_x = range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI);
                    ServiceCarStatus.obs_radar[radar_ID_index].nomal_y = range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI);
                    ServiceCarStatus.obs_radar[radar_ID_index].speed_relative = rate * 1.0 / 100.0;
                    ServiceCarStatus.obs_radar[radar_ID_index].speed_x = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * sin(angle / 1800.0 * M_PI);
                    ServiceCarStatus.obs_radar[radar_ID_index].speed_y = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * cos(angle / 1800.0 * M_PI);
				}
				else {
					ServiceCarStatus.obs_radar[radar_ID_index].valid = false;
				}
			}
        }
    }
}

void iv::control::CanUtil::obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC)
{
    int vec_size = obstacleStatusA.size();
    if(vec_size == 0)
    {
        return;
    }
    std::cout<< "*****************************************************************************************"<<std::endl;
    for(int i = 0; i < vec_size; i++)
    {

        std::cout<< "obstacle ID: " << obstacleStatusA[i].obstacle_ID << std::endl;
        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_x << std::endl;
        std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_y << std::endl;
        std::cout<< "obstacle_rel_vel_x: " << obstacleStatusA[i].obstacle_rel_vel_x << std::endl;
        std::cout<< "obstacle_type: " << obstacleStatusA[i].obstacle_type << std::endl;
        std::cout<< "obstacle_status: " << obstacleStatusA[i].obstacle_status << std::endl;
        std::cout<< "obstacle_brake_light: " << obstacleStatusA[i].is_obstacle_brake_lights << std::endl;
        std::cout<< "obstacle_valid: " << obstacleStatusA[i].obstacle_valid << std::endl;
//        //std::cout<< "obstacle_length: " << obstacleStatusB[i].obstacle_length << std::endl;
//        std::cout<< "obstacle_width: " << obstacleStatusB[i].obstacle_width << std::endl;
//        std::cout<< "obstacle_age: " << obstacleStatusB[i].obstacle_age << std::endl;
//        std::cout<< "obstacle_cipv: " << obstacleStatusB[i].is_cipv_flag << std::endl;
//        std::cout<< "obstacle_radar_pos_x: " << obstacleStatusB[i].radar_pos_x << std::endl;
//        std::cout<< "obstacle_radar_vel_x: " << obstacleStatusB[i].radar_vel_x << std::endl;
//        std::cout<< "obstacle_angle_rate: " << obstacleStatusC[i].obstacle_angle_rate << std::endl;
//        std::cout<< "obstacle_angle" << obstacleStatusC[i].obstacle_angle << std::endl;
//        std::cout<< "object_accel_x" << obstacleStatusC[i].object_accel_x << std::endl;
//        std::cout<< "obstacle_scale_change: " << obstacleStatusC[i].obstacle_scale_change << std::endl;
//        std::cout<< "is_obstacle_replaced: " << obstacleStatusC[i].is_obstacle_replaced<< std::endl;
    }
    std::cout<< "*****************************************************************************************"<<std::endl;
}

#ifdef USE_MOBILEYE
void iv::control::CanUtil::receive_mobileye(){
    while(m_connect)
    {
        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_a) * 15);
        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_b) * 15);
        memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_c) * 15);
        int obs_a_count = 0, obs_b_count = 0, obs_c_count = 0;
        int left_lane_a_count = 0, right_lane_a_count = 0, left_lane_b_count = 0, right_lane_b_count = 0;
        obstacle_data_a a_tmp;
        obstacle_data_b b_tmp;
        obstacle_data_c c_tmp;
        next_lane_a left_tmp_a, right_tmp_a;
        next_lane_b left_tmp_b, right_tmp_b;
        VCI_CAN_OBJ receivedata[2500];
        int res = VCI_Receive(m_devtype, m_devindmob, m_cannum, receivedata, 2500, 10);
        if (res <= 0)
        {
#ifdef linux
            usleep(1000);
#endif
#ifdef WIN32
            Sleep(1);
#endif
            continue;
        }
        for (int i = 0; i < res; i++)
        {
            switch(receivedata[i].ID)
            {
                case 0x700:
                    impl_0x700(receivedata[i]);
                    break;
                case 0x737:
                    impl_0x737(receivedata[i]);
                    break;
                case 0x738:
                    impl_0x738(receivedata[i]);
                    break;
                case 0x739: case 0x73c: case 0x73f: case 0x742: case 0x745: case 0x748: case 0x74b: case 0x74e: case 0x751: case 0x754: case 0x757: case 0x75a: case 0x75d: case 0x760: case 0x763:
                    a_tmp = impl_0x739(receivedata[i]);
                    memcpy(&(ServiceCarStatus.obstacleStatusA[obs_a_count]), &a_tmp, sizeof(obstacle_data_a));
                    obs_a_count++;
                    break;
                case 0x73a: case 0x73d: case 0x740: case 0x743: case 0x746: case 0x749: case 0x74c: case 0x74f: case 0x752: case 0x755: case 0x758: case 0x75b: case 0x75e: case 0x761: case 0x764:
                    b_tmp = impl_0x73a(receivedata[i]);
                    memcpy(&(ServiceCarStatus.obstacleStatusB[obs_b_count]), &b_tmp, sizeof(obstacle_data_b));
                    obs_b_count++;
                    break;
                case 0x73b: case 0x73e: case 0x741: case 0x744: case 0x747: case 0x74a: case 0x74d: case 0x750: case 0x753: case 0x756: case 0x759: case 0x75c: case 0x75f: case 0x762: case 0x765:
                    c_tmp = impl_0x73b(receivedata[i]);
                    memcpy(&(ServiceCarStatus.obstacleStatusC[obs_a_count]), &c_tmp, sizeof(obstacle_data_c));
                    obs_c_count++;
                    break;
                case 0x669:
                    impl_0x669(receivedata[i]);
                    break;
                case 0x766:
                    impl_0x766(receivedata[i]);
                    break;
                case 0x767:
                    impl_0x767(receivedata[i]);
                    break;
                case 0x768:
                    impl_0x768(receivedata[i]);
                    break;
                case 0x769:
                    impl_0x769(receivedata[i]);
                    break;
                case 0x76c: case 0x76e: case 0x771: case 0x773: case 0x775: case 0x777: case 0x779: case 0x77b:
                    if (receivedata[i].ID == 0x76c || receivedata[i].ID == 0x771 || receivedata[i].ID == 0x775 || receivedata[i].ID == 0x779)
                    {
                        left_tmp_a = impl_0x76c(receivedata[i]);
                        memcpy(&(ServiceCarStatus.nextLaneA_left[left_lane_a_count]), &left_tmp_a, sizeof(next_lane_a));
                        left_lane_a_count++;
                    }
                    else
                    {
                        right_tmp_a = impl_0x76c(receivedata[i]);
                        memcpy(&(ServiceCarStatus.nextLaneA_right[right_lane_a_count]), &right_tmp_a, sizeof(next_lane_a));
                        right_lane_a_count++;
                    }
                    //impl_0x76c(receivedata[i]);
                    break;
                case 0x76d: case 0x76f: case 0x772: case 0x774: case 0x776: case 0x778: case 0x77a: case 0x77c:
                    if (receivedata[i].ID == 0x76d || receivedata[i].ID == 0x772 || receivedata[i].ID == 0x776 || receivedata[i].ID == 0x77a)
                    {
                        left_tmp_b = impl_0x76d(receivedata[i]);
                        memcpy(&(ServiceCarStatus.nextLaneB_left[left_lane_b_count]), &left_tmp_b, sizeof(next_lane_b));
                        left_lane_b_count++;
                    }
                    else
                    {
                        right_tmp_b = impl_0x76d(receivedata[i]);
                        memcpy(&(ServiceCarStatus.nextLaneB_right[right_lane_b_count]), &right_tmp_b, sizeof(next_lane_b));
                        right_lane_b_count++;
                    }
                    break;
                case 0x76a:
                    impl_0x76a(receivedata[i]);
                    break;
                case 0x76b:
                    impl_0x76b(receivedata[i]);
                    break;
                default:
                    break;
            }
        }
       // obs_print(ServiceCarStatus.obstacleStatusA, ServiceCarStatus.obstacleStatusB, ServiceCarStatus.obstacleStatusC);
    }
}
#endif

//void iv::control::CanUtil::send() {
	
//    VCI_CAN_OBJ senddata;

//	QTime time;
//    int nTimeLastSend1;
//	time.start();
//    nTimeLastSend1 = time.elapsed();
//    int nSend1Int = 10;  //10ms

//	senddata.SendType = m_sendtype;
//	senddata.ExternFlag = m_frameflag;
//    senddata.RemoteFlag = m_frameformat;
//    ServiceControlStatus.control_cmd_.Head = 0xEB;

//	while (true) {
//        usleep(1000);
//		if ((time.elapsed() - nTimeLastSend1) >= nSend1Int)
//        {
//			nTimeLastSend1 = time.elapsed();
//			senddata.ID = ServiceControlStatus.id;
//            senddata.DataLen = 8;
//            ServiceControlStatus.control_cmd_.CommunicationCount++;
//            ServiceControlStatus.control_cmd_.CheckSum = (ServiceControlStatus.control_cmd_.Acceleration_frac + ServiceControlStatus.control_cmd_.Acceleration_int + ServiceControlStatus.control_cmd_.CommunicationCount + ServiceControlStatus.control_cmd_.Control + ServiceControlStatus.control_cmd_.Wheel_Angel[0] + ServiceControlStatus.control_cmd_.Wheel_Angel[1])%256;
//            memcpy(senddata.Data, &ServiceControlStatus.control_cmd_.Head, 8);
//            VCI_Transmit(m_devtype, m_devind, m_cannum, &senddata, 1);
//            ServiceControlStatus.control_cmd_.Control &= 0xCB;
//        }
//	}
//}


void iv::control::CanUtil::startsend(bool start_or_stop)
{
    if(start_or_stop == true && send_connect == true && is_repeat == false)
    {
        is_repeat = true;
        thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));			//启动发送线程
    }
    else if(start_or_stop == false && send_connect == true && is_repeat == true)
    {
        thread_send->interrupt();
        thread_send->join();
        is_repeat = false;
    }
}

bool iv::control::CanUtil::did_radar_ok()
{
    bool temp = obs_find_flag;
    obs_find_flag = false;
    return temp;
}