/***************************************************************************************************************:')

ESMgr.c

ES distance sensor handling.

Fabrice Le Bars

Created: 2009-06-24

Version status: Not finished

***************************************************************************************************************:)*/

#include "ESMgr.h"

THREAD_PROC_RETURN_VALUE ESMgrThreadProc(void* lpParam)	
{
	HESMGR hESMgr = (HESMGR)lpParam;
	double distance = 0;

	do
	{
		mSleep(hESMgr->PollingTimeInterval);

		if (!hESMgr->bConnected)
		{ 
			// The device is not connected, trying to connect or reconnect.
			ConnectToES(hESMgr);
		}
		else	
		{
			// Wait for new data.
			if (hESMgr->PollingMode & DISTANCE_POLLING_MODE_ES)
			{
				EnterCriticalSection(&hESMgr->ESCS);

				if (!hESMgr->bConnected)
				{ 
					LeaveCriticalSection(&hESMgr->ESCS);
					continue;
				}

				if (GetDistanceES(hESMgr->hES, &distance) != EXIT_SUCCESS)
				{
					PRINT_DEBUG_MESSAGE_ESMGR(("Connection to a ES lost\n"));

					// The device was supposed to be connected before.
					// Notify the thread to try to reconnect the device.
					hESMgr->bConnected = FALSE; 
					CloseES(&hESMgr->hES);

					LeaveCriticalSection(&hESMgr->ESCS);
					continue;
				}

				LeaveCriticalSection(&hESMgr->ESCS);

				EnterCriticalSection(&hESMgr->distanceCS);
				hESMgr->distancecounter++;
				hESMgr->distance = distance;
				LeaveCriticalSection(&hESMgr->distanceCS);
			}
		}
	} 
	while (hESMgr->bRunThread);

	EnterCriticalSection(&hESMgr->ESCS);

	if (hESMgr->bConnected)
	{ 
		hESMgr->bConnected = FALSE; 

		if (CloseES(&hESMgr->hES) != EXIT_SUCCESS)
		{
			PRINT_DEBUG_ERROR_ESMGR(("ESMgrThreadProc error (%s) : %s"
				"\n", 
				strtime_m(), 
				". "));
		}
	}
	
	LeaveCriticalSection(&hESMgr->ESCS);

	return 0; 
} 
