/* 
*	file: nikonD3Server.cpp 
*	author: Eric J Martin
*
*	copyright: MBARI 2013
*
*	Defines the entry point for the console application. 
*	Arguments to be read in are:
*
*		nikonD3server.exe <inifile>
*
*	<inifile> is the name and path if not in the same 
*	directory as the exe.
*
*/
#include "nikonD3Server.h"

/* 
*	Wrapper function to initialize socket based on ini loaded data. 
*/
BOOL SocketInitialize() {
	// Initialize Winsock
	char funcname[] = "SocketInitialize()";
	WSADATA wsaData;

	int nResult;

	nResult = WSAStartup(MAKEWORD(2,2), &wsaData);

	if (NO_ERROR != nResult)
	{
		logprintf(LOGFDS,"%s: Error occurred while executing WSAStartup().\n",funcname);
		return 1; //error
	}
	else
	{
		logprintf(LOGFDS,"%s: WSAStartup() successful.\n",funcname);
	}

	int    nPortNo = sTcpServerData.serverPort;

	struct sockaddr_in ServerAddress;

	//Create a socket
	ListenSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

	if (INVALID_SOCKET == ListenSocket) 
	{
		logprintf(LOGFDS,"%s: Error occurred while opening socket: %ld.\n",funcname, WSAGetLastError());
		return false;
	}
	else
	{
		logprintf(LOGFDS,"%s: socket() successful.\n",funcname);
	}

	//Cleanup and Init with 0 the ServerAddress
	ZeroMemory((char *)&ServerAddress, sizeof(ServerAddress));


	//Fill up the address structure
	ServerAddress.sin_family = AF_INET;
	ServerAddress.sin_addr.s_addr = INADDR_ANY; //WinSock will supply address
	ServerAddress.sin_port = htons(nPortNo);    //comes from commandline

	//Assign local address and port number
	if (SOCKET_ERROR == bind(ListenSocket, (struct sockaddr *) &ServerAddress, sizeof(ServerAddress))) 
	{
		closesocket(ListenSocket);

		logprintf(LOGFDS,"%s: Error occurred while binding.\n",funcname);
		return false;
	}
	else
	{
		logprintf(LOGFDS,"%s: bind() successful.\n",funcname);
	}

	//Make the socket a listening socket
	if (SOCKET_ERROR == listen(ListenSocket,SOMAXCONN))
	{
		closesocket(ListenSocket);

		logprintf(LOGFDS,"%s: Error occurred while listening.\n",funcname);
		return false;
	}
	else
	{
		logprintf(LOGFDS,"%s: listen() successful.\n",funcname);
	}


	// Make this a non-blocking socket;
	unsigned long iMode=1;
	ioctlsocket(ListenSocket,FIONBIO,&iMode);

	return true;
}

/*		
*	Set up the three FD sets used with select() with the sockets in the
*	connection list.  Also add one for the listener socket, if we have
*	one.
*/
void SetupFDSets(fd_set& fdRead, fd_set& fdWrite, 
				 fd_set& fdExcept, SOCKET ListeningSocket = INVALID_SOCKET) 
{
	FD_ZERO(&fdRead);
	FD_ZERO(&fdWrite);
	FD_ZERO(&fdExcept);

	// Add the listener socket to the read and except FD sets, if there
	// is one.
	if (ListeningSocket != INVALID_SOCKET) {
		FD_SET(ListeningSocket, &fdRead);
		FD_SET(ListeningSocket, &fdExcept);
	}

	// Add client connections, currently we are limited to one, so this
	// is marked for retirement.
	ConnectionList::iterator it = gConnections.begin();
	while (it != gConnections.end()) {

		// Copy status into the connection iterator
		FD_SET(it->sd, &fdRead);

		// Copy exception status into the connection iterator.
		FD_SET(it->sd, &fdExcept);

		++it;
	}
}

// BEGIN MUTEX FUNCTIONS

/*	
*	Overloaded function that returns the value of 
*	main loop status variable.
*/
bool inMainLoop() {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		retval = bInMainLoop;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;

}

/*	
*	Overloaded function that sets the value of 
*	main loop status variable. Also it returns the 
*	final set value.
*/
bool inMainLoop(bool setval) {
	DWORD dwWaitResult;
	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		bInMainLoop = setval;					
	}
	else setval = false;
	ReleaseMutex(statusMutex);
	//logprintf(LOGFDS,"inMainLoop(): Changing Value.\n");
	return setval;

}

/*	
*	Overloaded function that returns the value of 
*	camera logging status variable.
*/
bool cameraLogging() {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		retval = bCameraLogging;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;

}

/*	
*	Overloaded function that sets the value of 
*	RTS rs232 output from the falcon fanout.
*/

bool lasersPowered(bool setval) {
	DWORD dwWaitResult;
	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		bLasersPowered = setval;					
	}
	else setval = false;
	ReleaseMutex(statusMutex);
	return setval;
}

/*
*	Overloaded function that returns the value of
*	RTS rs232 output on the falcon fanout.
*/
bool lasersPowered() {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject(statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		retval = bLasersPowered;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;

}

/*
*	Overloaded function that sets the value of
*	camera logging status variable.
*/

bool cameraLogging(bool setval) {
	DWORD dwWaitResult;
	dwWaitResult = WaitForSingleObject(statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		bCameraLogging = setval;
	}
	else setval = false;
	ReleaseMutex(statusMutex);
	return setval;
}

/*
*	Overloaded function that returns the value of 
*	camera connection status variable.
*/
bool cameraConnected() {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		retval = bCameraConnected;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;

}

/*	
*	Overloaded function that sets the value of 
*	camera connection status variable. It also 
*	returns the final value.
*/
bool cameraConnected(bool setval) {
	DWORD dwWaitResult;
	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		bCameraConnected = setval;					
	}
	else setval = false;
	ReleaseMutex(statusMutex);
	return setval;
}


/*
*	Set the interval at which to trigger the camera in a mutex 
*	safe fashion. This gets called from the trigger thread but
*	is set in the camera thread.
*/

bool getInterval(unsigned long &ulInterval) {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		ulInterval = sTriggerServerData.triggerInterval;
		retval = true;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;
}

/*
*	Set the interval at which to trigger the camera in a mutex 
*	safe fashion. This gets called from the camera thread but
*	is used in the Trigger thread.
*/
bool setInterval(unsigned long ulInterval) {
	bool retval;
	DWORD dwWaitResult;

	dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		sTriggerServerData.triggerInterval = ulInterval;
		retval = true;
	}
	else retval = false;

	ReleaseMutex(statusMutex);
	return retval;
}

// END MUTEX FUNCTIONS

/*	Trigger function to output triggers on a timed cycle and assign a 
*	filename based on system clock time. No NIKON IO should take place in
*	this thread. 
*/
DWORD WINAPI TriggerWorkerThread(LPVOID lpParam) {
	const char funcname[] = "TriggerWorkerThread()";

	//remove these from nikon thread 

	double dElapsedMS;
	LARGE_INTEGER liNow, liLastTrigger, liFreq;
	char sImageName[128];
	char sDTOrig[128];
	char sDTOrigSS[128];
	char sAbsoluteFileName[128];
	DWORD dwWaitResult;
	unsigned long ulLocalInterval;
	SYSTEMTIME utcDateTime;
	bool _bLasersOn, _bLasersTemp;
	
	const long long _SECOND = -10000000LL;

	//Initialize timers // USE MS Precision timers
	QueryPerformanceFrequency(&liFreq);
	QueryPerformanceCounter(&liNow);
	QueryPerformanceCounter(&liLastTrigger);

	// Initialize trigger counter
	sTriggerServerData.ulTriggerCount = 0;


	//SET UP SERIAL PORT for Trigger
	// Initialize serial port object
	tspTrigger = new triggerSerialPort(sTriggerServerData.triggerPortName);
	if (!tspTrigger->isConnected()) {
		logprintf(LOGFDS,"%s: ERROR Cannot open serial port %s\n",funcname, sTriggerServerData.triggerPortName);
		inMainLoop(false);
	}

	// Change port settings for Trigger
	if (!tspTrigger->changePortSettings(sTriggerServerData.triggerBaudRate,
		(BYTE)sTriggerServerData.triggerBits, sTriggerServerData.triggerParity[0], (BYTE)sTriggerServerData.triggerStop)) {
			logprintf(LOGFDS, "%s: ERROR Cannot set serial port parameters.\n",funcname);
			inMainLoop(false);
	}

	logprintf(LOGFDS,"%s: Trigger serial port configured and open, %s %i kbps %i%s%i \n",funcname,
		sTriggerServerData.triggerPortName, sTriggerServerData.triggerBaudRate, 
		sTriggerServerData.triggerBits,sTriggerServerData.triggerParity, sTriggerServerData.triggerStop);

	//SET UP SERIAL PORT for Laser Power
	// Initialize serial port object
	tspLaser = new triggerSerialPort(sTriggerServerData.laserPortName);
	if (!tspLaser->isConnected()) {
		logprintf(LOGFDS, "%s: ERROR Cannot open serial port %s\n", funcname, sTriggerServerData.laserPortName);
		inMainLoop(false);
	}

	// Change port settings for Laser
	if (!tspLaser->changePortSettings(sTriggerServerData.laserBaudRate,
		(BYTE)sTriggerServerData.laserBits, sTriggerServerData.laserParity[0], (BYTE)sTriggerServerData.laserStop)) {
		logprintf(LOGFDS, "%s: ERROR Cannot set serial port parameters.\n", funcname);
		inMainLoop(false);
	}

	logprintf(LOGFDS, "%s: Laser power serial port configured and open, %s %i kbps %i%s%i \n", funcname,
		sTriggerServerData.laserPortName, sTriggerServerData.laserBaudRate,
		sTriggerServerData.laserBits, sTriggerServerData.laserParity, sTriggerServerData.laserStop);

	// Power off the lasers if on. 
	tspLaser->setRTS(0);
	lasersPowered(false);
	_bLasersOn = false;

	// Create a waitable timer.
	HANDLE hTimer = NULL;
	LARGE_INTEGER liDueTime;

	hTimer = CreateWaitableTimer(NULL,TRUE,NULL);
	if (hTimer == NULL) {
		logprintf(LOGFDS,"%s: CreateWaitableTimer Failed (%d)\n",funcname,GetLastError());
		return -1;
	}

	while (inMainLoop()) {

		//test if digital out should be high
		_bLasersTemp = lasersPowered();
		if (_bLasersTemp != _bLasersOn) {
			if (tspLaser->setRTS(_bLasersTemp? 1 : 0)) {
				_bLasersOn = _bLasersTemp;
				logprintf(LOGFDS, "%s: Changed state of laser power to %i.\n", funcname, _bLasersOn?1:0);
			}
			else {
				logprintf(LOGFDS, "%s: ERROR failed to change state of laser power to %i.\n", funcname, _bLasersTemp?1:0);
			}
		}


		// We're not connected or not logging, so we're not triggering.
		if (!cameraLogging() || !cameraConnected()) {
			Sleep(100); // Slow down the idle thread.
			continue;
		}

		// Grab Data from the NikonWorkerThread
		getInterval(ulLocalInterval);

		if (ulLocalInterval > 0 ) {
			liDueTime.QuadPart = ulLocalInterval * _SECOND / 1000 ;
			//printf("setting interval to %i ms [%i]\n",ulLocalInterval,liDueTime.QuadPart);
		}

		// Set the timer
		if (!SetWaitableTimer(hTimer, &liDueTime, 0, NULL, NULL, 0)) {
			logprintf(LOGFDS,"%s: ERROR SetWaitableTimer Failed (%d)\n",funcname,GetLastError());
			return -2;
		}

		// Block On the timer.
		if (WaitForSingleObject(hTimer, INFINITE) != WAIT_OBJECT_0) 
			logprintf(LOGFDS,"%s: ERROR WaitForSingleObject Failed (%d)\n",funcname,GetLastError());
		//else 
		//logprintf(LOGFDS,"%s: Timer Signaled\n",funcname);

		//test if we're logging
		if (!cameraLogging()) {
			continue;
		}

		// Entering time sensitive controls
		if (!QueryPerformanceCounter(&liNow)) 
			logprintf(LOGFDS,"%s: ERROR Performance counter failed.\n", funcname);


		// Check time for message acquisition, convert to milliseconds
		dElapsedMS =((double)((liNow.QuadPart - liLastTrigger.QuadPart)*1000 / liFreq.QuadPart));


		// Test if we're logging and enough time has passed to take an image. 
		logprintf(LOGFDS,"%s: Sending Trigger after %6.0f ms.\n",funcname,dElapsedMS);

		// Update the filename string using the current time of the trigger.
		GetSystemTime(&utcDateTime);

		//Filename
		sprintf(sImageName,"BIAUV_%04d%02d%02d_%02d.%02d.%02d.%03d\0",
			utcDateTime.wYear,utcDateTime.wMonth,utcDateTime.wDay,
			utcDateTime.wHour,utcDateTime.wMinute,utcDateTime.wSecond,
			utcDateTime.wMilliseconds);

		sprintf(sAbsoluteFileName,"%s%s\0",sNikonServerData.cameraFilePrefix,sImageName);

		//Exif Data
		sprintf(sDTOrig,"%04d:%02d:%02d %02d:%02d:02d\0",
			utcDateTime.wYear,utcDateTime.wMonth,utcDateTime.wDay,
			utcDateTime.wHour,utcDateTime.wMinute,utcDateTime.wSecond);
		sprintf(sDTOrigSS, "%03d\0",utcDateTime.wMilliseconds);


		logprintf(LOGFDS,"%s: Next File: %s.jpg\n",funcname,sAbsoluteFileName);

		// Now set the filename to correspond to the last saved name. This is copied into the global by the nikon thread, where it will be grabbed in Callback.c
		// Grab Mutex for stringQ
		dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
		if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
			if (!stringQ->enqueueString(sAbsoluteFileName,strlen(sAbsoluteFileName),sDTOrig,sDTOrigSS)) {
				logprintf(LOGFDS,"%s: ERROR Cannot Enqueue filename. Queue is full.\n",funcname,sAbsoluteFileName);
			}
			else { // we have a name to trigger so lets

				tspTrigger->sendTrigger();
				QueryPerformanceCounter(&liLastTrigger);
				sTriggerServerData.ulTriggerCount++;
			}
		}
		else {
			logprintf(LOGFDS,"%s: Error getting status mutex\n",funcname);
		}
		ReleaseMutex(statusMutex);


	}

	tspTrigger->closeSerialPort();

	logprintf(LOGFDS,"%s: Thread Execution Complete.\n", funcname);
	return 0xF;
}

/*	
*	Worker function for NikonD3 Camera. This thread manages all 
*	IO related to the camera and its triggering. All interactions
*	with this thread are adding messages to the queue. No direct 
*	IPC is planned. 
*/
DWORD WINAPI NikonWorkerThread(LPVOID lpParam ) {

	const char funcname[] ="NikonWorkerThread()";
	char msgbuff[ulMaxNikonTotalPacketSize_c];
	int msglen, iAbsNameLen;
	bool bGotMessage, bGotItem;
	int msgid;
	DWORD dwWaitResult;

	logprintf(LOGFDS,"%s: Starting Camera Control Thread\n",funcname);

	// CONNECT TO CAMERA MODULE
	nhCamera = new nikonHandler();

	// CONNECT CAMERA
	if (cameraConnected(nhCamera->connectCamera())) 
		logprintf(LOGFDS, "%s: Connected to camera on thread start.\n",funcname);
	else 
		logprintf(LOGFDS, "%s: ERROR Failed to connect to camera on thread start.\n",funcname);

	//Initialize counters
	sNikonServerData.ulImageCount = 0;

	// MAIN THREAD LOOP
	while (inMainLoop()) {
		Sleep(10);
		// Check if there are any messages to process in the queue, process accordingly.
		dwWaitResult = WaitForSingleObject( msgMutex, MAXMUTEXWAIT);

		if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
			//Test for messages from the server
			bGotMessage = msgQ->retrieveRecvPacket(msgbuff,msglen,msgid);
		}
		else {
			logprintf(LOGFDS,"NikonWorkerThread():: Error getting msg mutex\n");
		}
		ReleaseMutex(msgMutex);

		if (bGotMessage) { // There was a message in the queue, we're going to process it here. 
			processNikonMessage(msgbuff,msglen);
		} 

		// test connection status
		if (!cameraConnected()) {
			continue;
		}     

		// Check for an image regardless of logging status 
		if (true) {
			bGotItem = nhCamera->itemSelect();
			if ( nhCamera->itemSelected() ) { 				// Got item

				// Now set the filename to correspond to the last saved name. This is copied into the global, where it will be grabbed in Callback.c
				// Grab Mutex for stringQ
				dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
				if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
					if (!stringQ->retrieveString(sNextFileName,iAbsNameLen,sNextDTOrig,sNextDTOrigSS)) {
						sprintf(sNextFileName,"%s%s",sNikonServerData.cameraFilePrefix,"IMG");
						logprintf(LOGFDS,"%s: ERROR: No Image name avaiable, using generic.\n",funcname);
					}
				}
				else {
					logprintf(LOGFDS,"NikonWorkerThread():: Error getting status mutex\n");
				}
				ReleaseMutex(statusMutex);

				// Download Selected Image
				if (nhCamera->itemDownloadDefault()) {
					logprintf(LOGFDS,"%s: GOT FILE [%s]\n",funcname,sLastFileName);
					sNikonServerData.ulImageCount++;
					WriteFileToLog(false);

				}
				else {
					logprintf(LOGFDS,"%s: Error: failed download image.\n",funcname);
				}
			}
			else if ( bGotItem == true ) { // No item but we're talking to the camera
			}
			else {
				logprintf(LOGFDS,"%s: ERROR: Cannot communicate with the camera for item select.\n",funcname);

			}
		}

		// Check Trigger number is not more than one spot ahead of the frame count.
		// Enter Mutex - Potentially move to its own function.

		//if (sNikonServerData.ulTriggerCount > sNikonServerData.ulImageCount+1) {
		/*logprintf(LOGFDS,"%s: ERROR Image count is %l frames behind the trigger count.",funcname,
		(sNikonServerData.ulTriggerCount-sNikonServerData.ulImageCount)
		);
		*/
		//}
		//else if (sNikonServerData.ulTriggerCount < sNikonServerData.ulImageCount) {
		//	//logprintf(LOGFDS,"%s, ERROR Image count is ahead of trigger count, unaccounted images\n", funcname);
		//}
	}

	logprintf(LOGFDS,"%s: Thread Execution Complete.\n", funcname);
	return 0xE;
}

/*		processNikonMessage()
*	Process a message from the tcp client appropriately.
*/
void processNikonMessage(char* msgbuff, unsigned long msglen) {
	const char funcname[] = "processNikonMessage()";
	bool bMsgReady = false;
	bool bRet = false;
	long lValue;
	char sValue[NIKON_MESSAGE_STRING_LENGTH] = "\0";
	DWORD dwWaitResult;

	//Potential replies or input
	NikonMessageUARTType sUart;

	// Print the values of the bytes 
	/*printf("MSG: ");
	for (unsigned long i=0; i<msglen; i++) {
	printf("%X ",msgbuff[i]);
	}
	printf("\n");*/

	//Recast the message into a header and move the point into the message class
	NikonMessage nikonmsg, nmReply;
	NikonMessageHeaderType * pHeader = (NikonMessageHeaderType *) (msgbuff);
	if (nikonmsg.SetHeader(pHeader)) {					
		switch (nikonmsg.NikonCameraMessage()) {

		case NIKON_MESSAGE_NONE:		// Null message used to test communication
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET || nikonmsg.NikonCommand()==NIKON_COMMAND_SET ){
				logprintf(LOGFDS,"%s: received NIKON_MESSAGE_NONE\n",funcname);
				nmReply.SetMessage(NIKON_MESSAGE_NONE);
				bMsgReady = true;
			}
			break;

		case NIKON_MESSAGE_SYSTEM_RESET:	// Set to have server reload default config: this will stop acq
			//Do nothing on a get
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				//TODO
			}
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				//TODO 
				nmReply.SetMessage(NIKON_MESSAGE_SYSTEM_RESET,NIKON_COMMAND_GET);
				bMsgReady = true;
			}
			break;

		case NIKON_MESSAGE_CONNECT:			// Attempt to connect (1) or disconnect (0) the PTP connection to the dsc (NikonMessageLongType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_CONNECT\n",funcname);

			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				if ((long) nikonmsg == 1 && !cameraConnected()) {
					// try to connect
					nhCamera->connectCamera();
					if (cameraConnected(nhCamera->isConnected()))
						logprintf(LOGFDS,"%s: Camera connected.\n",funcname);
					else 
						logprintf(LOGFDS,"%s: ERROR Camera failed to connect.\n",funcname);
				}
				else if ((long) nikonmsg == 0 && cameraConnected()) {
					// try to disconnect
					nhCamera->closeModule();
					// see if it worked and set the global
					if (cameraConnected(nhCamera->isConnected()))
						logprintf(LOGFDS,"%s: Camera disconnected.\n",funcname);
					else if ( nhCamera->isConnected() )
						logprintf(LOGFDS,"%s: ERROR Camera failed to disconnect.\n",funcname);
					Sleep(50);
				}

			}
			//Either way reply with result
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				lValue =  (cameraConnected())? 1 : 0 ;
				nmReply.SetMessage(NIKON_MESSAGE_CONNECT,NIKON_COMMAND_GET,(unsigned char *) &lValue, (unsigned long) sizeof(lValue));
				bMsgReady = true;
			}
			break;

			// THIS IS NOW RETIRED.
		case NIKON_MESSAGE_PRETRIGGER:		
			// Enable (1) or Disable (0) Pretrigger image staggering for faster acquisition (NikonMessageLongType)
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				if ((long) nikonmsg == 1 ) {
					sNikonServerData.cameraPreTrigger = true;
				}
				else if ((long) nikonmsg == 0 && sNikonServerData.cameraPreTrigger) {
					sNikonServerData.cameraPreTrigger = false;
				}
			}
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				lValue = sNikonServerData.cameraPreTrigger ? 1 : 0;
				nmReply.SetMessage(NIKON_MESSAGE_PRETRIGGER,NIKON_COMMAND_GET,(unsigned char *) &lValue, (unsigned long) sizeof(lValue));
				bMsgReady = true;
			}
			break;

		case NIKON_MESSAGE_ACQUISITION:		
			// Enable (1) or Disable (0) aquisition (NikonMessageLongType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_ACQUISITION\n",funcname);
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				if ((long) nikonmsg == 1 ) {
					if (cameraLogging( true))
						logprintf(LOGFDS,"%s: Started acquisition.\n",funcname);
					else 
						logprintf(LOGFDS,"%s: ERROR Failed to start acquisition.\n",funcname);
				}
				else if ((long) nikonmsg == 0 ) {
					if (cameraLogging(false))
						logprintf(LOGFDS,"%s: Stopped acquisition.\n",funcname);
					else if ( cameraLogging() )  
						logprintf(LOGFDS,"%s: ERROR Failed to stop acquisition.\n",funcname);
				}
			}
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				lValue = cameraLogging() ? 1 : 0;
				nmReply.SetMessage(NIKON_MESSAGE_ACQUISITION,NIKON_COMMAND_GET,(unsigned char *) &lValue, (unsigned long) sizeof(lValue));
				bMsgReady = true;
			}
			break;

		case NIKON_MESSAGE_LASERPOWER:
			// Enable (1) or Diable (0) a digital output using a serial rs232 RTS line
			logprintf(LOGFDS, "%s: received NIKON_MESSAGE_ACQUISITION\n", funcname);
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET) {
				if ((long)nikonmsg == 1) {
					if (lasersPowered(true))
						logprintf(LOGFDS, "%s: Lasers on.\n", funcname);
					else
						logprintf(LOGFDS, "%s: ERROR Failed to set laser power state.\n", funcname);
				}
				else if ((long)nikonmsg == 0) {
					if (lasersPowered(false))
						logprintf(LOGFDS, "%s: Lasers off.\n", funcname);
					else if (cameraLogging())
						logprintf(LOGFDS, "%s: ERROR Failed to set laser power state.\n", funcname);
				}
			}

			// NOT IMPLEMENTED 
		case NIKON_MESSAGE_CONFIGFILE:		
			// The filename of the configuration for the server (NikonMessageStringType)
			// TODO implement file change
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				strcpy(sValue,(char*)sConfigFile);
				nmReply.SetMessage(NIKON_MESSAGE_CONFIGFILE,NIKON_COMMAND_GET,(unsigned char *) sValue, (unsigned long) NIKON_MESSAGE_STRING_LENGTH+1);
				bMsgReady = true;
			}
			break;
			/* TODO
			case NIKON_MESSAGE_TIME:			
			// Get the time of the clock on the server: set forces a sync with the camera
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET){
			}
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {

			}

			break;
			*/

		case NIKON_MESSAGE_INTERVAL:		
			// The interval on which to trigger frames in milliseconds (NikonMessageLongType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_INTERVAL \n",funcname);
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET ) {
				if (setInterval( (long) nikonmsg))
					logprintf(LOGFDS,"%s: Interval set to %li.\n",funcname,(long) nikonmsg);
				else 
					logprintf(LOGFDS,"%s: Failed to change interval.\n",funcname);
			}
			if (nikonmsg.NikonCommand() == NIKON_COMMAND_SET || nikonmsg.NikonCommand() == NIKON_COMMAND_GET) {
				getInterval((unsigned long &) lValue);
				nmReply.SetMessage(NIKON_MESSAGE_INTERVAL,NIKON_COMMAND_GET,
					(unsigned char *) &lValue,
					(unsigned long) sizeof(lValue));
				bMsgReady = true;
			}
			break;

		case NIKON_MESSAGE_FILEPREFIX:		
			// Get/set the file prefix for image acquisition (NikonMessageStringType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_FILEPREFIX \n",funcname);
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				strcpy(sNikonServerData.cameraFilePrefix,(char *) nikonmsg);
				logprintf(LOGFDS,"%s: Set File prefix to: %s\n",funcname,sNikonServerData.cameraFilePrefix);
				nmReply.SetMessage(NIKON_MESSAGE_FILEPREFIX,NIKON_COMMAND_GET,(unsigned char *) sNikonServerData.cameraFilePrefix, (unsigned long) NIKON_MESSAGE_STRING_LENGTH+1);
				bMsgReady = true;
			}
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET || nikonmsg.NikonCommand()==NIKON_COMMAND_SET){
				strcpy(sValue,sNikonServerData.cameraFilePrefix);
				nmReply.SetMessage(NIKON_MESSAGE_FILEPREFIX,NIKON_COMMAND_GET,(unsigned char *) sValue, (unsigned long) NIKON_MESSAGE_STRING_LENGTH+1);
				bMsgReady = true;
			}

			break;
			/* TODO
			case NIKON_MESSAGE_STATUS:			
			// Get NikonMessageStatusType of 
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET){
			//TODO

			}
			*/
			break;

		case NIKON_MESSAGE_IMAGECOUNT:		
			// Get the current frame number (NikonMessageLongType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_IMAGECOUNT \n",funcname);

			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET || nikonmsg.NikonCommand()==NIKON_COMMAND_SET){
				lValue = sNikonServerData.ulImageCount;
				nmReply.SetMessage(NIKON_MESSAGE_IMAGECOUNT,NIKON_COMMAND_GET,(unsigned char *) &lValue,(unsigned long) sizeof(lValue));
				bMsgReady = true;
			}


			break;

		case NIKON_MESSAGE_TRIGGERCOUNT:		
			// Get the current frame number (NikonMessageLongType)
			logprintf(LOGFDS,"%s: received NIKON_MESSAGE_TRIGGERCOUNT \n",funcname);

			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET || nikonmsg.NikonCommand()==NIKON_COMMAND_SET){
				dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
				if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
					lValue = sTriggerServerData.ulTriggerCount;
					nmReply.SetMessage(NIKON_MESSAGE_TRIGGERCOUNT,NIKON_COMMAND_GET,(unsigned char *) &lValue,(unsigned long) sizeof(lValue));
					bMsgReady = true;
				} else {
					logprintf(LOGFDS,"%s: Error: Failed to get status mutex.\n",funcname);
				}

				ReleaseMutex(statusMutex);
			}
			break;

			// TRIGGER SERIAL PORT OPTIONS NOT IMPLEMENTED
		case NIKON_MESSAGE_TRIGGERCONFIG:
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_GET || nikonmsg.NikonCommand()==NIKON_COMMAND_SET){
				// Populate response
				dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
				if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
					strcpy((char *) sUart.name,(char *) sTriggerServerData.triggerPortName);
					sUart.baudRate = sTriggerServerData.triggerBaudRate;
					sUart.bits = sTriggerServerData.triggerBits;
					sUart.parity = (unsigned char) sTriggerServerData.triggerParity[0];
					sUart.stop = sTriggerServerData.triggerStop;
					//Stuff Message
					nmReply.SetMessage(NIKON_MESSAGE_TRIGGERCONFIG,NIKON_COMMAND_GET,(unsigned char *)&sUart, (unsigned long) sizeof(NikonMessageUARTType));
					bMsgReady = true;
				} else {
					logprintf(LOGFDS,"%s: Error: Failed to get status mutex.\n",funcname);
				}

				ReleaseMutex(statusMutex);

			}
			if (nikonmsg.NikonCommand()==NIKON_COMMAND_SET) {
				//First validate input
				/*
				sUart = (NikonMessageUARTType *) nikonmsg.Message();
				if ((msgUart->baudRate != 600 & msgUart->baudRate != 1200 & msgUart->baudRate != 4800 &
				msgUart->baudRate != 9600 & msgUart->baudRate != 19200 ) | (
				msgUart->bits != 8 & msgUart->bits != 7 ) | (
				msgUart->stop != 0 & msgUart->stop != 1 ) | (
				msgUart->parity != 'N' & msgUart->parity != 'E' & msgUart->parity != 'O') )
				{
				logprintf(LOGFDS, "%s: ERROR, message supplied baudrate is unsupported.\n",funcname);
				nmReply.SetMessage(NIKON_ERROR_INVALID,NIKON_COMMAND_ERROR);
				bMsgReady = true;
				break;
				}
				*/
				// TODO Finish implementing a serial port change. 

			}
			break;

		default: 
			logprintf(LOGFDS,"%s: received UNKNOWN MESSAGE \n",funcname);
			nmReply.SetMessage(NIKON_ERROR_UNKNOWN,NIKON_COMMAND_ERROR);
			bMsgReady = true;
		}

	}
	else {
		logprintf(LOGFDS, "%s: ERROR, Malformed message received.\n",funcname);
	}



	if (bMsgReady) { // Lets enqueue the message
		// Now Copy that code into the message que. 

		dwWaitResult = WaitForSingleObject( msgMutex, MAXMUTEXWAIT);
		if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
			msgQ->enqueueSendPacket((char *) nmReply.Message(), nmReply.MessageSize(),NULL);
		} else {
			logprintf(LOGFDS,"%s: Error: Failed to enqueue message.\n",funcname);
		}

		ReleaseMutex(msgMutex);
	}

}

/* 
*	main execution loop	
*	reads in INI file, spawns threads and runs the tcp server. 
*/
int _tmain(int argc, _TCHAR* argv[])
{
	const char funcname[] = "nikonD3Server:main()";
	BOOL bRet;
	signal(SIGINT, siginthandler);

	// Initialize Mutex
	statusMutex = CreateMutex( 
		NULL,              // default security attributes
		FALSE,             // initially not owned
		NULL);             // unnamed mutex

	if (statusMutex == NULL) 
	{
		logprintf(LOGFDS, "%s: ERROR CreateMutex: %d\n", funcname, GetLastError());
		goto cleanup;
	}

	// Initialize Mutex
	msgMutex = CreateMutex( 
		NULL,              // default security attributes
		FALSE,             // initially not owned
		NULL);             // unnamed mutex

	if (msgMutex == NULL) 
	{
		logprintf(LOGFDS, "%s: ERROR CreateMutex: %d\n", funcname, GetLastError());
		goto cleanup;
	}

	strcpy(sConfigFile,".\\nikonD3Server.ini");

	// LOAD INI Initial Settings
	if (argc > 1) {
		// LOAD IN INI FILE
		strcpy(sConfigFile, argv[1]);
		logprintf(LOGFDS,"%s: Loading INI File (%s)...\n",funcname,sConfigFile);
	}
	else
	{
		logprintf(LOGFDS,"%s: No INI file given, using defaults.\n", funcname);
	}

	bRet = importIniSettings(sConfigFile);

	printIniSettings();

	if(!bRet) {
		logprintf(LOGFDS,"%s: ERROR importing INI settings, exiting...\n",funcname);
		goto cleanup;
	}

	//Try opening the log file for appending. 
	if (strncmp(sTcpServerData.logFileName,"stdout",6)!=0) {
		FILE * fdLog = fopen(sTcpServerData.logFileName,"a");
		if (fdLog == NULL) {
			logprintf(LOGFDS,"%s: Failed to open log for writing. [%s]\n", funcname,sTcpServerData.logFileName);
		}
		else {
			logprintf(LOGFDS,"%s: Now Writing to Server Log. [%s]\n", funcname,sTcpServerData.logFileName);
			LOGFDS = fdLog;
			logprintf(LOGFDS,"%s: Start of Server Log. [%s]\n", funcname,sTcpServerData.logFileName);
			printIniSettings();
		}
	}

	//Set Logging status 
	bCameraLogging = false;
	bInMainLoop = true;

	// Start Threads for Camera work and for TCP Server to Service Requests.
	DWORD  dwNikonThreadID,dwTriggerThreadID;

	// Initialize Message Queue
	msgQ = new nikonServerMessageQueue();

	// Initialize Filename Queue
	//Filename Queue for staggered acquisition/download.
	stringQ = new stringQueue();

	// Camera Thread Start
	camThread = CreateThread(
		NULL,	//default security
		0,		//default stack size
		NikonWorkerThread,
		NULL,	//no thread function arguments
		0,		//default creation flags
		&dwNikonThreadID); //receive thread ID

	if (dwNikonThreadID == NULL) {
		logprintf(LOGFDS,"%s: ERROR Failed to create NikonWorkerThread\n",funcname);
		goto cleanup;
	}

	// Trigger Thread Start
	trigThread = CreateThread(
		NULL,	//default security
		0,		//default stack size
		TriggerWorkerThread,
		NULL,	//no thread function arguments
		0,		//default creation flags
		&dwTriggerThreadID); //receive thread ID

	if (dwTriggerThreadID == NULL) {
		logprintf(LOGFDS,"%s: ERROR Failed to create TriggerWorkerThread\n",funcname);
		goto cleanup;
	}


	// TCP Server Thread Start

	if (!SocketInitialize()){
		logprintf(LOGFDS,"%s ERROR Failed to initialize socket.\n",funcname);
		goto cleanup;
	}

	//Server main loop. 
	while (inMainLoop()) {
		AcceptConnections(ListenSocket);
	}

	// We reached the end of the main loop successfully. 

cleanup:
	// Program Termination

	siginthandler(0);

	// Clean Up Camera Thread
	WaitForSingleObject(camThread, INFINITE);
	CloseHandle(camThread);

	// Clean Up Trigger Thread
	WaitForSingleObject(trigThread, INFINITE);
	CloseHandle(trigThread);

	// Close Socket
	try {
		closesocket(ListenSocket);
	}
	catch (int e) {
		logprintf(LOGFDS,"%s: ERROR closing listening socket... [ERRNO: %i]\n",funcname, e);
	}
	WSACleanup();

	logprintf(LOGFDS, "%s: End of program.\n",funcname);

	// Close Log File
	if (LOGFDS != stdout) {
		fclose(LOGFDS);
	}


	fprintf(stdout, "\nPress any key to exit...\n");
	while (!_kbhit()) {}

	return 0;
}

/* 
*	Print ini settings to global log 
*/
void printIniSettings(void) {

	logprintf(LOGFDS, "NIKON D3 SERVER SETTINGS\n");
	logprintf(LOGFDS, "-----------------------------\n");
	logprintf(LOGFDS, "| serverPort:        %i\n",sTcpServerData.serverPort);
	logprintf(LOGFDS, "| serverMaxUsers:    %i\n",sTcpServerData.serverMaxUsers);
	logprintf(LOGFDS, "| logfilename:		  %s\n",sTcpServerData.logFileName);
	logprintf(LOGFDS, "| cameraPreTrigger:  %i\n",sNikonServerData.cameraPreTrigger);
	logprintf(LOGFDS, "| cameraFilePrefix:  %s\n",sNikonServerData.cameraFilePrefix);
	logprintf(LOGFDS, "| cameraInterval:    %i\n",sTriggerServerData.triggerInterval);
	logprintf(LOGFDS, "| triggerPortName:   %s\n",sTriggerServerData.triggerPortName);
	logprintf(LOGFDS, "| triggerBaudRate:   %i\n",sTriggerServerData.triggerBaudRate);
	logprintf(LOGFDS, "| triggerBits:       %i\n",sTriggerServerData.triggerBits);
	logprintf(LOGFDS, "| triggerParity:     %s\n",sTriggerServerData.triggerParity);
	logprintf(LOGFDS, "| triggerStop:       %i\n",sTriggerServerData.triggerStop);
	logprintf(LOGFDS, "| lasersPortName:   %s\n", sTriggerServerData.laserPortName);
	logprintf(LOGFDS, "| laserBaudRate:   %i\n", sTriggerServerData.laserBaudRate);
	logprintf(LOGFDS, "| laserBits:       %i\n", sTriggerServerData.laserBits);
	logprintf(LOGFDS, "| laserParity:     %s\n", sTriggerServerData.laserParity);
	logprintf(LOGFDS, "| laserStop:       %i\n", sTriggerServerData.laserStop);
	logprintf(LOGFDS, "-----------------------------\n");

}

/*
*	Import settings from a standard windows INI file and 
*	emplace them into global structures. This can only be 
*	run at the startup of the server. 
*/
BOOL importIniSettings(_TCHAR * cFname) {
	const char funcname[] = "importIniSettings()";
	DWORD dSize;
	const DWORD dResultSize = 255;
	CHAR  cResult[dResultSize];

	dSize = GetPrivateProfileSectionNames(cResult,dResultSize,cFname);

	if (dSize <= 0) {
		logprintf(LOGFDS, "%s: ERROR reading file (0x%X).\n",funcname, GetLastError());
		return false;
	}

	// Tcp Server INI
	sTcpServerData.serverPort = GetPrivateProfileInt("server","port", 2020, cFname);
	sTcpServerData.serverMaxUsers = GetPrivateProfileInt("server","maxclients", 2, cFname);
	GetPrivateProfileString("server","logfilename", "stdout", sTcpServerData.logFileName,dResultSize,cFname);


	// Camera INI
	sNikonServerData.cameraPreTrigger = GetPrivateProfileInt("camera","pretrigger", 0, cFname)==1?true:false; //TODO REMOVE
	GetPrivateProfileString("camera","fileprefix", "AUV_", sNikonServerData.cameraFilePrefix,dResultSize,cFname);

	// Trigger INI
	GetPrivateProfileString("trigger","serialport", "COM1", sTriggerServerData.triggerPortName,dResultSize,cFname);
	sTriggerServerData.triggerBaudRate= GetPrivateProfileInt("trigger","baudrate", 1200, cFname);
	sTriggerServerData.triggerBits = GetPrivateProfileInt("trigger","bits", 8, cFname);
	GetPrivateProfileString("trigger","parity", "N",(CHAR*) sTriggerServerData.triggerParity,dResultSize,cFname);
	sTriggerServerData.triggerStop = GetPrivateProfileInt("trigger","stop", 1, cFname);
	sTriggerServerData.triggerInterval= GetPrivateProfileInt("camera","interval", 1000, cFname);

	// Laser Relay INI
	GetPrivateProfileString("laserpower", "serialport", "COM1", sTriggerServerData.laserPortName, dResultSize, cFname);
	sTriggerServerData.laserBaudRate = GetPrivateProfileInt("laserpower", "baudrate", 1200, cFname);
	sTriggerServerData.laserBits = GetPrivateProfileInt("laserpower", "bits", 8, cFname);
	GetPrivateProfileString("laserpower", "parity", "N", (CHAR*)sTriggerServerData.laserParity, dResultSize, cFname);
	sTriggerServerData.laserStop = GetPrivateProfileInt("laserpower", "stop", 1, cFname);
	
	return true;
}

/*
*	This function will loop on while creating a new 
*	thread for each client connection
*/
void AcceptConnections(SOCKET ListenSocket)
{
	sockaddr_in ClientAddress;
	int nClientLength = sizeof(ClientAddress);
	fd_set fdRead;	// Sockets waiting to be read
	fd_set fdWrite; //set of sockets ready for IO
	fd_set fdExcept; //
	struct timeval sTimeout;
	int retVal, iBytesWritten;

	// set up timeout for select operations
	sTimeout.tv_sec = 1;
	sTimeout.tv_usec = 0;

	// This is the main loop for processing messages
	while (inMainLoop()) 
	{
		SetupFDSets(fdRead, fdWrite, fdExcept, ListenSocket);
		//Accept remote connection attempt from the client

		// set up timeout for select operations
		sTimeout.tv_sec = 0;
		sTimeout.tv_usec = 1000;

		retVal = select(0,&fdRead, &fdWrite, &fdExcept, &sTimeout);

		if (0 == retVal) {//Nothing returned
			//continue;
		}
		else if (SOCKET_ERROR == retVal) {
			logprintf(LOGFDS,"AcceptConnections(): ERROR select: %i\n", WSAGetLastError());
			continue;
		}

		// Start Processing Sockets

		//The ListenerSocket
		if (FD_ISSET(ListenSocket, &fdRead)){
			SOCKET Socket = accept(ListenSocket, (sockaddr*)&ClientAddress, &nClientLength);

			if (INVALID_SOCKET == Socket)
			{
				printf("AcceptConnections(): Error occurred while accepting socket: %ld.\n", WSAGetLastError());
				continue;
			}

			//Display Client's IP
			logprintf(LOGFDS, "AcceptConnections(): Client connected from: %s\n", inet_ntoa(ClientAddress.sin_addr)); 

			//Add this socket to the vector
			gConnections.push_back(Connection(Socket));
			unsigned long ulNoBlock=1;
			ioctlsocket(Socket, FIONBIO, &ulNoBlock);

		}
		// Check for Exception on the Listener
		else if (FD_ISSET(ListenSocket, &fdExcept)){
			fprintf(stderr,"AcceptConnections(): Exception on listener socket: %ld.\n", WSAGetLastError());
			continue;	
		}

		// Check if a client socket is the result
		ConnectionList::iterator it = gConnections.begin();

		while (it != gConnections.end()) {

			bool bClientOK = true;

			//Since we are non-blocking and limited to one connection in this version.
			iBytesWritten = WriteSocketData(*it);

			//Check for Exceptions
			if (FD_ISSET( it->sd,&fdExcept)){
				bClientOK = false;
				FD_CLR(it->sd, &fdExcept);
			}
			else {
				if (FD_ISSET(it->sd, &fdRead)) {
					bClientOK = ReadSocketData(*it);
					FD_CLR(it->sd, &fdRead);
				}
				if (FD_ISSET(it->sd, &fdWrite)) { // Removed from the select set, this will no longer execute.
					iBytesWritten = WriteSocketData(*it);
					FD_CLR(it->sd, &fdWrite);
				}
			}

			if (!bClientOK) {
				// Something bad happened on the socket, or the
				// client closed its half of the connection.  Shut
				// the conn down and remove it from the list.
				logprintf(LOGFDS,"AcceptConnections(): Shutting down client\n");
				int err;
				int errlen = sizeof(err);
				getsockopt(it->sd, SOL_SOCKET, SO_ERROR,
					(char*)&err, &errlen);
				if (err != NO_ERROR) {
					logprintf(LOGFDS, "AcceptConnections(): Exception on client socket: %ld.\n", WSAGetLastError());
				}
				closesocket(it->sd);
				gConnections.erase(it);
				it = gConnections.begin();
			}
			else {
				// Go on to next connection
				++it;
			}
		}
	}

}

/*
*	Read in socket data on selected connection. 
*	The result of successful select()
*/
bool ReadSocketData(Connection& conn) 
{
	const char  funcname[] = "ReadSocketData()";
	int nBytes = recv(conn.sd, conn.acBuffer,
		ulMaxNikonTotalPacketSize_c, 0);
	if (nBytes == 0) {
		logprintf(LOGFDS, "%s: Socket was closed by the client.\n",funcname);
		return false;
	}
	else if (nBytes == SOCKET_ERROR) {
		// Something bad happened on the socket.  It could just be a
		// "would block" notification, or it could be something more
		// serious.  Let caller handle the latter case.  WSAEWOULDBLOCK
		// can happen after select() says a socket is readable under
		// Win9x: it doesn't happen on WinNT/2000 or on Unix.
		int err;
		int errlen = sizeof(err);
		getsockopt(conn.sd, SOL_SOCKET, SO_ERROR, (char*)&err, &errlen);
		return (err == WSAEWOULDBLOCK);
	}

	// We read some bytes.  Advance the buffer size counter.
	conn.nCharsInBuffer = nBytes;

	// Now Copy that code into the message queue. 
	DWORD dwWaitResult;
	bool retval = true;

	dwWaitResult = WaitForSingleObject( msgMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		msgQ->enqueueRecvPacket(conn.acBuffer, conn.nCharsInBuffer, NULL);
		//logprintf(LOGFDS,"%s: Enqueued %i byte message.\n",funcname, conn.nCharsInBuffer);
	} else {
		logprintf(LOGFDS,"%s: ERROR Failed to enqueue message.\n",funcname);
		retval = false;
	}

	ReleaseMutex(msgMutex);

	return retval;


}

/*
*	Writes data out to supplied connections.
*/
int WriteSocketData(Connection& conn) 
{
	//Retrieve data and enqueue
	int camID;
	DWORD dwWaitResult;
	conn.nCharsInBuffer = 0;
	bool bMsgReady = false;

	dwWaitResult = WaitForSingleObject( msgMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		bMsgReady = msgQ->retrieveSendPacket(conn.acBuffer, conn.nCharsInBuffer, camID);
	} else return 0;
	ReleaseMutex(msgMutex);

	// There is no message ready
	if (!bMsgReady) {
		return 0;
	}

	int nBytes = send(conn.sd, conn.acBuffer, conn.nCharsInBuffer, 0);
	/*printf("\nSEND: ");
	for (int i=0;i<msglen;i++){
	printf("%02X ",msgbuff[i]);
	}
	printf("\n");*/

	if (nBytes == SOCKET_ERROR) {
		// Something bad happened on the socket.  Deal with it.
		int err;
		int errlen = sizeof(err);
		getsockopt(conn.sd, SOL_SOCKET, SO_ERROR, (char*)&err, &errlen);
		return (-err);
	}

	if (nBytes == conn.nCharsInBuffer) {
		//	// Everything got sent, so take a shortcut on clearing buffer.
		conn.nCharsInBuffer = 0;
	}

	return nBytes;
}

/*
*	Wrapper function for fprintf that adds the sytem 
*	time in UTC to the start of the entry. Performs 
*	this in a thread mutex safe fashion.
*/
void logprintf(FILE * fds, const char *format, ...) {
	DWORD dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0) { //we got the mutex
		//First insert time 
		SYSTEMTIME utcDateTime;
		GetSystemTime(&utcDateTime);
		fprintf(fds, "[%04d%02d%02d %02d:%02d:%02d] ", 
			utcDateTime.wYear,utcDateTime.wMonth,utcDateTime.wDay,
			utcDateTime.wHour,utcDateTime.wMinute,utcDateTime.wSecond);

		// Now pass on the rest. 
		va_list args;
		va_start(args, format);
		vfprintf(fds, format, args);
		va_end(args);
	}
	else if (dwWaitResult != WAIT_OBJECT_0) {
		printf("logprintf():: Error getting status mutex\n");
	}
	ReleaseMutex(statusMutex);

}

/*
*	Writes the last filename in memory
*	which was written out by the camera thread 
*	to a file defined in the INI file.
*/
void WriteFileToLog(bool bCloseRequested) {
	FILE * fLogFile;
	bool bLogStarted = false; 
	bool bFileOpen = false;

	SYSTEMTIME utcDateTime;

	// Open the logfile, we are going to log the Lastfilename written each time this is called;
	char sFileName[128];
	sprintf(sFileName,"%sFileLog.log",sNikonServerData.cameraFilePrefix);
	fLogFile = fopen(sFileName,"a");
	if (fLogFile != NULL)
		bFileOpen = true;


	DWORD dwWaitResult = WaitForSingleObject( statusMutex, MAXMUTEXWAIT);
	if (dwWaitResult == WAIT_OBJECT_0 && bFileOpen) { //we got the mutex
		// Write the lastfilename;
		GetSystemTime(&utcDateTime);
		fprintf(fLogFile,"%04d %02d %02d %02d %02d %02d %03d %06lu %06lu %s\n", 
			utcDateTime.wYear,utcDateTime.wMonth,utcDateTime.wDay,
			utcDateTime.wHour,utcDateTime.wMinute,utcDateTime.wSecond,
			utcDateTime.wMilliseconds,
			sTriggerServerData.ulTriggerCount, sNikonServerData.ulImageCount,
			sLastFileName);
		fclose(fLogFile);
	}
	else if (dwWaitResult != WAIT_OBJECT_0) {
		logprintf(LOGFDS,"NikonWorkerThread():: Error getting status mutex\n");
	}
	else if (!bFileOpen) {
		logprintf(LOGFDS,"WriteFileToLog(): ERROR cannot open image log file for writing.\n");
	}
	ReleaseMutex(statusMutex);


	return;
}

/*
*	Function added to proper handle a command line interrupt
*	commonly a ctrl-c
*/
void siginthandler(int param)
{
	if (bDeadAlready) {
		return;
	}

	bDeadAlready = true;

	logprintf(LOGFDS,"Siginthandler: Ctrl-c Received Program Terminating...\n");

	inMainLoop(false);

}