#include <stdio.h>
#include <windows.h>
#include <assert.h>
#include <commctrl.h>
#include <stdlib.h>
#include <process.h>
#include <winsock.h>
#include <stddef.h>
#include <conio.h>
#include <string.h>
#include <ctype.h>
#include <time.h>

#define STRINGPAD           2
#define MAXPORTSIZE        10
#define GOOD                0
#define FAIL               -1
#define QUEUEEMPTY         -2
#define MOTORCOMMANDSIZE    7
#define ENDCHAR          0xFF
#define MOTORCOMMAND        2
#define QUEUELENGTH       200

#define ASCII_XON	 0x11
#define ASCII_XOFF 0x13

#define uchar unsigned char

struct MotorCom_struct
{
  uchar LEncoder;
  uchar REncoder;
  uchar LMotor;
  uchar RMotor;
  struct MotorCom_struct* next;
};

typedef struct MotorCom_struct MOTORCOM;

char gPort[MAXPORTSIZE+STRINGPAD];
int  gBaud;
int  gStopBit;
int  gParity;
int  gByteSize;
int  gNetPort;
MOTORCOM gMCQueue[QUEUELENGTH];
int  gCurrentWritePos;
int  gCurrentReadPos;
CRITICAL_SECTION	gComPort;

void InitQueue(void)
{
  gCurrentWritePos = 0;
  gCurrentReadPos  = 0;
}

int AddToQueue(uchar LEncoder, uchar REncoder, uchar LMotor, uchar RMotor)
{
	EnterCriticalSection(&gComPort);

  gMCQueue[gCurrentWritePos].LEncoder = LEncoder;
  gMCQueue[gCurrentWritePos].REncoder = REncoder;
  gMCQueue[gCurrentWritePos].LMotor   = LMotor;
  gMCQueue[gCurrentWritePos].RMotor   = RMotor;

  gCurrentWritePos++;
  if(gCurrentWritePos >= QUEUELENGTH)
  {
    gCurrentWritePos = 0;
  }

  /*
  ** Colide into the read 
  */
  if(gCurrentWritePos == gCurrentReadPos)
  {
    printf("Collide!");
    gCurrentReadPos++;
    if(gCurrentReadPos >= QUEUELENGTH)
    {
      gCurrentReadPos = 0;
    }
  }
	LeaveCriticalSection(&gComPort);
  return(GOOD);
}

int GetFromQueue(uchar* LEncoder, uchar* REncoder, uchar* LMotor, uchar* RMotor)
{
  int Result=GOOD;
	EnterCriticalSection(&gComPort);
  
  if(gCurrentWritePos == gCurrentReadPos)
  {
    Result = QUEUEEMPTY;
  }
  else
  {
    *LEncoder = gMCQueue[gCurrentReadPos].LEncoder;
    *REncoder = gMCQueue[gCurrentReadPos].REncoder;
    *LMotor   = gMCQueue[gCurrentReadPos].LMotor;
    *RMotor   = gMCQueue[gCurrentReadPos].RMotor;

    gCurrentReadPos++;
    if(gCurrentReadPos >= QUEUELENGTH)
    {
      gCurrentReadPos = 0;
    }
  }

	LeaveCriticalSection(&gComPort);
  return(Result);
}

/*
** Read in the settings from the INI file
*/
void ReadIniFile(void)
{
	        GetPrivateProfileString("ComPortReader","ComPort", "COM1:",gPort,MAXPORTSIZE,".\\ComPortReader.ini");
  gBaud    = GetPrivateProfileInt("ComPortReader","BaudRate",9600,".\\ComPortReader.ini");
  gStopBit = GetPrivateProfileInt("ComPortReader","StopBit", 0,".\\ComPortReader.ini");
  gParity  = GetPrivateProfileInt("ComPortReader","Parity",  0,".\\ComPortReader.ini");
  gByteSize= GetPrivateProfileInt("ComPortReader","ByteSize",8,".\\ComPortReader.ini");
  gNetPort = GetPrivateProfileInt("ComPortReader","NetPort", 5678,".\\ComPortReader.ini");
}

DWORD WINAPI SocketThread(void *psocket)
{
  uchar Buffer[128]; 
  SOCKET msgsock = *(SOCKET*) psocket; 
  int retval; 
  uchar LEncoder;
  uchar REncoder;
  uchar LMotor;
  uchar RMotor;
  int   BufferSize;  

  while(1) 
  { 
    retval = recv(msgsock,(char*)Buffer,sizeof (Buffer),0 ); 
    if (retval == SOCKET_ERROR) 
    { 
      fprintf(stderr,"recv() failed: error %d\n",WSAGetLastError()); 
      closesocket(msgsock); 
      Sleep(1000);
      continue; 
    } 

    if (retval == 0) 
    { 
      printf("Client closed connection\n"); 
      closesocket(msgsock); 
      break;
    } 

    if(102 == Buffer[0])
    {
      Buffer[0]  = 202;
      BufferSize = 1;  
      if(GOOD == GetFromQueue(&LEncoder, &REncoder, &LMotor, &RMotor))
      { 
        Buffer[1] = LEncoder;
        Buffer[2] = REncoder;
        Buffer[3] = LMotor;
        Buffer[4] = RMotor;
        BufferSize = 5;  
      }
    }
    else
    {
      Buffer[0]  = 0;
      BufferSize = 1;  
    }

    retval = send(msgsock,(char*)Buffer,BufferSize,0); 
    if (retval == SOCKET_ERROR) 
    { 
      fprintf(stderr,"send() failed: error %d\n",WSAGetLastError()); 
      WSACleanup(); 
      ExitThread(-1);
      return(-1);
    } 
    
    Sleep(10);
    continue; 
  } 
  ExitThread(4343);
  return(4242);
}

DWORD WINAPI ServerThread(void *psocket)
{
  char *the_interface= NULL; 
  int retval; 
  int fromlen; 
  int socket_type = SOCK_STREAM; 
  struct sockaddr_in local, from; 
  WSADATA wsaData; 
  SOCKET listen_socket, msgsock; 
  int bOnce = 0; 
  HANDLE hThread=NULL;
  BOOL abool;
  int nExitCode = 0;

  if ((retval = WSAStartup(0x202,&wsaData)) != 0) 
  { 
    fprintf(stderr,"WSAStartup failed with error %d\n",retval); 
    WSACleanup(); 
    return -1; 
  } 

  local.sin_family = AF_INET; 
  local.sin_addr.s_addr = (!the_interface)?INADDR_ANY:inet_addr(the_interface);  

  /*  
  ** Port MUST be in Network Byte Order 
  */ 
  local.sin_port = htons(gNetPort); 

  listen_socket = socket(AF_INET, socket_type,0); // TCP socket 

  if (listen_socket == INVALID_SOCKET)
  { 
    fprintf(stderr,"socket() failed with error %d\n",WSAGetLastError()); 
    WSACleanup(); 
    return -1; 
  } 

  if (bind(listen_socket,(struct sockaddr*)&local,sizeof(local)) == SOCKET_ERROR) 
  { 
    fprintf(stderr,"bind() failed with error %d\n",WSAGetLastError()); 
    WSACleanup(); 
    return -1; 
  } 

  if (listen(listen_socket,5) == SOCKET_ERROR) 
  { 
    fprintf(stderr,"listen() failed with error %d\n",WSAGetLastError()); 
    WSACleanup(); 
    return -1; 
  } 

  printf("'Listening' on port %d, protocol %s\n", gNetPort, (socket_type == SOCK_STREAM)?"TCP":"UDP"); 

  fromlen =sizeof(from); 
  while(1)
  {
    msgsock = accept(listen_socket,(struct sockaddr*)&from, &fromlen); 
    if (msgsock == INVALID_SOCKET) 
    { 
      fprintf(stderr,"accept() error %d\n",WSAGetLastError()); 
      WSACleanup(); 
      return -1; 
    } 
    printf("accepted connection from %s, port %d\n", inet_ntoa(from.sin_addr), htons(from.sin_port)) ; 

    if(NULL != hThread)
    {
      abool = GetExitCodeThread(hThread,(unsigned long*)&nExitCode);
      printf("lngReturn=%d %d\n",nExitCode,abool);
      while(STILL_ACTIVE == nExitCode)
      {
        printf("Waiting for free thread\n");
        abool = GetExitCodeThread(hThread,(unsigned long*)&nExitCode);
      }
      printf("lngReturn=%d %d\n",nExitCode,abool);
      hThread = NULL;
    }

    hThread = CreateThread(NULL, 0, &SocketThread, (LPVOID)&msgsock, 0, NULL);
    Sleep(10);
  } 
  return(0);
}


HANDLE OpenPort(void)
{
  HANDLE hCommPort;
    
  hCommPort = CreateFile( gPort,  
							GENERIC_READ | GENERIC_WRITE, 
							0, 
							0, 
							OPEN_EXISTING,
							FILE_ATTRIBUTE_NORMAL,
							0);

  return hCommPort;
}

int InitPort(HANDLE hCommPort)
{
  DCB dcb = {0};

  dcb.DCBlength = sizeof(dcb);
  
  /*
  ** Get the current com port settings
  */

  if (!GetCommState(hCommPort, &dcb))
  {
    return(FAIL);
  }

  /*
  ** set the com port to our settings
  */
  dcb.BaudRate	= gBaud;
  dcb.ByteSize	= gByteSize;
  dcb.Parity		= gParity;
  dcb.StopBits	= gStopBit;

  dcb.fDtrControl       = DTR_CONTROL_DISABLE;
  dcb.fRtsControl       = RTS_CONTROL_DISABLE;
  dcb.fOutxCtsFlow      = FALSE;
  dcb.fOutxDsrFlow      = FALSE;
  dcb.fDsrSensitivity   = FALSE;
  dcb.fOutX             = FALSE;
  dcb.fInX              = FALSE;
  dcb.fTXContinueOnXoff = FALSE;
  dcb.XonChar           = ASCII_XON;
  dcb.XoffChar          = ASCII_XOFF;
  dcb.XonLim            = 0;
  dcb.XoffLim           = 0;
  dcb.fParity			      = TRUE;

  /*
  ** set the com port to our settings
  */
  if (!SetCommState(hCommPort, &dcb))
  {
    return(FAIL);
  }

  return(GOOD);
}

void ProcessMotorCommand(uchar* ReadBuffer)
{
  int   i;
  uchar check;  

  check = 0;
  for(i=0;i<5;i++)
  {
    check+=ReadBuffer[i];
  }

  for(i=0;i<8;i++)
  {
    printf("%02x ",ReadBuffer[i]);
  }

  if(check != ReadBuffer[5])
  {
    printf("Failed Check");
  }
  else
  {
    AddToQueue(ReadBuffer[1], ReadBuffer[2], ReadBuffer[3], ReadBuffer[4]);
  }
  printf("\n");
}

void main(void)
{
  HANDLE hCommPort;
  uchar  ReadBuffer[100];
  DWORD  CharsRead;
	BOOL   ReturnCode;
  uchar  PrevChar;
  uchar  msgsock=0;
  HANDLE hThread=NULL;

	InitializeCriticalSection(&gComPort);
  InitQueue();
  hThread = CreateThread(NULL, 0, &ServerThread, (LPVOID)&msgsock, 0, NULL);

  ReadIniFile();
  printf("Opening Port:%s\n",gPort);
  printf("        Baud:%d\n",gBaud);
  printf("    Stop Bit:%d\n",gStopBit);
  printf("      Parity:%d\n",gParity);
  printf("    Net Port:%d\n",gNetPort);

  hCommPort = OpenPort();
  if (INVALID_HANDLE_VALUE == hCommPort)
	{
    printf("Error in opening the Com Port\n");
    return; 
  }

  if(GOOD != InitPort(hCommPort))
  {
    CloseHandle(hCommPort);
    printf("Error while setting up the com port\n");
    return;
  }
 
  ReturnCode = TRUE;
  while(TRUE == ReturnCode)
  {
    Sleep(10);
		ReturnCode = ReadFile(hCommPort, ReadBuffer, 1, &CharsRead, NULL);
    switch(ReadBuffer[0])
    {
      case(MOTORCOMMAND):
        ReturnCode = ReadFile(hCommPort, &ReadBuffer[1], MOTORCOMMANDSIZE, &CharsRead, NULL);
        ProcessMotorCommand(ReadBuffer);
        break;
      default:
        /*
        ** Somehow we have gotten off track, find the next two EndChars to get back on
        */
        PrevChar = ReadBuffer[0];
        ReturnCode = ReadFile(hCommPort, ReadBuffer, 1, &CharsRead, NULL);
        while(TRUE == ReturnCode)
        {
          printf("%d %d\n",PrevChar,ReadBuffer[0]);
          if((ENDCHAR == PrevChar)&&(ENDCHAR == ReadBuffer[0]))
          {
            break;
          }
          PrevChar = ReadBuffer[0];
          ReturnCode = ReadFile(hCommPort, ReadBuffer, 1, &CharsRead, NULL);
        }     
    }
  }

  CloseHandle(hCommPort);
}