// ****************************************************************************
// INCLUDE FILES: Header files of modules referenced by this module
// ****************************************************************************
#include "udi_cdc.h"
#include "Protocol_iSTP.h"
#include "CRC.h"
#include "spi.h"
#include "ConfigFile.h"
#include "sensors_universal.h"
#include "init.h"
#include "powerlinemanager.h"
#include "dataConversions.h"
#include "usart_rs485mode.h"
#include "SWversion.h"
#include "string.h"
#include "AssertISTP.h"
#include "HealthData.h"
#include <ISTPManager.h>
#include <StopWatch.h>


// ****************************************************************************
// PRIVATE TYPES: Private types used by this module only
// ****************************************************************************
typedef enum
{
    ISTPP_FRAME_CONSUMER_IDLE,
    ISTPP_FRAME_CONSUMER_RX_HEADER,
    ISTPP_FRAME_CONSUMER_RX_PACKET_INFO,
    ISTPP_FRAME_CONSUMER_RX_DATA,
    ISTPP_FRAME_CONSUMER_RX_CRC
}ISTPPframeConsumerState_t;


// ****************************************************************************
// PRIVATE MACROS: Private #define directives used by this module only
// ****************************************************************************
#define ISTPP_TX_BUFFER_SIZE 8192
#define ISTPP_RX_BUFFER_SIZE (USART_RX_BUFFER_SIZE * 2)
#define ISTPP_RX_PACKET_BUFFER_SIZE (ISTPP_RX_BUFFER_SIZE)
//#define USART_LOOPBACK_TEST 

// ISTP protocol frame definitions

// Header stands as the first item in the frame.
// For more convenient addressing of the frame content,
// byte positions of the items behind the header starts 
// from zero. The reason is that header is not stored to
// the frame buffer.
#define ISTPP_HEADER_SIZE  (4)
#define ISTPP_HEADER_BYTE  (0x23)

// First item behind the header is "NUMBER"
// All subsequent items are defined relative to this
#define ISTPP_NUMBER_POS  ( 0 )
#define ISTPP_NUMBER_SIZE (4)

#define ISTPP_TIME_STAMP_POS  ( ISTPP_NUMBER_POS + ISTPP_NUMBER_SIZE )
#define ISTPP_TIME_STAMP_SIZE (8)

#define ISTPP_TYPE_POS  ( ISTPP_TIME_STAMP_POS + ISTPP_TIME_STAMP_SIZE )
#define ISTPP_TYPE_SIZE (4)

#define ISTPP_ID_POS  ( ISTPP_TYPE_POS + ISTPP_TYPE_SIZE )
#define ISTPP_ID_SIZE (4)

#define ISTPP_LENGTH_POS  ( ISTPP_ID_POS + ISTPP_ID_SIZE )
#define ISTPP_LENGTH_SIZE (4)

#define ISTPP_DATA_POS  ( ISTPP_LENGTH_POS + ISTPP_LENGTH_SIZE )

#define ISTPP_CRC_SIZE (4)

#define ERROR_DATA_LENGTH 8
#define ACK_DATA_LENGTH 8


// ****************************************************************************
// PRIVATE VARIABLES: Variables used by this module only (static)
// ****************************************************************************
static ISTPPframeConsumerState_t istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
static uint8_t istppRxData1[ISTPP_RX_BUFFER_SIZE + (ISTPP_RX_BUFFER_SIZE / 2)];
static uint8_t istppRxData2[ISTPP_RX_BUFFER_SIZE + (ISTPP_RX_BUFFER_SIZE / 2)];
static uint8_t * istppRxDataActive = istppRxData1;
static uint32_t istppRxNonConsumedBytes = 0;
static uint8_t istppRxFrame[ISTPP_RX_PACKET_BUFFER_SIZE];
static uint32_t istppRxFrameLength = 0;
static uint32_t istppRxFrameCalculatedCrc;
static uint32_t istppRxFrameReceivedCrc;
static uint8_t istppRxFrameReceived;

static uint32_t SensorData[2048];          // field for all sensor data
static SensorMemory_t SensMem[256];        // array storing EEPROM data for sensors
static int8_t isUSBconnect = 0;
static bool ReadFirst = false;
static uint32_t istppLostTxMessagesCnt = 0;
static uint32_t HealthDataCounter = 0;


 

// ****************************************************************************
// PRIVATE FUNCTIONS PROTOTYPES: Functions used by this module only (static)
// ****************************************************************************
static inline void ReadMSG(PacketStruct_t *p_Packet);
static inline void ReadMSGrs485(PacketStruct_t *p_Packet);
static inline void ReadFirstMSG(PacketStruct_t *p_Packet);
static int8_t SendMSGData(uint32_t aCounter, uint64_t aTimeStamp, uint16_t aType, uint32_t aID, uint32_t wordCount, uint32_t *p_Sensor);
static int8_t SendDataMsg(uint32_t aLength, uint32_t *p_Sensor, uint64_t timeStamp, uint32_t aID);
static int8_t SendMSG(PacketStruct_t *p_Packet);
static uint8_t ISTPPframeConsumer(uint8_t * rxData, uint32_t size, uint32_t * consumedBytes);


// ****************************************************************************
// PUBLIC VARIABLES: Public variables shared with other modules
// ****************************************************************************

// Defined by this module
uint32_t MsgNumber;

// Defined by other modules
extern PacketStruct_t Packet;       // iSTP data frame structure
extern unsigned int currentFrame;
extern unsigned int virtualSensNum;


// ****************************************************************************
// PRIVATE FUNCTIONS: Private functions used by this module only
// ****************************************************************************
static uint8_t ISTPPframeConsumer(uint8_t * rxData, uint32_t size, uint32_t * consumedBytes)
{
    uint32_t iterator = 0;
    uint8_t rxByte;
    uint8_t rxFrameReceived = 0;
    static uint32_t frameBytesReceived;
    static uint32_t temporaryBytesReceived;
    
    // Parse the incoming data only if no frame is currently received.
    // The communication model does not count with any queues and is 
    // able to receive and process just one incoming frame at a time
    
    // Loop through all the provided received bytes 
    while(  (iterator < size ) && !istppRxFrameReceived )
    {
        // Read the received byte
        rxByte = rxData[iterator]; 
        
        // Pass the received byte through the state machine
        switch(istppframeConsumerState)
        {
            case ISTPP_FRAME_CONSUMER_IDLE:
            {
                // Look for the beginning of the header
                if( ISTPP_HEADER_BYTE == rxByte )
                {
                    // Beginning of the header found, look for the rest of the header
                    istppframeConsumerState = ISTPP_FRAME_CONSUMER_RX_HEADER; 
                    // First byte of the header received
                    temporaryBytesReceived = 1;
                }
                break;
            }
            case ISTPP_FRAME_CONSUMER_RX_HEADER:
            {
                if( ISTPP_HEADER_BYTE == rxByte )
                {
                    // Next byte of the header received
                    temporaryBytesReceived++;
                    
                    if(temporaryBytesReceived == ISTPP_HEADER_SIZE )
                    {
                        // All bytes of the header received
                        istppframeConsumerState = ISTPP_FRAME_CONSUMER_RX_PACKET_INFO;
                        // Clear the counter of the bytes within the specific frame item or group of
                        // items.
                        temporaryBytesReceived = 0;
                        // Clear the counter of the total frame bytes (excluding header)
                        frameBytesReceived = 0;
                        // Clear the variable where the length of the frame data will be stored
                        istppRxFrameLength = 0;
                        // Initialize CRC calculation
                        istppRxFrameCalculatedCrc = 0xFFFFFFFF;
                    }
                }
                else
                {
                    // Frame header is not complete, go back to idle state
                    istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
                    temporaryBytesReceived = 0;
                }
                break;
            }
            case ISTPP_FRAME_CONSUMER_RX_PACKET_INFO:
            {
                // Store the received byte
                istppRxFrame[frameBytesReceived++] = rxByte;
                // All bytes between header and CRC are under CRC control
                addDataToCRC32((unsigned int *)(&istppRxFrameCalculatedCrc), rxByte);
                
                // Check if the length of the data was received
                if( frameBytesReceived == ISTPP_DATA_POS )
                {
                    // Length of the data is received, get the length
                    istppRxFrameLength = changeEndian(* (uint32_t *) &istppRxFrame[ISTPP_LENGTH_POS]);
                    // Data reception follows
                    istppframeConsumerState = ISTPP_FRAME_CONSUMER_RX_DATA;
                }
                break;
            }
            case ISTPP_FRAME_CONSUMER_RX_DATA:
            {
                // Store the received byte
                if( frameBytesReceived < ISTPP_RX_PACKET_BUFFER_SIZE )
				{
					istppRxFrame[frameBytesReceived++] = rxByte;	
				}
				else
				{
					// buffer overflow, throw packet away
					istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
					break;
				}
                
                if( temporaryBytesReceived < istppRxFrameLength )
                {
                    // Data byte was received
                    temporaryBytesReceived++;
                    // All bytes between header and CRC are under CRC control
                    addDataToCRC32((unsigned int *)(&istppRxFrameCalculatedCrc), rxByte);
                }
                else
                {
                    // Data are already received, currently processed byte
                    // is the first byte of CRC
                    istppframeConsumerState = ISTPP_FRAME_CONSUMER_RX_CRC;
                    // One byte of the CRC is already received
                    temporaryBytesReceived = 1;
                }
                break;
            }
            case ISTPP_FRAME_CONSUMER_RX_CRC:
            {
                if( frameBytesReceived < ISTPP_RX_PACKET_BUFFER_SIZE )
				{
					// Store the received byte
					istppRxFrame[frameBytesReceived++] = rxByte;	
				}
				else
				{
					// buffer overflow, throw packet away
					istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
					break;
				}
                
                // Increment the number of the received CRC bytes
                temporaryBytesReceived++;
                
                // Check if entire CRC is received
                if( temporaryBytesReceived >= ISTPP_CRC_SIZE )
                {
                    // CRC is received, check calculated versus received CRC
                    istppRxFrameReceivedCrc = changeEndian(* (uint32_t *)&istppRxFrame[frameBytesReceived - ISTPP_CRC_SIZE]);
                    
					istppRxFrameCalculatedCrc = istppRxFrameCalculatedCrc ^ 0xFFFFFFFF;
					
					istppRxFrameCalculatedCrc = reverse32(istppRxFrameCalculatedCrc)^0xFFFFFFFF;
					
					if( istppRxFrameCalculatedCrc == istppRxFrameReceivedCrc )
                    {
                        // Lock the reception of the next frame, until the currently received
                        // one is processed
                        istppRxFrameReceived = 0x01;
                        // Prepare return value of the function, which notifies if the
                        // complete frame reception was finished during this call.
                        rxFrameReceived = 0x01;
                    }
					else
					{ // the calculated CRC is not the same as received CRC. Send the ERROR MSG - CRC error
						// Get the Type and ID of corrupted message
						uint32_t Data[2] = {*((uint32_t*)&istppRxFrame[frameBytesReceived-16]),*((uint32_t*)&istppRxFrame[frameBytesReceived-12])}; // !!!! change endian
						SendMSGData(MsgNumber++, 0, ERROR_MSG , ERROR_ID_BAD_CRC, (ERROR_DATA_LENGTH/sizeof(uint32_t)), Data);	
						
					}
					
                    istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
                }
                break;
            }
            default:
            {
				istppframeConsumerState = ISTPP_FRAME_CONSUMER_IDLE;
                break;
            }
        }
        // Move to next byte
        iterator++;
    }
    *consumedBytes = iterator; 
    return rxFrameReceived;
}


// ****************************************************************************
// PUBLIC FUNCTIONS: Public functions shared with other modules
// ****************************************************************************
void ISTPPrxMessageProcessed(void)
{
    istppRxFrameReceived = 0;
}

//******************************************************************************
//! @fn int8_t NullPacketData(PacketStruct_t *p_Packet)
//!
//! @description
//! This function sets members of PacketStruct to zero(null)
//!
//! @param    p_Packet     pointer to packets structure
//!
//! @return   SUCCESS
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! NullPacketData(&Packet);
//! @endcode
//******************************************************************************
int8_t NullPacketData(PacketStruct_t *p_Packet)
{
	p_Packet->MsgNumber = 0;
	p_Packet->CRC = 0;
	p_Packet->Time = 0;
	p_Packet->ID = 0;
	p_Packet->Type = 0;
	p_Packet->Length = 0;
	return SUCCESS;
}

//******************************************************************************
//! @fn int32_t * ReadMSG(PacketStruct_t *p_Packet)
//!
//! @description
//! This function reads message from input stream
//!
//! @param    p_Packet     pointer to packets structure         
//!
//! @return   pointer to data array from received packet or null
//!
//! @calls    udi_cdc_is_rx_ready
//!           udi_cdc_getc
//!           Get32bits
//!           Get16bits
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void ReadMSG(PacketStruct_t *p_Packet)
{
	//uint8_t buff[4] = {1,2,3,4};
	
	if(!udi_cdc_is_rx_ready())
		return;
	
	while(Get32bits() != 0x23232323); //wait for star word: ####
	//udi_cdc_write_buf(buff,4);
	
	p_Packet->MsgNumber = (uint32_t)Get32bits();
	p_Packet->Time = (uint64_t)Get64bits();
	p_Packet->Type = (uint32_t)Get32bits();
	p_Packet->ID = (uint32_t)Get32bits();
	p_Packet->Length = (uint32_t)Get32bits();
	//data
	p_Packet->CRC = (uint32_t)Get32bits();
}



//******************************************************************************
//! @fn int8_t SendMSGData(uint32_t aCounter, uint16_t aType, uint32_t aID, uint32_t aLen, uint32_t *p_Sensor)
//!
//! @description
//! This function sends message to host
//!
//! @param    aCounter     number of packet
//!           aType        type of message
//!           aID          ID of message
//!           aLen         length of data array
//!           p_Sensor     data array
//!
//! @return   ERR = cant send data
//!           SUCCESS = data was send
//!
//! @calls    udi_cdc_is_tx_ready
//!           udi_cdc_write_buf
//!
//! @req
//!
//! @example
//!
//! @code
//!
//! @endcode
//******************************************************************************
int8_t SendMSGData(uint32_t aCounter, uint64_t aTimeStamp, uint16_t aType, uint32_t aID, uint32_t wordCount, uint32_t *p_Sensor)
{
	if( isUSBconnect )
	{
    	// USB is connected, check the space in the transmit buffer
    	if( udi_cdc_multi_get_free_tx_buffer(0) < ( 32 + (wordCount * 4) ) )
    	{
            // There is not enough space in the Tx buffer, throw message away
        	istppLostTxMessagesCnt++;
        	return ERR;
    	}
	}
	
    static uint8_t buffer[ISTPP_TX_BUFFER_SIZE] __attribute__ ((aligned(4))) = { 0 };
	unsigned int crc32;
	
	* (uint32_t *) &buffer[0]  = 0x23232323;
	* (uint32_t *) &buffer[4]  = changeEndian(aCounter);
	* (uint64_t *) &buffer[8]  = changeEndianLong(aTimeStamp);
	* (uint32_t *) &buffer[16] = changeEndian(aType);
	* (uint32_t *) &buffer[20] = changeEndian(aID);
	* (uint32_t *) &buffer[24] = changeEndian(wordCount*4);
	
	if(wordCount)
	{
		for (uint32_t i = 0; i < wordCount; i++)
		{
			* (uint32_t *) &buffer[28 + (i*4)] = changeEndian(*(p_Sensor + i));
		}
	}
	crc32 =	compute_crc(&buffer[4], 6 + (wordCount), CRCCU_MR_PTYPE_CCITT8023); //Main function to perform CRC by CRCCU
	* (uint32_t *) &buffer[28 + (wordCount*4)] = changeEndian(crc32);

	if(isUSBconnect == 0)
		USART_transmit_buffer(&buffer, 32 + (wordCount*4));
	else
		udi_cdc_write_buf(&buffer, 32 + (wordCount*4));
	
	return SUCCESS;
}

//******************************************************************************
//! @fn int8_t SendSwVersionMsg( void )
//!
//! @description
//! This function sends software version message to host
//!
//! @param    void
//!
//! @return   ERR_NOT_ALOC
//!           ERR
//!           SUCCESS
//!
//! @calls    NullPacketData
//!           SendMSG
//!
//! @req
//!
//! @example
//!
//! @code
//!
//! @endcode
//******************************************************************************
int8_t SendSwVersionMsg( void )
{
    uint32_t version = 0;
    PacketStruct_t packet;

    if(NullPacketData(&packet) < 0)
    {
        return ERR_NOT_ALOC;
    }

    // Encode the SW version to the 32bit variable
    version =  ((uint32_t)GetMajorSoftwareVer()) << 24;
    version |= ((uint32_t)GetMainSoftwareVer()) << 16;
    version |= ((uint32_t)GetMinorSoftwareVer()) << 8;
    version |= (uint32_t)GetPatchSoftwareVer();
    
    // Prepare packet
    packet.MsgNumber = MsgNumber++;
    packet.Type = VERSION_MSG;
    packet.ID = version;

    // Send message
    if(SendMSG(&packet) < 0)
    {
        return ERR;
    }

    return SUCCESS;
}

//******************************************************************************
//! @fn int8_t SendErrMsg(PacketStruct_t *p_Packet, uint32_t aValue)
//!
//! @description
//! This function sends error message to host
//!
//! @param    p_Packet      pointer to packets structure 
//!
//! @return   ERR_NOT_ALOC
//!           ERR
//!           SUCCESS
//!
//! @calls    NullPacketData
//!           SendMSG
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
int8_t SendErrMsg(PacketStruct_t *p_Packet, uint32_t aValue)
{
	if(NullPacketData(p_Packet) < 0)
		return ERR_NOT_ALOC;
	p_Packet->MsgNumber = MsgNumber++;
	p_Packet->Type = ERROR_MSG;
	p_Packet->ID = aValue;
	if(SendMSG(p_Packet) < 0)
		return ERR;
		
	return SUCCESS;
}

int8_t SendHealthDataMsg( uint32_t * healthData, uint8_t healthDataLen )
{
    return SendMSGData( 0, 0, HEALTH_DATA, 0, healthDataLen, healthData );
}

//******************************************************************************
//! @fn int8_t SendInitMsg(PacketStruct_t *p_Packet, uint32_t aValue)
//!
//! @description
//! This function sends initialization message to host
//!
//! @param    p_Packet      pointer to packets structure 
//!
//! @return   ERR_NOT_ALOC
//!           SUCCESS
//!
//! @calls    NullPacketData
//!           SendMSG
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
int8_t SendInitMsg(PacketStruct_t *p_Packet, uint32_t aValue)
{
	if(NullPacketData(p_Packet) < 0)
	   return ERR_NOT_ALOC;
	
	p_Packet->MsgNumber = MsgNumber++;
	p_Packet->Type = INIT_MSG;
	p_Packet->ID = aValue;
  
	SendMSG(p_Packet);
	return SUCCESS;
}
//******************************************************************************
//! @fn int8_t SendDataMsg(uint32_t aLength, uint32_t *p_Sensor, uint32_t aID)
//!
//! @description
//! This function fill packet with data and sends it to host
//!
//! @param    aLength      length of data array
//!           p_Sensor     pointer to data array 
//!           aID          ID of message
//!
//! @return   ERR = cant send data
//!           SUCCESS = data was send
//!
//! @calls    NullPacketData
//!           SendMSGData
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
int8_t SendDataMsg(uint32_t aLength, uint32_t *p_Sensor, uint64_t timeStamp, uint32_t aID)
{
	PacketStruct_t Packet1;
	
	NullPacketData(&Packet1);
		
	Packet1.MsgNumber = MsgNumber++;
	Packet1.Type = DATA_MSG;
	Packet1.ID = aID;
	Packet1.Length = aLength;
	Packet1.Time = timeStamp;
	Packet1.CRC = 0;
	
	return	SendMSGData(Packet1.MsgNumber, Packet1.Time, Packet1.Type, Packet1.ID, Packet1.Length, p_Sensor);	
}

//******************************************************************************
//! @fn int8_t SendMSG(PacketStruct_t *p_Packet)
//!
//! @description
//! This function sends packet without data array
//!
//! @param    p_Packet      pointer to packets structure
//!
//! @return   ERR = cant send data
//!           SUCCESS = data was send
//!
//! @calls    SendMSGData
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
int8_t SendMSG(PacketStruct_t *p_Packet)
{
	return SendMSGData(p_Packet->MsgNumber, p_Packet->Time, p_Packet->Type, p_Packet->ID, 0, NULL);
}

void ReadMSGrs485(PacketStruct_t *p_Packet)
{
	uint32_t rxBytesNum;
    uint32_t rxTotalBytesToConsume;
    static uint32_t rxConsumedBytes;
    
    rxConsumedBytes = 0;

#ifdef USART_LOOPBACK_TEST
	
	static uint32_t totalRxBytesNum = 0;
	static uint8_t ISTPrxDataComplete[USART_RX_BUFFER_SIZE * 2];

#endif // USART_LOOPBACK_TEST
		
	// Get the bytes received since the last call of this function
	rxBytesNum = USART_receiveData( &istppRxDataActive[istppRxNonConsumedBytes] );
	
#ifdef USART_LOOPBACK_TEST
	
	if(rxBytesNum)
	{
		memcpy( &ISTPrxDataComplete[totalRxBytesNum], istppRxData, rxBytesNum);
		totalRxBytesNum += rxBytesNum;
		if( ISTPrxDataComplete[totalRxBytesNum - 1] == 0x1F )
		{
			USART_transmit_buffer(ISTPrxDataComplete, totalRxBytesNum);
			totalRxBytesNum = 0;
		}
	}
#else

    // No loopback test - normal reception
    
    // Pass the received data through the frame parser and check
    // if complete and correct frame was received
    rxTotalBytesToConsume = rxBytesNum + istppRxNonConsumedBytes; 

    if( ISTPPframeConsumer(istppRxDataActive, rxTotalBytesToConsume, &rxConsumedBytes) )
    {
        // Entire frame received with correct CRC
        // Now get the packet data from frame buffer istppRxFrame

        // Parse the buffer
        p_Packet->MsgNumber = changeEndian(* (uint32_t *) &istppRxFrame[ISTPP_NUMBER_POS]);
        p_Packet->Time =      changeEndianLong(* (uint64_t *) &istppRxFrame[ISTPP_TIME_STAMP_POS]);
        p_Packet->Type =      changeEndian(* (uint32_t *) &istppRxFrame[ISTPP_TYPE_POS]);
        p_Packet->ID =        changeEndian(* (uint32_t *) &istppRxFrame[ISTPP_ID_POS]);
        p_Packet->Length =    changeEndian(* (uint32_t *) &istppRxFrame[ISTPP_LENGTH_POS]);
        //data
        p_Packet->CRC =       changeEndian(* (uint32_t *)&istppRxFrame[ISTPP_DATA_POS + p_Packet->Length]);
    }

    // Check if there were received additional bytes, which were not processed by the frame consumer.
    // Basically it is the scenario where one complete and correct frame was received and additional bytes
    // behind the frame were received to. This can happen, since there is not implemented any ACK mechanism.
        
    istppRxNonConsumedBytes = rxTotalBytesToConsume - rxConsumedBytes; 

    if( istppRxNonConsumedBytes > 0 )
    {
        // Some received bytes were not consumed, store them for next frame consumer call
        // Switch the receive buffer
        if( istppRxDataActive == istppRxData1 )
        {
            memcpy(istppRxData2, &istppRxData1[rxConsumedBytes], istppRxNonConsumedBytes );
            istppRxDataActive = istppRxData2;
        }
        else if( istppRxDataActive == istppRxData2 )
        {
            memcpy(istppRxData1, &istppRxData2[rxConsumedBytes], istppRxNonConsumedBytes );
            istppRxDataActive = istppRxData1;
        }
        else
        {
            // Wrong pointer to the Rx buffer
            ASSERT_ISTP( 0x00, ASSERT_ISTPP_1 );
        }
    }
#endif // USART_LOOPBACK_TEST	
}

typedef enum 
{
	Idle,
	Config,
	Logging,
	Initialization
}ISTP_STATE_e;

static ISTP_STATE_e istpState = Idle;

void decodePacket(void)
{
	uint32_t sensIndex;
    uint16_t requestedFifoReadRateHz;
    uint16_t requestedSampleRateHz;
    uint8_t requestedHealthDataStatus;
    uint8_t fifoModeRequested;
	// store msg. TYPE and ID for ACK or ERROT msg.
	uint32_t LastPacketInfo[2] = {Packet.Type,Packet.ID};
	//Checked if packet arrives
	if(Packet.Type)
	{
		switch(Packet.Type)
		{
			case FORCESTOP:
			{	
				if((Packet.ID & FORCESTOP_ID_STOP) == 1)
				{
					// send ACK MSG
					SendMSGData(MsgNumber++, 0, ACK , 0, (ERROR_DATA_LENGTH/sizeof(uint32_t)), LastPacketInfo);
					user_callback_suspend_action();
					PLMshutDownAllRails( PLM_DEFAULT_DELAY_MS );
				}
				else
				{
					// the received command is unknown !!!. Send the ERROR MSG - CRC error
					SendMSGData(MsgNumber++, 0, ERROR_MSG , ERROR_ID_CMD_FAILURE, (ERROR_DATA_LENGTH/sizeof(uint32_t)), LastPacketInfo);
				}
				break;
			}
			
			case ERROR_MSG:
			{
				NullPacketData(&Packet);
				SendErrMsg(&Packet , 1);
				break;
			}
			case CONFFILE:
			{
				// send ACK MSG
				SendMSGData(MsgNumber++, 0, ACK , 0, (ERROR_DATA_LENGTH/sizeof(uint32_t)), LastPacketInfo);	
				istpState = Config;
				
				switch(Packet.ID & 0x7F)
				{
					case CONFFILE_ID_SEND:
						ISTPMSetAlertFifoOverflow(0);
						ISTPMSetAlertSchFrameOverflow(0);
						ISTPMSetAlertTxBufferOverflow(0);

						MsgNumber = 0;

						SendSwVersionMsg();
						PLMswitchRail(POWER_RAIL_3V3, POWER_RAIL_ON, 0);
						PLMswitchRail(POWER_RAIL_5VD, POWER_RAIL_ON, 0);
						PLMswitchRail(POWER_RAIL_5VA, POWER_RAIL_ON, PLM_DEFAULT_DELAY_MS);
						SendDataFromEEPROMs(&SensMem[0]);
						SendMSGData(MsgNumber++, 0, CONTROL_MSG, CONTROL_ID_STOP_CONF, 0, NULL);
						break;
					case CONFFILE_ID_BOARD:

						break;
					case CONFFILE_CHANGE:
						//Sets initialization for each board
						{
							int axis =	(Packet.ID & 0xF0000000) >> 28;
							int board = (Packet.ID & 0x0F000000) >> 24;
							int type =	(Packet.ID & 0x00FF0000) >> 16;
							int val =	(Packet.ID & 0x0000FF00) >> 8;
							sensIndex = 0;
							while(SensMem[sensIndex].Active == 1)
							{
								if(SensMem[sensIndex].Axis == axis && SensMem[sensIndex].Board == board)
								{									
									// create mask for parameters
									uint64_t Mask = 0xFF << (4*type);
									uint64_t ConfigNumber = SensMem[sensIndex].ConfigNumber;
									// Update value with new param
									ConfigNumber = (ConfigNumber & ~Mask) | ( (uint64_t)(val & 0x0F) << (4*type) );
									SensMem[sensIndex].ConfigNumber = ConfigNumber;							
									
									WriteNewParam(GetAxis(axis), GetBoard(board), SensMem[sensIndex].ConfigNumber);
									sensIndex++;
									break;
								}
								else
								{
									sensIndex++;
								}
					
							}
						}
						break;

					case CONFFILE_ID_HEALTH_DATA:
					{
						requestedHealthDataStatus = (Packet.ID >> 8);
						if( 0x01 == requestedHealthDataStatus )
						{
							ISTPMSetHealthDataStatus(ISTP_HEALTH_DATA_ACTIVE);
							HealthDataCounter = 0;
						}
						else
						{
							ISTPMSetHealthDataStatus(ISTP_HEALTH_DATA_INACTIVE);
						}
						break;
					}
					case CONFFILE_SAMPLE_RATE:
					{
						ISTPMSetSamplingStatus(ISTP_SAMPLING_INACTIVE);
						requestedSampleRateHz = (Packet.ID >> 16);
						// Set requested sample rate to the global properties
						ISTPMSetSampleRateHz(requestedSampleRateHz);
						if( ISTP_FIFO_MODE_INACTIVE == (ISTPMGetGlobalProperties()->FifoMode) )
						{
							// If the FIFO mode is not active, sample rate is equal to the scheduler rate,
							// because sensors are sampled once with every scheduler frame
							ISTPMSetSchedulerRateHz(requestedSampleRateHz);
						}
						break;
					}
					case CONFFILE_FIFO_MODE:
						{
							fifoModeRequested = (Packet.ID >> 16);
							// Set FIFO requested information to the global properties                         
							if(fifoModeRequested == true)
							{
								ISTPMSetFifoMode(ISTP_FIFO_MODE_ACTIVE);
							}
							else
							{
								ISTPMSetFifoMode(ISTP_FIFO_MODE_INACTIVE);
							}
							break;
						}
					case CONFFILE_FIFO_READ_RATE:
						{
							// Consider this message as valid only if FIFO mode is requested, 
							// otherwise the message was sent from the PC application in wrong circumstances.
							if( ISTP_FIFO_MODE_ACTIVE == (ISTPMGetGlobalProperties()->FifoMode) )
							{
								// In case FIFO mode is active, FIFO read rate is communicated separately
								// and it's meaning is scheduler rate, because FIFOs of the sensors are
								// read once with every scheduler frame.
								requestedFifoReadRateHz = (Packet.ID >> 16);
								// Set scheduler rate global property to the value of FIFO read rate.
								ISTPMSetSchedulerRateHz(requestedFifoReadRateHz);
							}
							break;
						}
					
#ifdef IMU_SIMULATOR
					case CONFFILE_SEN_NUM:
			
						//change timers period
						virtualSensNum = (Packet.ID >> 16);
						ISTPMSetSamplingStatus(ISTP_SAMPLING_INACTIVE);
						break;
	#endif
				}
				NullPacketData(&Packet);
				istpState = Idle;
				break;
			}
			case INIT_MSG:
			{
				SendMSGData(MsgNumber++, 0, ACK , 0, (ERROR_DATA_LENGTH/sizeof(uint32_t)), LastPacketInfo);
				if(istpState == Idle)
				{
					istpState = Initialization;
					ISTPMSetSamplingStatus(ISTP_SAMPLING_INACTIVE);
					//Initialization sensors
					if( Packet.ID > 0 )
					{
						PLMswitchRail(POWER_RAIL_3V3, POWER_RAIL_ON, 0);
						PLMswitchRail(POWER_RAIL_5VD, POWER_RAIL_ON, 0);
						PLMswitchRail(POWER_RAIL_5VA, POWER_RAIL_ON, PLM_DEFAULT_DELAY_MS);
					}
					else
					{
						PLMshutDownAllRails( PLM_DEFAULT_DELAY_MS );
					}
				
					sensIndex = 0;
	#ifndef IMU_SIMULATOR
					while(SensMem[sensIndex].Active == 1)
					{
	#endif
						if (Packet.ID) // workaround to prevent sensors initialization when power is off
							InitSensorBoard( &SensMem[sensIndex] );
						sensIndex++;
	#ifndef IMU_SIMULATOR
					}
	#endif		
				}
				NullPacketData(&Packet);
				currentFrame = getTick();
				break;
			}
			case DATA_MSG:
			{
				NullPacketData(&Packet);
				break;
			}
			case CONTROL_MSG:
			{
				SendMSGData(MsgNumber++, 0, ACK , 0, (ERROR_DATA_LENGTH/sizeof(uint32_t)), LastPacketInfo);
				//Start/Stop logging data
				if(Packet.ID & CONTROL_ID_START && istpState == Initialization)
				{
					istpState = Logging;
					HealthDataCounter = 0;
					if( ISTP_FIFO_MODE_ACTIVE == (ISTPMGetGlobalProperties()->FifoMode) )
					{
						// Clear all FIFOs prior start of the reading in order
						// to avoid FIFO overflows
						sensIndex = 0;
						while(SensMem[sensIndex].Active == 1)
						{
							ClearSensorBoardFifo( &SensMem[sensIndex] );
							sensIndex++;
						}
					}
					else
					{
						sensIndex = 0;
						while(SensMem[sensIndex].Active == 1)
						{
							ClearSensorBoard( &SensMem[sensIndex] );
							sensIndex++;
						}
					}
                
					ISTPMSetSamplingStatus(ISTP_SAMPLING_ACTIVE);
					initalizeTimers( ISTPMGetGlobalProperties()->SchedulerRateHz );
					tc_start(TC0, 0);
					currentFrame = getTick();
					MsgNumber = 0;
				}
				else if(Packet.ID & CONTROL_ID_STOP && istpState == Logging)
				{
					istpState = Idle;
					ISTPMSetSamplingStatus(ISTP_SAMPLING_INACTIVE);
					//tc_stop(TC0, 0);
					//PLMshutDownAllRails(PLM_DEFAULT_DELAY_MS);
				}
				//toggleMainLeds(PIOA, LED_C, 0);
				NullPacketData(&Packet);
				break;
			}
			default:
				// the received command is unknown !!!. Send the ERROR MSG - CRC error
				SendMSGData(MsgNumber++, 0, ERROR_MSG , ERROR_ID_CMD_FAILURE, (ERROR_DATA_LENGTH/4), LastPacketInfo);	
				break;
		}
	}
}

void readSensorDataAndSendItToPc(double *timestamp)
{
	NullPacketData(&Packet);
	
	uint32_t ID = 0;
	uint32_t length = 0;
	uint32_t ActLen = 0;
	uint32_t sensIndex = 0;
#ifndef IMU_SIMULATOR	
    _SWStart(SW_ID_MEASURE);
	while(SensMem[sensIndex].Active == 1)
	{
		ActLen = 0;
		ID = SensMem[sensIndex].Axis << 24;
		ID |= SensMem[sensIndex].Board << 16;
		ID |= SensMem[sensIndex].Sensor;
        ReadSensorBoard(&SensMem[sensIndex], &SensorData[length], &ActLen);
        sensIndex++;
		length += ActLen;
	}
    _SWElapsedTimeUs(SW_ID_MEASURE);
#endif
#ifdef IMU_SIMULATOR
	int _isthiseasteregg = virtualSensNum;
	while(_isthiseasteregg > 0)
	{
		ActLen = 0;
		ID = SensMem[0].Axis << 24;
		ID |= SensMem[0].Board << 16;
		ID |= SensMem[0].Sensor;
		ReadSensorBoard(&SensMem[sensIndex], &SensorData[length], &ActLen);
		sensIndex++;
		length += 7;
		_isthiseasteregg--;
	}
	(void)_isthiseasteregg;
#endif
	double timeSince = getTimeSince();
	*timestamp = timeSince;
    SendDataMsg(length, &SensorData[0], *(uint64_t*)&timeSince, ID);
}


int8_t GetUSBState(void)
{
	return isUSBconnect;
}

void ReadIncomeMSG(PacketStruct_t *p_Packet)
{
	if(!ReadFirst)
		return ReadFirstMSG(p_Packet);
		
	if (isUSBconnect == 0)
	{
		return ReadMSGrs485(p_Packet);
	}
	else
	{
		return ReadMSG(p_Packet);
	}
}

void ReadFirstMSG(PacketStruct_t *p_Packet)
{
	#ifndef MAINBOARD54
	ReadMSGrs485(p_Packet);
	if (p_Packet->Type != 0)
	{
		isUSBconnect = 0;
		ReadFirst = true;
        udc_stop();
		return;
	}
	
	ReadMSG(p_Packet);
	if (p_Packet->Type != 0)
	{
		isUSBconnect = 1;
		ReadFirst = true;
		return;
	}
	#else
	ReadMSGrs485(p_Packet);
	isUSBconnect = 0;
	ReadFirst = true;
	#endif
}

uint32_t ISTPPgetLostTxMessagesCnt(void)
{
    return istppLostTxMessagesCnt;
}

void HealthDataProvider(double timestamp)
{
	uint32_t length = 0;
	uint32_t lenbuffer = 0;
	
#ifdef MAINBOARD54
	SPI_setDec(chip_hum);
	length += lenbuffer;
	lenbuffer = 0;
#endif

	HDcollectData(&SensorData[length], &lenbuffer);
	length += lenbuffer;
	SendMSGData(HealthDataCounter++, *(uint64_t*)&timestamp, HEALTH_DATA, 0, length, &SensorData[0]);
}

