#include "StdAfx.h"
#include "TimeSync.h"
#include <string.h>

TimeSync::TimeSync( Status *pStatus ) :
	  m_pStatus			( pStatus )
	, m_hCom				( NULL )
	, m_bInitialSync	( true )
	, m_bInitialized1	( false )
	, m_bInitialized2	( false )
	, m_thread( "Time Sync Thread", SysThread::PRI_HIGHEST )
{
	memset( m_chTimeMessage, 0, 128 );
#ifdef SIM_UDP_SERIAL
	m_bSimSerial = true;
#else
	m_bSimSerial = false;
#endif
}

TimeSync::~TimeSync()
{
}

bool TimeSync::Initialize( char *commPort, char *strIP1, int UDPport1, char *strIP2, int UDPport2 )
{
	bool bRtn = false;
	strcpy_s( m_strIP1, 128, strIP1 );
	m_UDPport1 = UDPport1;
	strcpy_s( m_strIP2, 128, strIP2 );
	m_UDPport2 = UDPport2;

	int syncGPSClock = 0;
	if( InitSerial( commPort ) && InitUDPSocket1() && InitUDPSocket2())
	{
		bRtn			= true;

		m_thread.Start ( ThreadEntry,  (void*) this );
		if( strlen( m_strIP1 ) ) { m_pStatus->PrintStatus( "Broadcasting UDP Time String to %s\n", strIP1 ); }
		if( strlen( m_strIP2 ) ) { m_pStatus->PrintStatus( "Broadcasting UDP Time String to %s\n", strIP2 ); }
	}
	else
	{
		m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync reading cfg - GPS_SYNC_CLOCK\n" );
	}

	return bRtn;
}

bool TimeSync::InitUDPSocket1()
{
	bool bRtn = true;
	WSADATA wsa;

	if( !strlen( m_strIP1 ) ) return true;
      
	//Initialise winsock
	if (WSAStartup(MAKEWORD(2,2),&wsa) != 0)
	{
		m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error initializing WinSock, Error Code : %d\n", WSAGetLastError() );
		bRtn = false;
	}

	if( bRtn )
	{
		//setup address structure
		memset((char *) &m_sockaddr1, 0, sizeof(m_sockaddr1));
		m_sockaddr1.sin_family = AF_INET;
		m_sockaddr1.sin_port = htons(m_UDPport1);
		m_sockaddr1.sin_addr.S_un.S_addr = inet_addr(m_strIP1);
    
		//create socket
		if ( (m_UPSSocket1 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == SOCKET_ERROR)
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error creating UDP socket 1, error: %d\n", (int)WSAGetLastError() );
			bRtn = false;
		}
	}

	if( bRtn ) { m_bInitialized1 = true; }
	return bRtn;
}
bool TimeSync::InitUDPSocket2()
{
	bool bRtn = true;
	WSADATA wsa;

	if( !strlen( m_strIP2 ) ) return true;
      
	//Initialise winsock
	if (WSAStartup(MAKEWORD(2,2),&wsa) != 0)
	{
		m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error initializing WinSock, Error Code : %d\n", WSAGetLastError() );
		bRtn = false;
	}

	if( bRtn )
	{
		//setup address structure
		memset((char *) &m_sockaddr2, 0, sizeof(m_sockaddr1));
		m_sockaddr2.sin_family = AF_INET;
		m_sockaddr2.sin_port = htons(m_UDPport2);
		m_sockaddr2.sin_addr.S_un.S_addr = inet_addr(m_strIP2);
    
		//create socket
		if ( (m_UPSSocket2 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == SOCKET_ERROR)
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error creating UDP socket 2, error: %d\n", (int)WSAGetLastError() );
			bRtn = false;
		}
	}

	if( bRtn ) { m_bInitialized2 = true; }
	return bRtn;
}


bool TimeSync::InitSerial( char *commPort )
{
	if( m_bSimSerial ) return true;

	bool bRtn      = false;
	BOOL bSuccess1 = false;

	while( true )
	{
			m_hCom = CreateFile( commPort,  
				GENERIC_READ | GENERIC_WRITE, //GENERIC_ALL
				0,    // exclusive access 
				NULL, // no security attributes 
				OPEN_EXISTING,
				0, //FILE_FLAG_OVERLAPPED, //0
				NULL
				);

		if (m_hCom == INVALID_HANDLE_VALUE) 
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "ERROR: Time Sync, opening port: %s\n", commPort );
			break;
		}		

		// Build on the current configuration, and skip setting the size
		// of the input and output buffers with SetupComm.
		DCB dcb;

		//  Initialize the DCB structure.
		SecureZeroMemory( &dcb, sizeof(DCB) );
		dcb.DCBlength = sizeof(DCB);

		bSuccess1 = GetCommState(m_hCom, &dcb);

		if( !bSuccess1 ) 
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync, GetCommState failed with error:\n", GetLastError() );
			CloseHandle( m_hCom );
			break;
			}

		// fixed for PCI-FiT
		dcb.BaudRate = CBR_57600;		//CBR_115200;     // set the baud rate
		dcb.ByteSize = 8;             // data size, xmit, and rcv
		dcb.Parity   = NOPARITY;      // no parity bit
		dcb.StopBits = ONESTOPBIT;    // one stop bit

		bSuccess1 = SetCommState( m_hCom, &dcb );
		if( !bSuccess1 ) 
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync SetCommState failed with error:\n", GetLastError() );
			CloseHandle( m_hCom );
			break;
		}

		COMMTIMEOUTS commTimeOut;
		BOOL flag = GetCommTimeouts( m_hCom, &commTimeOut );

			// from MSDN:
			//    If an application sets ReadIntervalTimeout and ReadTotalTimeoutMultiplier to MAXDWORD and sets ReadTotalTimeoutConstant 
			//    to a value greater than zero and less than MAXDWORD, one of the following occurs when the ReadFile function is called:
			//    If there are any bytes in the input buffer, ReadFile returns immediately with the bytes in the buffer.
			//    If there are no bytes in the input buffer, ReadFile waits until a byte arrives and then returns immediately.
			//    If no bytes arrive within the time specified by ReadTotalTimeoutConstant, ReadFile times out.
			commTimeOut.ReadIntervalTimeout			= MAXDWORD;
			commTimeOut.ReadTotalTimeoutConstant	= 50;
			commTimeOut.ReadTotalTimeoutMultiplier = MAXDWORD;
			SetCommTimeouts( m_hCom, &commTimeOut );

			bSuccess1 = SetupComm(m_hCom,1024,1024);
			if (!bSuccess1) 
			{
				m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync SetupComm failed with error:\n", GetLastError() );
				CloseHandle( m_hCom );
				break;
			}

			PurgeComm( m_hCom, PURGE_RXCLEAR | PURGE_TXCLEAR );
			bRtn = true;
			break;
	}

	return bRtn;
}


bool TimeSync::ReadChar( char *character )
{
	BOOL bSuccess;
	DWORD bytesRead = 0;

	bSuccess = ReadFile( m_hCom, character, 1, &bytesRead, NULL );
	if( bSuccess && bytesRead == 1 ) return true;
	else                             return false;
}
bool TimeSync::ReadString( char *cmd, int maxBytesToRead, DWORD *bytesRead )
{
	BOOL bSuccess;

	bSuccess = ReadFile( m_hCom, cmd, maxBytesToRead, bytesRead, NULL );
	if( bSuccess ) cmd[*bytesRead] = 0;

	return bSuccess ? true : false;
}
int TimeSync::ReadDataWaiting()
{
	if( m_hCom == NULL ) return( 0 );

	DWORD dwErrorFlags;
	COMSTAT ComStat;

	ClearCommError( m_hCom, &dwErrorFlags, &ComStat );

	return( (int) ComStat.cbInQue );
}
void TimeSync::EnumerateSerialPorts(CUIntArray& ports)
{

  //Make sure we clear out any elements which may already be in the array
  ports.RemoveAll();

  //Determine what OS we are running on
  OSVERSIONINFO osvi;
  osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);
  BOOL bGetVer = GetVersionEx(&osvi);

  //On NT use the QueryDosDevice API
  if (bGetVer && (osvi.dwPlatformId == VER_PLATFORM_WIN32_NT))
  {
    //Use QueryDosDevice to look for all devices of the form COMx. This is a better
    //solution as it means that no ports have to be opened at all.
    TCHAR szDevices[65535];
    DWORD dwChars = QueryDosDevice(NULL, szDevices, 65535);
    if (dwChars)
    {
      int i=0;

      for (;;)
      {
        //Get the current device name
        TCHAR* pszCurrentDevice = &szDevices[i];

        //If it looks like "COMX" then
        //add it to the array which will be returned
        int nLen = _tcslen(pszCurrentDevice);
        if (nLen > 3 && _tcsnicmp(pszCurrentDevice, _T("COM"), 3) == 0)
        {
          //Work out the port number
          int nPort = _ttoi(&pszCurrentDevice[3]);
          if (nPort > 0)
            ports.Add(nPort);
        }

        // Go to next NULL character
        while(szDevices[i] != _T('\0'))
          i++;

        // Bump pointer to the next string
        i++;

        // The list is double-NULL terminated, so if the character is
        // now NULL, we're at the end
        if (szDevices[i] == _T('\0'))
          break;
      }
    }
    else
        TRACE(_T("Failed in call to QueryDosDevice, GetLastError:%d\n"), GetLastError());
  }
}

// static thread entry function
void TimeSync::ThreadEntry( void *pObj )
{
	((TimeSync*) pObj)->Thread();
}

void TimeSync::Thread()
{
	if( m_bSimSerial ) { ThreadSim(); }

	char ZDAbuf[2048];
	DWORD len, len2;
	int cnt = 0;
	bool bStartup = true;
	bool bSync    = true;

	m_SerialTimer.StartTimer();

	while( true )
	{
		if(  !m_bInitialized1 ) { m_pStatus->PrintStatus( Status::COLOR_ERROR, "Time Sync ZDA TCP 1 Lost Comms\n" ); }
		while( !m_bInitialized1 )
		{	
			m_bInitialized1 = InitUDPSocket1(); 
			if( m_bInitialized1 ) { m_pStatus->PrintStatus( "Time Sync ZDA TCP 1 comms initialized\n" ); }
		}
 		if(  !m_bInitialized2 ) { m_pStatus->PrintStatus( Status::COLOR_ERROR, "Time Sync ZDA TCP 2 Lost Comms\n" ); }
		while( !m_bInitialized2 )
		{	
			m_bInitialized2 = InitUDPSocket2(); 
			if( m_bInitialized2 ) { m_pStatus->PrintStatus( "Time Sync ZDA TCP 2 comms initialized\n" ); }
		} 
		// clear the buffer
		if( bStartup )
		{
			bStartup = false;
			do
			{
				ReadString( ZDAbuf, 1024, &len );
			} while( len != 0 );
		}

		//  $GPZDA,201530.00,04,07,2002,00,00*60rn
		//  12345678901234567892123456789312345678
		while( bSync )
		{
			// read single chars util we see a new line
			char cr = 0;
			if( ReadChar( &cr ) )
			{
				if( cr == '\n' ) 
				{
					bSync           = false;
					m_bInitialSync  = false;
					m_pStatus->PrintStatus( "completed sync...\n" );
					break;
				}
			}
			if( !m_bInitialSync && m_SerialTimer.GetElapsedTime() > 5.0 )
			{
				m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync, NO Serial ZDA present\n" );
				bSync = true;
			}
		}

		memset( ZDAbuf, 0, sizeof(ZDAbuf) );
		m_SerialTimer.StartTimer();

		// gather the ZDA message
		m_SerialTimer.StartTimer();
		len  = 0;
		len2 = 0;
		while( true )
		{
			ReadString( &ZDAbuf[len], ZDA_MESSAGE_SIZE-len, &len2 );
			len += len2;

			if( (int)len == ZDA_MESSAGE_SIZE )
			{
				ZDAbuf[ZDA_MESSAGE_SIZE-2] = 0;
				SendTimeZDA_UDP( ZDAbuf );
				m_SerialTimer.StartTimer();
				break;
			}
			if( (int)len > ZDA_MESSAGE_SIZE )
			{
				ZDAbuf[128] = 0;
				m_pStatus->PrintStatus( Status::STATUS_ERROR, "Read too many characters: %s\n", ZDAbuf ); 
				m_SerialTimer.StartTimer();
				bSync = true;
				break;
			}

			if( m_SerialTimer.GetElapsedTime() > 5.0 )
			{
				m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync, NO Serial ZDA present\n" );
				m_SerialTimer.StartTimer();
				bSync = true;
			}		
		}
	}

}

// simulate serial data collection - use current clock
void TimeSync::ThreadSim()
{
	char ZDAbuf[128];

	SysTime sysTime;
	sysTime.Update();

	// align to seconds
	int seconds = sysTime.Second();
	// align with seconds
	while( true) { sysTime.Update(); if( seconds != sysTime.Second() ) { break; } Sleep(5); }

	while( true ) 
	{
		// align to next second
		while( true) { sysTime.Update(); if( seconds != sysTime.Second() ) { break; } Sleep(5); }
		seconds = sysTime.Second();

		m_SerialTimer.StartTimer();
		memset( ZDAbuf, 0, sizeof(ZDAbuf) );

		// build the ZDA message
		// $GPZDA,hhmmss.ss,dd,mm,yyyy,xx,yy*CC
		// $GPZDA,201530.00,04,07,2002,00,00*60
		//	where:
		//		hhmmss    HrMinSec(UTC)
		//			  dd,mm,yyy Day,Month,Year
		//			  xx        local zone hours -13..13
		//			  yy        local zone minutes 0..59
		//			  *CC       checksum
		sprintf_s( ZDAbuf, 128, "GPZDA,%02d%02d%02d.%02d,%02d,%02d,%04d,00,00*00", 
						  sysTime.Hour(), sysTime.Minute(), sysTime.Second(), sysTime.Millisecond(),
						  sysTime.Day(), sysTime.Month(), sysTime.Year() );

		ZDAbuf[ZDA_MESSAGE_SIZE-2] = 0;
		SendTimeZDA_UDP( ZDAbuf );
		m_SerialTimer.StartTimer();

		if( m_SerialTimer.GetElapsedTime() > 5.0 )
		{
			m_pStatus->PrintStatus( Status::STATUS_ERROR, "Error: Time Sync, NO Serial ZDA present\n" );
			m_SerialTimer.StartTimer();
		}		
	}
}

bool TimeSync::SendTimeZDA_UDP( char *bufZDAmsg )
{
 	bool bRtn = false;
   int slen  = 0;

	if( m_bInitialized1 )
	{
		//send the message
		int sRtn = sendto( m_UPSSocket1, bufZDAmsg, strlen( bufZDAmsg ), 0, (struct sockaddr *)&m_sockaddr1, sizeof(m_sockaddr1) );	
		if( sRtn == SOCKET_ERROR )
		{
			closesocket(m_UPSSocket1);
			WSACleanup();				
			m_bInitialized1 = false;
		}
		else 
		{ 
			m_semTimeStr.Lock();
			sprintf_s( m_chTimeMessage, 128, bufZDAmsg );
			m_semTimeStr.Unlock();
	
			bRtn = true; 
		}
	}
	if( m_bInitialized2 )
	{
		//send the message
		int sRtn = sendto( m_UPSSocket2, bufZDAmsg, strlen( bufZDAmsg ), 0, (struct sockaddr *)&m_sockaddr2, sizeof(m_sockaddr2) );	
		if( sRtn == SOCKET_ERROR )
		{
			closesocket(m_UPSSocket2);
			WSACleanup();				
			m_bInitialized2 = false;
		}
		else 
		{ 
			m_semTimeStr.Lock();
			sprintf_s( m_chTimeMessage, 128, bufZDAmsg );
			m_semTimeStr.Unlock();
	
			bRtn = true; 
		}
	}
	return bRtn;
}