//******************************************************************************
// Include(s) for Internal Component API(s)
//******************************************************************************
#include "init.h"
#include "conf_sleepmgr.h"
#include "sysclk.h"
#include "udc.h"
#include "Protocol_iSTP.h"
#include "conf_usb.h"
#include "toggle.h"
#include "usart_rs485mode.h"
#include <ISTPManager.h>
#include <StopWatch.h>
#include <CRC.h>
#include <eeprom.h>
#include "spi.h"
#include "powerlinemanager.h"

extern void inv_icm30xxx_sleep(int ms);
//******************************************************************************
// Global Data
//******************************************************************************
PacketStruct_t Packet = {0,};       // iSTP data frame structure
unsigned int currentFrame = 0;
unsigned int virtualSensNum = 16;

int main (void)
{
	Bool taskFinished;
	uint16_t HealthDataSender = 0;
	
	board_init();
	initalizeTimers( ISTPMGetGlobalProperties()->SchedulerRateHz );
    _SWInit();
	sleepmgr_init();
	sysclk_init();
	udc_start();
	USART_Init();
    // System clock settings are finalized, update the peripheral clock
    // value used by SPI module.
    SPI_UpdatePeriphClk();
    // By default prepare SPI for communication with EEPROM
    EEPROM_init_spi();
    // Enable the CRCCU peripheral clock before using this peripheral 	
    pmc_enable_periph_clk(ID_CRCCU);	
	
	#ifdef MAINBOARD54
	if(!udc_include_vbus_monitoring())
	{
		UDC_VBUS_EVENT(true);
	}
	#endif
	
	NullPacketData(&Packet);

    ISTPMResetLedAlertCode();
	currentFrame = getTick();
	
	while(1)
	{ 
		toggleMainLeds(PIOA, LED_R, (currentFrame & 0x80));
		while(currentFrame == getTick()); // detect the start of a new frame
		currentFrame = getTick();

		//Sends data from sensors to host
		if(ISTPMGetSamplingStatus() == ISTP_SAMPLING_ACTIVE)
		{
			double timestamp = 0.0;
            _SWStart(SW_ID_MEASURE_AND_SEND);
			readSensorDataAndSendItToPc(&timestamp);
            _SWElapsedTimeUs(SW_ID_MEASURE_AND_SEND);
            // Kind of watchdog, however SW realized
            ISTPMSafetyCheckPointSet();
			
			//Send Health Data
			if(ISTPHealthDataStatusGet() && ISTPSampleRateGet() == ++HealthDataSender)
			{
				HealthDataSender = 0;
				HealthDataProvider(timestamp);
			}
		}
		NullPacketData(&Packet);
                
		// Check for the received packet
        ReadIncomeMSG(&Packet);
		// Perform the eventual actions specific to the received packet
        decodePacket();
        // Acknowledge the packet processing is done
        ISTPPrxMessageProcessed();
        
        if( !(currentFrame % 128) )
        {
            ISTPMLedAlertCode(); 
        }       

        
		taskFinished = (currentFrame == getTick()); // Are we still in the same frame? if so, we've finished in time    
            
        if (!taskFinished && (ISTPMGetSamplingStatus() == ISTP_SAMPLING_ACTIVE))
		{
		    ISTPMSetAlertSchFrameOverflow(1);
            SendErrMsg(&Packet , 2);
		    currentFrame = getTick(); // reset current frame, to make sure we'll start from the beginning of a new frame
		}

        // Refresh watchdog timer
        wdt_restart(WDT);
	}
	return 1;
}


