/****************************************************************************
 Module
   TopHSMTemplate.c

 Revision
   2.0.1

 Description
   This is a template for the top level Hierarchical state machine

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 02/08/12 01:39 jec      converted from MW_MasterMachine.c
 02/06/12 22:02 jec      converted to Gen 2 Events and Services Framework
 02/13/10 11:54 jec      converted During functions to return Event_t
                         so that they match the template
 02/21/07 17:04 jec      converted to pass Event_t to Start...()
 02/20/07 21:37 jec      converted to use enumerated type for events
 02/21/05 15:03 jec      Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "TopHSM.h"
#include <stdio.h>

#include "MotorService.h"


#include "ReloadHSM.h"

#include "JoustingHSM.h" 

#include "SPIProjectService.h"
#include "BallShootService.h"


#include "MoveForwardHSM.h" 
#include "ShootingHSM.h" 

/*----------------------------- Module Defines ----------------------------*/

/*---------------------------- Module Functions ---------------------------*/
static ES_Event DuringStage1( ES_Event Event);
static ES_Event DuringStage2( ES_Event Event);
static ES_Event DuringReload( ES_Event Event);
static ES_Event DuringJoustingStage( ES_Event Event);
static ES_Event DuringTestingStage( ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, though if the top level state machine
// is just a single state container for orthogonal regions, you could get
// away without it
static MasterState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static bool DarkNight;
//static bool goalReached;
//static bool stageOver;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitMasterSM

 Parameters
     uint8_t : the priorty of this service

 Returns
     boolean, False if error in initialization, True otherwise

 Description
     Saves away the priority,  and starts
     the top level state machine
 Notes

 Author
     J. Edward Carryer, 02/06/12, 22:06
****************************************************************************/
bool InitMasterSM ( uint8_t Priority )
{
  ES_Event ThisEvent;
  //PP0 = Lateral Tape Sensor 
  //PP1 = Central Tape Sensor
  //PP2 = Push Buttons Side One
  //PP3 = Push Buttons Side Two
  //PP4 = REQUEST BALL LED (output)
  //PP5 = GAME ON LED (output)
  DDRP = 0b00110000; 
  PTP &= (BIT4LO & BIT5LO); //LEDs initially low 
  
  DDRS &= BIT2LO; //Knight Type Switch 
  
  if (PTS & BIT2HI) { // Check NightType Switch
	DarkNight = true;
  } else {
	DarkNight = false;
  }
  MyPriority = Priority;  // save our priority
  ThisEvent.EventType = ES_ENTRY;
  // Start the Master State machine
  StartMasterSM( ThisEvent );

  return true;
}

/****************************************************************************
 Function
     PostMasterSM

 Parameters
     ES_Event ThisEvent , the event to post to the queue

 Returns
     boolean False if the post operation failed, True otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostMasterSM( ES_Event ThisEvent )
{
  return ES_PostToService( MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunMasterSM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   the run function for the top level state machine 
 Notes
   uses nested switch/case to implement the machine.
 Author
   J. Edward Carryer, 02/06/12, 22:09
****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855
ES_Event RunMasterSM( ES_Event CurrentEvent )
{
   ES_Event ShootingEvent;	   
   unsigned char MakeTransition = false;/* are we making a state transition? */
   MasterState_t NextState = CurrentState;
   ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
   ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event
  /* if (CurrentEvent.EventType != 24) 
  	{
   		printf("%d , %d \n", CurrentEvent.EventType, CurrentEvent.EventParam);
  	}
  	*/
  	/*
  //TEST HARNESS
	 
    switch (CurrentState)    
   {
		case TestingStage:
			CurrentEvent = DuringTestingStage(CurrentEvent);
			break;
		case Stage1:
			CurrentEvent = DuringStage2(CurrentEvent);
			break;
		case Stage2:
			CurrentEvent = DuringStage2(CurrentEvent);
			break;
		case Recess2:
			CurrentEvent = DuringReload(CurrentEvent);
			break;
		case Stage3:
			CurrentEvent = DuringJoustingStage(CurrentEvent);
			break;
    }
    if((CurrentEvent.EventType == ES_CHANGE_STATUS)) 
    {
    	//printf("Change Status");
    	if((CurrentEvent.EventParam == 0x03))	
    	{
    	    //printf("Stage 2!");
    	    if(Ready2Reload()) 
    	    {
    			//printf("Start Stage");
    			NextState = Recess2;
				MakeTransition = true;
    	    }
    	}
	    switch ( CurrentEvent.EventParam )
	   {
	       
	   		case 0x50:
	   			 NextState = TestingStage;
				 MakeTransition = true; 
	       		 break;
	   		case 0x20:
	   			 NextState = Stage2;
				 MakeTransition = true; 
	       		 break;
	       	case 0x30:
	   			 NextState = Recess2;
				 MakeTransition = true; 
	       		 break;
	       	case 0x40:
	   			 NextState = Stage3;
				 MakeTransition = true; 
	       		 break;
	   }
    }
    if (MakeTransition == true)
    {
       //   Execute exit function for current state
       CurrentEvent.EventType = ES_EXIT;
       RunMasterSM(CurrentEvent);
       CurrentState = NextState; //Modify state variable
       //   Execute entry function for new state
       // this defaults to ES_ENTRY
       RunMasterSM(EntryEventKind);
     }
	  */
 
 //ACUTAL GAME CODE BELOW:
   
   if ((CurrentEvent.EventType != 24) && (CurrentEvent.EventType != 24) && (CurrentEvent.EventType != 23) && (CurrentEvent.EventType != 22) && (CurrentEvent.EventType != 25) && (CurrentEvent.EventType != 26) )
  	{
   		printf("%d , %d \n", CurrentEvent.EventType, CurrentEvent.EventParam);
  	}
   switch ( CurrentState )
   {
       case Waiting:
			if ((CurrentEvent.EventType == ES_CHANGE_STATUS) && (CurrentEvent.EventParam == 0x01)) {
					NextState = Stage1;
					MakeTransition = true; 
					PTP |= BIT5HI; //Turn Status LED On
			}
			break;
	   case Stage1:       
			CurrentEvent = DuringStage1(CurrentEvent);
			if (CurrentEvent.EventType == ES_STAGE_OVER) {
				NextState = Recess1;
				MakeTransition = true;
					
			}
			break;
		case Recess1:
			if (CurrentEvent.EventType == ES_ENTRY) 
			{
					ShootingEvent.EventType = ES_START_SHOOTER;
					PostBallShootService(ShootingEvent);	
			}
			if ((CurrentEvent.EventType == ES_CHANGE_STATUS) && (CurrentEvent.EventParam == 0x01)) {
				NextState = Stage2;
				MakeTransition = true;
			}
			break;
		case Stage2:
			CurrentEvent = DuringStage2(CurrentEvent);
			if (CurrentEvent.EventType == ES_STAGE_OVER) {
				NextState = Recess2;
				MakeTransition = true;	
			}
			break;
		case Recess2:
			CurrentEvent = DuringReload(CurrentEvent);
			if (CurrentEvent.EventType == ES_STAGE_OVER) {
				NextState = Stage3;
				MakeTransition = true;	
			}
			break;
		case Stage3:
			if(CurrentEvent.EventType == ES_ENTRY) 
			{
				CurrentEvent.EventParam = 1;
			}
			CurrentEvent = DuringJoustingStage(CurrentEvent);
			if (CurrentEvent.EventType == ES_STAGE_OVER) {
				NextState = Recess3;
				MakeTransition = true;	
			}
			break;
		case Recess3:
			if (CurrentEvent.EventType == ES_ENTRY) 
			{
					ShootingEvent.EventType = ES_START_SHOOTER;
					PostBallShootService(ShootingEvent);	
			}
			if ((CurrentEvent.EventType == ES_CHANGE_STATUS) && (CurrentEvent.EventParam == 0x01)) {
				NextState = SuddenDeath;
				MakeTransition = true;
			}
			break;
		case SuddenDeath:
			if(CurrentEvent.EventType == ES_ENTRY) 
			{
				CurrentEvent.EventParam = 0;
			}
			CurrentEvent = DuringJoustingStage(CurrentEvent);
			break;	
    }
	if (CurrentEvent.EventType == ES_GAME_OVER) {
		PTP &= BIT5LO; //Turn Status LED Off
		NextState  = EndOfMatch;
		MakeTransition = true;
	}
    if (MakeTransition == true)
    {
       //   Execute exit function for current state
       CurrentEvent.EventType = ES_EXIT;
       RunMasterSM(CurrentEvent);
       CurrentState = NextState; //Modify state variable
       //   Execute entry function for new state
       // this defaults to ES_ENTRY
       RunMasterSM(EntryEventKind);
     }
		 
   CurrentEvent.EventType = ES_NO_EVENT;
   return(CurrentEvent);
}
/****************************************************************************
 Function
     StartMasterSM

 Parameters
     ES_Event CurrentEvent

 Returns
     nothing

 Description
     Does any required initialization for this state machine
 Notes

 Author
     J. Edward Carryer, 02/06/12, 22:15
****************************************************************************/
void StartMasterSM ( ES_Event CurrentEvent )
{
  // local variable to get debugger to display the value of CurrentEvent
  volatile ES_Event LocalEvent = CurrentEvent;
  // if there is more than 1 state to the top level machine you will need 
  // to initialize the state variable
  CurrentState = Waiting;
  // now we need to let the Run function init the lower level state machines
  // use LocalEvent to keep the compiler from complaining about unused var
  RunMasterSM(LocalEvent);
  return;
}


/***************************************************************************
 private functions
 ***************************************************************************/

 static ES_Event DuringTestingStage( ES_Event Event)
{
    ES_Event ReturnEvent = Event; 
    ES_Event ShootingEvent;
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
        ShootingEvent.EventType = ES_START_SHOOTER;
		PostBallShootService(ShootingEvent);	
        StartMoveHSM( Event );
        StartShootingHSM(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
		RunMoveHSM(Event);
		RunShootingHSM(Event);
		ShootingEvent.EventType = ES_START_SHOOTER;
		PostBallShootService(ShootingEvent);	
    }else
    {
        ReturnEvent = RunMoveHSM(Event);
        RunShootingHSM(Event);
    }
    return(ReturnEvent);
}



static ES_Event DuringStage1( ES_Event Event)
{
    ES_Event ReturnEvent = Event; 
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {	
        StartMoveHSM( Event );
    }
    else if ( Event.EventType == ES_EXIT )
    {
		RunMoveHSM(Event);
    }else
    {
        ReturnEvent = RunMoveHSM(Event);
    }
    return(ReturnEvent);
}


static ES_Event DuringStage2( ES_Event Event)
{
    ES_Event ReturnEvent = Event; 
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
        StartMoveHSM( Event );
		StartShootingHSM(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
		RunMoveHSM(Event);
		RunShootingHSM(Event);
    }else
    {
        RunShootingHSM(Event);
		ReturnEvent = RunMoveHSM(Event);
    }
    return(ReturnEvent);
}

static ES_Event DuringReload( ES_Event Event)
{
    ES_Event ReturnEvent = Event; 
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
        StartReloadHSM( Event );
    }
    else if ( Event.EventType == ES_EXIT )
    {
		RunReloadHSM(Event);
    }else
    {
        ReturnEvent = RunReloadHSM(Event);
    }
    return(ReturnEvent);
}



static ES_Event DuringJoustingStage( ES_Event Event)
{
    ES_Event ReturnEvent = Event; 
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
        StartMoveHSM( Event );
		StartJoustingHSM( Event );
    }
    else if ( Event.EventType == ES_EXIT )
    {
		RunMoveHSM(Event);
		RunJoustingHSM(Event);
    }else
    {
		RunJoustingHSM(Event);
		ReturnEvent = RunMoveHSM(Event);
    }
    return(ReturnEvent);
}



MasterState_t QueryMasterHSM ( void )
{
   return(CurrentState);
}

bool DarkNightPlayer( void )
{
   return(DarkNight);
}