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

ESCore.h

ES distance sensor handling.

Fabrice Le Bars
CalcCRC16() is based on the code provided in the documentation of the distance sensor

Created: 2009-03-28

Version status: Not finished

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

#ifndef ESCORE_H
#define ESCORE_H

#include "OSRS232Port.h"

/*
Debug macros specific to ESUtils.
*/
#ifdef _DEBUG_MESSAGES
#	define _DEBUG_MESSAGES_ESUTILS
#endif // _DEBUG_MESSAGES

#ifdef _DEBUG_WARNINGS
#	define _DEBUG_WARNINGS_ESUTILS
#endif // _DEBUG_WARNINGS

#ifdef _DEBUG_ERRORS
#	define _DEBUG_ERRORS_ESUTILS
#endif // _DEBUG_ERRORS

#ifdef _DEBUG_MESSAGES_ESUTILS
#	define PRINT_DEBUG_MESSAGE_ESUTILS(params) PRINT_DEBUG_MESSAGE(params)
#else
#	define PRINT_DEBUG_MESSAGE_ESUTILS(params)
#endif // _DEBUG_MESSAGES_ESUTILS

#ifdef _DEBUG_WARNINGS_ESUTILS
#	define PRINT_DEBUG_WARNING_ESUTILS(params) PRINT_DEBUG_WARNING(params)
#else
#	define PRINT_DEBUG_WARNING_ESUTILS(params)
#endif // _DEBUG_WARNINGS_ESUTILS

#ifdef _DEBUG_ERRORS_ESUTILS
#	define PRINT_DEBUG_ERROR_ESUTILS(params) PRINT_DEBUG_ERROR(params)
#else
#	define PRINT_DEBUG_ERROR_ESUTILS(params)
#endif // _DEBUG_ERRORS_ESUTILS

/*
Debug macros specific to ESCore.
*/
#ifdef _DEBUG_MESSAGES_ESUTILS
#	define _DEBUG_MESSAGES_ESCORE
#endif // _DEBUG_MESSAGES_ESUTILS

#ifdef _DEBUG_WARNINGS_ESUTILS
#	define _DEBUG_WARNINGS_ESCORE
#endif // _DEBUG_WARNINGS_ESUTILS

#ifdef _DEBUG_ERRORS_ESUTILS
#	define _DEBUG_ERRORS_ESCORE
#endif // _DEBUG_ERRORS_ESUTILS

#ifdef _DEBUG_MESSAGES_ESCORE
#	define PRINT_DEBUG_MESSAGE_ESCORE(params) PRINT_DEBUG_MESSAGE(params)
#else
#	define PRINT_DEBUG_MESSAGE_ESCORE(params)
#endif // _DEBUG_MESSAGES_ESCORE

#ifdef _DEBUG_WARNINGS_ESCORE
#	define PRINT_DEBUG_WARNING_ESCORE(params) PRINT_DEBUG_WARNING(params)
#else
#	define PRINT_DEBUG_WARNING_ESCORE(params)
#endif // _DEBUG_WARNINGS_ESCORE

#ifdef _DEBUG_ERRORS_ESCORE
#	define PRINT_DEBUG_ERROR_ESCORE(params) PRINT_DEBUG_ERROR(params)
#else
#	define PRINT_DEBUG_ERROR_ESCORE(params)
#endif // _DEBUG_ERRORS_ESCORE

#define MAX_NB_BYTES_ES 1024

/*
Read the value at a specific channel of a ES.

HANDLE hDev : (IN) Identifier of the device serial port.
double* pValue : (INOUT) Valid pointer receiving the value.

Return : EXIT_SUCCESS or EXIT_FAILURE if there is an error.
*/
inline int ReadChannelES(HANDLE hDev, double* pValue)
{
/*	char recvbuf[MAX_NB_BYTES_ES];
	char savebuf[MAX_NB_BYTES_ES];
	int BytesReceived = 0, Bytes = 0, recvbuflen = 0;
	char* ptr_dist = NULL;

	// Prepare the buffers.
	memset(recvbuf, 0, sizeof(recvbuf));
	memset(savebuf, 0, sizeof(savebuf));
	recvbuflen = MAX_NB_BYTES_ES-1; // The last character must be a 0 to be a valid string for sscanf.
	BytesReceived = 0;

	// As the sensor always sends data, any old data must be discarded.

	if (ReadAllRS232Port(hDev, readbuf, 9) != EXIT_SUCCESS)
	{
		PRINT_DEBUG_ERROR_ESCORE(("ReadChannelES error (%s) : %s"
									"(hDev=%#x)\n", 
									strtime_m(), 
									"Error reading data from the serial port. ", 
									hDev));
		return EXIT_FAILURE;
	}






	Bytes = recv(pWeatherStation->s, recvbuf, recvbuflen, 0);
	if (Bytes <= 0)
	{
		printf("recv() failed or timed out.\n");
		return EXIT_FAILURE;
	}
	BytesReceived += Bytes;

	while (Bytes == recvbuflen)
	{
		memcpy(savebuf, recvbuf, Bytes);

		Bytes = recv(pWeatherStation->s, recvbuf, recvbuflen, 0);
		if (Bytes <= 0)
		{
			printf("recv() failed or timed out.\n");
			return EXIT_FAILURE;
		}
		BytesReceived += Bytes;
	}

	if (BytesReceived < recvbuflen)
	{
		ptr_dist = FindLatestESSentence("$GPGGA", recvbuf);

		while (!(ptr_dist))
		{
			if (BytesReceived >= recvbuflen) return EXIT_FAILURE;

			Bytes = recv(pWeatherStation->s, recvbuf+BytesReceived, recvbuflen-BytesReceived, 0);
			if (Bytes <= 0)
			{
				printf("recv() failed or timed out.\n");
				return EXIT_FAILURE;
			}
			BytesReceived += Bytes;

			ptr_dist = FindLatestESSentence("$GPGGA", recvbuf);
		}
	}
	else
	{
		memmove(recvbuf+recvbuflen-Bytes, recvbuf, Bytes);
		memcpy(recvbuf, savebuf+Bytes, recvbuflen-Bytes);

		ptr_dist = FindLatestESSentence("$GPGGA", recvbuf);

		if (!(ptr_dist)) return EXIT_FAILURE;
	}

	if (sscanf((char*)ptr_dist, "%f\r\n", pValue) != 1)
	{
		PRINT_DEBUG_ERROR_ESCORE(("ReadChannelES error (%s) : %s"
									"(hDev=%#x)\n", 
									strtime_m(), 
									"Invalid data. ", 
									hDev));
		return EXIT_FAILURE;
	}
*/





	uint8 readbuf[11];

	if (PurgeRS232Port(hDev) != EXIT_SUCCESS)
	{
		PRINT_DEBUG_ERROR_ESCORE(("ReadChannelES error (%s) : %s"
									"(hDev=%#x)\n", 
									strtime_m(), 
									"Error reading data from the serial port. ", 
									hDev));
		return EXIT_FAILURE;
	}

	memset(readbuf, 0, sizeof(readbuf));

	if (ReadAllRS232Port(hDev, readbuf, 10) != EXIT_SUCCESS)
	{
		PRINT_DEBUG_ERROR_ESCORE(("ReadChannelES error (%s) : %s"
									"(hDev=%#x)\n", 
									strtime_m(), 
									"Error reading data from the serial port. ", 
									hDev));
		return EXIT_FAILURE;
	}

	if (sscanf((char*)readbuf, "%lfm\r\n", pValue) != 1)
	{
		PRINT_DEBUG_ERROR_ESCORE(("ReadChannelES error (%s) : %s"
									"(hDev=%#x)\n", 
									strtime_m(), 
									"Invalid data. ", 
									hDev));
		return EXIT_FAILURE;
	}

	PRINT_DEBUG_MESSAGE_ESCORE(("Distance : %f\n", *pValue));

	return EXIT_SUCCESS;
}

#endif // ESCORE_H
