#include "WifibotServer.h"

WifibotServer::WifibotServer(void)
{ 
	tcpon=false;
	sprintf(buffer_address,"0.0.0.0");
	sprintf(buffer_localip,"0.0.0.0");
	Data_Of_Thread_In = 1; 
	
	//Trooper
	servermode = 0;
	watchdog = 0;
	cycles = 0;
	speed1 = 0;
	speed2 = 0;

	Thread_TCP_Run=false;
	Thread_TCP_Trooper_In_Run=false;
	Thread_TCP_Trooper_Out_Run=false;
	Thread_TCP_Trooper_I2C_Run=false;
	Thread_Dog_Run=false;	
}

WifibotServer::~WifibotServer(void)
{
	TerminateThread(m_hThread2,0);
	TerminateThread(m_hThread3,0);
	TerminateThread(m_hThread4,0);
	TerminateThread(m_hThread5,0);
	TerminateThread(m_hThread1,0);
}

HANDLE WifibotServer::SetupI2CCommPort( LPCSTR comport)
{
	HANDLE hCom=0;
	DCB dcb;
	COMMTIMEOUTS ct;

	if (!hCom) CloseHandle(hCom);
	hCom = CreateFileA( comport, GENERIC_READ | GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);
	GetCommState(hCom, &dcb);
	dcb.BaudRate = CBR_19200;
	dcb.fParity = FALSE;
	dcb.fOutxCtsFlow = FALSE;
	dcb.fOutxDsrFlow = FALSE;
	dcb.fDtrControl = DTR_CONTROL_DISABLE;
	dcb.fDsrSensitivity = FALSE;
	dcb.fOutX = FALSE;
	dcb.fInX = FALSE;
	dcb.fRtsControl = RTS_CONTROL_DISABLE;
	dcb.fAbortOnError = FALSE;
	dcb.ByteSize = 8;
	dcb.Parity = NOPARITY;
	dcb.StopBits = TWOSTOPBITS;
	SetCommState(hCom, &dcb);
  
	GetCommTimeouts(hCom, &ct);
	ct.ReadIntervalTimeout = 500;
    ct.ReadTotalTimeoutMultiplier =500; 
    ct.ReadTotalTimeoutConstant = 500; 
	SetCommTimeouts(hCom, &ct);

	SetCommMask(hCom, EV_RXCHAR);
	return hCom;
}

HANDLE WifibotServer::InitComTrooperGPS(void)
{
	DCB	   dcb;							// configuration de la liaison RS-232
	int TimeToWait = 1000;
	COMMTIMEOUTS timeOut;
	/////////////////////////////////////////////////////////////////////////

    hCom_GPS = CreateFileA(PORT_GPS, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL );
   	
    if (hCom_GPS == INVALID_HANDLE_VALUE)	return false;

	if (!GetCommState(hCom_GPS, &dcb))		return false;

	//Configuration du Port
	dcb.DCBlength         = sizeof(DCB);		dcb.BaudRate        = 4800;
	dcb.fBinary           = true;				dcb.fParity         = false;
	dcb.fOutxCtsFlow      = false;				dcb.fOutxDsrFlow    = false;
	dcb.fDtrControl       = DTR_CONTROL_ENABLE;	dcb.fDsrSensitivity = false;
	dcb.fTXContinueOnXoff = false;				dcb.fOutX           = false;
	dcb.fInX              = false;				dcb.fErrorChar      = false;
	dcb.fNull             = false;				dcb.fRtsControl     = RTS_CONTROL_ENABLE;
	dcb.fAbortOnError     = false;				dcb.fDummy2         = 0;
	dcb.wReserved         = 0;					dcb.XonLim          = 0x100;
	dcb.XoffLim           = 0x100;				dcb.ByteSize        = 8;
	dcb.Parity            = NOPARITY;			dcb.StopBits        = ONESTOPBIT;
	dcb.XonChar           = 0x11;				dcb.XoffChar        = 0x13;
	dcb.ErrorChar         = '?';				dcb.EofChar         = 0x1A;
	dcb.EvtChar           = 0x10;							

	timeOut.ReadIntervalTimeout         = TimeToWait;
	timeOut.ReadTotalTimeoutMultiplier  = 0;
	timeOut.ReadTotalTimeoutConstant    = TimeToWait;
	timeOut.WriteTotalTimeoutMultiplier = 0;
	timeOut.WriteTotalTimeoutConstant   = 0;

	if (!SetCommState(hCom_GPS, &dcb))	return false;
	
	if ( !SetCommTimeouts(hCom_GPS, &timeOut) )	return false;

	return hCom_GPS;
}

HANDLE WifibotServer::InitComTrooperRS232(void)
{
	DCB	   dcb;							// configuration de la liaison RS-232
	int TimeToWait = 1000;
	COMMTIMEOUTS timeOut;

	hCom_PIC = CreateFileA(PORT_PIC, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL );
   	//hCom = CreateFile( (LPCWSTR)PORT, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL|FILE_FLAG_NO_BUFFERING, NULL );

	if (hCom_PIC == INVALID_HANDLE_VALUE)	return false;

	if (!GetCommState(hCom_PIC, &dcb))		return false;

	//Configuration du Port
	dcb.DCBlength         = sizeof(DCB);		 dcb.BaudRate        = 19200;
	dcb.fBinary           = true;				 dcb.fParity         = false;
	dcb.fOutxCtsFlow      = false;				 dcb.fOutxDsrFlow    = false;
	dcb.fDtrControl       = DTR_CONTROL_DISABLE; dcb.fDsrSensitivity = false;
	dcb.fTXContinueOnXoff = false;				 dcb.fOutX           = false;
	dcb.fInX              = false;				 dcb.fErrorChar      = false;
	dcb.fNull             = false;				 dcb.fRtsControl     = RTS_CONTROL_DISABLE;
	dcb.fAbortOnError     = false;				 dcb.fDummy2         = 0;
	dcb.wReserved         = 0;					 dcb.XonLim          = 0x100;
	dcb.XoffLim           = 0x100;				 dcb.ByteSize        = 8;
	dcb.Parity            = NOPARITY;			 dcb.StopBits        = ONESTOPBIT;
	dcb.XonChar           = 0x11;				 dcb.XoffChar        = 0x13;
	dcb.ErrorChar         = '?';				 dcb.EofChar         = 0x1A;
	dcb.EvtChar           = 0x10;							

	timeOut.ReadIntervalTimeout         = TimeToWait;
	timeOut.ReadTotalTimeoutMultiplier  = 0;
	timeOut.ReadTotalTimeoutConstant    = TimeToWait;
	timeOut.WriteTotalTimeoutMultiplier = 0;
	timeOut.WriteTotalTimeoutConstant   = 0;


	if (!SetCommState(hCom_PIC, &dcb))		return false;
	
	if ( !SetCommTimeouts(hCom_PIC, &timeOut) )	return false;
       
	return hCom_PIC;
}

int WifibotServer::GetFirmware(HANDLE hUSB)
{
	BYTE sbuf[100];
	DWORD n;

	sbuf[0] = USBI2C_CMD;			// send nodule command			
	sbuf[1] = VERSION;				// send read version command
	sbuf[2] = 0x00;
	sbuf[3] = 0x00;
	WriteFile(hUSB, &sbuf, 4, &n, NULL);
    ReadFile(hUSB, &sbuf, 1, &n, NULL);
		
	if(sbuf[0]>20) {
		#ifdef debug_msg
		printf(" No USB-I2C Module Found \n");
		#endif
		return -1;
	}
	else {
			#ifdef debug_msg
			printf("Firmware Ver %i\n", sbuf[0]);
			#endif
			return sbuf[0];
		}

}

int WifibotServer::GetI2cMotor(HANDLE hUSB, BYTE add,SensorData *data)
{	    
	DWORD n;
	BYTE sbuf[100];

	sbuf[0] = I2C_CMD;			
	sbuf[1] = add+1;
	sbuf[2] = 0x00;
	sbuf[3] = 0x04;
	WriteFile(hUSB, &sbuf, 4, &n, NULL);
	ReadFile(hUSB, &sbuf, 4, &n, NULL);
	data->SpeedFront=sbuf[0];
	data->BatVoltage=sbuf[1];
	data->IR=sbuf[2];
	data->IR2=sbuf[3];
	return 0;
}
int WifibotServer::SetI2cMotor(HANDLE hUSB, BYTE add,BYTE speed)
{
	DWORD n;
	BYTE sbuf[100];

	sbuf[0] = I2C_CMD;			
	sbuf[1] = add;
	sbuf[2] = 0x00;
	sbuf[3] = 0x01;
	sbuf[4] = speed;
	WriteFile(hUSB, &sbuf, 5, &n, NULL);
	ReadFile(hUSB, &sbuf, 1, &n, NULL);
	return sbuf[0];
}

int WifibotServer::GetADC(HANDLE hUSB, BYTE add)
{
	DWORD n;
	BYTE sbuf[100];

	sbuf[0] = I2C_CMD;			
	sbuf[1] = add;
	sbuf[2] = 0x00;
	sbuf[3] = 0x02;
	sbuf[4] = 0x40;
	sbuf[5] = 250;
	WriteFile(hUSB, &sbuf, 6, &n, NULL);
	ReadFile(hUSB, &sbuf, 1, &n, NULL);
	
	sbuf[0] = I2C_CMD;		
	sbuf[1] = add+1;
	sbuf[2] = 0x00;
	sbuf[3] = 0x01;
	WriteFile(hUSB, &sbuf, 4, &n, NULL);
	ReadFile(hUSB, &sbuf, 1, &n, NULL);
	return sbuf[0];	
}
int WifibotServer::StopMotor(HANDLE hUSB)
{
	SetI2cMotor(hUSB,0xA2,0x00);
	SetI2cMotor(hUSB,0xA4,0x00);
	return 0;
}


void WifibotServer::Thread_TCP(void) 
{
	WSADATA wsa;
    SOCKET sock;
    SOCKADDR_IN sin;
    int sinsize;
	int recvMsgSize=0;
	
	//Initialisation du serveur
	WSAStartup(MAKEWORD(2,2),&wsa);
    //Création du socket
	sock=socket(PF_INET,SOCK_STREAM,0);
    sin.sin_family=AF_INET;
	//Lancement du bind
	sin.sin_port=htons((unsigned short)15020);
    //bind(sock,(SOCKADDR*)&sin,sizeof(sin));//release
	sin.sin_addr.s_addr=INADDR_ANY;
    bind(sock,(SOCKADDR*)&sin,sizeof(sin));//debug
	sinsize=sizeof(sin);
    printf("go \n");
	while(1)
	{
		listen(sock,1);
		SOCKET hAccept = accept(sock, NULL, NULL);
		printf("client tcp ok \n");
		Sleep(500);

	do{
			if ((recvMsgSize = recv(hAccept, (char*)buffso_rcv, 2, 0)) < 1) shutdown(hAccept,1);
	
			//  mutexSend.acquire();
			speed1=buffso_rcv[0];
			speed2=buffso_rcv[1];
			//mutexSend.release();
			watchdog = 0;
	 	    cycles = 3;
			//mutexRcv.acquire();
	   		send(hAccept,buffso_send,7,0);
			//mutexRcv.acquire();	   
		}while(recvMsgSize>0);
	  }
}

DWORD WINAPI WifibotServer::Thread_TCP(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_TCP();
   return 0;
}


void WifibotServer::Thread_Dog(void) 
{
	while (1) {
		if(watchdog < cycles) watchdog = watchdog + 1;
	else if(watchdog == cycles)
	{
		speed1 = 0;
		speed2 = 0;
		SetRobotSpeed(0,0,true,true,false);

	}//end of else if watchdog
	Sleep(200);
	}
}

DWORD WINAPI WifibotServer::Thread_Dog(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_Dog();
   return 0;
}

void WifibotServer::Thread_TCP_Trooper_In(void) 
{
	WSADATA wsa;
    SOCKET sock;
    SOCKADDR_IN sin;
    int dataLen;
    int sinsize;
	
	//Initialisation du serveur
	WSAStartup(MAKEWORD(2,2),&wsa);
	//Création du socket
	sock=socket(AF_INET,SOCK_DGRAM,0);
    sin.sin_family=AF_INET;
	//Lancement du bind
	sin.sin_port=htons((unsigned short)15000);
    //bind(sock,(SOCKADDR*)&sin,sizeof(sin));
	sin.sin_addr.s_addr=INADDR_ANY;
    bind(sock,(SOCKADDR*)&sin,sizeof(sin));
	sinsize=sizeof(sin);

while(1)
{    
	//We wait for the client to send "init" and send "ok" in return
	dataLen = recvfrom(sock, buffso_rcvIN, 20,0,(SOCKADDR*)&sin, &sinsize);

    if(!(strstr(buffso_rcvIN,"init")==NULL))
    {
     sprintf(buffso_sendIN,"ok");
	 dataLen = sendto(sock,buffso_sendIN,3,0,(SOCKADDR*)&sin,sizeof(sin));
	printf("client udp ok %d \n",dataLen);
	}
    else if(!(strstr(buffso_rcvIN,"speed")==NULL))
    {
     watchdog = 0;
	 mutexSend.acquire();
	 speed2 = buffso_rcvIN[6];
     speed1 = buffso_rcvIN[8];
	 mutexSend.release();
	 
	#ifdef debug_msg 
//	printf("speed1 %d speed2 %d \n",speed1,speed2);
	#endif
	 
	cycles = 3;
    servermode = 0;
	}//end of else if

}//end of main loop 
}

DWORD WINAPI WifibotServer::Thread_TCP_Trooper_In(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_TCP_Trooper_In();
   return 0;
}

void WifibotServer::Thread_TCP_Trooper_Out(void) 
{
	char ac[80];
    struct in_addr addr;
    struct hostent *phe;

	WSADATA wsa;
    SOCKET sock;
    SOCKADDR_IN sin;
    int dataLen;
    int sinsize;
    
	//Initialisation du serveur
	WSAStartup(MAKEWORD(2,2),&wsa);
    //Création du socket
	sock=socket(AF_INET,SOCK_DGRAM,0);
    sin.sin_family=AF_INET;
	//Lancement du bind
    sin.sin_port=htons((unsigned short)15010);
    //bind(sock,(SOCKADDR*)&sin,sizeof(sin));
	sin.sin_addr.s_addr=INADDR_ANY;
    bind(sock,(SOCKADDR*)&sin,sizeof(sin));
	sinsize=sizeof(sin);

	while(1)
	{	
		//We wait for the client to send "init" and send "ok" in return
		dataLen = recvfrom(sock, buffso_rcvOUT, 20,0,(SOCKADDR*)&sin, &sinsize);
		if(!(strstr(buffso_rcvOUT,"init")==NULL))
		{
		  sprintf(buffso_sendOUT,"ok");
		  dataLen = sendto(sock,buffso_sendOUT,3,0,(SOCKADDR*)&sin,sizeof(sin));
		}
		else if(!(strstr(buffso_rcvOUT,"data")==NULL))
		{  
		   //We look if the reference address has changed
		  if(strstr(buffso_rcvOUT,buffer_address)==NULL)
		  {
		   //We get the new reference address
		   CopyMemory(buffer_address,buffso_rcvOUT+5*sizeof(char),15*sizeof(char));
		   //printf("address: %s\n",buffer_address);
		  }//enf of if address
	      
		  //We get our IP address
		  if (gethostname(ac, sizeof(ac)) != SOCKET_ERROR)
		  {
			phe = gethostbyname(ac);
			if (phe!=0) memcpy(&addr, phe->h_addr_list[0], sizeof(struct in_addr));
			sprintf(buffer_localip,"%d.%d.%d.%d",addr.S_un.S_un_b.s_b1,addr.S_un.S_un_b.s_b2,addr.S_un.S_un_b.s_b3,addr.S_un.S_un_b.s_b4);
		  }
		  CopyMemory(buffso_sendOUT+5*sizeof(char),buffer_localip,15*sizeof(char));

		  mutexRcv.acquire();
		  dataLen = sendto(sock,buffso_sendOUT,100,0,(SOCKADDR*)&sin,sizeof(sin));//
		  mutexRcv.release();
		  }//end of else if

	}//end of main loop
}

DWORD WINAPI WifibotServer::Thread_TCP_Trooper_Out(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_TCP_Trooper_Out();
   return 0;
}

void WifibotServer::Thread_TCP_Trooper_I2C(void) 
{
	//speed1=0;speed2=0;	
	SetI2cMotor(hUSB,0xA2,0x00);
	SetI2cMotor(hUSB,0xA4,0x00);
	while(1)
	{
		#ifdef debug_msg 
		printf("speed1 %d speed2 %d\n",speed1,speed2);
		//printf(" IR1L %d  IR2L %d IR1R %d  IR2R %d  bat L %d  bat R %d\n",dataL.IR,dataL.IR2,dataR.IR,dataR.IR2,buffso_send[0],dataL.SpeedRear+20,dataR.SpeedRear+20);
		#endif
		#ifndef debug_msg 
        #ifndef simu
		mutexSend.acquire();
		int res = SetI2cMotor(hUSB,0xA2,speed1);
	    GetI2cMotor(hUSB,0xA2,&dataL);
	    res = SetI2cMotor(hUSB,0xA4,speed2);
		GetI2cMotor(hUSB,0xA4,&dataR);
		userdataL=dataL;
		userdataL=dataR;
		mutexSend.release();
		#endif
		#endif
		#ifndef simu
	   //We prepare the data to be sent to the user
	   mutexRcv.acquire();	  
	   buffso_sendOUT[0] = dataL.BatVoltage;//+20 or (unsigned char)(strIN[6]-155);
	   buffso_sendOUT[1] = dataR.SpeedFront+(speed2&0x40);
	   buffso_sendOUT[2] = dataL.SpeedFront+(speed1&0x40);
   
	   buffso_sendOUT[93] = dataL.BatVoltage+20;
	   buffso_sendOUT[94] = dataL.SpeedFront;
	   buffso_sendOUT[95] = dataL.SpeedFront;
	   buffso_sendOUT[96] = dataR.SpeedFront;
	   buffso_sendOUT[97] = dataR.SpeedFront;
	   buffso_sendOUT[98] = dataL.IR;
	   buffso_sendOUT[99] = dataR.IR;
	   
	   buffso_send[0]=dataL.BatVoltage+20;//GetADC(hUSB,0x48);
	   buffso_send[1]=dataL.SpeedFront;
	   buffso_send[2]=dataL.SpeedFront;
	   buffso_send[3]=dataR.SpeedFront;
	   buffso_send[4]=dataR.SpeedFront;
	   buffso_send[5]=dataL.IR;
	   buffso_send[6]=dataR.IR;
	   mutexRcv.release();
	   #endif
	   Sleep(100);	   
	}
}

DWORD WINAPI WifibotServer::Thread_TCP_Trooper_I2C(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_TCP_Trooper_I2C();
   return 0;
}

void WifibotServer::Thread_Trooper_RS232(void) 
{
	 BOOL result = FALSE;
    unsigned long  lgReceive = 0;			// nb bytes reçus COM 
    unsigned long  lgSent;
	char strIN[100];
    char strOUT[100];
    int i;

	strOUT[0] = 255;
    strOUT[1] = 0;
    strOUT[2] = 0;
    strOUT[3] = 0;
    strOUT[4] = 0;
    strOUT[5] = 0;
    strOUT[6] = 0;
    

if (hCom_PIC != INVALID_HANDLE_VALUE)
{
	

	while(1)
    {
    
	watchdog = 0;
    speed1 = 4;
    speed2 = 4;
    cycles = 3;
    servermode = 1;
		
	  if(servermode == 0)
      {
       strOUT[1] = speed1;
       strOUT[2] = speed2;
      }
      else if (servermode == 1)
      {
       strOUT[1]=128 + speed1;
       strOUT[2]=128 + speed2;
      }	
    
      //we send and retreive de data from the PIC
      for(i = 0;i<7;i++)
      {
	    WriteFile(hCom_PIC,(void*)(strOUT+i), 1,&lgSent, NULL);
	    ReadFile(hCom_PIC, (strIN+i), 1, &lgReceive, NULL);
	  }//end of for	
	
	  //We prepare the data to be sent to the user
      buffso_sendOUT[0] = (unsigned char)(strIN[6]-155);
      buffso_sendOUT[1] = strIN[0]+(speed2&0x40);
	  buffso_sendOUT[2] = strIN[1]+(speed1&0x40);
	  
	  userdataR.BatVoltage=(unsigned char)(strIN[6]-155);
      userdataL.BatVoltage=(unsigned char)(strIN[6]-155);
	  userdataR.SpeedFront=strIN[0]+(speed2&0x40);
	  userdataL.SpeedFront=strIN[1]+(speed1&0x40);

	  //printf("PIC: ");
	  //for(i = 0;i<7;i++)printf("%d ",(unsigned char)strIN[i]);
      //printf("\n");
	  
	  Sleep(20);
	}//end of main loop
}//end of if hCom_PIC
}

DWORD WINAPI WifibotServer::Thread_Trooper_RS232(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_Trooper_RS232();
   return 0;
}

void WifibotServer::Thread_GPS(void) 
{
BOOL result = FALSE;
unsigned long	lgReceive = 0 ;			// nb bytes reçus COM 
char str[100];
char gps_buffer[100];
char data_buffer[100];
int index = 0;
int i;
int j;
int degrees;
double new_lat = 0;
double new_lng = 0;
double old_lat = 0;
double old_lng = 0;

if (hCom_GPS != INVALID_HANDLE_VALUE)
{
while(1)
{
 
 result = ReadFile(hCom_GPS, str, 1, &lgReceive, NULL);
 
 if(str[0]=='$')
 {
   //We process the data
   if(!(strstr(gps_buffer,"$GPGGA")==NULL))
   {
    
	old_lat = new_lat;
    old_lng = new_lng;
	
	i = 0;
    while(gps_buffer[i]!=',') i++;
    i = i + 1;
    while(gps_buffer[i]!=',') i++;
    i = i + 1;
    j = 0;
    while(gps_buffer[i]!=',')
    {
     data_buffer[j] = gps_buffer[i];
     j++;
     i++; 
    }
    new_lat = atof(data_buffer);
    new_lat = (float)new_lat/(float)100;
    degrees = (int)new_lat;
    new_lat = (new_lat - degrees)*100;
    new_lat = (float)new_lat/(float)60;
    new_lat = degrees + new_lat; 

    i = i + 1;
    if(gps_buffer[i] == 'S') new_lat = -new_lat;

    i = i + 2;
   j = 0;
   while(gps_buffer[i]!=',')
   {
    data_buffer[j] = gps_buffer[i];
    j++;
    i++;
   }
   new_lng = atof(data_buffer);
   new_lng = (float)new_lng/(float)100;
   degrees = (int)new_lng;
   new_lng = (new_lng - degrees)*100;
   new_lng = (float)new_lng/(float)60;
   new_lng = degrees + new_lng;

   i = i + 1;
   if(gps_buffer[i]=='W') new_lng = -new_lng;

   memset(buffso_sendOUT+21*sizeof(char),0,20);
   if(new_lat != 0)
   {
    sprintf(data_buffer,"%f",new_lat);
    CopyMemory(buffso_sendOUT+21*sizeof(char),data_buffer,20);
   }

   memset(buffso_sendOUT+40*sizeof(char),0,20);
   if(new_lng != 0)
   {
    sprintf(data_buffer,"%f",new_lng);
    CopyMemory(buffso_sendOUT+40*sizeof(char),data_buffer,20);
   }
   
   //printf("GPS: %s %s\n",buffso_sendOUT+21,buffso_sendOUT+40);
   }//end of if $GPGGA
 
   //Then we start a new sequence
   gps_buffer[0] = str[0];
   index = 1;
 }//end of if $
 else if (lgReceive>0)
 {
   gps_buffer[index] = str[0];
   index = index + 1;
 }//end of else $

}//end of while
}//end of if GPS
}

DWORD WINAPI WifibotServer::Thread_GPS(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_GPS();
   return 0;
}

void WifibotServer::Thread_SNR(void) 
{
	while(1)
	{
     buffso_sendOUT[3] = 50;
	 Sleep(1000);
	}//end of main loop
}

DWORD WINAPI WifibotServer::Thread_SNR(LPVOID p)
{
   WifibotServer *me = (WifibotServer *)p;
   me->Thread_SNR();
   return 0;
}

void WifibotServer::GetDataSimIn(char* L,char* R)
{
	*L=speed1;
    *R=speed2;
}

void WifibotServer::SetDataSimOut(int bat,int SpeedFrontR,int SpeedFrontL,int IRL,int IRR)
{
	  //We prepare the data to be sent to the user
	   mutexRcv.acquire();	  
	   buffso_sendOUT[0] = bat;//dataL.SpeedRear;//+20 or (unsigned char)(strIN[6]-155);
	   buffso_sendOUT[1] = SpeedFrontR;//dataR.SpeedFront+(speed2&0x40);
	   buffso_sendOUT[2] = SpeedFrontL;//dataL.SpeedFront+(speed1&0x40);
   
	   buffso_sendOUT[93] = bat+20;//dataL.SpeedRear+20;
	   buffso_sendOUT[94] = SpeedFrontL;//dataL.SpeedFront;
	   buffso_sendOUT[95] = SpeedFrontL;//dataL.SpeedFront;
	   buffso_sendOUT[96] = SpeedFrontR;//dataR.SpeedFront;
	   buffso_sendOUT[97] = SpeedFrontR;//dataR.SpeedFront;
	   buffso_sendOUT[98] = IRL;//dataL.IR;
	   buffso_sendOUT[99] = IRR;//dataR.IR;
	   
	   buffso_send[0]=bat+20;//dataL.SpeedRear+20;//GetADC(hUSB,0x48);
	   buffso_send[1]=SpeedFrontL;//dataL.SpeedFront;
	   buffso_send[2]=SpeedFrontL;//dataL.SpeedFront;
	   buffso_send[3]=SpeedFrontR;//dataR.SpeedFront;
	   buffso_send[4]=SpeedFrontR;//dataR.SpeedFront;
	   buffso_send[5]=IRL;//=dataL.IR;
	   buffso_send[6]=IRR;//dataR.IR;
	   mutexRcv.release();
}

void WifibotServer::GetRobotSensor(SensorData* L,SensorData* R)
{
	*L=userdataL;
	*R=userdataR;
}

void WifibotServer::SetRobotSpeed(char speedL,char speedR,bool SideL,bool SideR,bool ctrl)
{	
	speed1=(ctrl*128)+(SideL*64)+speedL;
	speed2=(ctrl*128)+(SideR*64)+speedR;	
}

void WifibotServer::runLab(void)
{
    hUSB=NULL;
	CloseHandle(hUSB);
	hUSB = SetupI2CCommPort((LPCSTR)PORT_I2C_USB);
	printf("\nCOM HANDLE %d \n",hUSB);
	int firm=GetFirmware(hUSB);
	printf("firmware=%d\n",firm);
	
	m_hThread1=CreateThread(0,0,Thread_TCP_Trooper_I2C,this,0,&m_dwThreadID1);
	m_hThread2=CreateThread(0,0,Thread_TCP,this,0,&m_dwThreadID2);
	m_hThread3=CreateThread(0,0,Thread_Dog,this,0,&m_dwThreadID3);
	m_hThread4=CreateThread(0,0,Thread_TCP_Trooper_In,this,0,&m_dwThreadID4);
	m_hThread5=CreateThread(0,0,Thread_TCP_Trooper_Out,this,0,&m_dwThreadID5);	
}

void WifibotServer::runTrooper(void)
{
	m_hThread6=CreateThread(0,0,Thread_Trooper_RS232,this,0,&m_dwThreadID6);
	m_hThread3=CreateThread(0,0,Thread_Dog,this,0,&m_dwThreadID3);
	m_hThread4=CreateThread(0,0,Thread_TCP_Trooper_In,this,0,&m_dwThreadID4);
	m_hThread5=CreateThread(0,0,Thread_TCP_Trooper_Out,this,0,&m_dwThreadID5);	
}

void WifibotServer::runSim(void)
{
    m_hThread2=CreateThread(0,0,Thread_TCP,this,0,&m_dwThreadID2);
	m_hThread3=CreateThread(0,0,Thread_Dog,this,0,&m_dwThreadID3);
	m_hThread4=CreateThread(0,0,Thread_TCP_Trooper_In,this,0,&m_dwThreadID4);
	m_hThread5=CreateThread(0,0,Thread_TCP_Trooper_Out,this,0,&m_dwThreadID5);	
}

void WifibotServer::runManualSim(void)
{
	SetRobotSpeed(20,20,true,true,false);
}

void WifibotServer::runManualLab(void)
{
    hUSB=NULL;
	CloseHandle(hUSB);
	hUSB = SetupI2CCommPort((LPCSTR)PORT_I2C_USB);
	printf("\nCOM HANDLE %d \n",hUSB);
	int firm=GetFirmware(hUSB);
	printf("firmware=%d\n",firm);
	
	m_hThread1=CreateThread(0,0,Thread_TCP_Trooper_I2C,this,0,&m_dwThreadID1);	
	//Loop to Get Set Robot Data
	SetRobotSpeed(20,20,true,true,false);
	SensorData L,R;
	GetRobotSensor(&L,&R);
}

void WifibotServer::runManualTrooper(void)
{
    hUSB=NULL;
	CloseHandle(hUSB);
	hUSB = SetupI2CCommPort((LPCSTR)PORT_I2C_USB);
	printf("\nCOM HANDLE %d \n",hUSB);
	int firm=GetFirmware(hUSB);
	printf("firmware=%d\n",firm);
	
	m_hThread1=CreateThread(0,0,Thread_Trooper_RS232,this,0,&m_dwThreadID6);
	
	SetRobotSpeed(10,10,true,true,false);
}

WMutex::WMutex ()
{
	wmutex = (void*)CreateMutex (0, false, 0);
}

WMutex::~WMutex ()
{
	CloseHandle (wmutex);
}

void WMutex::acquire ()
{
	WaitForSingleObject (wmutex, INFINITE);
}

int WMutex::acquired ()
{
	return (WaitForSingleObject (wmutex, 0) != WAIT_TIMEOUT);
}

void WMutex::release ()
{
	ReleaseMutex (wmutex);
}
