/*****************************************************************************/
/* FILE NAME: main.c                            COPYRIGHT (c) Freescale 2010 */
/*                                                       All Rights Reserved */
/* DESCRIPTION:                                                              */
/* This is the main file for the XTRKUSB_MPC5602P Development Board.         */
/* Performs Temperature measurement and sends over SCI as default.			 */
/* Commands received over serial are decoded, and can put program into 'pong'*/ 
/* mode when used with demo pc application.						             */			
/*****************************************************************************/

/* Main.h includes all other relevant include files. */
#include "main.h"


extern const vuint32_t IntcIsrVectorTable[]; /* ISR Vector Table */


/********************************* Defines ***********************************/
/* Global Variables */

int32_t Result;            	/* ADC conversion results */
int32_t ResultInMv;        	/* ADC conversion results in mV */
int32_t ResultInDeg;		/* ADC conversion results in Degrees */
int32_t Offset;				/* Temp Sensor Offset */
uint32_t ctr1 = 0;	 		/* Counter */
uint32_t ctr2 = 0;	 		/* Counter */
uint32_t ctr3 = 0;	 		/* Counter */
uint32_t Pit1Ctr = 0;   	/* Counter for PIT 1 interrupts */
char input_command[80];		/* Buffer for input commands */
uint8_t input_index = 0;	/* Index for storing into buffer */
uint8_t LED_count = 0;		/* Counter for LED strobing */
uint8_t play_pong = 0;		/* Boolean for entering pong */
uint8_t pong_quit = 0;		/* Boolean for quitting pong */

/******************************* Functions ***********************************/


/****************************************************************
* FUNCTION :strcmp
* DESCRIPTION : Compares two input strings and outputs 0 if they
*				match.
* INPUTS : string 1 and comparison string.
* OUTPUTS : 0 if strings match
****************************************************************/

int strcmp (const char * s1, const char * s2)
{
    for(; *s1 == *s2; ++s1, ++s2)
        if(*s1 == 0)
            return 0;
    return *(unsigned char *)s1 < *(unsigned char *)s2 ? -1 : 1;
}


/****************************************************************
* FUNCTION :init_Modes_Clock
* DESCRIPTION : This initializes the Clock Generation Module and 
*				sets PLL0 to 64MHz and System Clock. 
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void init_Modes_Clock(void)

{

  ME.MER.R = 0x000025FF;            /* Enable DRUN, RUN0, SAFE, RESET modes */
                                    /* Initialize PLL before turning it on: */
  CGM.CMU_0_CSR.R = 0x000000004;    /* Clock Monitor FXOSC > FIRC/4 (4MHz); no PLL monitor */
  CGM.FMPLL[0].CR.R = 0x02400100;   /* 8 MHz xtal: Set PLL0 to 64 MHz */   

  ME.RUNPC[0].R = 0x000000FE;       /* All peripherals on */
  ME.RUN[0].R = 0x001F0074;         /* RUN0 cfg: 16MHzIRCON,OSC0ON,PLL0ON,syclk=PLL0 */
  ME.RUNPC[1].R = 0x00000010; 	    /* Peri. Cfg. 1 settings: only run in RUN0 mode */
  ME.PCTL[92].R = 0x01;           	/* PIT, RTI: select ME_RUN_PC[1] */
  ME.PCTL[32].R = 0x01; 	      	/* MPC56xxB/P/S ADC 0: select ME.RUNPC[1] */                               
  ME.PCTL[48].R = 0x01; 	      	/* MPC56xxB/P/S LINFlex: select ME.RUNPC[1] */ 
                                
  /* Mode Transition to enter RUN0 mode: */
  
  ME.MCTL.R = 0x40005AF0;         	/* Enter RUN0 Mode & Key */
  ME.MCTL.R = 0x4000A50F;         	/* Enter RUN0 Mode & Inverted Key */  
  
  while (ME.GS.B.S_MTRANS == 1) {}  /* Wait for mode transition to complete */    
                                   
  while(ME.GS.B.S_CURRENTMODE != 4) {} /* Verify RUN0 is the current mode */                          

}

/****************************************************************
* FUNCTION :init_IO
* DESCRIPTION : This initializes the IO Module (SIUL)
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void init_IO(void)
{

	/* Setup IO For Switches and LED's */
	
	SIU.PCR[50].R = 0x0100;			/* PD2 used for switch input (IBE=1) */
	SIU.PCR[51].R = 0x0100;			/* PD3 used for switch input (IBE=1) */

	SIU.PCR[53].R = 0x0220;			/* Enable PD5 as output for LED (OBE=1, ODE=1) */
	SIU.PCR[54].R = 0x0220;			/* Enable PD6 as output for LED (OBE=1, ODE=1) */
	SIU.PCR[55].R = 0x0220;			/* Enable PD7 as output for LED (OBE=1, ODE=1) */
  
	/* Setup ATD Input Port For Temp Sensor */
	
	SIU.PCR[66].R = 0x2000;         /* Initialize PE[2] as ADC0_AN5 */	 
	
}
/****************************************************************
* FUNCTION :disableWatchdog
* DESCRIPTION : This disables the software watchdog
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void disableWatchdog(void) {
  SWT.SR.R = 0x0000c520;     /* Write keys to clear soft lock bit */
  SWT.SR.R = 0x0000d928; 
  SWT.CR.R = 0x8000010A;     /* Clear watchdog enable (WEN) */
}        

/****************************************************************
* FUNCTION :init_Interrupts
* DESCRIPTION : This initializes Interrupts 
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void init_Interrupts(void)
{
	
	initIrqVectors();			/* Initialize exceptions: only need to load IVPR */
	INTC.MCR.B.HVEN = 0;       	/* Initialize for SW vector mode */
	INTC.MCR.B.VTES = 0;       	/* Use default vector table 4B offsets */
	INTC.IACKR.R = (uint32_t) &IntcIsrVectorTable[0];    /* INTC ISR table base */
	
	INTC.PSR[64].R = 4;			/* Set ADC Watchdog Priority */
	INTC.PSR[62].R = 5;		    /* ADC EOC Priority */
	INTC.PSR[79].R = 8;			/* Lin Rx Handler */
    INTC.PSR[4].R = 6;		    /* Software interrupt 4 IRQ priority = 2 */

	/* Initialize PIT */
	
	PIT.PITMCR.R = 0x00000001;       	/* Enable PIT and configure stop in debug mode */
  	PIT.CH[1].LDVAL.R = 64000;       	/* Timeout= 64K sysclks x 1sec/64M sysclks= 1 ms */
  	PIT.CH[1].TCTRL.R = 0x000000003; 	/* Enable PIT1 interrupt & start PIT counting */ 
  	INTC.PSR[60].R = 1;           		/* PIT 1 interrupt vector with priority 1 */

	INTC.CPR.B.PRI = 0;					/* Decrease Current Priority to 0 */
  	asm(" wrteei 1");	    	   		/* Enable external interrupts */

}

/****************************************************************
* FUNCTION :init_ADC
* DESCRIPTION : This sets up the ADC module
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void init_ADC(void)
{
	/* Set Up ADC */
	ADC_0.MCR.R = 0x00000000;       	 /* Initialize ADC0 for one shot mode */
  	ADC_0.NCMR[0].R = 0x00000020;   	 /* Enable Sampling on CH5 (PE(0)) */
  	ADC_0.CTR[0].R = 0x00008606;    	 /* Conversion times for 32MHz ADClock */
  	
  	/* Initialize Watchdog */
  	ADC_0.THRHLR[0].B.THRH    = 0xC8;    /* Upper threshold 0 value (30oC) (With 8oC Offset) */  
  	ADC_0.THRHLR[0].B.THRL    = 0xAF;    /* Lower threshold 0 value (18oC) (With 8oC Offset) */  
  	ADC_0.TRC[0].B.THREN      = 1;       /* Threshold 0 enable                				 */   
  	ADC_0.TRC[0].B.THRCH      = 5;  	 /* ADC channel for threshold 0 (CH5) 				 */
  	ADC_0.IMR.R 			  = 0x0012;	 /* Enable EOC Interrupt 			  				 */  
  	ADC_0.WTIMR.B.MSKWDG0H    = 1;       /* Watchdog interrupt on threshold 0 				 */ 
  	ADC_0.WTIMR.B.MSKWDG0L    = 1;       /* Watchdog interrupt on threshold 0 				 */   

}

/****************************************************************
* FUNCTION :init_SCI
* DESCRIPTION : This initializes the LINFlex Module For SCI Comms 
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void init_SCI(void)
{

	/* Enable LINFlex Tx and Rx I/O */

	SIU.PCR[18].R = 0x400;			/* PB2 = LIN_0 Tx (Pin set to ALT1) */
	SIU.PCR[19].R = 0x503;			/* PB3 = LIN_0 Rx (Pin set to input) */

	/* Config LINFlex */
		
	LINFLEX_0.LINCR1.B.SLEEP 	= 0; /* LINFLEX in init mode */
	LINFLEX_0.LINCR1.B.INIT 	= 1; /* LINFLEX in init mode */
	LINFLEX_0.LINCR1.B.AWUM		= 1;
	LINFLEX_0.UARTCR.B.UART 	= 1; /* UART */
	
	LINFLEX_0.UARTCR.B.TDFL 	= 0; /* 1byte buffer */
	LINFLEX_0.UARTCR.B.RDFL 	= 0; /* 1byte buffer */
	LINFLEX_0.UARTCR.B.RXEN 	= 1; /* Rx enable */
	LINFLEX_0.UARTCR.B.TXEN 	= 1; /* Tx enable */
	LINFLEX_0.UARTCR.B.PCE   	= 0;  
	LINFLEX_0.UARTCR.B.PCE  	= 0; /* no Parity */
	LINFLEX_0.UARTCR.B.WL   	= 1; /* 8bit data */
	
	LINFLEX_0.LINIER.B.DRIE		= 1; /* Enable Data Receive Completed Interrupt */

	/* Set Up Baud Rate Dividers */
	
	LINFLEX_0.LINIBRR.B.DIV_M 	= 416; /* 9200 Baud (more or less at 16 MHz) */
	LINFLEX_0.LINFBRR.B.DIV_F 	= 11;  	

	LINFLEX_0.LINCR1.B.INIT  	= 0; /* LINFLEX out of init mode */
		
}


/****************************************************************
* FUNCTION :strobe_LEDs
* DESCRIPTION : This strobes LEDs either right or left, based on input
* INPUTS : left as integer (1 or 0), and right as integer (1 or 0)
* OUTPUTS : None
****************************************************************/

void strobe_LEDs(uint8_t left, uint8_t right)
{
	uint8_t is_left = left;			/* Store inputs */
	uint8_t is_right = right;
		
	ctr3 = 0;	
	
	SIU.GPDO[53].B.PDO = 1;			/* Turn all LED's off */
	SIU.GPDO[54].B.PDO = 1;
	SIU.GPDO[55].B.PDO = 1;

	
	if (is_left == 1)				/* If input command = left, cycle through LED's right to left */
	{
	LED_count = 55;
		while (LED_count < 56 && LED_count > 52)
		{
			while (ctr3 < 1000000)
			{
				SIU.GPDO[LED_count].B.PDO = 0;		
			    ctr3 ++;
			}
		
			SIU.GPDO[LED_count].B.PDO = 1;
			LED_count--;		
			ctr3 = 0;
		}
	}
	
	SIU.GPDO[53].B.PDO = 1;
	SIU.GPDO[54].B.PDO = 1;
	SIU.GPDO[55].B.PDO = 1;

	if (is_right == 1)				/* If input command = right, cycle through LED's left to right*/
	{
	LED_count = 53;
		while (LED_count < 56 && LED_count > 52)
		{
			while (ctr3 < 1000000)
			{
				SIU.GPDO[LED_count].B.PDO = 0;		
			    ctr3 ++;
			}
		
			SIU.GPDO[LED_count].B.PDO = 1;
			LED_count++;		
			ctr3 = 0;
		}
	}

		
}


/***********************************************************************
* FUNCTION :playpong
* DESCRIPTION : This plays pong by sending left or right commands to PC
* INPUTS : None
* OUTPUTS : None
************************************************************************/

void playpong (void)
{

/* First need to disable all interrupts apart from LINFlex Rx */

	INTC.PSR[64].R = 0;			/* Set ADC Watchdog Priority */
	INTC.PSR[62].R = 0;		    /* ADC EOC Priority */
    INTC.PSR[4].R =  0;		    /* Software interrupt 4 IRQ priority = 2 */
	INTC.PSR[60].R = 0;         /* PIT 1 interrupt vector with priority 1 */
	
	while (play_pong ==1)
	{
		  	/* Check for switch inputs */
	  	if (SIU.GPDI[50].B.PDI == 0)
		{
			while (SIU.GPDI[50].B.PDI == 0)
			{
			/* Send right command over SCI */
			u_printf("%s\n","right");
			/* Set LED to show Right */ 
			SIU.GPDO[53].B.PDO = 1;	
			SIU.GPDO[54].B.PDO = 0;	
			SIU.GPDO[55].B.PDO = 1;	
			}
	    	

		}
		
	    if (SIU.GPDI[51].B.PDI == 0)
		{
			while (SIU.GPDI[51].B.PDI == 0)
			{
			/* Send left command over SCI */
			u_printf("%s\n","left"); 
			/* Set LED to show Left */
			SIU.GPDO[53].B.PDO = 1;	
			SIU.GPDO[54].B.PDO = 1;
			SIU.GPDO[55].B.PDO = 0;	
			}
	    	
		}
		
		if (pong_quit == 1)
		{
			play_pong = 0;
		}
	}
	

	/* Re-enable all interrupts */
		
	INTC.PSR[64].R = 4;			/* Set ADC Watchdog Priority */
	INTC.PSR[62].R = 5;		    /* ADC EOC Priority */
	INTC.PSR[4].R =  6;		    /* Software interrupt 4 IRQ priority */
	INTC.PSR[60].R = 1;         /* PIT 1 interrupt vector with priority 1 */

	/* Reset LED's */
	SIU.GPDO[53].B.PDO = 1;
	SIU.GPDO[54].B.PDO = 1;	
	SIU.GPDO[55].B.PDO = 1;

}

/******************************* Main ***********************************/

void main (void) {	
  
  vuint32_t i = 0;		/* Dummy idle counter */
  init_Modes_Clock();  	/* MPC56xxP/B/S: Initialize mode entries, set sysclk = 64 MHz*/
  init_IO();			/* Initialize IO */
  disableWatchdog();    /* Disable watchdog */
  init_Interrupts();	/* Initialize exceptions: only need to load IVPR */
  init_ADC();			/* Initialize ADC Module */
  init_SCI();			/* Initialize SCI Module */
  
  Offset = 0x0258;		
  
  while (1) { 			/* Loop Forever */
  	i++;
  	
  	if (play_pong == 1) /* If pong command received, jump to subroutine */
  	{
  		playpong();
  	}
  }	
}



/******************************* Interrupt Service Routines ***********************************/


/****************************************************************
* FUNCTION :Pit1ISR
* DESCRIPTION : This handles the PIT interrupt, and triggers ADC conversion
* INPUTS : None
* OUTPUTS : None
****************************************************************/

void Pit1ISR(void) {

  ctr2++;
  
  if (ctr2 == 10000)
  {
	INTC.SSCIR[4].R = 2;      				/*  Then invoke software interrupt 4 to strobe LED's */
	ctr2 = 0;
  }
  
  if (Pit1Ctr == 100) {       				/* 0.1 second for sysclk = 64 MHz FIRC */ 
	
	/* Start ADC conversion, EOC Interrupt will update Results */
	ADC_0.MCR.B.NSTART=1;     				/* Trigger normal conversions for ADC0 */
  	Pit1Ctr = 0;              				/* Reset counter */
  	
  		if (ctr1 == 1000)					/* Only want to send every second */
  		{	
			u_printf("%d\n", ResultInDeg);	/* Send Result to Terminal */
			ctr1 = 0; 						/* Clear Counter */
  		}
  }
  
  ctr1 ++;
  Pit1Ctr ++;
  PIT.CH[1].TFLG.B.TIF = 1;   		/* Clear PIT 1 flag by writing 1 */
}


/****************************************************************
* FUNCTION :ADC0_WD_isr
* DESCRIPTION : This handles the ADC EOC Interrupt
* INPUTS : None
* OUTPUTS : None
/****************************************************************/

void ADC0_WD_isr(void)
{ 

	while (ADC_0.CDR[5].B.VALID != 1) {};   		/* Wait for last scan to complete */

	Result = ADC_0.CDR[5].B.CDATA; 
    ResultInMv =  (5000*Result/0x3FF);				/* Converted result in mv */
    ResultInDeg = (int32_t) ((ResultInMv-Offset)/10) - 8;	/* Calculate result in Degrees (-7oC offset for accuracy)*/
	
	/* Check the ADC Watchdog Flags */
	
	if(ADC_0.WTISR.B.WDG0H) 
	/* Upper threshold breached */
	{
	  	/* Check if above 50oC */
	  	//if (ADC_0.CDR[5].B.CDATA > 0xE1)
	  	if (ResultInDeg > 50)
	  	/* Light Amber Only */	
	  	{	
	    	SIU.GPDO[55].B.PDO = 1;
	    	SIU.GPDO[54].B.PDO = 1;	
	    	SIU.GPDO[53].B.PDO = 0;
	    	ADC_0.WTISR.R = 0x00000010; /* clear flag */
	    	ADC_0.ISR.R   = 0x00000012; /* clear flag */ 
	  	}
	  	
	  	else
	  	{
	  	/*Must be between 30oC and 50oC so light green and amber */
	  	SIU.GPDO[55].B.PDO = 1;		
	    SIU.GPDO[54].B.PDO = 0;	
	    SIU.GPDO[53].B.PDO = 0;
	    ADC_0.WTISR.R = 0x00000010; /* clear flag */
	    ADC_0.ISR.R   = 0x00000012; /* clear flag */ 
	  	}
	  }
	  
	  else if(ADC_0.WTISR.B.WDG0L) 
	  /* Lower threshold breached */				
	  {
	  	/* Check if below 9oC */
	  	//if (ADC_0.CDR[5].B.CDATA < 0x7C)	
	  	if (ResultInDeg < 8)
	  	/* Light Blue Only */
	  	{	
	    	SIU.GPDO[55].B.PDO = 0;
	    	SIU.GPDO[54].B.PDO = 1;	
	    	SIU.GPDO[53].B.PDO = 1;
	    	ADC_0.WTISR.R = 0x00000001; /* clear flag */
	    	ADC_0.ISR.R   = 0x00000012; /* clear flag */ 
	  	}
	  	else
	  	{
	  	/*Must be between 18oC and 8oC so light green and blue */	  	
	    SIU.GPDO[55].B.PDO = 0;
	    SIU.GPDO[54].B.PDO = 0;
	    SIU.GPDO[53].B.PDO = 1;
	    ADC_0.WTISR.R = 0x0000001;  /* clear flag */ 
	    ADC_0.ISR.R   = 0x00000012; /* clear flag */ 
	  	}
	  }
	  
	  else 
	  {
	  /* Must be within normal range as no watchdog interrupts - Light Green */
	  	SIU.GPDO[55].B.PDO = 1;
	    SIU.GPDO[54].B.PDO = 0;	
	    SIU.GPDO[53].B.PDO = 1;
	    ADC_0.ISR.R   = 0x00000012; /* clear flag */ 
	  }



}

/****************************************************************
* FUNCTION :LinRx_ISR
* DESCRIPTION : This handles LIN Received messages
* INPUTS : None
* OUTPUTS : None
/****************************************************************/

void LinRx_ISR(void)
{

  	INTC.SSCIR[79].R = 1;		  				/* Clear channel's flag */ 
  	LINFLEX_0.UARTSR.R = 4;	  					/* Clear Receive flag */
  	
  												
  	if (LINFLEX_0.BDRM.B.DATA4 == '\n') 		/* When end of line character received */
  	{
  		/* Check the command received */
  		
		if (strcmp(input_command, "right")==0)	
		{
		 strobe_LEDs(1,0);
		}
		
		if (strcmp(input_command, "left")==0)
		{
		 strobe_LEDs(0,1);
		}
		
  		if (strcmp(input_command, "pong")==0)
  		{
  			play_pong = 1;
  			pong_quit = 0;
  		}
  		
  		if (strcmp(input_command, "quit")==0)
  		{
  			pong_quit = 1;
  			play_pong = 0;
  		}
  		
  	  	input_command[input_index] = 0;
  		input_index = 0;
  	}
  	
  	else	/* Update array with next character */
  	{
	  	input_command[input_index]= LINFLEX_0.BDRM.B.DATA4;
	  	input_index++;  		
  	}
  	
	 
}

/****************************************************************
* FUNCTION :SWIRQ4_ISR
* DESCRIPTION : This handles the Software Interrupt and Strobes LEDs
* INPUTS : None
* OUTPUTS : None
/****************************************************************/

void SWIRQ4_ISR(void)
{
	ctr3 = 0;
	
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 0;		
	    SIU.GPDO[54].B.PDO = 1;	
	    SIU.GPDO[53].B.PDO = 1;
	    ctr3 ++;
	}
	
	ctr3 = 0;
	
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 1;		
	    SIU.GPDO[54].B.PDO = 0;	
	    SIU.GPDO[53].B.PDO = 1;
	    ctr3 ++;
	}
	
	ctr3 = 0;
	
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 1;		
	    SIU.GPDO[54].B.PDO = 1;	
	    SIU.GPDO[53].B.PDO = 0;
	    ctr3 ++;
	}
	
	ctr3 = 0;	
		
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 0;		
	    SIU.GPDO[54].B.PDO = 1;	
	    SIU.GPDO[53].B.PDO = 1;
	    ctr3 ++;
	}
	
	ctr3 = 0;
	
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 1;		
	    SIU.GPDO[54].B.PDO = 0;	
	    SIU.GPDO[53].B.PDO = 1;
	    ctr3 ++;
	}
	
	ctr3 = 0;
	
	while (ctr3 < 500000)
	{
		SIU.GPDO[55].B.PDO = 1;		
	    SIU.GPDO[54].B.PDO = 1;	
	    SIU.GPDO[53].B.PDO = 0;
	    ctr3 ++;
	}
	
	ctr3 = 0;		
  	INTC.SSCIR[4].R = 1;		  /* Clear channel's flag */  
	
}