/* ---------------------------------------------------------------------------
 * ProtocolThread.c - v0.1                             (c) 2008 Micro-key bv
 * ---------------------------------------------------------------------------
 * Micro-key bv
 * Industrieweg 28, 9804 TG Noordhorn
 * Postbus 92, 9800 AB Zuidhorn
 * The Netherlands
 * Tel: +31 594 503020
 * Fax: +31 594 505825
 * Email: support@microkey.nl  
 * Web: www.microkey.nl
 * ---------------------------------------------------------------------------
 * Description: 
 * ---------------------------------------------------------------------------
 * Version(s):  0.1, Jan 29, 2008, FSc
 *                   Creation.
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * System include files
 * ---------------------------------------------------------------------------
 */
#include <limits.h>
#include <errno.h> 
#include <string.h>

/* ---------------------------------------------------------------------------
 * Application include files
 * ---------------------------------------------------------------------------
 */
#include "BpPort.h"
#include "BusProtocol.h"
#include "ProtocolThread.h"
#include "bus.h"
#include "MessageQueue.h"
#include "MessageHandlerQueue.h"
#include "BpMessageFormat.h"
#include "Crc.h"
#include "ElecStatusCache.h"
#include "mem_mod.h"

/* ---------------------------------------------------------------------------
 * Local constant and macro definitions
 * ---------------------------------------------------------------------------
 */
#define TX_QUEUE_SIZE    (20)
#define MAX_TX_MESSAGES  (12)    /**< Maximum number of message to be send in 1 turn */
#define THREAD_NAME_BUS1 "Bus1Pb"
#define THREAD_NAME_BUS2 "Bus2Pb"
#define MESSAGE_THREAD_NAME_BUS1 "Bus1MsgH"
#define MESSAGE_THREAD_NAME_BUS2 "Bus2MsgH"
#define MAX_NR_CHANNELS  (32)
#define MSG_QUEUE_NAME   "/BpMsgQueue"

/* ---------------------------------------------------------------------------
 * Global variable definitions
 * ---------------------------------------------------------------------------
 */
typedef enum
{
	IDLE,
	SENDER_ID,
	TARGET_ID,
	PACKET_NR,
	STATUS,
	MESSAGE_ID,
	PAYLOAD_SIZE,
	PAYLOAD,
	CRC
} t_bpthread_decodestatus;


typedef struct t_BPTHREAD_ADMIN {
	t_bus_devices              bus;
	UINT8                      deviceId;
	UINT8                      highestDeviceId;
	int                        busProtocolHandle;
	int                        messageHandlerHandle;
	t_bp_myturn_callback       myTurnCallback;
	pthread_t                  threadHandle;
	pthread_t                  threadMessageHandle;
	t_mq_messagequeue         *messageQueue;
	t_mq_messagequeue		  *messageHandlerQueue;
	UINT8                      lastReceivedPacketNr;
	UINT8                      lastReceivedSenderId;
	t_bpthread_decodestatus    decodeStatus;
	t_bpmsg_message            rxMessage;
	BOOLEAN                    rxStartByteDetected;
	UINT8                      rxFillIdx;
	UINT16                     rxCrc;
	UINT16                     cyclusTimeout;
	UINT32                     cyclusEndTick;
	UINT16                     messageTimeout;
	UINT32                     messageEndTick;
	UINT16                     backoffTime;
	portTickType              *timestampLastRecvMsgDevices;
} t_bpthread_admin;

extern memman *bpMessagePool;
memman  *bpRecvMessagePool;

/* ---------------------------------------------------------------------------
 * Local variable definitions
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * Local function definitions
 * ---------------------------------------------------------------------------
 */
static void *protocolThread( void *pvParameters );
static void *messageHandlerThread( void *pvParameters );
static t_bpmsg_message *decodeByte( t_bpthread_admin *threadAdmin, UINT8 byte );
static void handleMessage(t_bpthread_admin *threadAdmin, t_bpmsg_message *message);
static void detectMyTurn(t_bpthread_admin *threadAdmin, t_bpmsg_message *message);
static void doMyTurnActions(t_bpthread_admin *threadAdmin);
static void checkTimeouts(t_bpthread_admin *threadAdmin);
static void resetMessageTimeout(t_bpthread_admin *threadAdmin);
static void resetCyclusTimeout(t_bpthread_admin *threadAdmin);
static void sendMessage( t_bus_devices bus, t_bpmsg_message *message );
static UINT32 calcEndTick( UINT16 timeoutPeriod );
static BOOLEAN hasTimeoutPast( UINT32 endTick );
static BOOLEAN ignoreFirstStartByte(t_bpthread_admin *threadAdmin, UINT8 byte);
static void putOnLine( t_bus_devices bus, UINT8 dataLength, UINT8 *data);
static void DecodeAndCacheElectronicStatus(t_bpmsg_message *message);
static void SendLocalElectronicStatus(t_bpthread_admin *threadAdmin);

static int bpthreadMessageReceived = 0;
static int bpthreadMessageSend = 0;
static int bpthreadBadCrcMessage = 0;
static int bpthreadDecodeError = 0;
static UINT16 bpthreadArrayAdcValue[MAX_NR_CHANNELS];
static UINT16 bpthreadArrayDacValue[MAX_NR_CHANNELS];
static BOOLEAN bpthreadArrayDoValue[MAX_NR_CHANNELS];
static BOOLEAN bpthreadArrayDiValue[MAX_NR_CHANNELS];

/** \brief Starts the protocol handling thread 
 * 
 * \param bus The communicaiton bus
 * \param deviceId Identifier for this device
 * \param highestDeviceId The highest deviceId on the bus (only required for master device (deviceId = 0x01))
 * \param bpHandle Handle of busprotocol
 * \parma mhqHandle Handle of MessageHandlerQueue
 */
int bpthreadStart(t_bus_devices bus, UINT8 deviceId, UINT8 highestDeviceId, int bpHandle, int mhqHandle)
{
	t_bpthread_admin *newAdmin = (t_bpthread_admin *)pvPortMalloc( sizeof(t_bpthread_admin) );
	signed char *threadName;
	signed char *messageHandlerThreadName;
	int i;
	
	/* init administration */
	newAdmin->bus = bus;
	newAdmin->deviceId = deviceId;
	newAdmin->highestDeviceId = highestDeviceId; 
	newAdmin->busProtocolHandle = bpHandle;
	newAdmin->myTurnCallback = NULL;
	newAdmin->messageQueue = mqInit(); 
	newAdmin->decodeStatus = IDLE;
	newAdmin->lastReceivedSenderId = 0;
	newAdmin->lastReceivedPacketNr = 0;
	newAdmin->rxStartByteDetected = FALSE; 
	newAdmin->rxMessage.payload = NULL;
	newAdmin->messageHandlerHandle = mhqHandle;
	newAdmin->messageTimeout = 50;   //was 50;  /// \todo tune timeouts 
	newAdmin->cyclusTimeout = 500;  // was 500;
	newAdmin->backoffTime = 5;      // Was 5
 	newAdmin->messageHandlerQueue =  mqInit();  // xQueueCreate( 25, sizeof(t_bpmsg_message));
 	newAdmin->timestampLastRecvMsgDevices = (portTickType *)pvPortMalloc( sizeof(portTickType) * highestDeviceId );

 	// Reset buffer
 	for (i =0; i < highestDeviceId; i++)
 	{
 		newAdmin->timestampLastRecvMsgDevices[i] = xTaskGetTickCount();
 	}
 	

 	bpRecvMessagePool = Memmod_Create(8,64); // Make sure size is dividable by 4
	
	if (bus == BUS1)
	{
		threadName = (signed char *)THREAD_NAME_BUS1;
		messageHandlerThreadName = (signed char *)MESSAGE_THREAD_NAME_BUS1;
	}
	else 
	{
		threadName = (signed char *)THREAD_NAME_BUS2;
		messageHandlerThreadName = (signed char *)MESSAGE_THREAD_NAME_BUS2;
	}

	pthread_create(  &(newAdmin->threadHandle), NULL, protocolThread, newAdmin );  // Was: xTaskCreate( protocolThread, threadName, configMINIMAL_STACK_SIZE + 100, newAdmin, tskIDLE_PRIORITY + 3, &(newAdmin->threadHandle) );
	pthread_create(  &(newAdmin->threadMessageHandle), NULL, messageHandlerThread, newAdmin );  // Was: xTaskCreate( messageHandlerThread, messageHandlerThreadName, configMINIMAL_STACK_SIZE + 400, newAdmin, tskIDLE_PRIORITY + 2, &(newAdmin->threadMessageHandle) );

	return (int)newAdmin;
}

/** \brief Stops the protocol handling thread
 *
 * \note this function is never tested (reconsider implementation) 
 * \post handle is not valid anymore
 * \param handle Handle to the protocol thread
 */
void bpthreadStop( int handle )
{
	// \todo Test
	//vTaskDelete( ((t_bpthread_admin *)handle)->threadHandle );
	//vTaskDelete( ((t_bpthread_admin *)handle)->threadMessageHandle );

	vPortFree( (void *)handle );
}

/** \brief Add a message to the tx-queue. Message will be send when its this device its turn */
void bpthreadAddMessage( int handle, t_bpmsg_message *message )
{
	t_bpthread_admin *theAdmin = (t_bpthread_admin *)handle;
	mqAdd( theAdmin->messageQueue, message );
}

/** \brief Indicates whether a message a device is received in the last 10 seconds 
 * Only used by the master 
 */
BOOLEAN bpthreadDeviceIsDetected( int handle, UINT8 deviceId )
{
	t_bpthread_admin *theAdmin = (t_bpthread_admin *)handle;
	portTickType currentTimeout;
	BOOLEAN retval;
	
	// if difference is larger than 10 seconds return FALSE
	currentTimeout = xTaskGetTickCount() - theAdmin->timestampLastRecvMsgDevices[deviceId - 1];
	if (currentTimeout < 0)
	{
		currentTimeout = LONG_MAX - theAdmin->timestampLastRecvMsgDevices[deviceId] + xTaskGetTickCount();
	}
	
	if (currentTimeout > 10000) // Timeout larger than 10 seconds
	{
		retval = FALSE;
	}
	else
	{
		retval = TRUE;
	}
	
	return retval;
}


/** \brief Attaches a callback function to MyTurn-event, which notifies when it is this device its turn
 * 
 * \param handle Handle to the protocol thread
 * \param callback Pointer to callback-function
 */
void bpthreadAttachMyTurn( int handle, t_bp_myturn_callback callback)
{
	t_bpthread_admin *theAdmin = (t_bpthread_admin *)handle;

	theAdmin->myTurnCallback = callback;
}

/** \brief Detaches the callback function to MyTurn-event
 * 
 * \param handle Handle to the protocol thread
 * \param callback Pointer to callback-function
 */
void bpthreadDetachMyTurn( int handle, t_bp_myturn_callback callback)
{
	t_bpthread_admin *theAdmin = (t_bpthread_admin *)handle;

	theAdmin->myTurnCallback = NULL;
}
 
/** \brief The thread which handles the in & output on the bus.
 * 
 * \param pvParameters Pointer to parameters
 */
void *protocolThread( void *pvParameters )
{
	t_bpthread_admin *threadAdmin = (t_bpthread_admin *)pvParameters;
	t_bpmsg_message *rxMessage;
	UINT8 rxByte;
	
	if (threadAdmin->deviceId == MASTER_DEVICE_ID)
	{
		/* This is the master so start sending messages */
		doMyTurnActions( threadAdmin );
		
		resetMessageTimeout( threadAdmin );
		resetCyclusTimeout( threadAdmin );
	}
	else
	{
		resetMessageTimeout( threadAdmin );
	}
	
	for (;;)
	{
		// Read all bytes received on bus
		while (busGet( threadAdmin->bus, &rxByte ) == TRUE)
		{
			rxMessage = decodeByte( threadAdmin, rxByte );
			if (rxMessage != NULL)
			{
				bpthreadMessageReceived++;
				threadAdmin->lastReceivedSenderId = rxMessage->senderId;
				
				// Complete message is received, handle message
				handleMessage( threadAdmin, rxMessage);
				resetMessageTimeout( threadAdmin );
				if (rxMessage->status == BPMSG_STATUS_FINISHEDSENDING )
				{	
					detectMyTurn( threadAdmin, rxMessage );
				}
			}
		}
		
		// Verify timeouts
		checkTimeouts( threadAdmin );
		
		vTaskDelay( 5 ); // 5 milliseconden sleep
	}
	
	return NULL;
}
 
t_bpmsg_message *decodeByte( t_bpthread_admin *threadAdmin, UINT8 byte )
{
	switch (threadAdmin->decodeStatus)
	{
	case(IDLE):
		if (byte == BPMSG_STARTBYTE)
		{
			threadAdmin->decodeStatus = SENDER_ID;
		}
		else
		{
			bpthreadDecodeError++;
		}
		break;
	case(SENDER_ID):
		if (byte != BPMSG_STARTBYTE) /* 0xAA is not allowed as SENDER_ID, must be a START BYTE */
		{
			threadAdmin->rxMessage.senderId = byte;
			threadAdmin->decodeStatus = TARGET_ID;
		}
		else
		{
			bpthreadDecodeError++;
		}
		break;
	case(TARGET_ID):
		if (byte != BPMSG_STARTBYTE) /* 0xAA is not allowed as SENDER_ID, must be a START BYTE */
		{
			threadAdmin->rxMessage.targetId = byte;
			threadAdmin->decodeStatus = PACKET_NR;
		}
		else
		{
			bpthreadDecodeError++;
			threadAdmin->decodeStatus = SENDER_ID;
		}
		break;
	case(PACKET_NR):
		if (byte != BPMSG_STARTBYTE) /* 0xAA is not allowed as PACKET_NR, must be a START BYTE */
		{
			threadAdmin->rxMessage.packetNr = byte;
			threadAdmin->decodeStatus = STATUS;
		}
		else
		{
			bpthreadDecodeError++;
			threadAdmin->decodeStatus = SENDER_ID;
		}
		break;
	case(STATUS):
		if ( (byte & 0x40) == 0x40)  /* bit 6 must be high */
		{
			threadAdmin->rxMessage.status = byte;
			threadAdmin->decodeStatus = MESSAGE_ID;
		}
		else
		{
			bpthreadDecodeError++;
			threadAdmin->decodeStatus = IDLE;
		}
		break;
	case(MESSAGE_ID):
		if (byte != BPMSG_STARTBYTE) /* 0xAA is not allowed as PACKET_NR, must be a START BYTE */
		{
			threadAdmin->rxMessage.messageId = byte;
			threadAdmin->decodeStatus = PAYLOAD_SIZE;
			threadAdmin->rxStartByteDetected = FALSE;
		}
		else
		{
			bpthreadDecodeError++;
			threadAdmin->decodeStatus = SENDER_ID;
		}
		break;
	case(PAYLOAD_SIZE):
		if (!ignoreFirstStartByte(threadAdmin, byte ))
		{
			threadAdmin->rxCrc = crcCalc(&byte, 1, 0);
			threadAdmin->rxMessage.payloadSize = byte;
			if (byte > 0)
			{
				threadAdmin->decodeStatus = PAYLOAD;
				threadAdmin->rxMessage.payload = (UINT8 *)Memmod_Alloc(bpRecvMessagePool);
				threadAdmin->rxFillIdx = 0;
			}
			else
			{
				threadAdmin->decodeStatus = CRC;
				threadAdmin->rxMessage.payload = NULL;
			}
		}
		break;
	case(PAYLOAD):
		if (!ignoreFirstStartByte(threadAdmin, byte ))
		{
			threadAdmin->rxCrc = crcCalc(&byte, 1, threadAdmin->rxCrc);
			threadAdmin->rxMessage.payload[threadAdmin->rxFillIdx] = byte;
			threadAdmin->rxFillIdx++;
		}
	
		if (threadAdmin->rxFillIdx == threadAdmin->rxMessage.payloadSize)
		{
			threadAdmin->rxFillIdx = 0;
			threadAdmin->decodeStatus = CRC;
		}
		break;
	case(CRC):
		if (!ignoreFirstStartByte(threadAdmin, byte) )
		{
			if (threadAdmin->rxFillIdx == 0)
			{
				threadAdmin->rxMessage.crc = ((UINT16)byte) << 8;
			}
			else
			{
				threadAdmin->rxMessage.crc |= (UINT16)byte;
			}	
			threadAdmin->rxFillIdx++;
		}
	
		if (threadAdmin->rxFillIdx == 2)
		{
			threadAdmin->rxFillIdx = 0;
			threadAdmin->decodeStatus = IDLE;
			
			if (threadAdmin->rxCrc == threadAdmin->rxMessage.crc)
			{
				return &(threadAdmin->rxMessage);
			}
			else
			{
				if (threadAdmin->rxMessage.payload != NULL)
				{
				    Memmod_Free( bpRecvMessagePool, threadAdmin->rxMessage.payload );
				    threadAdmin->rxMessage.payload = NULL;
				}

				bpthreadBadCrcMessage++;
			}
		}
		break;
	}
	
	return NULL;
}


void handleMessage(t_bpthread_admin *threadAdmin, t_bpmsg_message *message)
{
	// Record packet nr.
	threadAdmin->lastReceivedPacketNr = message->packetNr;
	
	// Reset Device detected timeout
	if ((message->senderId > 0) && (message->senderId <= threadAdmin->highestDeviceId))  // Safety first
	{
		threadAdmin->timestampLastRecvMsgDevices[message->senderId - 1] = xTaskGetTickCount();
	}

	// is message ment for this device
	if (    (message->targetId == BPMSG_BROADCAST_ID)
		 ||	(message->targetId == threadAdmin->deviceId)
	   )
	{
		// Add to queue
		mqAdd( threadAdmin->messageHandlerQueue, message);  // Was: xQueueSendToBack( threadAdmin->messageHandlerQueue, message, 100);

		// Make sure the payload isn't freed
		//message->payload = NULL;
	}
	else
	{
		// Delete message stuff
		if (message->payload != NULL)
		{
			Memmod_Free( bpRecvMessagePool, message->payload );
			message->payload = NULL;
		}
	}
}

void detectMyTurn(t_bpthread_admin *threadAdmin, t_bpmsg_message *message)
{
	BOOLEAN isMyTurn = FALSE;
	
	if ((message->status & BPMSG_STATUS_FINISHEDSENDING) == BPMSG_STATUS_FINISHEDSENDING)
	{
		if (threadAdmin->deviceId == BPMSG_MASTER_DEVID)
		{
			if (message->senderId == threadAdmin->highestDeviceId)
			{
				resetCyclusTimeout( threadAdmin );
				isMyTurn = TRUE;
			}
		}
		else
		{
			if ( (message->senderId + 1) == threadAdmin->deviceId)
			{
				isMyTurn = TRUE;
			}
		}
	}

	if (isMyTurn == TRUE)
	{
		doMyTurnActions(threadAdmin);
	}
}

void checkTimeouts( t_bpthread_admin *threadAdmin )
{
	if (threadAdmin->deviceId == MASTER_DEVICE_ID)
	{
		if (hasTimeoutPast( threadAdmin->cyclusEndTick ) == TRUE)
		{
			BP_DEBUG_OUT( 't');	BP_DEBUG_OUT( 'c');
			resetCyclusTimeout( threadAdmin );
			doMyTurnActions( threadAdmin );
		}
	}
	else
	{
		// If slave device than check if master is seen in the last 10 seconds
		if (bpthreadDeviceIsDetected( (int)threadAdmin, MASTER_DEVICE_ID ) == FALSE)
		{
			// Put slave in save mode and reset device
			// secureSlave();    NOTE NOTE : This is not intended for the Webcontroller
			
		}
	}

	// Can safely do test again, cause above actions have reset this 
	// timeout in doMyTurnActions.
	if (hasTimeoutPast( threadAdmin->messageEndTick ) == TRUE)
	{
		resetMessageTimeout( threadAdmin );
		
		// Test if it is possible my turn
		if ((threadAdmin->lastReceivedSenderId + 1) == threadAdmin->deviceId)
		{
			BP_DEBUG_OUT('t');	BP_DEBUG_OUT('m');
			doMyTurnActions( threadAdmin );
		}
		else
		{
			if (    (threadAdmin->deviceId == MASTER_DEVICE_ID)
			     && (threadAdmin->lastReceivedSenderId == threadAdmin->highestDeviceId)
			   )
			{
                BP_DEBUG_OUT('t');  BP_DEBUG_OUT('m');
				resetCyclusTimeout( threadAdmin );
				doMyTurnActions( threadAdmin );
			}
			else
			{
				threadAdmin->lastReceivedSenderId++;
			}
		}
	}
}

void doMyTurnActions(t_bpthread_admin *threadAdmin)
{
	int nrOfMessagesSend = 0;
	t_bpmsg_message message;
	UINT16 multipleTimeout;
	
	// Backoff for some time
	vTaskDelay( threadAdmin->backoffTime );
	
	/* Notify MyTurn-event listeners */
	if (threadAdmin->myTurnCallback != NULL)
	{
		// If MyTurn is handled by application -> then application is responsible for send give electronicStatus
		threadAdmin->myTurnCallback();
	}

	if (mqEmpty(threadAdmin->messageQueue) == TRUE)
	    bpSendPassTurn(threadAdmin->busProtocolHandle);
	

	// Send MAX messages on the bus
	while (    (nrOfMessagesSend < MAX_TX_MESSAGES) 
			&& (mqEmpty(threadAdmin->messageQueue) == FALSE)
		  )
	{
		if (mqGet(threadAdmin->messageQueue, &message) != ERROR)
		{
			// If last message in a row then set status LAST_MESSAGE
			if (    (mqEmpty(threadAdmin->messageQueue) == TRUE )
				 ||	((nrOfMessagesSend + 1)>= MAX_TX_MESSAGES)
			   )
			{
				message.status = BPMSG_STATUS_FINISHEDSENDING;
			}
			else
			{
				message.status = BPMSG_STATUS_BUSYSENDING;
			}
			
			// Fill packetNr
			threadAdmin->lastReceivedPacketNr++;
			if (threadAdmin->lastReceivedPacketNr == BPMSG_STARTBYTE) threadAdmin->lastReceivedPacketNr++; // 0xAA cannot be used as packetNr 
			message.packetNr = threadAdmin->lastReceivedPacketNr;
			
			sendMessage( threadAdmin->bus, &message );
			nrOfMessagesSend++;
		
			// Throw payload away (dynamic part of message)
			if (message.payload != NULL)
			{
			    Memmod_Free( bpMessagePool, message.payload );
			}
		}
	}
	
	threadAdmin->lastReceivedSenderId = threadAdmin->deviceId;
	
	// Reset message timeout multiple times
	multipleTimeout = nrOfMessagesSend * threadAdmin->messageTimeout;
	threadAdmin->messageEndTick = calcEndTick(  multipleTimeout );
}

void resetMessageTimeout(t_bpthread_admin *threadAdmin)
{
	threadAdmin->messageEndTick = calcEndTick( threadAdmin->messageTimeout);
}

void resetCyclusTimeout(t_bpthread_admin *threadAdmin)
{
	threadAdmin->cyclusEndTick = calcEndTick( threadAdmin->cyclusTimeout);
} 

void sendMessage( t_bus_devices bus, t_bpmsg_message *message )
{
	bpthreadMessageSend++;
	UINT8 crcByte;
	
	// Put message on the bus
	busPut( bus, message->uniqueStartByte);
	busPut( bus, message->senderId);
	busPut( bus, message->targetId);
	busPut( bus, message->packetNr);
	busPut( bus, message->status);
	busPut( bus, message->messageId);
	putOnLine( bus, 1, &(message->payloadSize));
	putOnLine( bus, message->payloadSize, message->payload );
	crcByte = (UINT8)(message->crc >> 8);
	putOnLine( bus, 1, &crcByte);
	crcByte = (UINT8)message->crc;
	putOnLine( bus, 1, &crcByte);
}

void putOnLine( t_bus_devices bus, UINT8 dataLength, UINT8 *data)
{
	int index;
	
	for (index = 0; index < dataLength; index++)
	{
		if (data[index] == BPMSG_STARTBYTE)
		{
			// Write double AA
			busPut( bus, BPMSG_STARTBYTE);
			busPut( bus, BPMSG_STARTBYTE);
		}
		else
		{
			busPut( bus, data[index]);
		}
	}
}

UINT32 calcEndTick( UINT16 timeoutPeriod )
{
	UINT32 result = xTaskGetTickCount() + timeoutPeriod;
	
	return result;
}

BOOLEAN hasTimeoutPast( UINT32 endTick )
{
	UINT32 nowTick = xTaskGetTickCount();

	if (nowTick >= endTick)
	{
		if ((nowTick - endTick) > 0x0000FFFF)
		{
			// the endTick has gone through 0 point, nowTick is at end of range 
			return FALSE;
		}
		else
		{
			// nowTick passed endTick.
			return TRUE;
		}
	}
	else
	{
		if ((endTick - nowTick) > 0x0000FFFF)
		{
			
			// the endTick was at end of range, nowTick has gone through 0 point 
			return TRUE;
		}
		else
		{
			// nowTick still has to pass endTick
			return FALSE;
		}
	}
}

BOOLEAN ignoreFirstStartByte(t_bpthread_admin *threadAdmin, UINT8 byte)
{
	if (threadAdmin->rxStartByteDetected == FALSE)
	{
		if (byte == BPMSG_STARTBYTE)
		{
			threadAdmin->rxStartByteDetected = TRUE;
		}
		else
		{
			threadAdmin->rxStartByteDetected = FALSE;
		}
	}
	else
	{
		if (byte == BPMSG_STARTBYTE)
		{
			// Correctly received the second StartByte
			threadAdmin->rxStartByteDetected = FALSE;
		}
		else
		{
			// Expected a second StartByte but didn't => ERROR
			threadAdmin->rxStartByteDetected = TRUE;
			threadAdmin->decodeStatus = IDLE;
		}
	}
	
	return threadAdmin->rxStartByteDetected; 
}

void *messageHandlerThread( void *pvParameters )
{
	t_bpthread_admin *threadAdmin = (t_bpthread_admin *)pvParameters;
	t_bpmsg_message message;
	
	for (;;)
		{
		if (mqGet( threadAdmin->messageHandlerQueue, &message) == OK) // Was: if (xQueueReceive(threadAdmin->messageHandlerQueue, &message, 200))
	        {
	    	// If "Give Electronic status"-update then store in bpec of driver
	    	if (message.messageId == BPMSG_MSGID_GIVEELECTRONICSTATUS)
	    		{
	    		DecodeAndCacheElectronicStatus( &message );
	    		}
    	
	    	// Lookup message and execute handler
	    	mhqExecute( threadAdmin->messageHandlerHandle, message.messageId, &message );
	    	
	    	if (message.payload != NULL)
	    		{
                Memmod_Free( bpRecvMessagePool, message.payload );
	    		}
	        }
		vTaskDelay(5);
		}
		
	return NULL;
}

void DecodeAndCacheElectronicStatus(t_bpmsg_message *message)
{
	UINT8 index;
	UINT8 arraySize;
	UINT8 payloadIndex = 0;
	
	BP_DEBUG_OUT( 'e');	BP_DEBUG_OUT( '<');

	// Decode ADC-values
	arraySize = message->payload[payloadIndex++];
	for (index = 0; index < arraySize; index++)
	{
		bpthreadArrayAdcValue[index] = bpmsgGet16bit( message->payload, &payloadIndex);
	} 
	bpecSetAdcReadCache( message->senderId, bpthreadArrayAdcValue, arraySize);

	// Decode DAC-values
	arraySize = message->payload[payloadIndex++];
	for (index = 0; index < arraySize; index++)
	{
		bpthreadArrayDacValue[index] = bpmsgGet16bit( message->payload, &payloadIndex);
	} 
	bpecSetDacReadBackCache( message->senderId, bpthreadArrayDacValue, arraySize);

	// Decode DIO input-values
	arraySize = message->payload[payloadIndex++];
	for (index = 0; index < arraySize; index++)
	{
		bpthreadArrayDiValue[index] = bpmsgGet8bit( message->payload, &payloadIndex);
	} 
	bpecSetDioReadCache( message->senderId, bpthreadArrayDiValue, arraySize);

	// Decode DIO output-values
	arraySize = message->payload[payloadIndex++];
	for (index = 0; index < arraySize; index++)
	{
		bpthreadArrayDoValue[index] = bpmsgGet8bit( message->payload, &payloadIndex);
	} 
	bpecSetDioReadBackCache( message->senderId, bpthreadArrayDoValue, arraySize);
}

void SendLocalElectronicStatus(t_bpthread_admin *threadAdmin)
{
#ifdef DO_NOT_COMPILE
	int i;
	
	BP_DEBUG_OUT( 'e');	BP_DEBUG_OUT( '>');

	// Assemble information
	for (i = 0; i < maxADC_Channels; i++ )
	{
		bpthreadArrayAdcValue[i] = adcRead( 0, i );
	}

	for (i = 0; i < maxDAC_Channels; i++ )
	{
		bpthreadArrayDacValue[i] = dacReadBack( 0, i );
	}

	for (i = 0; i < maxDI_Channels; i++ )
	{
		bpthreadArrayDiValue[i] = dioRead( 0, i );
	}

	for (i = 0; i < maxDO_Channels; i++ )
	{
		bpthreadArrayDoValue[i] = dioReadBack( 0, i );
	}
	
	// Send message
	bpSendGiveElectronicStatus( threadAdmin->busProtocolHandle, 
                                maxADC_Channels,
                                bpthreadArrayAdcValue,
                                maxDAC_Channels,
                                bpthreadArrayDacValue,
                                maxDI_Channels,
                                (UINT8 *)bpthreadArrayDiValue,
                                maxDO_Channels,
                                (UINT8 *)bpthreadArrayDoValue
                               );
#endif							  
}