#pragma once
#ifdef WIN32
#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN
#endif
#include <WS2tcpip.h>
#include <WinSock2.h>
#define socketerrno WSAGetLastError()
#define SOCKET_EAGAIN_EINPROGRESS WSAEINPROGRESS
#define SOCKET_EWOULDBLOCK WSAEWOULDBLOCK
#ifndef _SSIZE_T_DEFINED
typedef int ssize_t;
#define _SSIZE_T_DEFINED
#endif
#ifndef _SOCKET_T_DEFINED
typedef SOCKET socket_t;
#define _SOCKET_T_DEFINED
#endif
#else
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h> 
#include <fcntl.h>
#include <poll.h>
#define socketerrno errno
#define SOCKET_EAGAIN_EINPROGRESS EAGAIN
#define SOCKET_EWOULDBLOCK EWOULDBLOCK
#define INVALID_SOCKET -1
#define SOCKET_ERROR -1
#ifndef _SOCKET_T_DEFINED
typedef int socket_t;
#define _SOCKET_T_DEFINED
#endif
#endif
#include <thread>
#include <mutex>
#include <iostream>
#include <unordered_map>
#include "../common/comm.h"

#ifdef _MAINDLG
class CThreadWindow;
#else
class CMessageQueue;
#endif
class CIOBuffer;
template<typename T>
class SensorSocket
{
public:
#ifdef _MAINDLG
	SensorSocket(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port);
#else
	SensorSocket(CMessageQueue * q,std::string can_ip,int32_t can_port,int32_t host_port);
#endif
	bool Start(const char* ip=nullptr);
	void Run();
	void Stop();
	void Write(CIOBuffer * pBuffer);
	void Read(CIOBuffer* pBuffer);
	T* Get();
#ifndef WIN32
    void SetStartRead(bool b);
	void SetStartWrite(bool b);
#endif
private:
	socket_t _fd;
	std::thread  _thread;
	bool _run;
	std::string _canip;
	int32_t _canport;
	int32_t _hostport;
	std::mutex _lock;
	std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;
    sockaddr_in _canaddr;
#ifndef WIN32
	bool _startRead;
	bool _startWrite;
#endif
};
template<typename T>
#ifdef _MAINDLG
SensorSocket<T>::SensorSocket(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port)
#else
SensorSocket<T>::SensorSocket(CMessageQueue* q, std::string can_ip, int32_t can_port, int32_t host_port)
#endif
{
  	_sensorNotify=std::make_unique<T>(q);
	_canip=can_ip;
	_canport=can_port;
	_hostport=host_port;
#ifndef WIN32
 _startWrite=_startRead=true;
#endif
}


template<typename T>

bool SensorSocket<T>::Start(const char * ip)
{
#ifdef WIN32
	WSAData data;
	WSAStartup(MAKEWORD(2, 2), &data);
#endif

    std::cout<<"SensorSocket<T>::Start"<<std::endl;
	
	_sensorNotify->SetSensorSocket(this);
	_fd = socket(AF_INET, SOCK_DGRAM, 0);
	sockaddr_in sin;
	sin.sin_family = AF_INET;
	sin.sin_port = htons(_hostport);
#ifdef WIN32
	sin.sin_addr.s_addr = inet_addr(ip);
#else
	sin.sin_addr.s_addr = htonl(INADDR_ANY);
#endif
	if (::bind(_fd, (sockaddr*)&sin, sizeof(sin)) == -1)
		return false;
	
	_canaddr.sin_family=AF_INET;
	_canaddr.sin_addr.s_addr=inet_addr(_canip.c_str());
	_canaddr.sin_port=htons(_canport);	
	_sensorNotify->Start();
	_thread = std::thread(&SensorSocket::Run, this);
	
	return true;
}
template<typename T>
void SensorSocket<T>::Read(CIOBuffer* pBuffer)
{
	sockaddr_in from;
	socklen_t fromlen=sizeof(sockaddr_in);
	int32_t ret = recvfrom(_fd,(char *)pBuffer->Buffer, CIOBuffer::IO_BUFFER_SIZE,0,(sockaddr*)&from,&fromlen);
	if (ret <= 0)
	{
		 return;
    }
	pBuffer->Length=ret;
}
template<typename T>
void SensorSocket<T>::Run()
{
	_run = true;
	struct pollfd fds[1];
	fds[0].fd = _fd;
    fds[0].events = POLLIN;

	while (_run)
	{
		if(poll(&fds[0], 1, 1000) > 0)
	   {
			if (fds[0].revents & POLLIN)
			{
				CIOBuffer pBuffer;
				sockaddr_in from;
				socklen_t fromlen=sizeof(sockaddr_in);
				int32_t ret = recvfrom(_fd,(char *)pBuffer.Buffer, CIOBuffer::IO_BUFFER_SIZE,0,(sockaddr*)&from,&fromlen);
				if (ret <= 0||!_run)
				{
					continue;
				}
	   			_sensorNotify->Notify(pBuffer.Buffer,ret);
			}
	   }
	}
    std::cout<<"SensorSocket<T>::Run Finished"<<std::endl;
}
template<typename T>
void SensorSocket<T>::Write(CIOBuffer * pBuffer)
{
	
#ifndef WIN32
	if(_startWrite==false) return;
#endif
	socklen_t len=sizeof(_canaddr);
	 
    int ret=::sendto(_fd,(char *)pBuffer->Buffer,pBuffer->Length,0,(const sockaddr *)&_canaddr,len);
	{
	  //   std::cout<<"ret = "<<ret<<" size ="<<pBuffer->Length<<std::endl;
	}
 
}


template<typename T>
void SensorSocket<T>::Stop()
{
	if(!_run) return;
	_sensorNotify->Stop();
	
	_run = false;
#ifdef WIN32
	closesocket(_fd);
#else
	close(_fd);
#endif
    std::cout<<"SensorSocket<T>::Stop"<<std::endl;
	_thread.join();
}
#ifndef WIN32
template<typename T>
void SensorSocket<T>::SetStartRead(bool b)
{
	_startRead=b;
}
template<typename T>
void SensorSocket<T>::SetStartWrite(bool b)
{
	_startWrite=b;
}
#endif
template<typename T>
T* SensorSocket<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
class SensorTCP
{
public:
#ifdef _MAINDLG
	SensorTCP(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port);
#else
	SensorTCP(CMessageQueue * q,std::string can_ip,int32_t can_port,int32_t host_port);
#endif
	bool Start(const char* ip=nullptr);
	void Run();
	void Stop();
	void Write(CIOBuffer * pBuffer);
	T* Get();
#ifndef WIN32
    void SetStartRead(bool b);
	void SetStartWrite(bool b);
#endif
private:
	socket_t _fd;
	std::thread  _thread;
	bool _run;
	std::string _canip;
	int32_t _canport;
	int32_t _hostport;
	std::mutex _lock;
	std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;
    sockaddr_in _canaddr;
#ifndef WIN32
	bool _startRead;
	bool _startWrite;
#endif
};
template<typename T>
#ifdef _MAINDLG
SensorTCP<T>::SensorTCP(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port)
#else
SensorTCP<T>::SensorTCP(CMessageQueue* q, std::string can_ip, int32_t can_port, int32_t host_port)
#endif
{
  	_sensorNotify=std::make_unique<T>(q);
	_canip=can_ip;
	_canport=can_port;
	_hostport=host_port;

	_run = false;
#ifndef WIN32
 _startWrite=_startRead=true;
#endif
}
template<typename T>

bool SensorTCP<T>::Start(const char * ip)
{
#ifdef WIN32
	WSAData data;
	WSAStartup(MAKEWORD(2, 2), &data);
#endif

    std::cout<<"SensorSocket<T>::Start"<<_canip<<","<<_canport<<std::endl;
	//_sensorNotify->SetSensorSocket(this);
	_fd = socket(AF_INET, SOCK_STREAM, 0);
	sockaddr_in sin;
	sin.sin_family = AF_INET;
	sin.sin_port = htons(_canport);
#ifdef WIN32
	sin.sin_addr.s_addr = inet_addr(ip);
#else
	sin.sin_addr.s_addr =inet_addr(_canip.c_str());// htonl(INADDR_ANY);
#endif
	while (::connect(_fd, (sockaddr*)&sin, sizeof(sin)) == -1)
	{
		std::cout<< "connect "<<_canip<<" failed"<<std::endl;
		  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

	}	 

	if(_run == false)
		_thread = std::thread(&SensorTCP::Run, this);
	_sensorNotify->Start();
	
	return true;
}
template<typename T>
void SensorTCP<T>::Run()
{
	_run = true;
	struct pollfd fds[1];
	fds[0].fd = _fd;
    fds[0].events = POLLIN;
	
//	long long  k = 0;
	//long long tick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
	while (_run)
	{
#ifdef WEBRTC_LINUX
      //  _sensorNotify->PreProcess();
		if(poll(&fds[0], 1, 1000) > 0)
	   {
		    if (fds[0].revents & POLLIN)
			{
#endif
				CIOBuffer pBuffer;
				sockaddr_in from;
				socklen_t fromlen=sizeof(sockaddr_in);
				int32_t ret = recv(_fd,(char *)pBuffer.Buffer, CIOBuffer::IO_BUFFER_SIZE,0);
				if (ret <= 0||!_run)
				{
					close(_fd);
					this->Start();

	
				}
        
	   			//_sensorNotify->Notify(pBuffer.Buffer,ret);
#ifdef WEBRTC_LINUX
			}
	   }
#endif
 
	}
    std::cout<<"SensorSocket<T>::Run Finished"<<std::endl;
}
template<typename T>
void SensorTCP<T>::Write(CIOBuffer * pBuffer)
{
	
#ifndef WIN32
	if(_startWrite==false) return;
#endif
	socklen_t len=sizeof(_canaddr);
	 
    int ret=::send(_fd,(char *)pBuffer->Buffer,pBuffer->Length,0);
	if(ret<=0)
	{
	     std::cout<<"ret = "<<ret<<" size ="<<pBuffer->Length<<std::endl;
	}
 
}
template<typename T>
void SensorTCP<T>::Stop()
{
	if(!_run) return;
	_sensorNotify->Stop();
	
	_run = false;
#ifdef WIN32
	closesocket(_fd);
#else
	close(_fd);
#endif
    std::cout<<"SensorSocket<T>::Stop"<<std::endl;
	_thread.join();
	std::cout<<"SensorSocket<T>::Stop finished"<<std::endl;
}
#ifndef WIN32
template<typename T>
void SensorTCP<T>::SetStartRead(bool b)
{
	_startRead=b;
}
template<typename T>
void SensorTCP<T>::SetStartWrite(bool b)
{
	_startWrite=b;
}
#endif
template<typename T>
T* SensorTCP<T>::Get()
{
	return _sensorNotify.get();
}


#include "../thirdparty/Mqtt/include/MQTTAsync.h"
#include <string.h>

//20230414 中软
template<typename T>
class SensorMQTT
{
public:
	SensorMQTT(CMessageQueue* q, std::string Server_Address, std::string Esn, std::string Password, std::string Clientid);

	bool Start();
	//void Run();
	void Stop();
	void Write(CIOBuffer* pBuffer, const char* pubTopic);
	T* Get();

private:
	MQTTAsync mqttClient;
	//std::thread  _thread;
	std::string _Server_Address;
	std::string _Esn;
	std::string _Password;
	std::string _ClientID;
	//std::mutex _lock;
	//std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorMQTT<T>::SensorMQTT(CMessageQueue* q, std::string Server_Address, std::string Esn, std::string Password, std::string Clientid)
{
	_sensorNotify = std::make_unique<T>(q);
	_Server_Address = Server_Address;
	_Esn = Esn;
	_Password = Password;
	_ClientID = Clientid;
}

template<typename T>
bool SensorMQTT<T>::Start()
{
	std::cout << "SensorMQTT<T>::Start" << _Server_Address << "," << _Esn << "," << _Password << ","  << _ClientID  <<std::endl;

	int rc = 0;
	MQTTAsync_create(&mqttClient, _Server_Address.c_str(), _ClientID.c_str(), MQTTCLIENT_PERSISTENCE_NONE, NULL);

	_sensorNotify->SetSensorMQTT(mqttClient, _Esn);
	
	MQTTAsync_setCallbacks(mqttClient, NULL, _sensorNotify->Disconnect, _sensorNotify->RecevieMessage, NULL); //�������ӶϿ��ͽ������ݻص�
	
	MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer;  //��ʼ���ṹ��
	conn_opts.cleansession = 1;

	conn_opts.username = _Esn.c_str();
	conn_opts.password = _Password.c_str();
	conn_opts.onFailure = _sensorNotify->onConnectFailure; //����ʧ�ܻص�
	conn_opts.context = mqttClient;
	conn_opts.automaticReconnect = true;    //�����Ͽ��Զ�����
	conn_opts.minRetryInterval = 5; //��С�������ʱ��(��)��ÿ��ʧ���������ʱ�䶼��ӱ�
	conn_opts.maxRetryInterval = 365 * 24 * 60 * 60;    //����������ʱ��(��)
	
	
	MQTTAsync_setConnected(mqttClient, (char *)conn_opts.username , _sensorNotify->onConnectCallCBack);   //�������ӳɹ��ص�,�����ǵ�һ�����ӳɹ����������ɹ�������ô˻ص�
	if ((rc = MQTTAsync_connect(mqttClient, &conn_opts)) != MQTTASYNC_SUCCESS)  //��������
	{
		std::cout << "MQTTAsync_connect() fail, error code: " << rc << std::endl;
	}
	//_thread = std::thread(&SensorMQTT::Run, this);
	//_sensorNotify->Start();
	return true;
}

template<typename T>
void SensorMQTT<T>::Stop()
{
	_sensorNotify->Stop(_Esn.c_str());
	std::cout << "SensorMQTT<T>::Stop" << std::endl;
}

template<typename T>
T* SensorMQTT<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorMQTT<T>::Write(CIOBuffer* pBuffer, const char* pubTopic)
{
	_sensorNotify->sendMessage((char *)pBuffer->Buffer,1,pubTopic);
}


#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <linux/can/error.h>

//20231128 CANBUS
template<typename T>
class SensorCanBus
{
public:
	SensorCanBus(CMessageQueue* q,std::string CanName);

	bool Start();
	void Run();
	void Stop();
	void Write(can_frame *date);
	T* Get();

	int sockfd;

private:
	
	bool _run;

	struct ifreq ifr;
    struct sockaddr_can can_addr;
	int ret;
	int nbytes;
	struct can_frame Reciveframe;

	std::string _CanName;

	std::thread  _thread;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorCanBus<T>::SensorCanBus(CMessageQueue* q,std::string CanName)
{
	_sensorNotify = std::make_unique<T>(q);
	_CanName = CanName;

	sockfd = -1;
	memset(&ifr,0,sizeof(ifr));
	memset(&can_addr,0,sizeof(can_addr));
	memset(&Reciveframe,0,sizeof(Reciveframe));
	_run = false;
}

template<typename T>
bool SensorCanBus<T>::Start()
{
	std::cout << "SensorCanBus<T>::Start" << std::endl;
	_sensorNotify->SetCanBusSensor(this);

	sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if(sockfd < 0) 
	{
		std::cout << "SensorCanBus Socket Error" << std::endl;
	}
    
	memcpy(ifr.ifr_name,_CanName.c_str(),_CanName.length());
	//std::cout << ifr.ifr_name << std::endl;
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    ret = bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr));
    if (ret < 0) 
    {
		std::cout << "SensorCanBus Bind Error" << std::endl;
        close(sockfd);
		return false;
	}

	int loopback = 0;
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback));
    int ro = 1; 
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &ro, sizeof(ro));
	/*
	can_err_mask_t err_mask = (CAN_ERR_TX_TIMEOUT | CAN_ERR_BUSOFF);
	setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask));
	*/
	int flag = fcntl(sockfd, F_GETFL, 0);
    if (flag < 0) 
	{
		std::cout << "fcntl F_GETFL fail" << std::endl;
    }

    if (fcntl(sockfd, F_SETFL, flag | O_NONBLOCK) < 0)
	{
		std::cout << "fcntl F_SETFL fail" << std::endl;
	}
	_run = true;

	_sensorNotify->Start();
	_thread = std::thread(&SensorCanBus::Run, this);
	return true;
}

template<typename T>
void SensorCanBus<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;
	_thread.join();

	close(sockfd);
	std::cout << "SensorCanBus<T>::Stop" << std::endl;
}

template<typename T>
T* SensorCanBus<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorCanBus<T>::Write(can_frame *date)
{
	ret = write(sockfd, date, sizeof(can_frame)); 

    if(sizeof(can_frame) != ret) 
	{
		perror("write");
		//printf("\r\n");
	}
		
}

template<typename T>
void SensorCanBus<T>::Run()
{
	//struct pollfd fds[1];
	//fds[0].fd = sockfd;
    //fds[0].events = POLLIN;
	
	
	while (_run)
	{
		fd_set fds;
		struct timeval timeout = {0,0};
	    timeout.tv_usec = 20000; 

        
	    FD_ZERO(&fds);
	    FD_SET(sockfd, &fds);
		
	    int err = select(sockfd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(sockfd, &fds)) 
	   	{
            nbytes = read(sockfd, &Reciveframe, sizeof(Reciveframe));
        
            if(nbytes > 0)
			{
				_sensorNotify->Notify(&Reciveframe);
				//printf("CAN frame:\nID = %x\nDLC = %x\nDATA = %s\n", Reciveframe.can_id,Reciveframe.can_dlc, Reciveframe.data);
			}
	   	}
	}
}



#include <PCANBasic.h>
#include <assert.h>
//20230414 PEAKCAN
template<typename T>
class SensorPeakCan
{
public:
	SensorPeakCan(CMessageQueue* q);

	bool Start();
	void Run();
	void Stop();
	void Write(TPCANMsg* dataMessage);
	T* Get();

private:
	TPCANStatus result;
	TPCANHandle _handle = PCAN_NONEBUS;
	bool _isFD;
	bool _run;

	std::thread  _thread;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorPeakCan<T>::SensorPeakCan(CMessageQueue* q)
{
	_sensorNotify = std::make_unique<T>(q);
}

template<typename T>
bool SensorPeakCan<T>::Start()
{
	std::cout << "SensorPeakCan<T>::Start" << std::endl;
	_sensorNotify->SetSensorSocket(this);

	int iBuffer;
	TPCANHandle _HandlesArray[16];
	char strMsg[256];

	_HandlesArray[0] = PCAN_USBBUS1;
	_HandlesArray[1] = PCAN_USBBUS2;
	_HandlesArray[2] = PCAN_USBBUS3;
	_HandlesArray[3] = PCAN_USBBUS4;
	_HandlesArray[4] = PCAN_USBBUS5;
	_HandlesArray[5] = PCAN_USBBUS6;
	_HandlesArray[6] = PCAN_USBBUS7;
	_HandlesArray[7] = PCAN_USBBUS8;
	_HandlesArray[8] = PCAN_USBBUS9;
	_HandlesArray[9] = PCAN_USBBUS10;
	_HandlesArray[10] = PCAN_USBBUS11;
	_HandlesArray[11] = PCAN_USBBUS12;
	_HandlesArray[12] = PCAN_USBBUS13;
	_HandlesArray[13] = PCAN_USBBUS14;
	_HandlesArray[14] = PCAN_USBBUS15;
	_HandlesArray[15] = PCAN_USBBUS16;
	for (int i = 0; i < (sizeof(_HandlesArray) / sizeof(TPCANHandle)); i++)
	{
		result = CAN_GetValue(_HandlesArray[i], PCAN_CHANNEL_CONDITION, &iBuffer, sizeof(iBuffer));
		if (((result) == PCAN_ERROR_OK) && ((iBuffer & PCAN_CHANNEL_AVAILABLE) == PCAN_CHANNEL_AVAILABLE))
		{
			result = CAN_GetValue((TPCANHandle)_HandlesArray[i], PCAN_CHANNEL_FEATURES, (void*)&iBuffer, sizeof(iBuffer));
			_isFD = (result == PCAN_ERROR_OK) && (iBuffer & FEATURE_FD_CAPABLE);
			_handle = _HandlesArray[i];
			break;
		}
	}
	if (_handle != PCAN_NONEBUS)
	{
		result = ::CAN_Initialize(_handle, PCAN_BAUD_250K);

		if (result == PCAN_ERROR_OK)
		{
			result = CAN_SetValue(_handle, PCAN_BUSOFF_AUTORESET | PCAN_PARAMETER_ON, (void*)&iBuffer, sizeof(iBuffer));
		}
		else
			return false;	
		_run = true;

		_sensorNotify->Start();
		_thread = std::thread(&SensorPeakCan::Run, this);
	}
	return true;
}

template<typename T>
void SensorPeakCan<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;
	_thread.join();

	CAN_Uninitialize(_handle);
	std::cout << "SensorPeakCan<T>::Stop" << std::endl;
}

template<typename T>
T* SensorPeakCan<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorPeakCan<T>::Write(TPCANMsg *dataMessage)
{
	/*dataMessage.ID = 0x601;
	dataMessage.MSGTYPE = PCAN_MESSAGE_STANDARD;
	dataMessage.LEN = 0x03;

	memset(dataMessage.DATA,0x00,8);
	dataMessage.DATA[0] = 0xB1;
	dataMessage.DATA[1] = 0x10;
	dataMessage.DATA[2] = 0xFF;*/
	int fd;
	if(CAN_GetValue(_handle, PCAN_RECEIVE_EVENT, &fd, sizeof(fd)) == PCAN_ERROR_OK)
	{
		result = CAN_Write(_handle, dataMessage);
		/*
		if(result == PCAN_ERROR_OK)
			std::cout << std::hex << dataMessage->ID << std::endl;
		else
			std::cout << std::hex << result << std::endl;
		*/
		if(result != PCAN_ERROR_OK)
			CAN_Reset(_handle);
	}
	
}

template<typename T>
void SensorPeakCan<T>::Run()
{
	int fd;
	TPCANMsg CANMsg;
	TPCANTimestamp CANTimeStamp;

	TPCANStatus stsResult = CAN_GetValue(_handle, PCAN_RECEIVE_EVENT, &fd, sizeof(fd));

	if (stsResult != PCAN_ERROR_OK)
	    std::cout << "CAN_GetValue Error" << "\n";
	
	 while (_run)
	{
		/*
		std::this_thread::sleep_for(std::chrono::milliseconds(20));
		TPCANStatus ret = PCAN_ERROR_OK;

		do{
			 result = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
			 if (result != PCAN_ERROR_QRCVEMPTY)
			 	_sensorNotify->Notify(&CANMsg, CANMsg.LEN);
			
			memset(&CANMsg, 0, sizeof(CANMsg));
			memset(&CANTimeStamp, 0, sizeof(CANTimeStamp));
		}
		while(result & PCAN_ERROR_QRCVEMPTY);
		*/
		
	    struct timeval timeout = {0,0};
	    timeout.tv_usec = 20000; // 20ms

	    fd_set fds;
	    FD_ZERO(&fds);
	    FD_SET(fd, &fds);

	    int err = select(fd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(fd, &fds)) 
	    {
	        result = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
	        if (result != PCAN_ERROR_QRCVEMPTY)
	        {
				_sensorNotify->Notify(&CANMsg, CANMsg.LEN);

				memset(&CANMsg, 0, sizeof(CANMsg));
				memset(&CANTimeStamp, 0, sizeof(CANTimeStamp));
	        }
	    }
		
	}
}