/*
 * scheduler.c
 *
 * Created: 5/6/2016 1:04:36 PM
 *  Author: e813622
 */ 

#include "scheduler.h"
#include "Protocol_iSTP.h"
#include "USBinterrupts.h"
#include "ConfigFile.h"
#include "init.h"
#include "tc.h"

extern unsigned  int timerfreq;
extern PacketStruct_t Packet;
extern unsigned int SensorData[512];
SensorMemory_t SensMem[128];


void scheduler(void) 
{
	//Reads receive packet
	ReadMSG(&Packet);
	//if data from PC app received
	if(Packet.Type)
	switch(Packet.Type)
	{
		case FORCESTOP:
		{
			if((Packet.ID & FORCESTOP_ID_STOP) == 1)
			{
				user_callback_suspend_action();
				PowerLinesManager(0); ///TBD power up requested lines
			}
			
			break;
		}
		case ERROR_MSG:
		{
			NullPacketData(&Packet);
			break;
		}
		case CONFFILE:
		{
			static int msg;
			//toggleMainLeds(PIOA, LED_C, 1);
			//SEnds configuration packets
			if (Packet.ID & CONFFILE_ID_SEND)
			{
				PowerLinesManager(1);
				int OnlyOnceSensorMem = 0;
				//SetDataIntoEEPROMs_bmg160(axis_B, board_1);
				SendDataFromEEPROMs(&SensMem[0]);
				SendMSGData(Packet.MsgNumber++, CONTROL_MSG, CONTROL_ID_STOP_CONF, 0, NULL);
			}
			else if (Packet.ID & CONFFILE_ID_BOARD)
			{
				//Send again only one board
				//TO DO: CODE HERE
			}
			else if (Packet.ID & CONFFILE_CHANGE)
			{
				//Sets initialization for each board
				msg = (int)Packet.ID >> 4;
				if (msg == 0) ;
				int axis = (msg & 0x0F000000) >> 24;
				int board = (msg & 0x00F00000) >> 20;
				int type = (msg & 0x0000FF00) >> 8;
				int val = msg & 0x000000FF;
				unsigned int sensIndex = 0;
				while(SensMem[sensIndex].Active == 1)
				{
					if(SensMem[sensIndex].Axis == axis && SensMem[sensIndex].Board == board)
					{
						if(type == 1)
						{
							int a1 = (((int)SensMem[sensIndex].ConfigNumber & 0x0000FF00) >> 8);
							int a2 = val;
							SensMem[sensIndex].ConfigNumber = (uint16_t)(a1 << 8);
							SensMem[sensIndex].ConfigNumber |= (uint16_t)a2;
						}
						else
						{
							int a1 = val;
							int a2 = ((int)SensMem[sensIndex].ConfigNumber & 0x000000FF);
							SensMem[sensIndex].ConfigNumber = (uint16_t)(a1 << 8);
							SensMem[sensIndex].ConfigNumber |= (uint16_t)a2;
						}
						sensIndex++;
						break;
					}
					else
					{
						sensIndex++;
					}
					
				}
			}
			else if(Packet.ID & CONFFILE_ID_TIMER)
			{
				//change timers period
				timerfreq = (Packet.ID >> 16);
				initalizeTimers();
			}
			/*toggleMainLeds(PIOA, LED_C, 0);
			delay_ms(300);
			toggleMainLeds(PIOA, LED_C, 1);
			delay_ms(300);
			toggleMainLeds(PIOA, LED_C, 0);*/
			NullPacketData(&Packet);
			break;
		}
		case INIT_MSG:
		{
			//Initialization sensors
			//toggleMainLeds(PIOA, LED_C, 1);
			PowerLinesManager(Packet.ID);
			uint32_t sensIndex = 0;
			while(SensMem[sensIndex].Active == 1)
			{
				InitSensors(SensMem[sensIndex].Driver,SensMem[sensIndex].ConfigNumber,GetAxis(SensMem[sensIndex].Axis-1),GetBoard(SensMem[sensIndex].Board-1));
				sensIndex++;
			}
			//toggleMainLeds(PIOA, LED_C, 0);
			NullPacketData(&Packet);
			break;
		}
		case DATA_MSG:
		{
			//Actualy unused
			//toggleMainLeds(PIOA, LED_C, 1);
			//toggleMainLeds(PIOA, LED_C, 0);
			NullPacketData(&Packet);
			break;
		}
		case CONTROL_MSG:
		{
			//Start/Stop logging data
			//toggleMainLeds(PIOA, LED_C, 1);
			if(Packet.ID & CONTROL_ID_START)
			{
				CanSendData = true;
				tc_start(TC0, 0);
			}
			else if(Packet.ID & CONTROL_ID_STOP)
			{
				CanSendData = false;
				tc_stop(TC0, 0);
				PowerLinesManager(0);
			}
			//toggleMainLeds(PIOA, LED_C, 0);
			NullPacketData(&Packet);
			break;
		}
		
		if(CanSendData == true && send_next == true)
		{
			send_next = false;
			NullPacketData(&Packet);
			
			uint32_t ID = 0;
			uint32_t length = 0;
			uint32_t ActLen = 0;
			uint32_t sensIndex = 0;
			cpu_irq_disable();
			while(SensMem[sensIndex].Active == 1)
			{
				ActLen = 0;
				ID = SensMem[sensIndex].Axis << 24;
				ID |= SensMem[sensIndex].Board << 16;
				ID |= SensMem[sensIndex].Sensor;
				float *p = NULL;
				p = ReadSensors(SensMem[sensIndex].Driver,
				SensMem[sensIndex].ConfigNumber,
				GetAxis((int)SensMem[sensIndex].Axis-1),
				GetBoard((int)SensMem[sensIndex].Board-1),
				&ActLen);
				for(uint32_t i = 0; i < ActLen; i++)
				SensorData[length + i] = float2Uint32(*(p + i));
				sensIndex++;
				length += ActLen;
			}
			cpu_irq_enable();
			SendDataMsg(length, &SensorData[0], ID);
		}
	}
}

//******************************************************************************
//! @fn void PowerLinesManager(uint32_t power)
//!
//! @description
//! Toggle power lines on main board
//!
//! @param    power       which line will turn on
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
static void PowerLinesManager(uint32_t power)
{
	if(power > 0)
		power = INIT_MSG_ID_3V3 | INIT_MSG_ID_5VD | INIT_MSG_ID_5VA | INIT_MSG_ID_EXT;
		
	if((power & INIT_MSG_ID_3V3) == 1)
	{
		PIOB->PIO_SODR = PWR_3V3;
	}
	else
	{
		PIOB->PIO_CODR = PWR_3V3;
	}
	
	if((power & INIT_MSG_ID_5VD) == 2)
	{
		PIOB->PIO_SODR = PWR_5VD;
	}
	else
	{
		PIOB->PIO_CODR = PWR_5VD;
	}
	
	if((power & INIT_MSG_ID_5VA) == 4)
	{
		PIOB->PIO_SODR = PWR_5VA;
	}
	else{
		PIOB->PIO_CODR = PWR_5VA;
	}
	
	if((power & INIT_MSG_ID_EXT) == 8)
	{
		PIOB->PIO_SODR = PWR_EXT;
	}
	else
	{
		PIOB->PIO_CODR = PWR_EXT;
	}
	delay_ms(100);
	return;
}

//******************************************************************************
//! @fn uint32_t float2Uint32 (float input)
//!
//! @description
//! Converts float to unsigned 32bits integer
//!
//! @param    input     float number
//!
//! @return   input number as uint32
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
static unsigned int float2Uint32 (float input)
{
	union
	{
		float d;
		uint32_t t;
	}
	test_union;
	
	test_union.d = input;
	return (test_union.t);
}
//******************************************************************************
//! @fn void SysTick_Handler(void)
//!
//! @description
//! 1ms timer interrupt routine
//!
//! @param    None
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void SysTick_Handler(void)
{
	
}
//******************************************************************************
//! @fn void TC0_Handler(void)
//!
//! @description
//! 100Hz timer interrupt routine
//!
//! @param    None
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
void TC0_Handler(void)
{
	uint32_t dummy;
	dummy = tc_get_status(TC0, 0);
	(void) dummy;
	
	static uint16_t counter = 0;
	counter++;
		
	send_next = true;
	
	if((counter >> 5) & 0x0001)
	{
	//	toggleMainLeds(PIOA, LED_C, 1);
	}
}


//******************************************************************************
//! @fn void VBUS_Handler (void)
//!
//! @description
//! VBUS interrupt
//!
//! @param    None
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
static void VBUS_Handler (void)
{
	if(PIOA->PIO_PDSR & VBUS)
  {
		udc_attach();
	}
	else
  {
		udc_detach();
	}		

}
//******************************************************************************
//! @fn void toggleGPIO (Pio* port, uint32_t pin)
//!
//! @description
//! Turn on/off LED
//!
//! @param    port      ports number
//!           pin       pins number
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
static void toggleGPIO (Pio* port, uint32_t pin)
{
	if(port->PIO_ODSR & pin)
  {
		port->PIO_CODR = pin;
	}
	else
  {
		port->PIO_SODR = pin;
	}

}
//******************************************************************************
//! @fn void toggleMainLeds (Pio* port, uint32_t pin, uint8_t TurnOn)
//!
//! @description
//! Turn on/off main LED
//!
//! @param    port      ports number
//!           pin       pins number
//!           TurnOn    0 = turn off
//!
//! @return   None
//!
//! @calls    None
//!
//! @req
//!
//! @example 
//! 
//! @code
//! 
//! @endcode
//******************************************************************************
static void toggleMainLeds (Pio* port, uint32_t pin, uint8_t TurnOn)
{
	if(TurnOn == 0)
	{
		if(port->PIO_ODSR & pin)
			port->PIO_CODR = pin;
	}
	else
	{
		if(!(port->PIO_ODSR & pin))
			port->PIO_SODR = pin;
	}
}