/****************************************************************************
 Module
   IRBeaconService.c

 Revision
   1.0.1

 Description
   This module contains the service for the IR Beacon detection 

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 02/22/18       r.adenekan    first pass

****************************************************************************/
// the common headers for C99 types
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>

// the headers to access the TivaWare Library
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"

/* include header files for hardware access
*/
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_timer.h"
#include "inc/hw_nvic.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"  // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"

/* include header files for the framework
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_Timers.h"
#include "ES_Events.h"

#include "IRBeaconService.h"
#include "MotorCommands.h"
#include "LolaGameMaster.h"
#include "TeamColorAndGameLEDs.h"

#include "PlayingSM.h"
// **********************************************************************
// PROTOTYPES

// READABILITY DEFINES
#define NO_SIGNAL 50// Time between which we go before we know that we have no signal <--do not change both to 5

// Period in Ticks * ( 1 / SYSCLK) =  Time is Seconds
// Desired Signal is 800 us-->32000 in ticks
#define Period_RedGoal 32000// us * 40 MHz CLK
#define Period_RedGoal_Tol 300

// Desired Signal is 700 us-->28000 in ticks
#define Period_BlueGoal  28000 // us * 40 MHz CLK
#define Period_BlueGoal_Tol 300// us * 40 MHz CLK

// Desired Signal is 600 us-->24000 in ticks
#define Period_RedReload 24000 // us * 40 MHz CLK
#define Period_RedReload_Tol 300// us * 40 MHz CLK

// Desired Signal is 500 us-->20000 in ticks
#define Period_BlueReload  20000 // us * 40 MHz CLK //should be 20k. looked like 80k ?
#define Period_BlueReload_Tol 300// us * 40 MHz CLK //

//Beacons of Interest params
#define LolaReload 0
#define OppReload 1
#define LolaGoal 2
#define OppGoal 3

// MODULE LEVEL VARIABLES
static uint8_t MyPriority;
static IRBeaconState_t CurrentState; 

static uint8_t NumTimesSeen_BeaconDesired=0;

static uint32_t Period;
static uint32_t Period_Desired;
static uint32_t Period_Desired_Tol;

static uint32_t FoundPeriod;

static uint32_t Period_LolaReload;
static uint32_t Period_LolaReload_Tol;

static uint32_t Period_OppReload;
static uint32_t Period_OppReload_Tol;

static uint32_t Period_LolaGoal;
static uint32_t Period_LolaGoal_Tol;

static uint32_t Period_OppGoal;
static uint32_t Period_OppGoal_Tol;

static uint32_t LastCapture;
static uint32_t ThisCapture;

static uint16_t StartTime;
static uint16_t StopTime;
static uint16_t AlignTime;

// **********************************************************************
// PUBLIC FUNCTIONS //
// **********************************************************************

/****************************************************************************
 Function
     InitIRBeaconService

 Parameters
     uint8_t : the priority of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, sets up the initial transition and does any
     other required initialization for this state machine

****************************************************************************/

// Saves away the priority, sets up the initial transition and does any
//     other required initialization for this state machine
bool InitIRBeaconService(uint8_t Priority)
{
	//Create a ThisEvent variable
  ES_Event_t ThisEvent;
	//Set MyPriority to be the Priority of the event
  MyPriority = Priority;

  //Initialize NumTimesSeen_BeaconDesired to 0
  NumTimesSeen_BeaconDesired=0;
  
    //If team is blue
  if (GetTeamColor() == 0) // var that needs to be read in from LolaGameMaster (0 for blue, 1 for red)
  {
		//Set the LolaReload period to be the BlueReload period
    Period_LolaReload = Period_BlueReload;
		//Set the LolaReload tolerance to be the BlueReload tolerance
    Period_LolaReload_Tol = Period_BlueReload_Tol;

		//Set the OppReload period to be the RedReload period
    Period_OppReload = Period_RedReload;
		//Set the OppReload period to be the RedReload period
    Period_OppReload_Tol =Period_RedReload_Tol;

		//Set the LolaGoal period to be the BlueGoal period
    Period_LolaGoal = Period_BlueGoal;
		//Set the LolaGoal period to be the BlueGoal period
    Period_LolaGoal_Tol = Period_BlueGoal_Tol;

		//Set the OppGoal period to be the RedGoal period
    Period_OppGoal = Period_RedGoal;
		//Set the OppGoal tolerance to be the RedGoal tolerance
    Period_OppGoal_Tol = Period_RedGoal_Tol;

  }
 //If team is red
  else 
{
		//Set the LolaReload period to be the RedReload period
    Period_LolaReload = Period_RedReload;
		//Set the LolaReload tolerance to be the RedReload tolerance		
    Period_LolaReload_Tol = Period_RedReload_Tol;

		//Set the OppReload period to be the BlueReload period
    Period_OppReload = Period_BlueReload;
		//Set the OppReload period to be the BlueReload period
    Period_OppReload_Tol = Period_BlueReload_Tol;

		//Set the LolaGoal period to be the RedGoal period		
    Period_LolaGoal = Period_RedGoal;
		//Set the LolaGoal period to be the RedGoal period
    Period_LolaGoal_Tol = Period_RedGoal_Tol;
  
		//Set the OppGoal period to be the BlueGoal period
    Period_OppGoal = Period_BlueGoal;
		//Set the OppGoal tolerance to be the BlueGoal tolerance
    Period_OppGoal_Tol = Period_BlueGoal_Tol;
    
}

  // Initialized the Interrupt in main

  // post the initial transition event
  ThisEvent.EventType = ES_INIT;
  CurrentState = InitializingIRBeaconService;
  
  if (ES_PostToService(MyPriority, ThisEvent) == true)
  {
    return true;
  }
  else
  {
    return false;
  }
}

/****************************************************************************
 Function
     PostIRBeaconService

 Parameters
     EF_Event ThisEvent , the event to post to the queue

 Returns
     boolean False if the Enqueue operation failed, True otherwise

 Description
     Posts an event to this state machine's queue
****************************************************************************/

// Posts an event to this state machine's queue
bool PostIRBeaconService(ES_Event_t ThisEvent)
{
  return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunIRBeaconService

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   Runs events for this state machine
****************************************************************************/

// Runs events for this state machine
ES_Event_t RunIRBeaconService(ES_Event_t ThisEvent)
{
	//Create a ReturnEvent variable
  ES_Event_t ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors //go back and put in the intial event!!
	//Create a NextState variable
  static IRBeaconState_t NextState; 
	//Set NextState equal to CurrentState
  NextState=CurrentState;

  switch (CurrentState)
  {
    case InitializingIRBeaconService:        // If current state is initial Psedudo State
    {
      if (ThisEvent.EventType == ES_INIT)    // only respond to ES_Init
      {
        // now put the machine into the actual initial state (WaitingForInstruction)      
        NextState = WaitingForInstruction;
      }
    }
    break;  

    case WaitingForInstruction:        
    {

      if (ThisEvent.EventType == ALIGN_WITH_BEACON) //If the EventType is ALIGN_WITH_BEACON
      {        
        //if beacon of interest is LolaReload
        if (ThisEvent.EventParam == 0)
        {
					//Set the Period_Desired to the LolaReload period
          Period_Desired=Period_LolaReload;
					//Set the Period_Desired_Tol to the LolaReload_Tol
          Period_Desired_Tol=Period_LolaReload_Tol;
        }
        //if beacon of interest is OppReload
         else if (ThisEvent.EventParam ==1)
        {
					//Set the Period_Desired to the OppReload period					
          Period_Desired=Period_OppReload;
					//Set the Period_Desired_Tol to the OppReload_Tol					
          Period_Desired_Tol=Period_OppReload_Tol;
        }

        //if beacon of interest is LolaGoal
         else if (ThisEvent.EventParam ==2)
        {
					//Set the Period_Desired to the LolaGoal period										
          Period_Desired=Period_LolaGoal;
					//Set the Period_Desired_Tol to the LolaGoal_Tol					
          Period_Desired_Tol=Period_LolaGoal_Tol;
        }

        //if beacon of interest is OppGoal
         else if (ThisEvent.EventParam ==3)
        {
					//Set the Period_Desired to the LolaGoal period	
          Period_Desired=Period_OppGoal;
					//Set the Period_Desired_Tol to the LolaGoal_Tol										
          Period_Desired_Tol=Period_OppGoal_Tol;
        }
				//Rotate Clockwise slowly
        RotateCCWSlow();

        //set next state to StartingAlignProcess
        NextState = StartingAlignProcess;
      }
    }
    break;

		//If the EventType is StartingAlignProcess
    case StartingAlignProcess:        
    {
      if (ThisEvent.EventType == DETECTED_BEACON) //If the EventType is DETECTED_BEACON
      {
        if (QueryPlayingSM() == Reload) //If the CurrentState of PlayingSM is Reload
        {
          //stop motor
          Stop();
          //post DETECTED_BEACON event to LolaGameMasterSM
          ES_Event_t Event2Post;
          Event2Post.EventType = DETECTED_BEACON;
          PostLolaGameMasterSM(Event2Post);	
          //reset NumTimesSeen_BeaconDesired counter
          NumTimesSeen_BeaconDesired=0;
          //set next state to WaitingForInstruction
          NextState = WaitingForInstruction;
        }
        else
        {
         //save time
        StartTime=ES_Timer_GetTime();
				//Set the NextState to FindingEdgeOfBeacon
        NextState = FindingEdgeOfBeacon;
        }
      }
    }
    
   break; 

	 //If ThisEvent is FindingEdgeOfBeacon
    case FindingEdgeOfBeacon:        
    {
			//If this event is a BEACON_TIMER timeout
      if ((ThisEvent.EventType == ES_TIMEOUT)  && (ThisEvent.EventParam ==  BEACON_TIMER))
      {
        //stop motor
        Stop();
        //save time
        StopTime=ES_Timer_GetTime(); 
        //calculate Align Time
        AlignTime=(StopTime-StartTime)/2; 
				//Restart the ALIGN_TIMER
        ES_Timer_InitTimer(ALIGN_TIMER,AlignTime); 
        //rotate CW slowly
        RotateCWSlow();
        //set next state to Aligning
        NextState = Aligning;
      }
    }
    break;

		//If ThisEvent is Aligning 
    case Aligning:        
    {
			//If this event is an ALIGN_TIMER timeout
      if ((ThisEvent.EventType == ES_TIMEOUT)  && (ThisEvent.EventParam ==  ALIGN_TIMER))
      {
        //stop motor
        Stop();
        //post ALIGNED_BEACON event to LolaGameMasterSM
        ES_Event_t Event2Post;
        Event2Post.EventType = ALIGNED_BEACON;
        PostLolaGameMasterSM(Event2Post);
        //reset NumTimesSeen_BeaconDesired counter
        NumTimesSeen_BeaconDesired=0;
        
        //set next state to WaitingForInstruction
        NextState = WaitingForInstruction;
      }
    }
    break;
  }  
                                   // end switch on Current State
  CurrentState=NextState;
  return ReturnEvent;
}

//Interrupt Response Routine for the beacon input capture
void IRBeacon_ISR(void)
{
// clear the source of the interrupt, the input capture event
HWREG(WTIMER0_BASE + TIMER_O_ICR) = TIMER_ICR_CAECINT; // clear by writing a one
// grab the captured value
ThisCapture = HWREG(WTIMER0_BASE + TIMER_O_TAR);
// Calculate Period
Period = ThisCapture - LastCapture;
	// Update lastCapture
LastCapture = ThisCapture;

//if period is within range by LolaGameMaster
if ((Period > (Period_Desired-Period_Desired_Tol)) && (Period < (Period_Desired+Period_Desired_Tol)))
{
  NumTimesSeen_BeaconDesired++; // We saw desired beacon , so increment our counter
  // restart the timer for reading no desired beacon signal ... 
  ES_Timer_InitTimer(BEACON_TIMER, NO_SIGNAL);
  //if seen beacon of interest enough times
  if(NumTimesSeen_BeaconDesired == 7) //not greater than or equal cause don't want to post event multiple times
  {
  //post DETECTED_BEACON event to LolaGameMaster
    ES_Event_t Event2Post;
    Event2Post.EventType = DETECTED_BEACON;
		FoundPeriod = Period;
    PostIRBeaconService(Event2Post);
  }
}
else
{
  //reset NumTimesSeen_BeaconDesired counter
  NumTimesSeen_BeaconDesired=0;
}
}

// **********************************************************************
// PRIVATE FUNCTIONS //
// **********************************************************************
void InitIRBeaconInterrupt(void)
{
  // enable the clock for the timer (Wide Timer 0)
  HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R0;
  
  // enable the clock to PortC
  HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R2;
  // can immediately start configuring the timer from here
  
  // make sure timer A is DISABLED before configuring
  HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEN;
  
  // set it up in 32bit wide (indiv, not concat) mode
  // STRANGE CONSTANT NAME ALERT - do not be alarmed
  HWREG(WTIMER0_BASE + TIMER_O_CFG) = TIMER_CFG_16_BIT;
  
  // use the full 32 bit count, init the interval load to full
  HWREG(WTIMER0_BASE + TIMER_O_TAILR) = 0xffffffff;
  
  // Timer A to capture mode (TAMR = 3, TAAMS = 0)
  // for edge time (TACMR = 1) and up-counting (TACDIR = 1)
  HWREG(WTIMER0_BASE + TIMER_O_TAMR) = 
  (HWREG(WTIMER0_BASE + TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) | (TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
  
  // set the event ot a rising edge - modify TAEVENT bits to GPTMCTL
  // Rising edge = 00, so clear the bits
  HWREG(WTIMER0_BASE + TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
  
  // **************
  // SETUP THE PORT
  
  // alternate fxn for the PortC bit 4 (WT0CCP0)
  HWREG(GPIO_PORTC_BASE + GPIO_O_AFSEL) |= BIT4HI;
  
  // map bit 4's alt fxn to WT0CCP0
  // 7 is the mux value, 16 to shift over to the right nibble
  HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) = (HWREG(GPIO_PORTC_BASE + GPIO_O_PCTL) & 0xfff0ffff) + (7<<16);
  
  // enable pin on PortC for digital I/O
  HWREG(GPIO_PORTC_BASE + GPIO_O_DEN) |= BIT4HI;
  
  // make pin4 on PortC into an input
  HWREG(GPIO_PORTC_BASE + GPIO_O_DIR) &= BIT4LO;
  
  // *********
  // INTERRUPT
  
  // back to the timer enable a local capture interrupt
  HWREG(WTIMER0_BASE + TIMER_O_IMR) |= TIMER_IMR_CAEIM;
  
  // enable Timer A in Wide Timer 0 interrupt in the NVIC
  // interrupt #94: EN2 @ Bit30 (#>>5 = ENx  :  #mod32 = Bit#)
  HWREG(NVIC_EN2) |= BIT30HI;
  
  // set the priority of the other timer to 1 (2nd) INTC
  HWREG(NVIC_PRI23) |= 1 << 21;
  
  // make sure interrupts enabled globally
  __enable_irq();
  
  // ****************
  // ENABLE THE TIMER
  
  // enable timer and enable it to be stopped by the debugger
  HWREG(WTIMER0_BASE + TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
  
  // start the timers for reading no signal ... 
  ES_Timer_InitTimer(BEACON_TIMER, NO_SIGNAL); //removed only for testing. put back!

}