//******************************************************************************
//  Include(s) for External Component API(s)
//******************************************************************************
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
//******************************************************************************
// Include(s) for Common Component API(s)
//******************************************************************************
#include <sam4sd32c.h>
#include <conf_board.h>
#include <udi_cdc.h>
#include "delay.h"
#include "eeprom.h"
#include "spi.h"
//******************************************************************************
// Include(s) for Internal Component API(s)
//******************************************************************************
#include "Protocol_iSTP.h"
#include "ConfigFile.h"
#include "usart_rs485mode.h"
#include "toggle.h"
#include "dataConversions.h"
#include "CRC.h"
//******************************************************************************
// Support Function Forward Declaration
//******************************************************************************

//******************************************************************************
// Defines
//******************************************************************************

//******************************************************************************
// Typedefs
//******************************************************************************
typedef enum
{
	Main,
	Comment,
	Param	
}ReadPhase_e;
//******************************************************************************
// Local Data
//******************************************************************************
static int End = 0;
static int EndIndex = 0;
static int OnlyOnce = 0;
static int IndexData = 0;
static int p_end = 0;

#ifdef IMU_SIMULATOR

static int NumOfSensorIMU = 0;

#endif //IMU_SIMULATOR

//externs 
extern int readSensIndex;
extern uint32_t MsgNumber;
extern unsigned int virtualSensNum;


//extern SensorMemory_t SensMem;
//******************************************************************************
//! @fn void toggleLeds (Pio* port, uint32_t pin)
//!
//! @description
//! Toggle on/off LED
//!
//! @param    port     port number
//!           pin      pin number
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void toggleLeds (Pio* port, uint32_t pin)
{
	if(port->PIO_ODSR & pin)
  {
		port->PIO_CODR = pin;
	}
	else
  {
		port->PIO_SODR = pin;
	}

}
//******************************************************************************
//! @fn sensor_axis_e GetAxis(int aAxis)
//!
//! @description
//! Converts number to enum member
//!
//! @param    aAxis     Axis number x = 0
//!
//! @return   member of  sensor_axis_e
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
sensor_axis_e GetAxis(int aAxis)
{
	if(aAxis == 0)
		return axis_A;
	else if(aAxis == 1)
		return axis_B;
	else if(aAxis == 2)
		return axis_C;
	else
		return axis_mainboard;
}
//******************************************************************************
//! @fn sensor_board_e GetBoard(int aBoard)
//!
//! @description
//! Converts number to enum member
//!
//! @param    aBoard     Board number(1. board = 0)
//!
//! @return   member of sensor_board_e
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
sensor_board_e GetBoard(int aBoard)
{
	switch (aBoard)
	{
		case 0: return board_1;
		case 1: return board_2;
		case 2: return board_3;
		case 3: return board_4;
		case 4: return board_5;
		case 5: return board_6;
		default: return board_1;
	}
}

//******************************************************************************
//! @fn void SendDataFromEEPROMs(SensorMemory *p_sens)
//!
//! @description
//! reads all data from available EEPROM and sends it into PC.
//!
//! @param    p_sens    save informations about active sensor in it
//!
//! @return   None
//!
//! @calls    GetAxis
//!           GetBoard
//!           EEPROM_read_status
//!           ReadEEPROM
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void SendDataFromEEPROMs(SensorMemory_t *p_sens)
{
	uint8_t eepromReadResult;
	uint32_t sensIndexer = 0;
	uint32_t ID = 0;
	int poprve = 0;
	uint16_t Address = 0;
	sensor_axis_e axis = GetAxis(0);
	sensor_board_e board = GetBoard(0);
	
#ifdef IMU_SIMULATOR
	for (uint32_t i = 0; i < virtualSensNum; i++)
	{
		switch (i)
		{
			case 0:
			case 15:
			case 30:
			case 45:
			case 60:
			case 75:
			ID = axis_mainboard << 24;
			ID |= (i / 15) << 16;
			axis = axis_mainboard;
			board = GetBoard(i);
			break;
			case 90:
			case 105:
			case 120:
			case 135:
			case 150:
			case 165:
			ID = axis_A << 24;
			ID |= ((i - 90)/15) << 16;
			axis = GetAxis(0);
			board = GetBoard(i - 6);
			break;
			case 180:
			case 195:
			case 210:
			case 225:
			case 240:
			case 255:
			ID = axis_B << 24;
			ID |= ((i - 180)/15) << 16;
			axis = GetAxis(1);
			board = GetBoard(i - 12);
			break;
			case 270:
			case 285:
			case 300:
			case 315:
			case 330:
			case 345:
			ID = axis_C << 24;
			ID |= ((i - 270)/15) << 16;
			axis = GetAxis(2);
			board = GetBoard(i - 18);
			break;
		}
		
		uint32_t _helper = 0;
		uint32_t _offset = i;
		ID &= 0xFFFF0000;
		while (_helper < 15)
		{		
			ID |= (1 << _helper);
			_helper++;
			if ((_offset + _helper) >= virtualSensNum)
			{
				i += _helper;
				break;
			}
		}
		NumOfSensorIMU = _helper;
		ReadEEPROM(&p_sens[sensIndexer++], ID, axis, board, &Address, &poprve);
		i += 14;
	}
	return;
#endif
	
#ifndef MAINBOARD40
    // Read main board
    ID = axis_mainboard << 24;
	ID |=  GetBoard(board_mb) << 16;
	axis = GetAxis(axis_mainboard);
    board = GetBoard(board_mb);
	ReadEEPROM(&p_sens[sensIndexer++], ID, axis, board, &Address, &poprve);
#endif
	
	for(int i  = 0; i < 3; i++)
	{
		for(int j = 0; j < 6; j++)
		{	
			Address = 0;
			poprve = 0;
			ID = (i << 24);
			ID |= (j << 16);
			axis = GetAxis(i);
			board = GetBoard(j);
			
			static uint16_t newStatus = 0;
			
			// Read EEPROM status register in order to decide, whether the board is present
			#ifdef MAINBOARD54
			newStatus = EEPROM_read_data(axis, board, 0);
			if(newStatus == 0x23)
			#else
			newStatus = EEPROM_read_status(axis, board);
 			if(newStatus == 0x00)
			#endif
			{
						// Assuming status register of zero is not enough for detection of connected EEPROM !
						// If the correct configuration is read from EEPROM, assume the EEPROM is really connected
						eepromReadResult = ReadEEPROM(&p_sens[sensIndexer], ID, axis, board, &Address, &poprve);
						
						if( eepromReadResult )
						{
							// Correct configuration read from EEPROM, let the content be stored and move forward
							// with the sensor board index
							sensIndexer++;
						}
				else
				{
					// Clean the possible invalid data
					p_sens[sensIndexer].Active = 0;
					p_sens[sensIndexer].Axis = 0;
					p_sens[sensIndexer].Board = 0;
					p_sens[sensIndexer].ConfigNumber = 0;
					p_sens[sensIndexer].Driver = 0;
					p_sens[sensIndexer].LengthData = 0;
					p_sens[sensIndexer].Sensor = 0;
				}
			
			}				
		}
	}
	
	return;
}
//******************************************************************************
//! @fn uint16_t ConvertAxis(sensor_axis_e ax)
//!
//! @description
//! Converts enums member to number
//!
//! @param    ax    axis
//!
//! @return   number equal to input axis
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
uint16_t ConvertAxis(sensor_axis_e ax)
{
	if(ax == axis_A)
		return CONFIG_HEADER_AXIS_X;
	else if(ax == axis_B)
		return CONFIG_HEADER_AXIS_Y;
	else
		return CONFIG_HEADER_AXIS_Z;
}
//******************************************************************************
//! @fn uint16_t ConvertBoard(sensor_board_e bo)
//!
//! @description
//! Converts enums member to number
//!
//! @param    bo    board
//!
//! @return   number equal to input board
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
uint16_t ConvertBoard(sensor_board_e bo)
{
	if(bo == board_1)
		return CONFIG_HEADER_BOARD_1;
	else if(bo == board_2)
		return CONFIG_HEADER_BOARD_2;
	else if(bo == board_3)
		return CONFIG_HEADER_BOARD_3;
	else if(bo == board_4)
		return CONFIG_HEADER_BOARD_4;
	else if(bo == board_5)
		return CONFIG_HEADER_BOARD_5;
	else
		return CONFIG_HEADER_BOARD_6;
}
void WriteNewParam(sensor_axis_e aAxis, sensor_board_e aBoard, uint32_t aNewConf)
{
	uint16_t newStatus = EEPROM_read_status(aAxis, aBoard);
	int _Scrooge = 0;
	char _charParamStart = 0x26; // &
	char _charParamEnd = 0x24; // $
	int _index = 0;
	int _indexMax = 0x00001FFF;
	
	if(newStatus != 0)
	{
		return;
	}
	
	while (_index < _indexMax)
	{
		if(EEPROM_read_data(aAxis, aBoard, _index) == _charParamStart)
			break;
		_index++;
	}
	
	while (_index < _indexMax)
	{
		uint8_t _eepromDat = EEPROM_read_data(aAxis, aBoard, _index);
		if(_eepromDat == _charParamEnd)
			break;
		else if(_eepromDat == 0x26)
		{
			_Scrooge++;
		}
		_index++;
	}
	
	uint8_t params[8];
	params[7] = (uint8_t)((aNewConf & 0xF0000000) >> 28);
	params[6] = (uint8_t)((aNewConf & 0x0F000000) >> 24);
	params[5] = (uint8_t)((aNewConf & 0x00F00000) >> 20);
	params[4] = (uint8_t)((aNewConf & 0x000F0000) >> 16);
	params[3] = (uint8_t)((aNewConf & 0x0000F000) >> 12);
	params[2] = (uint8_t)((aNewConf & 0x00000F00) >> 8);
	params[1] = (uint8_t)((aNewConf & 0x000000F0) >> 4);
	params[0] = (uint8_t)((aNewConf & 0x0000000F) >> 0);
	
	for(int i = 0; i < _Scrooge; i++)
	{
		EEPROM_write_data(aAxis, aBoard, ++_index, params[i]);
	}
}
//******************************************************************************
//! @fn SensorMemory ReadEEPROM(uint32_t ID, sensor_axis axis, sensor_board board, uint16_t *p_Address, int *p_poprve)
//!
//! @description
//! Reads data from specific EEPROM and call SendDataFromEEPROMs() at the end
//!
//! @param    ID        ID for configuration packet
//!           axis      is member of sensor_axis_e enum.
//!           board     is member of sensor_board_e enum.
//!           p_Address start address
//!           poprve
//!
//! @return   Information about actives sensors
//!
//! @calls    SendConfigMSG
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
uint8_t ReadEEPROM(SensorMemory_t * ap_Sens,uint32_t aID, sensor_axis_e aAxis, sensor_board_e aBoard, uint16_t *ap_Address, int *ap_poprve)
{
	// Failure as default
	uint8_t readingResult = 0;
	
	End = 0;
	EndIndex = 0;
	IndexData = 0;
	p_end = 0;
	ReadPhase_e _readPhase = Main;
	uint16_t _Address = (*ap_Address);
	uint16_t _Status = 0;
	//Read data
	uint8_t _Data[0x00001FFF];
	
	for(int x = 0; x < 0x00001FFF; x++)
		_Data[x] = 0;
		
	//READ WHOLE MEMORY
	if( axis_mainboard != aAxis )
		for(int i = 0; i < 0x00001FFF; i++)
			_Data[i] = EEPROM_read_data(aAxis, aBoard, _Address++);
	else if (axis_mainboard == aAxis)
	{
        
	}
	
	int _index = 0;
	while(1)
	{
		// Search for the end of the data
		if(_Data[_index] == 0x24 && _Data[_index + 1] == 0)
		{
			EndIndex = _index;
			break;
		}
		else
		{
			_index++;
		}
	}
	
	// Valid data-end delimiter was not found.
	// Do not continue with data parsing
	if(EndIndex >= 0x00001FFF)
	{
		return readingResult;
	}
	
#ifdef IMU_SIMULATOR
	int _simIndex = 7; 
	uint8_t _DataTemp[_index + 1];
	for(int q = 0; q <= _index; q++)
	{
		_DataTemp[q] = _Data[q];
	}
	//CHANGE number of sensor
	for(int q = 2; q <= NumOfSensorIMU; q++)
	{
		_Data[_simIndex++] = 0x2C;
		_Data[_simIndex++] = (0x30 & (uint8_t)q);
	}
	_Data[_simIndex++] = 0x25;
	//CHANGE data length
	int _simValuesNum = NumOfSensorIMU * 7;
	int _ten = _simValuesNum % 10;
	_Data[_simIndex++] = (uint8_t)_ten;
	_Data[_simIndex++] = (uint8_t)(_simValuesNum - 10*_ten);
	_Data[_simIndex++] = 0x25;
	//CHANGE sensors information
	for (int q = 1; q <= NumOfSensorIMU; q++)
	{
		for (int d = 11; d < (_index - 2); d++)
		{
			_Data[_simIndex++] = _DataTemp[d];
		}
		_Data[_simIndex++] = 0x25;
		_Data[_simIndex++] = 0x25;
	}
	_Data[_simIndex++] = 0x24;
	_index = _simIndex;
	(void)_DataTemp;
	(void)_simIndex;
	(void)_ten;
	(void)_simValuesNum;
#endif
		
	IndexData++;
	
	if(aAxis == axis_A)
		ap_Sens->Axis = 0;
	else if(aAxis == axis_B)
		ap_Sens->Axis = 1;
	else if(aAxis ==axis_C)
		ap_Sens->Axis = 2;
	else if(aAxis == axis_mainboard)
		ap_Sens->Axis = 3;
		
	if(aBoard == board_1)
		ap_Sens->Board = 0;
	else if(aBoard == board_2)
		ap_Sens->Board = 1;
	else if(aBoard == board_3)
		ap_Sens->Board = 2;
	else if(aBoard == board_4)
		ap_Sens->Board = 3;
	else if(aBoard == board_5)
		ap_Sens->Board = 4;
	else if(aBoard == board_6)
		ap_Sens->Board = 5;
	else if(aBoard == board_mb)
		ap_Sens->Board = 6;
	
	uint8_t driverNum[4];
	driverNum[0] = 0;
	driverNum[1] = 0;
	driverNum[2] = 0;
	driverNum[3] = 0;
	
	driverNum[0] = _Data[IndexData++];
	driverNum[1] = _Data[IndexData++];
	driverNum[2] = _Data[IndexData++];
	driverNum[3] = _Data[IndexData++];
	IndexData++;
	ap_Sens->Driver = ((uint32_t)driverNum[0] << 24);
	ap_Sens->Driver |= ((uint32_t)driverNum[1] << 16);
	ap_Sens->Driver |= ((uint32_t)driverNum[2] << 8);
	ap_Sens->Driver |= (uint32_t)driverNum[3];
	
	while(End == 0)
	{
		switch(_readPhase)
		{
			case Main:
				_Status = CONFFILE;
				_readPhase = Comment;
				//int SenCounter = 0;
				while(_Data[IndexData] != 0x25) //%
				{
					if(_Data[IndexData] == 0x31)
					{
							aID |= CONFIG_HEADER_SENSOR_1;
					}
					else if(_Data[IndexData] == 0x32)
					{
							aID |= CONFIG_HEADER_SENSOR_2;
					}
					else if(_Data[IndexData] == 0x33)
					{
							aID |= CONFIG_HEADER_SENSOR_3;
					}
					else if(_Data[IndexData] == 0x34)
					{
							aID |= CONFIG_HEADER_SENSOR_4;
					}
					else if(_Data[IndexData] == 0x35)
					{
							aID |= CONFIG_HEADER_SENSOR_5;
					}
					else if(_Data[IndexData] == 0x36)
					{
							aID |= CONFIG_HEADER_SENSOR_6;
					}
					else if(_Data[IndexData] == 0x37)
					{
							aID |= CONFIG_HEADER_SENSOR_7;
					}
					else if(_Data[IndexData] == 0x38)
					{
							aID |= CONFIG_HEADER_SENSOR_8;
					}
					else if(_Data[IndexData] == 0x39)
					{
							aID |= CONFIG_HEADER_SENSOR_9;
					}
					else if(_Data[IndexData] == 0x3A)
					{
							aID |= CONFIG_HEADER_SENSOR_10;
					}
					else if(_Data[IndexData] == 0x3B)
					{
							aID |= CONFIG_HEADER_SENSOR_11;
					}
					else if(_Data[IndexData] == 0x3C)
					{
							aID |= CONFIG_HEADER_SENSOR_12;
					}
					else if(_Data[IndexData] == 0x3D)
					{
							aID |= CONFIG_HEADER_SENSOR_13;
					}
					else if(_Data[IndexData] == 0x3E)
					{
							aID |= CONFIG_HEADER_SENSOR_14;
					}
					else if(_Data[IndexData] == 0x3F)
					{
							aID |= CONFIG_HEADER_SENSOR_15;
					}

					IndexData++;
				}
			
					ap_Sens->LengthData = (uint32_t)_Data[++IndexData] * 10;
					ap_Sens->LengthData += (uint32_t)_Data[++IndexData];
				IndexData++;
				for(int i = IndexData; i < 0x00001FFF; i++)
				{
					if(_Data[i] == 0x24)
					{
						p_end = i;
						break;
					}
				}
			
				if(IndexData >= EndIndex)
					End = 1;
				else
				{
					SendConfigMSG(_Status, &_Data[0], p_end, aID, IndexData);
					IndexData = p_end;
				}
			break;
			case Comment:
			//Send comments ------------------------------------------------------------------------------------------
				_Status = COMENT;
				if(_Data[IndexData] == 0x24 && _Data[IndexData + 1] == 0)
				{
					End = 1;
					break;
				}
				else if(_Data[IndexData] == 0x24 && _Data[IndexData + 1] == 0x26)
				{
					_readPhase = Param;
					break;
				}
				else if(_Data[IndexData] == 0)
				{
					End = 1;
					break;
				}
				while(_Data[IndexData] != 0x25) //%
				{
					if(_Data[IndexData] != 0x2C || _Data[IndexData] != 0x23) // ,
					{
						aID = aID & 0xFFFF0000;
						if(_Data[IndexData] == 0x31)
							aID |= CONFIG_HEADER_SENSOR_1;
						else if(_Data[IndexData] == 0x32)
							aID |= CONFIG_HEADER_SENSOR_2;
						else if(_Data[IndexData] == 0x33)
							aID |= CONFIG_HEADER_SENSOR_3;
						else if(_Data[IndexData] == 0x34)
							aID |= CONFIG_HEADER_SENSOR_4;
						else if(_Data[IndexData] == 0x35)
							aID |= CONFIG_HEADER_SENSOR_5;
						else if(_Data[IndexData] == 0x36)
							aID |= CONFIG_HEADER_SENSOR_6;
						else if(_Data[IndexData] == 0x37)
							aID |= CONFIG_HEADER_SENSOR_7;
						else if(_Data[IndexData] == 0x38)
							aID |= CONFIG_HEADER_SENSOR_8;
						else if(_Data[IndexData] == 0x39)
							aID |= CONFIG_HEADER_SENSOR_9;
						else if(_Data[IndexData] == 0x3A)
							aID |= CONFIG_HEADER_SENSOR_10;
						else if(_Data[IndexData] == 0x3B)
							aID |= CONFIG_HEADER_SENSOR_11;
						else if(_Data[IndexData] == 0x3C)
							aID |= CONFIG_HEADER_SENSOR_12;
						else if(_Data[IndexData] == 0x3D)
							aID |= CONFIG_HEADER_SENSOR_13;
						else if(_Data[IndexData] == 0x3E)
							aID |= CONFIG_HEADER_SENSOR_14;
						else if(_Data[IndexData] == 0x3F)
							aID |= CONFIG_HEADER_SENSOR_15;
					}
					IndexData++;
				}
				for(int i = IndexData; i < 0x00001FFF; i++)
				{
					if(_Data[i] == 0x24)
					{
						p_end = i;
						break;
					}
				}
			
				if(IndexData >= EndIndex)
					End = 1;
				else
				{
					SendConfigMSG(_Status, &_Data[0], p_end-1, aID, IndexData+1);
					IndexData = p_end;
					if(_Data[IndexData + 1] == 0x26)
					{
						IndexData++;
						OnlyOnce++;
					}
				}
			break;
			case Param:
			//Send Parameter List -------------------------------------------------------------------------------------------
				_Status = COMENT;
				
				if(_Data[IndexData] == 0x24 && _Data[IndexData + 1] == 0)
				{
					End = 1;
					break;
				}
				else if(_Data[IndexData] == 0x24 && _Data[IndexData + 1] != 0x26)
				{
					_readPhase = Comment;
					break;
				}
				else if(_Data[IndexData] == 0)
				{
					End = 1;
					break;
				}
				
				int _Scrooge = 0;
				IndexData++;
				for(int i = IndexData; i < 0x00001FFF; i++)
				{
					if(_Data[i] == 0x24)
					{
						p_end = i;
						break;
					}
					else if(_Data[i] == 0x26)
					{
						_Scrooge++;
					}
				}

				//save default parameters
				ap_Sens->ConfigNumber = 0;
				
				for(int i = 0; i < _Scrooge; i++)
				{
					ap_Sens->ConfigNumber |= (uint64_t)_Data[++p_end] << (4 * i); 
				}

				p_end++;
								if(IndexData >= EndIndex)
					End = 1;
				else
				{
					SendConfigMSG(_Status, &_Data[0], p_end, aID & 0xFFFF0000, IndexData);
					IndexData = p_end;
					IndexData++;
				}
				
			break;
		}
	} //end while
	
	ap_Sens->Sensor = aID;
	ap_Sens->Active = 1;
	// If the code reached here, it is success
	readingResult = 1;
	return readingResult;
}


//******************************************************************************
//! @fn void SendConfigMSG(uint16_t aStatus, uint8_t aData[0x00001FFF], int aEndIndex, uint32_t aID, int aStartIndex)
//!
//! @description
//! sends input data array as configure packet to host
//!
//! @param    aStatus     defining type of message, configure packet or comment packet
//!           aData[0x00001FFF] vector including data for sending process from EEPROM
//!           aEndIndex   Index to end
//!           aID         ID for sending packet
//!           aStartIndex Index to start  owing to useful data in data vector
//!
//! @return   Information about actives sensors
//!
//! @calls    udi_cdc_write_buf
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void SendConfigMSG(uint32_t aType, uint8_t *ap_Data, int aEndIndex, uint32_t aID, int aStartIndex)
{
	uint32_t dataLength = (aEndIndex - aStartIndex) + 1;
	uint32_t MyMsgNumber = MsgNumber++;
	
	//while ((dataLength % 4) != 0) //align data
	//{
		//dataLength++;
	//}
	static uint8_t buffer[4096] __attribute__ ((aligned (4))) ;
	unsigned int crc32;
	
	* (uint32_t *) &buffer[0]  = 0x23232323;
	* (uint32_t *) &buffer[4]  = changeEndian(MyMsgNumber);
	* (uint64_t *) &buffer[8]  = changeEndianLong(0);
	* (uint32_t *) &buffer[16] = changeEndian(aType);
	* (uint32_t *) &buffer[20] = changeEndian(aID);
	* (uint32_t *) &buffer[24] = changeEndian(dataLength);
	
	
	uint32_t i = 0;
	for(int x = aStartIndex; x < (aEndIndex + 1); x++) 
	{
		 buffer[28+i] = *(ap_Data + x);
		 i++;
	}
	
	crc32 = 0xFFFFFFFF;
	for ( uint j = 4; j < (28 + dataLength); j++)
	{
		addDataToCRC32 (&crc32, buffer[j]);
	}
		/* XOR the output value */
		crc32 = crc32 ^ 0xFFFFFFFF;
		
		crc32 = reverse32(crc32)^0xFFFFFFFF;

	* (uint32_t *) &buffer[28 + dataLength] = changeEndian(crc32);
	
	if(GetUSBState() == 0)
	{
		USART_transmit_buffer(&buffer, 32 + dataLength);
		delay_ms(10);
	}
	else
	{
		udi_cdc_write_buf(&buffer, 32 + dataLength);
	}
}
