DEFINE MODULE FollowerSPIService

INITIALIZE:
    Define CurrentState as WaitingCommand
    Define MyPriority as the priority of this service
    Define NewCommand as 0 (volatile)
    Define CommandToSend as 0 (default response message)

FUNCTION InitFollowerSPIService(Priority):
    Set MyPriority to Priority
    PRINT "Initializing Follower SPI Service..."
    
    Set CurrentState to WaitingCommand
    
    DISABLE SPI1

    // Configure SPI1 for SLAVE mode
    DISABLE analog functionality on SPI pins
    Set SPI1 pins (SDI, SDO, SS, SCK)
    Configure SPI1 registers:
        - Set as SLAVE
        - Clock idle high
        - Data changes on falling edge
        - Enable SS control
        - Use 8-bit mode
        - Enable enhanced buffer mode
    CLEAR SPI buffer and overflow flag
    ENABLE SPI1 receive interrupt
    ENABLE SPI1
    
    ENABLE global interrupts
    
    CREATE Event ES_INIT
    RETURN Post Event to Service Queue

FUNCTION PostFollowerSPIService(Event):
    Post Event to Service Queue
    RETURN True if successful, False otherwise

FUNCTION RunFollowerSPIService(CurrentEvent):
    DEFINE ReturnEvent as ES_NO_EVENT

    SWITCH(CurrentState):

        CASE WaitingCommand:
            SWITCH(CurrentEvent.Type):
                CASE ES_INIT:
                    PRINT "Follower SPI Initialized"
                    BREAK
                
                CASE ES_LEADERSENT:  // Leader sent a command
                    READ receivedData from Event.Param
                    PRINT "Received SPI Data:", receivedData
                    
                    CREATE Event NewEvent
                    
                    IF receivedData == 10:  // ALIGN_BEACON
                        PRINT "Aligning with beacon"
                        Set CurrentState to WaitingBeaconAlign
                        NewEvent.Type = BeaconAlign
                        PostDriveService(NewEvent)
                        Set CommandToSend = COMMAND_RECEIVED
                    ELSE IF receivedData == 20:  // ALIGN_TAPE
                        PRINT "Aligning with tape"
                        Set CurrentState to CompletingAction
                        Start Timer POLL_TIMER (2s)
                        Set CommandToSend = COMMAND_RECEIVED
                    ELSE IF receivedData == 30:  // MOVE_TO_NODE
                        PRINT "Moving to node"
                        Set CurrentState to CompletingAction
                        Start Timer POLL_TIMER (2s)
                        NewEvent.Type = MoveToNode
                        PostDriveService(NewEvent)
                        Set CommandToSend = COMMAND_RECEIVED
                    ELSE IF receivedData == 40:  // TURN
                        PRINT "Turning"
                        Set CurrentState to CompletingAction
                        Start Timer POLL_TIMER (2s)
                        Set CommandToSend = COMMAND_RECEIVED
                    ELSE IF receivedData == 50:  // DROP_BLOCK
                        PRINT "Dropping block"
                        Set CurrentState to CompletingAction
                        Set CommandToSend = COMMAND_RECEIVED
                    BREAK

        CASE WaitingBeaconAlign:
            SWITCH(CurrentEvent.Type):
                CASE ES_LEADERSENT:  // Check if leader sent beacon alignment update
                    READ receivedData from Event.Param
                    
                    IF receivedData == 60:  // Beacon Found
                        PRINT "Beacon Found"
                        NewEvent.Type = ES_BEACONFOUND
                        PostDriveService(NewEvent)
                        Set CommandToSend = 0
                        Set CurrentState to WaitingCommand
                    BREAK

        CASE CompletingAction:
            SWITCH(CurrentEvent.Type):
                CASE ES_DriveServiceFinished: // Drive service completed action
                    PRINT "Task Completed! Sending response to leader."
                    Set CommandToSend = COMMAND_COMPLETED
                    Set CurrentState to WaitingCommand
                    BREAK

    RETURN ReturnEvent

FUNCTION ISR SPI1_RX_ISR():
    IF SPI1 receive buffer is NOT empty:
        READ NewCommand from SPI1BUF
        CREATE Event ES_LEADERSENT
        Set Event.Param to NewCommand
        Post Event to FollowerSPIService
        WRITE CommandToSend to SPI1BUF (send response)
    
    CLEAR SPI1RX interrupt flag

// Hardware
#include <xc.h>

// Event & Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_Port.h"
#include "terminal.h"
#include "dbprintf.h"
#include <sys/attribs.h>
#include "DriveService.h"
#include "FollowerSPIService.h"


/*----------------------------- Module Defines ----------------------------*/
// These times assume a 10ms/tick timing
#define ONE_SEC 1000
#define HALF_SEC (ONE_SEC / 2)
#define TWO_SEC (ONE_SEC * 2)
#define FIVE_SEC (ONE_SEC * 5)

#define COMMAND_RECIEVED 1
#define COMMAND_COMPLETED 2

/*---------------------------- Module Variables ---------------------------*/
// Module-level Priority variable
static uint8_t MyPriority;
static volatile uint8_t NewCommand = 0;
static uint8_t CommandToSend = 0;

static FollowerSPIState_t CurrentState;

/*------------------------------ Module Code ------------------------------*/

bool InitFollowerSPIService(uint8_t Priority)
{
    ES_Event_t ThisEvent;
    MyPriority = Priority;

    clrScrn();
    DB_printf("Init Function for SPI (Follower) with Interrupts\n");
    
    CurrentState = WaitingCommand;

    __builtin_disable_interrupts();

    SPI1CONbits.ON = 0;  // Disable SPI1 before configuring

    // Disable analog functionality on SPI pins
    ANSELAbits.ANSA1 = 0;  // SDI (RA1)
    ANSELAbits.ANSA0 = 0;  // SS (RA0)
    ANSELBbits.ANSB14 = 0; // SCK (RB14)

    // Set SPI pins
    TRISAbits.TRISA1 = 1;  // SDI (RA1) as input
    SDI1R = 0b0000;        // Map RA1 to SPI1 SDI
    TRISAbits.TRISA2 = 0;  // Set RA2 (SDO) as output
    RPA2R = 0b0011;        // Map RA2  pin 9 to SPI1 SDO
    TRISAbits.TRISA0 = 1;  // SS (RA0) as input
    SS1R = 0b0000;
    TRISBbits.TRISB14 = 1; // SCK (RB14) as input

    // Configure SPI1 as SLAVE
    SPI1CONbits.MSTEN = 0; // Slave mode
    SPI1CONbits.CKP = 1;   // Clock idle high
    SPI1CONbits.CKE = 0;   // Data changes on falling edge, sampled on rising edge
    SPI1CONbits.SSEN = 1;  // Enable SS control
    SPI1CONbits.MODE16 = 0; // 8-bit mode
    SPI1CONbits.MODE32 = 0;
    SPI1CONbits.ENHBUF = 0; // Enable enhanced buffer mode (FIFO)

    // Clear SPI buffer and overflow flag
    volatile uint8_t dummy = SPI1BUF;
    SPI1STATbits.SPIROV = 0;

    // Configure and enable SPI1 receive interrupt
      SPI1CONbits.SRXISEL= 0b01; //interrupt to trigger as soon as the buffer receives data after having no data
      //clearing interrupt flag
      IFS1CLR= _IFS1_SPI1RXIF_MASK;
      //setting priority
      IPC7bits.SPI1IP= 7; // highest priority 
      // enabling interrupt flag
      IEC1SET = _IEC1_SPI1RXIE_MASK;

    SPI1CONbits.ON = 1; // Enable SPI1
    

    __builtin_enable_interrupts();

    // Post an initial transition event
    ThisEvent.EventType = ES_INIT;
    return ES_PostToService(MyPriority, ThisEvent);
}

bool PostFollowerSPIService(ES_Event_t ThisEvent)
{
    return ES_PostToService(MyPriority, ThisEvent);
}

ES_Event_t RunFollowerSPIService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // Default return

    switch (CurrentState)
    {
        case WaitingCommand:
        {
            switch (ThisEvent.EventType)
            {
                case ES_INIT:
                {
                    DB_printf("\rES_INIT received in Follower SPI Service %d\r\n", MyPriority);
                }
                break;

                case ES_LEADERSENT:  // Leader sent data
                {
                    uint8_t receivedData = ThisEvent.EventParam;
                    //DB_printf("Follower Received SPI Data: %d\n", receivedData);

                    ES_Event_t NewEvent;
                    NewEvent.EventType = ES_GameCommand; // Default event type
                    

                    if (receivedData == 10)  // ALIGN_BEACON
                    {
                        DB_printf("Command: Aligning with beacon\n");
                        //ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = WaitingBeaconAlign;
                        //NewEvent.EventType = BeaconAlign; // Default event type
                        NewEvent.EventType = BeaconAlign;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 20)  // ALIGN_TAPE
                    {
                        DB_printf("Command: Aligning with tape\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        //PostDriveService(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 30)  // MOVE_TO_NODE
                    {
                        DB_printf("Command: Moving to node\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = MoveToNode;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 40)  // TURN
                    {
                        DB_printf("Command: Turning\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        //PostDriveService(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 50)  // DROP_BLOCK
                    {
                        DB_printf("Command: Dropping block\n");
                        //ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        //PostDriveService(NewEvent);
                        CommandToSend = 1;
                    }      
                    else if (receivedData <= 100 && receivedData >= 110) {
                        uint16_t DriveTime = 10 * (receivedData - 100);
                        DB_printf("Command: Drive forward for %d milliseconds\n", DriveTime);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = MoveForwardTimed;
                        NewEvent.EventParam = DriveTime;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 55) {
                        DB_printf("Command: turn left\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = TurnLeft;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 56) {
                        DB_printf("Command: turn right\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = TurnRight;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 57) {
                        DB_printf("Command: turn full\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = TurnFull;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 58) {
                        DB_printf("Command: moving to block\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = MoveToBlock;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 59) {
                        DB_printf("Command: turn left at the beacon\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = BeaconLeft;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 75) {
                        DB_printf("Command: inserting block\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = InsertBlock;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    else if (receivedData == 77) {
                        DB_printf("Command: inserting block\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = TurnLeftBeacon;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    
                    else if (receivedData == 79) {
                        DB_printf("Command: driving in reverse\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = DriveReverse;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                    
                    else if (receivedData == 78){
                        DB_printf("Command: stop\n");
                        ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        CurrentState = CompletingAction;
                        NewEvent.EventType = Stop;
                        PostDriveService2(NewEvent);
                        CommandToSend = 1;
                    }
                }
                break;
            }
        }
        break;
        
        case WaitingBeaconAlign:
        {
            switch (ThisEvent.EventType)
            {
                case ES_LEADERSENT:  // Leader sent data
                {
                    uint8_t receivedData = ThisEvent.EventParam;
                    //DB_printf("Follower Received SPI Data: %d\n", receivedData);

                    ES_Event_t NewEvent;
                    NewEvent.EventType = ES_GameCommand; // Default event type
                    
                    if (receivedData == 60)  // Beacon Found
                    {
                        DB_printf("SPI Received: Beacon Found\n");
                        //ES_Timer_InitTimer(POLL_TIMER, TWO_SEC);
                        NewEvent.EventType = ES_BEACONFOUND;
                        PostDriveService2(NewEvent);
                        CommandToSend = 0;
                        CurrentState = WaitingCommand;
                    }
                    
                }
                break;
            }
        }
        break;

        case CompletingAction:
        {
            switch (ThisEvent.EventType)
            {
//                case ES_TIMEOUT: // Task finished, send response to leader
//                {
//                    DB_printf("Task Completed! Sending Response to Leader.\n");
//                    CommandToSend = 200;  // Load response message
//                    CurrentState = WaitingCommand;
//                }
//                break;

                case ES_DriveServiceFinished: // Task finished, send response to leader
                {
                    DB_printf("Task Completed! Sending response to leader.\n");
                    CommandToSend = COMMAND_COMPLETED;  // Load response message
                    CurrentState = WaitingCommand;
                }
                break;
            }
        }
        break;

        default:
            break;
    }

    return ReturnEvent;
}

/*------------------------------ ISR ------------------------------*/
void __ISR(_SPI_1_VECTOR, IPL7SOFT) SPI1_RX_ISR(void) { 

    
    if (1 == SPI1STATbits.SPIRBF){
        NewCommand = SPI1BUF; 
        // Read out the RxBuffer and put it into the newCommand variable

        ES_Event_t NewCmdEvent;
        NewCmdEvent.EventType = ES_LEADERSENT;
        NewCmdEvent.EventParam = NewCommand;

        PostFollowerSPIService(NewCmdEvent);
        SPI1BUF = CommandToSend;
//        uint32_t dummy = SPI1BUF;
//         dummy = SPI1BUF;
        
    }
    IFS1bits.SPI1RXIF = 0; // clearing the interrupt flag
}

#ifndef FollowerSPIService_H
#define FollowerSPIService_H

#include <stdint.h>
#include <stdbool.h>

#include "ES_Events.h"
#include "ES_Port.h"                // needed for definition of REENTRANT
// Public Function Prototypes

typedef enum
{
    WaitingCommand,
    CompletingAction,
    WaitingBeaconAlign,
} FollowerSPIState_t;


bool InitFollowerSPIService(uint8_t Priority);
bool PostFollowerSPIService(ES_Event_t ThisEvent);
ES_Event_t RunFollowerSPIService(ES_Event_t ThisEvent);

#endif /* ServTemplate_H */

DEFINE MODULE CommunicationService

INITIALIZE:
    Define CurrentState as AwaitingGameCommand
    Define MyPriority as the priority of this service
    Define DeferralQueue with capacity 3 + 1
    Define dataToSend as 120 (default SPI command byte)
    Define beacon_counter as 0
    Define LeaderLooking4Beacon as 0 (tracking beacon alignment state)

FUNCTION InitCommunicationService(Priority):
    Set MyPriority to Priority
    Clear screen for debugging

    // SPI Configuration
    DISABLE SPI1
    DISABLE analog functionality on SPI pins
    Configure SPI1 pins (SDI, SDO, SS, CLK)
    Configure SPI1 registers for Master Mode
    Enable SPI1

    // Timer Configuration for Input Capture
    DISABLE Timer3
    Set Timer3 period
    Configure Timer3 prescaler
    ENABLE Timer3

    // Initialize deferral queue
    Initialize DeferralQueue(DeferralQueue, ARRAY_SIZE(DeferralQueue))

    // Post initial transition event
    CREATE Event ES_INIT
    Post Event to Service Queue
    RETURN True if successful, False otherwise

FUNCTION PostCommunicationService(Event):
    Post Event to Service Queue
    RETURN True if successful, False otherwise

FUNCTION RunCommunicationService(CurrentEvent):
    DEFINE ReturnEvent as ES_NO_EVENT

    SWITCH(CurrentState):
        CASE AwaitingGameCommand:
            SWITCH(CurrentEvent.Type):
                CASE ES_INIT:
                    PRINT "ES_INIT received in Communication Service"
                    BREAK
                
                CASE EV_ALIGN_BEACON:
                    PRINT "Aligning with beacon..."
                    IF LeaderLooking4Beacon == 0:
                        Set dataToSend to 10
                        Set CurrentState to SendingCommand
                        Set LeaderLooking4Beacon to 1
                        Start Timer COM_TIMER (200ms)
                    BREAK

                CASE EV_ALIGN_TAPE:
                    PRINT "Aligning with tape..."
                    Set dataToSend to 20
                    Set CurrentState to SendingCommand
                    Start Timer COM_TIMER (200ms)
                    BREAK
                
                CASE EV_MOVE_TO_NODE:
                    PRINT "Moving to node..."
                    Set dataToSend to 30
                    Set CurrentState to SendingCommand
                    Start Timer COM_TIMER (200ms)
                    BREAK
                
                CASE EV_TURN:
                    PRINT "Turning..."
                    Set dataToSend to 40
                    Set CurrentState to SendingCommand
                    Start Timer COM_TIMER (200ms)
                    BREAK
                
                CASE EV_DROP_BLOCK:
                    PRINT "Dropping block..."
                    Set dataToSend to 50
                    Set CurrentState to SendingCommand
                    Start Timer COM_TIMER (200ms)
                    BREAK

        CASE SendingCommand:
            IF CurrentEvent.Type == ES_TIMEOUT AND CurrentEvent.Param == COM_TIMER:
                Start Timer COM_TIMER (200ms)
                
                // Ensure SPI buffer is ready
                WAIT UNTIL SPI buffer is not full
                
                // Send SPI data
                Write SPI1BUF = dataToSend
                
                // Check if response received
                IF SPI Receive Buffer is NOT empty:
                    READ receivedData from SPI1BUF
                    IF receivedData == COMMAND_RECIEVED:
                        Set dataToSend to 0 (reset command)
                        
                        IF LeaderLooking4Beacon == 1:
                            Start Timer COM_TIMER (5000ms)
                            Set CurrentState to AligningBeacon
                            Set LeaderLooking4Beacon to 0
                        ELSE IF LeaderLooking4Beacon == 2:
                            Set LeaderLooking4Beacon to 0
                            Set CurrentState to AwaitingGameCommand
                            CREATE Event ES_TaskAccomplished
                            Post Event to MasterSM
                        ELSE:
                            Set CurrentState to AwaitingActionCompletion
                    PRINT "Follower received command"
                    BREAK

        CASE AligningBeacon:
            PRINT "Communication Service: Aligning with beacon..."
            
            // Post beacon alignment event
            CREATE Event EV_ALIGN_BEACON with Param = 1
            Post Event to IRService
            
            IF CurrentEvent.Type == EV_ALIGN_BEACON:
                PRINT "Beacon found..."
                Set dataToSend to 60
                Start Timer COM_TIMER (200ms)
                Set CurrentState to SendingCommand
                Set LeaderLooking4Beacon to 2
                BREAK

        CASE AwaitingActionCompletion:
            Stop Timer COM_TIMER
            
            IF CurrentEvent.Type == ES_TIMEOUT AND CurrentEvent.Param == COM_TIMER:
                Start Timer COM_TIMER (200ms)
                
                // Ensure SPI buffer is ready
                WAIT UNTIL SPI buffer is not full
                
                // Send SPI data
                Write SPI1BUF = dataToSend
                
                // Check if response received
                IF SPI Receive Buffer is NOT empty:
                    READ receivedData from SPI1BUF
                    
                    IF receivedData == COMMAND_COMPLETED:
                        Set dataToSend to 0 (reset command)
                        CREATE Event ES_TaskAccomplished
                        Post Event to MasterSM
                        Set CurrentState to AwaitingGameCommand
                        PRINT "Follower has completed task"

    RETURN ReturnEvent

// Hardware
#include <xc.h>
//#include <proc/p32mx170f256b.h>

// Event & Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_Port.h"
#include "terminal.h"
#include "dbprintf.h"
#include <sys/attribs.h>
#include "GameHSM.h"
#include "CommunicationService.h"
#include "IRService.h"
#include "HSMTemplate.h"
#include "TopHSMTemplate.h"

/*----------------------------- Module Defines ----------------------------*/
// these times assume a 10.000mS/tick timing
#define ONE_SEC 1000
#define HALF_SEC (ONE_SEC / 2)
#define TWO_SEC (ONE_SEC * 2)
#define FIVE_SEC (ONE_SEC * 5)

#define ENTER_POST     ((MyPriority<<3)|0) //  TODO: what are these variables
#define ENTER_RUN      ((MyPriority<<3)|1)
#define ENTER_TIMEOUT  ((MyPriority<<3)|2)
#define PB_PS_FREQ       20000000 / 4 // PB clock frequency / prescaler

#define COMMAND_RECIEVED 1
#define COMMAND_COMPLETED 2

// LeaderLooking4Beacon variables 
#define NOT_LOOKING 0
#define ALIGNING 1
#define BEACON_FOUND 2


/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
// add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
static ES_Event_t DeferralQueue[3 + 1];
static volatile uint8_t NewCommand = 0;

//beacon sensing vars
static volatile uint16_t DummyCounter = 0;
volatile static uint32_t RolloverCounter;
volatile static uint32_t  BeaconTimePassed;
static volatile uint32_t BeaconPeriod = 0;
static volatile uint32_t BeaconFreq = 0;
static volatile uint32_t  BeaconFreq_Last;

static CommunicationServiceState_t CurrentState = AwaitingGameCommand;
static uint8_t dataToSend = 120; // Default SPI command byte
static uint8_t beacon_counter = 0;
static uint8_t LeaderLooking4Beacon = NOT_LOOKING;

typedef union {
    uint32_t fullTime;
    uint16_t byBytes[2];
}beaconInterval;


/*------------------------------ Module Code ------------------------------*/
bool InitCommunicationService(uint8_t Priority)
{
  ES_Event_t ThisEvent;

  MyPriority = Priority;

  // When doing testing, it is useful to announce just which program
  // is running.
  clrScrn();

  /********************************************
   in here you write your initialization code
   *******************************************/
  
    SPI1CONbits.ON = 0;  // Disable SPI1
      // Disable analog functionality on SPI pins
    
      ANSELAbits.ANSA1 = 0;  // Disable analog on RA1 (SDI)
      // ANSELAbits.ANSA2 = 0;  // Disable analog on RA2 (SDO)
      ANSELAbits.ANSA0 = 0;  // Disable analog on RA0 (SS)
      ANSELBbits.ANSB14 = 0; // Disable analog on RB14 (CLK)


      // PPS -> SPI mapping
      TRISAbits.TRISA1 = 1;  // Set RA1 (SDI) as input
      SDI1R = 0b0000;       // Map RA1 pin 3 to SPI1 SDI
      TRISAbits.TRISA2 = 0;  // Set RA2 (SDO) as output
      RPA2R = 0b0011;        // Map RA2  pin 9 to SPI1 SDO
      TRISAbits.TRISA0 = 0;  // Set RA0 (SS) as output
      RPA0R = 0b0011;         // map RA0 pin 2 to SPI1 SS.  RB3 is pin 7
      
      TRISBbits.TRISB14 = 0;      // Set RB14 /25 as output for SCK
      
      TRISBbits.TRISB13 = 1;
      ANSELBbits.ANSB13 = 0;


      // SPI1 configuration 
      __builtin_disable_interrupts();
      SPI1CONbits.ON = 0;  // Disable SPI1 to allow configuration changes
      volatile uint8_t dummy = SPI1BUF;  // Clear any existing data in the SPI receive buffer
      SPI1CONbits.ENHBUF = 0;  // Turn enhanced buffer mode ON
      SPI1BRG = 50; // clock frequency set to RECALC ***************************************************************
      SPI1STATbits.SPIROV = 0;  // Clear the SPI receive overflow flag
      SPI1CONbits.MSSEN = 1;      // Automatically drive the SS pin
      SPI1CONbits.MCLKSEL = 0;    // Use PBCLK as clock source
      SPI1CONbits.ENHBUF = 0;     // Enable enhanced buffer mode (FIFO)
      SPI1CONbits.MSTEN = 1;      // Enable master mode
      SPI1CONbits.CKE = 0;        // Data changes on falling edge, sampled on rising edge
      SPI1CONbits.CKP = 1;        // Clock is idle high

      //additional stuff Ethan added (PLS CHECK THIS)
      SPI1CONbits.FRMPOL=0; //we did this in lecture... means frame pulse (cs) is active low
      SPI1CON2bits.AUDEN= 0; //no audio,, did in lec
      SPI1CONbits.MODE16= 0;
      SPI1CONbits.MODE32= 0; // mode16 and mode32 =0 means we enable 8 bit transfers
      
      
      

    //INITIALIZE TIMER FOR INPUT CAPTURE
      T3CONbits.ON=0; //disable timer
      PR3= 0XFFFF; //setting period for timer 2
      T3CONbits.TCS= 0; //use internal PBclk
      T3CONbits.TCKPS= 0b010; //CHECK THIS PRESCALER 1:4 FOR INPUT CAPTURE?
      T3CONbits.TGATE= 0; //disable gated time accumulation
      TMR3=0; //set first timer read to be zero

    //  //init interrupts for timer 3 THIS IS ESSENTIALLY THE ROLLOVER INTERRUPT
      IFS0CLR= _IFS0_T3IF_MASK; //clear T2IF in IFSx register
      IPC3bits.T3IP= 6; //set timer to have second highest priority, see 7-2 for map
      IEC0SET= _IEC0_T3IE_MASK; //set T2IF in IFSx register
      T3CONbits.ON= 1; //enable timer

        
      SPI1CONbits.ON = 1;  // Re-enable SPI1

    __builtin_enable_interrupts(); //enabling interrupts after initializing spi
  
  // initialize deferral queue for testing Deferral function
  ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));
  // initialize LED drive for testing/debug output
  
  //setting first val of totaltime to be zero (for first period calc)
//  TotalTime=0;
  
  
  // post the initial transition event
  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService(MyPriority, ThisEvent) == true)
  {
    return true;
  }
  else
  {
    return false;
  }
}


bool PostCommunicationService(ES_Event_t ThisEvent)
{
  return ES_PostToService(MyPriority, ThisEvent);
}


ES_Event_t RunCommunicationService(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // Assume no errors
    
    switch (CurrentState)
    {
        case AwaitingGameCommand:
        {
            switch (ThisEvent.EventType)
            {
                case ES_INIT:
                {
                    //ES_Timer_InitTimer(COM_TIMER, HALF_SEC);
                    DB_printf("\rES_INIT received in Communication Service %d\r\n", MyPriority);
                }
                break;

                case EV_ALIGN_BEACON:
                {
                    DB_printf("aligning with beacon...\n");
                    //ES_Timer_InitTimer(COM_TIMER, HALF_SEC);
                    if (LeaderLooking4Beacon == NOT_LOOKING){ //0 means we want to look for beacon again
                        //DB_printf("aligning with beacon...\n");
                        dataToSend = 10;
                        CurrentState = SendingCommand;
                        LeaderLooking4Beacon= ALIGNING; // TODO: do we need?
                        ES_Timer_InitTimer(COM_TIMER, 200);
                    }
                }
                break;

                case EV_ALIGN_TAPE:
                {
                    DB_printf("aligning with tape...\n");
                    dataToSend = 20;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;

                case EV_MOVE_TO_NODE:
                {
                    DB_printf("moving to node...\n");
                    dataToSend = 30;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;

//                case EV_TURN:
//                {
//                    DB_printf("turning...\n");
//                    dataToSend = 40;
//                    CurrentState = SendingCommand;
//                    ES_Timer_InitTimer(COM_TIMER, 200);
//                }
//                break;

                case EV_DROP_BLOCK:
                {
                    DB_printf("dropping block...\n");
                    dataToSend = 50;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_TURN_LEFT:
                {
                    DB_printf("turning left...\n");
                    dataToSend = 55;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_TURN_RIGHT:
                {
                    DB_printf("turning right...\n");
                    dataToSend = 56;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_TURN_FULL:
                {
                    DB_printf("turning full...\n");
                    dataToSend = 57;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_MOVE_TO_BLOCK:
                {
                    DB_printf("move toblock in comm service...\n");
                    dataToSend = 58;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_BEACON_LEFT:
                {
                    DB_printf("turning left at the beacon...\n");
                    dataToSend = 59;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_TOWER_RAISE:
                {
                    DB_printf("raising tower...\n");
                    CurrentState = AwaitingActionCompletion;
                    PostTowerFSM(ThisEvent);
                }
                break;
                
                case EV_TOWER_LOWER:
                {
                    DB_printf("lowering tower...\n");
                    CurrentState = AwaitingActionCompletion;
                    PostTowerFSM(ThisEvent);
                }
                break;
                
                case EV_MAGNET_UP:
                {
                    DB_printf("magnet up...\n");
                    CurrentState = AwaitingActionCompletion;
                    PostTowerFSM(ThisEvent);
                }
                break;
                
                case EV_MAGNET_DOWN:
                {
                    DB_printf("magnet down...\n");
                    CurrentState = AwaitingActionCompletion;
                    PostTowerFSM(ThisEvent);
                }
                break;
                
                case EV_INSERT_BLOCK:
                {
                    DB_printf("placing the block...\n");
                    dataToSend = 75;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_BEACON_LEFT_TURN:
                {
                    DB_printf("placing the block...\n");
                    dataToSend = 77;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_DRIVE_REVERSE:
                {
                    DB_printf("driving in reverse...\n");
                    dataToSend = 79;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER, 200);
                }
                break;
                
                case EV_STOP:
                {
                    DB_printf("stop drive \n");
                    dataToSend= 78;
                    CurrentState = SendingCommand;
                    ES_Timer_InitTimer(COM_TIMER,200);
                }
                break;

                default:
                    break;
            }
        }
        break;

        case SendingCommand:
        {
            if (ThisEvent.EventType == ES_TIMEOUT)
                // TODO: Add timer parameter to check for what kind of timeout
            {
                if (ThisEvent.EventParam == COM_TIMER)
                {   
                    ES_Timer_InitTimer(COM_TIMER, 200);
                        // Ensure SPI transmit buffer is empty before writing new data
                        while (SPI1STATbits.SPITBF); // Wait until transmit buffer is not full

                        // Transmit SPI data
                        SPI1BUF = dataToSend;

                        // Check received data
                        if (!SPI1STATbits.SPIRBE)  // If receive buffer is NOT empty
                        {
                            uint8_t receivedData = SPI1BUF; // Read received byte
                            //DB_printf("received data: %d\n", receivedData);

                            if (receivedData == COMMAND_RECIEVED) // If the follower received DROP_BLOCK command
                            {
                                dataToSend = 0;  // Reset to a default/neutral command
                                //ES_Timer_StopTimer(COM_TIMER);
                                if (LeaderLooking4Beacon == ALIGNING) {
                                    ES_Timer_InitTimer(COM_TIMER, 5000);
                                    CurrentState = AligningBeacon;
                                    LeaderLooking4Beacon = NOT_LOOKING;
                                } else if (LeaderLooking4Beacon == BEACON_FOUND){
                                    LeaderLooking4Beacon = NOT_LOOKING;
                                    CurrentState = AwaitingGameCommand;
                                    ES_Event_t GameEvent;
                                    GameEvent.EventType = ES_TaskAccomplished;
                                    PostMasterSM(GameEvent);
                                } else{
                                    CurrentState = AwaitingActionCompletion;
                                }
                               DB_printf("follower received command\n");
                            }
                        }
                }
              
            }
        }
        break;
        
        case AligningBeacon:
        {
            DB_printf("Com Service, aligning with beacon...\n");
            
            // TO DO ****** SEE IF THIS WORKS!!
            //attempt to make it work if already aligned to beacon
            ES_Event_t NewEvent;
            NewEvent.EventType = EV_ALIGN_BEACON;
            NewEvent.EventParam= 1; // this works now
            PostIRService(NewEvent);
            
            if (ThisEvent.EventType == EV_ALIGN_BEACON) // posted by ISR
            {
                DB_printf("we found the beacon...\n");
                dataToSend = 60;
                ES_Timer_InitTimer(COM_TIMER, 200);
                CurrentState = SendingCommand;
                LeaderLooking4Beacon = BEACON_FOUND;
            }
        }
        break;
        
        case AwaitingActionCompletion:
        {
            ES_Timer_StopTimer(COM_TIMER);
            if (ThisEvent.EventType == ES_TIMEOUT)
            {
                if (ThisEvent.EventParam == COM_TIMER)
                {
                        ES_Timer_InitTimer(COM_TIMER, 200);
                        // Ensure SPI transmit buffer is empty before writing new data
                        while (SPI1STATbits.SPITBF); // Wait until transmit buffer is not full

                        // Transmit SPI data
                        SPI1BUF = dataToSend;

                        // Check received data
                        if (!SPI1STATbits.SPIRBE)  // If receive buffer is NOT empty
                        {
                            uint8_t receivedData = SPI1BUF; // Read received byte
                            //DB_printf("received data: %d\n", receivedData);

                            if (receivedData == COMMAND_COMPLETED) // 
                            {
                                dataToSend = 0;  // Reset to a default/neutral command
                                
                                ES_Event_t NewEvent;
                                NewEvent.EventType = ES_TaskAccomplished;
                                PostMasterSM(NewEvent);
                                
                                CurrentState = AwaitingGameCommand;
                            }
                        }
                }
            }
            
            if (ThisEvent.EventType == ES_TOWER_FINISHED) {
                DB_printf("finished processing tower event...\n");
                ES_Event_t NewEvent;
                NewEvent.EventType = ES_TaskAccomplished;
                PostMasterSM(NewEvent);

                CurrentState = AwaitingGameCommand;
            }
        }   
        break;       
    }

    return ReturnEvent;
}

#ifndef CommunicationService_H
#define CommunicationService_H

#include <stdint.h>
#include <stdbool.h>

#include "ES_Events.h"
#include "ES_Port.h"                // needed for definition of REENTRANT
// Public Function Prototypes

typedef enum
{
    AwaitingGameCommand,
    SendingCommand,
    AwaitingActionCompletion,
    AligningBeacon,
} CommunicationServiceState_t;

bool InitCommunicationService(uint8_t Priority);
bool PostCommunicationService(ES_Event_t ThisEvent);
ES_Event_t RunCommunicationService(ES_Event_t ThisEvent);

#endif /* ServTemplate_H */

DEFINE MODULE StageOne

INITIALIZE:
    Define counter = 0
    Define task_sequence as list of events to be processed
    Define task_params as corresponding task parameters
    Define length_task_sequence as the number of tasks in task_sequence

FUNCTION RunStageOne(CurrentEvent):
    DEFINE MakeTransition as False
    DEFINE EntryEventKind as ES_ENTRY
    DEFINE ReturnEvent as CurrentEvent

    PRINT "Enter RunStageOne..."

    IF CurrentEvent.Type == ES_ENTRY:
        PRINT "Sending first task from stage 1..."
        PRINT "Counter is", counter
        PRINT "First Task is", task_sequence[counter]
        
        CREATE Event taskEvent with EventType = task_sequence[counter]
        POST taskEvent to CommunicationService
        Increment counter by 1

    ELSE IF CurrentEvent.Type == ES_TaskAccomplished:
        IF counter < length_task_sequence:
            CREATE Event taskEvent with EventType = task_sequence[counter]
            POST taskEvent to CommunicationService
            Increment counter by 1
        ELSE:
            PRINT "Stage should be completed..."
            CREATE Event GameEvent with EventType = EV_STAGE_COMPLETE
            POST GameEvent to MasterSM

    RETURN ReturnEvent

FUNCTION StartStageOne(CurrentEvent):
    PRINT "Starting Stage 1..."
    CALL RunStageOne(CurrentEvent)

FUNCTION QueryStageOne():
    RETURN CurrentState

/****************************************************************************
 Module
   HSMTemplate.c

 Revision
   2.0.1

 Description
   This is a template file for implementing state machines.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 02/27/17 09:48 jec      another correction to re-assign both CurrentEvent
                         and ReturnEvent to the result of the During function
                         this eliminates the need for the prior fix and allows
                         the during function to-remap an event that will be
                         processed at a higher level.
 02/20/17 10:14 jec      correction to Run function to correctly assign 
                         ReturnEvent in the situation where a lower level
                         machine consumed an event.
 02/03/16 12:38 jec      updated comments to reflect changes made in '14 & '15
                         converted unsigned char to bool where appropriate
                         spelling changes on true (was True) to match standard
                         removed local var used for debugger visibility in 'C32
                         commented out references to Start & RunLowerLevelSM so
                         that this can compile. 
 02/07/13 21:00 jec      corrections to return variable (should have been
                         ReturnEvent, not CurrentEvent) and several EV_xxx
                         event names that were left over from the old version
 02/08/12 09:56 jec      revisions for the Events and Services Framework Gen2
 02/13/10 14:29 jec      revised Start and run to add new kind of entry function
                         to make implementing history entry cleaner
 02/13/10 12:29 jec      added NewEvent local variable to During function and
                         comments about using either it or Event as the return
 02/11/10 15:54 jec      more revised comments, removing last comment in during
                         function that belongs in the run function
 02/09/10 17:21 jec      updated comments about internal transitions on During funtion
 02/18/09 10:14 jec      removed redundant call to RunLowerlevelSM in EV_Entry
                         processing in During function
 02/20/07 21:37 jec      converted to use enumerated type for events & states
 02/13/05 19:38 jec      added support for self-transitions, reworked
                         to eliminate repeated transition code
 02/11/05 16:54 jec      converted to implment hierarchy explicitly
 02/25/03 10:32 jec      converted to take a passed event parameter
 02/18/99 10:19 jec      built template from MasterMachine.c
 02/14/99 10:34 jec      Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "CommunicationService.h"

/* 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 "HSMTemplate.h"
#include "TopHSMTemplate.h"

/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE STATE_ZERO

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static uint8_t counter = 0;

// Define the sequence of commands
static const ES_EventType_t task_sequence[] = {
    EV_ALIGN_BEACON, // Works
//    EV_DRIVE_REVERSE,
    EV_BEACON_LEFT_TURN,
    EV_MOVE_TO_NODE, // Works
    EV_TURN_RIGHT,
    EV_MOVE_TO_NODE,
    EV_STOP,
    //EV_TOWER_RAISE,
    EV_MAGNET_UP,
    EV_INSERT_BLOCK,
    EV_MAGNET_DOWN,
    EV_TURN_FULL,
    EV_MOVE_TO_NODE,
    EV_TURN_RIGHT,
    EV_MOVE_TO_NODE,
    EV_TURN_LEFT,
    EV_TURN_LEFT,
    EV_MOVE_TO_NODE,
    EV_TURN_FULL,
    EV_MOVE_TO_NODE,
    EV_TURN_LEFT,
    EV_TURN_LEFT,
    EV_TURN_LEFT,
    EV_TURN_LEFT,
    EV_TURN_LEFT,
    
    EV_STOP
//    EV_MOVE_TO_BLOCK,
//    EV_TURN_FULL,
//    EV_TURN_LEFT
    // EV_TOWER_RAISE,
//    EV_MAGNET_UP
};

// List of parameters (for turn angles, 0 otherwise)
static const int task_params[] = {
//    0, 0, 0, 0, 0, 0, 90, 0, 180, 0, 0, 90, 0
    //for checkoff 3
    0, 0, 10
};

int length_task_sequence = sizeof(task_sequence) / sizeof(task_sequence[0]); //this is the number of events in the task sequence

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunTemplateSM

 Parameters
   ES_Event_t: the event to process

 Returns
   ES_Event_t: an event to return

 Description
   add your description here
 Notes
   uses nested switch/case to implement the machine.
 Author
   J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
ES_Event_t RunStageOne( ES_Event_t CurrentEvent )
{
   bool MakeTransition = false;/* are we making a state transition? */
   ES_Event_t EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
   ES_Event_t ReturnEvent = CurrentEvent; // assume we are not consuming event

  
   
   if (CurrentEvent.EventType == ES_ENTRY) {
       
       ES_Event_t taskEvent;
        taskEvent.EventType = task_sequence[counter];
        PostCommunicationService(taskEvent);
        counter = counter + 1; //sets counter to one
   }
   if (CurrentEvent.EventType == ES_TaskAccomplished){
       if (counter < length_task_sequence) {
            ES_Event_t taskEvent;
             taskEvent.EventType = task_sequence[counter];
             PostCommunicationService(taskEvent);
             counter = counter + 1;
        } else {
           DB_printf("stage should be completed...\n");
             ES_Event_t GameEvent;
             GameEvent.EventType = EV_STAGE_COMPLETE;
             PostMasterSM(GameEvent);
        }
   }

//   switch ( CurrentState )
//   {
//       case STATE_ONE :       // If current state is state one
//         // Execute During function for state one. ES_ENTRY & ES_EXIT are
//         // processed here allow the lower level state machines to re-map
//         // or consume the event
//         ReturnEvent = CurrentEvent = DuringStateOne(CurrentEvent);
//         //process any events
//         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
//         {
//            switch (CurrentEvent.EventType)
//            {
//               case ES_LOCK : //If event is event one
//                  // Execute action function for state one : event one
//                  NextState = STATE_TWO;//Decide what the next state will be
//                  // for internal transitions, skip changing MakeTransition
//                  MakeTransition = true; //mark that we are taking a transition
//                  // if transitioning to a state with history change kind of entry
//                  EntryEventKind.EventType = ES_ENTRY_HISTORY;
//                  // optionally, consume or re-map this event for the upper
//                  // level state machine
//                  ReturnEvent.EventType = ES_NO_EVENT;
//                  break;
//                // repeat cases as required for relevant events
//            }
//         }
//       break;
//      // repeat state pattern as required for other states
//    }
//    //   If we are making a state transition
//    if (MakeTransition == true)
//    {
//       //   Execute exit function for current state
//       CurrentEvent.EventType = ES_EXIT;
//       RunTemplateSM(CurrentEvent);
//
//       CurrentState = NextState; //Modify state variable
//
//       //   Execute entry function for new state
//       // this defaults to ES_ENTRY
//       RunTemplateSM(EntryEventKind);
//     }
     return(ReturnEvent);
}
/****************************************************************************
 Function
     StartTemplateSM

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
 Notes

 Author
     J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartStageOne ( ES_Event_t CurrentEvent )
{
   // to implement entry to a history state or directly to a substate
   // you can modify the initialization of the CurrentState variable
   // otherwise just start in the entry state every time the state machine
   // is started
    DB_printf("starting stage 1...\n");
   RunStageOne(CurrentEvent);
}

/****************************************************************************
 Function
     QueryTemplateSM 

 Parameters
     None

 Returns
     TemplateState_t The current state of the Template state machine

 Description
     returns the current state of the Template state machine
 Notes

 Author
     J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
//TemplateState_t QueryStageOne ( void )
//{
//   return(CurrentState);
//}

/****************************************************************************
 Template header file for Hierarchical Sate Machines AKA StateCharts
 02/08/12 adjsutments for use with the Events and Services Framework Gen2
 3/17/09  Fixed prototpyes to use Event_t
 ****************************************************************************/

#ifndef HSMTemplate_H
#define HSMTemplate_H


// typedefs for the states
// State definitions for use with the query function
// typedef enum { EXECUTING } TemplateState_t ;


// Public Function Prototypes

ES_Event_t RunStageOne( ES_Event_t CurrentEvent );
void StartStageOne ( ES_Event_t CurrentEvent );
// TemplateState_t QueryStageOne ( void );

#endif /*SHMTemplate_H */

/*----------------------------- Module Defines ----------------------------*/
// these times assume a 10.000mS/tick timing
#define ONE_SEC 1000
#define HALF_SEC (ONE_SEC / 2)
#define TWO_SEC (ONE_SEC * 2)
#define FIVE_SEC (ONE_SEC * 5)

#define ENTER_POST     ((MyPriority<<3)|0)
#define ENTER_RUN      ((MyPriority<<3)|1)
#define ENTER_TIMEOUT  ((MyPriority<<3)|2)
#define PB_PS_FREQ     20000000 / 4 // PB clock frequency / prescaler
#define PR_VAL 24999 // 20ms period TO DO: CHECK THIS VALUE


#define GREEN 0
#define BLUE 1

//#define TEST_INT_POST
//#define BLINK LED
/*---------------------------- Module Functions ---------------------------*/
void TurnOnRegionIndicator(uint8_t region);
void InitRegionIndicator(void);

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
// add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
static ES_Event_t DeferralQueue[3 + 1];

//beacon sensing vars
include all variables here 




//Initial init function for the IR Service
bool InitIRService(uint8_t Priority)
{
  //Initialize Timer 3 for Output Compare
  //Initialize Input Capture 5 for IR Beacon
  //Enable Interrupts and set priority number (7)
  
  
  
  
  InitRegionIndicator();

  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService(MyPriority, ThisEvent) == true)
  {
    return true;
  }
  else
  {
    return false;
  }
}


bool PostIRService(ES_Event_t ThisEvent)
{
  return ES_PostToService(MyPriority, ThisEvent);
}


ES_Event_t RunIRService(ES_Event_t ThisEvent)
{
  ES_Event_t ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  static char DeferredChar = '1';

  //switch statement for all events possible
  switch (ThisEvent.EventType)
  
    //init case
    case ES_INIT:
    {
      //do nothing
    }
    break;
    
    //If the Leader is actively looking for the beacon, it will send this service thie event EV_Align_Beacon
    case EV_ALIGN_BEACON:
    {
       if event param ==1: //means the comm service wants to look for a beacon
            set  InterruptEnable = 0;
            set BeaconFreq_Last_Sent= 0; //basically resets values so we can run the ISR again
            end if
      
        else: //means
        
            //check to see if we have a frequency that was equal to the last we sent as an event
            // this means we are getting a repetitive signal so just reset the isr and search for beacon again
            if (BeaconFreq ==  BeaconFreq_Last_Sent){
                // do nothing
                set isr_counter = 0; 
               

            end nested if

            else: //means that this frequency is different than the one we last sent
                reset BeaconFreqLast_Sent to BeaconFreq
                // now let's turn on the region indicator
                Post event to Communication service that the beacon is aligned
                Include event parameter that is the beacon time passed so that we can calculate frequency and thus which side we are on
                reset the isr so we can run it again
            end else statement
            //  
        end else statement


  return ReturnEvent;
}

// This function is now the ISR that determines the frequency of the tower IR
void __ISR(_INPUT_CAPTURE_5_VECTOR, IPL7SOFT) BCON__SENSE_ISR(void) { 
    disable interrupts
    set the lastTime variable to be zero
    declare a union called realTime
    write the buffer value to a variable
    store buffer value into realTime union
    clear the interrupt flag

    if the captured time is greater than 0x800: //this means that the timer interrupt hasn't been dealt with yet
        increment rollover counter plus one
        clear roll over interrupt with mask
        combine rollover counter with captured time to create a uint32
        calculate beacontimepassed by subtracting the realtime.fulltime - lasttime
        set beaconFreq to pb_ps frequency divided by BeaconTimePassed
    end if


    // now we need to calculate which tower we are sensing 
    //discretize tower IR data into distinct buckets
    if (2700 <= BeaconFreq) {
            BeaconPeriod = 300;
            BeaconFreq = 3333;
            // beacon G, green
            Region = GREEN;
        } 
    else if (1750 <= BeaconFreq) {
            BeaconPeriod = 500;
            BeaconFreq = 2000; 
            // beacon L, blue
            Region = BLUE;
        } 
    else if (1250 <= BeaconFreq) {
            BeaconPeriod = 700;
            BeaconFreq = 1427;
            // beacon B, blue
            Region = BLUE;
        } 
    else { 
            BeaconPeriod = 1100;
            BeaconFreq = 909;
            // beacon R, green
            Region = GREEN;
        }

    end discretization if statements


    if isr_counter == 7 && InterruptEnable == 0: 
        // ensures that we only post to the IR service every 100 times the ISR fires  
        // instead of every time 
        post beacon align event with beacontimePassed parameter to IRService 
        set lastTime = realTime.fullTime;
    end if

    else{
        increment isr_counter++;
    end else


clear interrupt mask again // just in case
IFS0CLR = _IFS0_IC5IF_MASK;
__builtin_enable_interrupts();
} //end isr



//TIMER ROLLOVER ISR
void __ISR(_TIMER_3_VECTOR, IPL6SOFT) RolloverISR(void){
    __builtin_disable_interrupts();
    if flag is still pending:
        RolloverCounter++; 
        //clear roll-over interrupt (timer 3)
    end if statement
    
    __builtin_enable_interrupts(); //via ed's code
}


// InitRegionIndicator() to set up pins correctly
void InitRegionIndicator()
{
    // configure pin 10, RA3 to output
    TRISAbits.TRISA3 = 0;

    T2CONbits.ON = 0;
	//2. Clear the TCS control bit (TxCON<1> = 0) to select the internal PBCLK source.
    T2CONbits.TCS = 0;
	//3. Select the desired timer input clock prescale.
	T2CONbits.TCKPS = 0b011; // 1:8
	T2CONbits.TGATE = 0;
	//4. Load/Clear the timer register TMRx.
    TMR2 = 0;
	//5. Load the period register PRx with the desired 16-bit match value.
    PR2 = PR_VAL; // 20ms period. PR_VAL is 6500
	//6. Set the ON control bit (TxCON<15> = 1) to enable the timer.
    T2CONbits.ON = 1; 
	
	// init OC3R register to control the servo
    OC3CONbits.ON = 0; // Turn off the OC1 when performing the setup
    OC3CONbits.OCM = 0b110; // PWM mode
 
	// set timer 2 as the clock source
    RPA3R = 0b0101; // assign OC3 to pin 10
    OC3CONbits.OCTSEL = 0;
    OC3CONbits.OC32 = 0;
    OC3CONbits.OCSIDL = 0;
    OC3R = 0; // Initialize primary Compare register
    OC3RS = 0; // Initialize secondary Compare register

    OC3CONbits.ON = 1;
    OC3RS = 4000;
    return;
}

// private function to control the servo motor. 
void TurnOnRegionIndicator(uint8_t region){
    if(region == BLUE){
        OC3RS = 5500; // 1 ms pulse to move to zero
    }else if(region == GREEN                                                ){
        DB_printf("This is Supposed to Spin\n");
        OC3RS = 1500; // 2 ms pulse to move to 180 degrees
    }else {
        OC3RS = 2500; // otherwise set to 90 degrees
    }
    return;
}

#include "../ProjectHeaders/IRService.h"

// Hardware
#include <xc.h>
//#include <proc/p32mx170f256b.h>

// Event & Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_Port.h"
#include "terminal.h"
#include "dbprintf.h"
#include <sys/attribs.h>
#include "IRService.h"
#include "CommunicationService.h"
#include "TopHSMTemplate.h"

/*----------------------------- Module Defines ----------------------------*/
// these times assume a 10.000mS/tick timing
#define ONE_SEC 1000
#define HALF_SEC (ONE_SEC / 2)
#define TWO_SEC (ONE_SEC * 2)
#define FIVE_SEC (ONE_SEC * 5)

#define ENTER_POST     ((MyPriority<<3)|0)
#define ENTER_RUN      ((MyPriority<<3)|1)
#define ENTER_TIMEOUT  ((MyPriority<<3)|2)
#define PB_PS_FREQ     20000000 / 4 // PB clock frequency / prescaler
#define PR_VAL 24999 // 20ms period TO DO: CHECK THIS VALUE


#define GREEN 0
#define BLUE 1

//#define TEST_INT_POST
//#define BLINK LED
/*---------------------------- Module Functions ---------------------------*/
void TurnOnRegionIndicator(uint8_t region);
void InitRegionIndicator(void);

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
// add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
static ES_Event_t DeferralQueue[3 + 1];

//beacon sensing vars
volatile static uint32_t RolloverCounter;

volatile static uint32_t  BeaconTimePassed = 0;
static volatile uint32_t BeaconPeriod = 0;
static volatile uint32_t BeaconFreq = 0; 
static uint32_t  BeaconFreq_Last_Sent= 0; 
static uint8_t beacon_counter = 0;
static volatile uint8_t isr_counter= 0;
volatile static uint8_t InterruptEnable = 1;

volatile static uint8_t Region = BLUE;

typedef union {
    uint32_t fullTime;
    uint16_t byBytes[2];
}beaconInterval;


bool InitIRService(uint8_t Priority)
{
  ES_Event_t ThisEvent;

  MyPriority = Priority;
  DB_printf("initializing the IR service\n");
  
  
  //INITIALIZE TIMER FOR INPUT CAPTURE
  T3CONbits.ON=0; //disable timer
  PR3= 0XFFFF; //setting period for timer 2
  T3CONbits.TCS= 0; //use internal PBclk
  T3CONbits.TCKPS= 0b010; //CHECK THIS PRESCALER 1:4 FOR INPUT CAPTURE?
  T3CONbits.TGATE= 0; //disable gated time accumulation
  TMR3=0; //set first timer read to be zero
  
  //init interrupts for timer 3 THIS IS ESSENTIALLY THE ROLLOVER INTERRUPT
  IFS0CLR= _IFS0_T3IF_MASK; //clear T2IF in IFSx register
  IPC3bits.T3IP= 6; //set timer to have second highest priority, see 7-2 for map
  IEC0SET= _IEC0_T3IE_MASK; //set T2IF in IFSx register
  T3CONbits.ON= 1; //enable timer
  
  //INITIALIZE INPUT CAPTURE REGISTER
  IC5CONbits.ON=0; //disable input capture
  IC5CONbits.ICM= 0b011; //captures on every rising edge
  IC5CONbits.ICTMR= 0; //Timer 3 is used for input capture
  // IC5CONbits.C32=0; //set to 16 bit timer resource capture
  IC5CONbits.ICI= 00; //interrupt on every capture
  IC5R= 0b0011; //maps pin RB13 to IC5 port
  IFS0CLR= _IFS0_IC5IF_MASK; // clear any interrupt flags
  IPC5bits.IC5IP= 7; //sets interrupt priority number 
  IEC0SET= _IEC0_IC5IE_MASK; //enables interrupts to occur IC3
  IC5CONbits.ON=1; //enable input capture
  
  // TODO: ummm i suppose i will ignore this for now
  ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));

  InitRegionIndicator();

  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService(MyPriority, ThisEvent) == true)
  {
    return true;
  }
  else
  {
    return false;
  }
}


bool PostIRService(ES_Event_t ThisEvent)
{
  return ES_PostToService(MyPriority, ThisEvent);
}


ES_Event_t RunIRService(ES_Event_t ThisEvent)
{
  ES_Event_t ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  static char DeferredChar = '1';

  switch (ThisEvent.EventType)
  {
    case ES_INIT:
    {
      DB_printf("\rES_INIT IR service %d\r\n", MyPriority);
    }
    break;
    
    case EV_ALIGN_BEACON:
    {// TODO check this if statement//
        DB_printf("\r case EV_Align_Beacon in IR Service %d\r\n", MyPriority);
        
        if (ThisEvent.EventParam == 1){ //this means the comm service wants to look for beacon again
            InterruptEnable = 0;
            BeaconFreq_Last_Sent = 0; //basically resetting this value so we can run search isr again
            DB_printf("getting into here...\n");
        }
        else 
        {
            //check to see if we have a frequency that was equal to the last we sent as an event
            if (BeaconFreq ==  BeaconFreq_Last_Sent){
                // do nothing
                isr_counter = 0; //reset the isr counter so it can go back to looking for a beacon
                DB_printf("\r Beacon Found, same frequency so do nothing %d\r\n", BeaconFreq);

            }

            else { //means that this frequency is different than the one we last sent
                BeaconFreq_Last_Sent = BeaconFreq; // resetting the value that we last sent
                // BeaconFreq_Last_Sent = 0;
                // now let's turn on the region indicator
                if (InterruptEnable == 0) {
                    TurnOnRegionIndicator(Region);
                    ES_Event_t CommEvent;
                    CommEvent.EventType = EV_ALIGN_BEACON;
                    CommEvent.EventParam = BeaconTimePassed; //**TOD** SEE IF THIS WOULD BE BETER SENT AS A FREQUENCY
                    PostCommunicationService(CommEvent);
                    isr_counter= 0; //reset counter so isr can run again
                    DB_printf("\r New Beacon Found, frequency %d\r\n", BeaconFreq);
                    InterruptEnable = 1;
                }
            }
            //  
        }

        // now let's turn on the region indicator
//        TurnOnRegionIndicator(Region);
        break;
    }
    break;
    
    // ONLY FOR TESTING SERVO MOTOR
    case ES_NEW_KEY:{
        if ('b' == ThisEvent.EventParam)
          {
            ES_Event_t NewEvent;
            DB_printf("beacon triggered\n");
            NewEvent.EventType = EV_ALIGN_BEACON;
            NewEvent.EventParam = BeaconTimePassed;
            PostIRService(NewEvent);
          }
    }
    break;
    
    
    default:
    {}
     break;
  }

  return ReturnEvent;
}

/***************************************************************************
 private functions
 ***************************************************************************/
void __ISR(_INPUT_CAPTURE_5_VECTOR, IPL7SOFT) BCON__SENSE_ISR(void) { 
    __builtin_disable_interrupts();
    static uint32_t lastTime = 0;
    //static uint8_t counter = 0;
    beaconInterval realTime; // declare a new union called realTime. 
    //first set vars and read buffer
    // LastCapturedTime= TotalTime; //copies previous capture time value
    volatile uint16_t CapturedTime = IC5BUF; //writes buffer value to a volatile var. TODO: this is not volatile

    realTime.byBytes[0] = CapturedTime; // store buffer value into realTime union.
    IFS0CLR= _IFS0_IC5IF_MASK; // clear any interrupt flags

    if (IFS0bits.T3IF && CapturedTime < 0x8000){
        //this means that the timer interrupt hasn't been dealt with yet
        RolloverCounter++; //increment rollover counter
        IFS0CLR= _IFS0_T3IF_MASK; //clear roll-over interrupt (timer 3) 
    }

    realTime.byBytes[1] = RolloverCounter; 
    //combine roll_over counter with CapturedTime to create a uint32 (totalTime)
    // TotalTime= ((uint32_t)RolloverCounter << 16)|CapturedTime; 

    // BeaconTimePassed = TotalTime- LastCapturedTime-65536;
    BeaconTimePassed = realTime.fullTime - lastTime; // fullTime is the full 32 bits. 

    // BeaconFreq_Last= BeaconFreq; // resetting last frequency variable
    BeaconFreq = PB_PS_FREQ / BeaconTimePassed;

    //calculating which tower we are sensing 
    if (2700 <= BeaconFreq) {
            BeaconPeriod = 300;
            BeaconFreq = 3333;
            // beacon G, green
            Region = GREEN;
        } 
    else if (1750 <= BeaconFreq) {
            BeaconPeriod = 500;
            BeaconFreq = 2000; 
            // beacon L, blue
            Region = BLUE;
        } 
    else if (1250 <= BeaconFreq) {
            BeaconPeriod = 700;
            BeaconFreq = 1427;
            // beacon B, blue
            Region = BLUE;
        } 
    else { 
            BeaconPeriod = 1100;
            BeaconFreq = 909;
            // beacon R, green
            Region = GREEN;
        }


    if (isr_counter == 7 && InterruptEnable == 0) { 
        // ensures that we only post to the IR service every 100 times the ISR fires  
        // instead of every time  
        ES_Event_t NewEvent;
        NewEvent.EventType = EV_ALIGN_BEACON;
        NewEvent.EventParam = BeaconTimePassed;
        PostIRService(NewEvent);
        lastTime = realTime.fullTime;
        //isr_counter = isr_counter + 1;
    //} else if (isr_counter == 101){ // TODO: huh? why are we adding one instead of setting it back to zero
    } else{
        isr_counter++;
    }


    while (IC5CONbits.ICBNE != 0) {
        uint16_t dummyclear = IC5BUF;
    }    
    IFS0CLR = _IFS0_IC5IF_MASK;
    __builtin_enable_interrupts();
}



//TIMER ROLLOVER ISR
void __ISR(_TIMER_3_VECTOR, IPL6SOFT) RolloverISR(void){
    __builtin_disable_interrupts();
    if(IFS0bits.T3IF == 1){
        //if the flag is still pending
        RolloverCounter++; 
        IFS0CLR= _IFS0_T3IF_MASK; //clear roll-over interrupt (timer 3)
    }
    
    __builtin_enable_interrupts(); //via ed's code
}


// InitRegionIndicator() to set up pins correctly
void InitRegionIndicator()
{
    // configure pin 10, RA3 to output
    TRISAbits.TRISA3 = 0;

    T2CONbits.ON = 0;
	//2. Clear the TCS control bit (TxCON<1> = 0) to select the internal PBCLK source.
    T2CONbits.TCS = 0;
	//3. Select the desired timer input clock prescale.
	T2CONbits.TCKPS = 0b011; // 1:8
	T2CONbits.TGATE = 0;
	//4. Load/Clear the timer register TMRx.
    TMR2 = 0;
	//5. Load the period register PRx with the desired 16-bit match value.
    PR2 = PR_VAL; // 20ms period. PR_VAL is 6500
	//6. Set the ON control bit (TxCON<15> = 1) to enable the timer.
    T2CONbits.ON = 1; 
	
	// init OC3R register to control the servo
    OC3CONbits.ON = 0; // Turn off the OC1 when performing the setup
    OC3CONbits.OCM = 0b110; // PWM mode
 
	// set timer 2 as the clock source
    RPA3R = 0b0101; // assign OC3 to pin 10
    OC3CONbits.OCTSEL = 0;
    OC3CONbits.OC32 = 0;
    OC3CONbits.OCSIDL = 0;
    OC3R = 0; // Initialize primary Compare register
    OC3RS = 0; // Initialize secondary Compare register

    OC3CONbits.ON = 1;
    OC3RS = 4000;
    return;
}

// private function to control the servo motor. 
void TurnOnRegionIndicator(uint8_t region){
    if(region == BLUE){
        OC3RS = 5500; // 1 ms pulse to move to zero
    }else if(region == GREEN                                                ){
        DB_printf("This is Supposed to Spin\n");
        OC3RS = 1500; // 2 ms pulse to move to 180 degrees
    }else {
        OC3RS = 2500; // otherwise set to 90 degrees
    }
    return;
}

#ifndef IRService_H
#define IRService_H

#include <stdint.h>
#include <stdbool.h>

#include "ES_Events.h"
#include "ES_Port.h"                // needed for definition of REENTRANT
// Public Function Prototypes

bool InitIRService(uint8_t Priority);
bool PostIRService(ES_Event_t ThisEvent);
ES_Event_t RunIRService(ES_Event_t ThisEvent);

#endif /* ServTemplate_H */

Pseudocode for TowerFSM
	Initialization (InitTowerFSM)
	Configure Timer2 for PWM output (20ms period).
	Initialize Output Compare (OC5) for controlling the servo.
	Configure GPIO Pins for motor and sensor inputs/outputs.
	Set initial state to IdleState.
	Post an initialization event (ES_INIT).
    
Event Posting (PostTowerFSM)
	Adds an event to the FSM’s queue.
	State Machine Execution (RunTowerFSM)
	State: IdleState
        If the event posted is…
            EV_TOWER_RAISE → Move tower up one level, 
            controlled via a timer→ Transition to MovingUpState.
        EV_TOWER_LOWER → Move tower down one level → Transition 
        to MovingDownState.
        EV_MAGNET_UP → Rotate magnet servo up by 90 degrees→ 
          	Transition to RotatingMagnetUp.
        EV_MAGNET_DOWN → Rotate magnet down by 90 degrees →
          	Transition to RotatingMagnetDown.
	State: MovingUpState
		If the timer associated with the stepper motor (responsible for raising the tower) times out, 
		Move to the next step in the full-step drive sequence.
		If sensor detects threshold, enter timeout mode.
		If timeout limit reached, stop movement → Return to IdleState.
		Restart the stepper motor timer
	State: MovingDownState
		Every timer tick (ES_TIMEOUT)
		Move the stepper motor to the previous step in the full-step drive sequence.
		If sensor detects threshold, enter timeout mode.
		If timeout limit reached, stop movement → Return to IdleState.
		Restart the stepper motor timer. 
	State: RotatingMagnetUp / RotatingMagnetDown
		On ES_TIMEOUT event associated with the servo rotating the electromagnet: 
		Stop PWM signal.
		Return to IdleState.
        
Query Function (QueryTowerFSM)
	Returns the current state of the FSM.

/****************************************************************************
 Module
   TowerFSM.c

 Revision
   1.0.1

 Description
   This is a template file for implementing flat state machines under the
   Gen2 Events and Services Framework.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/15/12 11:12 jec      revisions for Gen2 framework
 11/07/11 11:26 jec      made the queue static
 10/30/11 17:59 jec      fixed references to CurrentEvent in RunTemplateSM()
 10/23/11 18:20 jec      began conversion from SMTemplate.c (02/20/07 rev)
****************************************************************************/
/*----------------------------- 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 "TowerFSM.h"
#include <xc.h>
#include <sys/attribs.h>
#include <PIC32_AD_Lib.h>

/*----------------------------- Module Defines ----------------------------*/
#define LIFT                    1
#define LOWER                   0
#define STEP_TIME               7
//#define STEP_DELAY              100
#define LEVEL1                  1000   // Adjust these based on testing
#define LEVEL2                  5000
#define LEVEL3                  10000
#define TIMEOUT_LIMIT_UP 1900
#define TIMEOUT_LIMIT_DOWN 100
#define PR_VAL 24999 // 20ms period TO DO: CHECK THIS VALUE
#define SENSOR_THRESHOLD 400 // TODO: change this value appropriately.



/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine.They should be functions
   relevant to the behavior of this state machine
*/

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well.
// type of state variable should match htat of enum in header file
static TemplateState_t CurrentState;

// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static uint32_t ADC_Results[0];  // Array to store ADC readings
static uint8_t levelDir = 0;
static uint16_t currentStep = 0;
static uint16_t counter = 0;
static uint16_t target = 0;
static const uint8_t fullStepSequence[4][4] = {
    {1, 0, 0, 1}, // Step 1
    {0, 1, 0, 1}, // Step 2
    {0, 1, 1, 0}, // Step 3
    {1, 0, 1, 0},  // Step 4
};

static uint8_t pseudoState = 1;
static uint32_t timeoutCounter = 0;



/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitTowerFSM

 Parameters
     uint8_t : the priorty 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
 Notes

 Author
     J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
bool InitTowerFSM(uint8_t Priority)
{
    // set up the timer for PWM OC stuff
    // configure pin 12, RA4 to output
    
    // ***TODO*** map to the correct pin.
    
    TRISAbits.TRISA4 = 0;

    T2CONbits.ON = 0;
	//2. Clear the TCS control bit (TxCON<1> = 0) to select the internal PBCLK source.
    T2CONbits.TCS = 0;
	//3. Select the desired timer input clock prescale.
	T2CONbits.TCKPS = 0b011; // 1:8
	T2CONbits.TGATE = 0;
	//4. Load/Clear the timer register TMRx.
    TMR2 = 0;
	//5. Load the period register PRx with the desired 16-bit match value.
    PR2 = PR_VAL; // 20ms period. PR_VAL is 6500
	//6. Set the ON control bit (TxCON<15> = 1) to enable the timer.
    T2CONbits.ON = 1; 
	
	// init OC5R register to control the servo
    OC5CONbits.ON = 0; // Turn off the OC1 when performing the setup
    OC5CONbits.OCM = 0b110; // PWM mode
 
	// set timer 2 as the clock source
    RPA4R = 0b0110; // assign OC5 to pin 14
    OC5CONbits.OCTSEL = 0;
    OC5CONbits.OC32 = 0;
    OC5CONbits.OCSIDL = 0;
    OC5R = 0; // Initialize primary Compare register
    OC5RS = 0; // Initialize secondary Compare register

    OC5CONbits.ON = 1;
    
    ES_Event_t ThisEvent;

    MyPriority = Priority;
    TRISBbits.TRISB4 = 0;
    TRISBbits.TRISB5 = 0;
    TRISBbits.TRISB9 = 0;
    TRISBbits.TRISB10 = 0;
    
    ANSELBbits.ANSB15 = 0;  // Disable analog function on RB15
    TRISBbits.TRISB15 = 1;  // Set RB15 as input

    
    // put us into the Initial PseudoState
    CurrentState = IdleState;
    // post the initial transition event
    ThisEvent.EventType = ES_INIT;
    DB_printf("This is Initializing the Tower FSM\n");
    if (ES_PostToService(MyPriority, ThisEvent) == true)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/****************************************************************************
 Function
     PostTowerFSM

 Parameters
     EF_Event_t 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
 Notes

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

/****************************************************************************
 Function
    RunTowerFSM

 Parameters
   ES_Event_t : the event to process

 Returns
   ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes
   uses nested switch/case to implement the machine.
 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunTowerFSM(ES_Event_t ThisEvent)
{
    ES_Event_t ReturnEvent;
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch (CurrentState)
    {
        case IdleState:
        {
            switch (ThisEvent.EventType)
            {
                case EV_TOWER_RAISE:  
                {
                    DB_printf("Raising the tower...\n");
                    timeoutCounter = 0; // Counter for stopping after ADC threshold
                    pseudoState = 1; // Start in normal stepping mode

                    ES_Timer_InitTimer(MOTOR_TIMER, STEP_TIME);
                    CurrentState = MovingUpState;
                }
                break;

                case EV_TOWER_LOWER:
                {
                    DB_printf("Lowering the tower...\n");
                    levelDir = LOWER;  // Set direction for lowering
                    timeoutCounter = 0; // Counter for stopping after ADC threshold
                    pseudoState = 0; // Start in normal stepping mode

                    ES_Timer_InitTimer(MOTOR_TIMER, STEP_TIME);
                    CurrentState = MovingDownState;
                }
                break;

                case EV_MAGNET_UP:
                {
                    DB_printf("Magnet moving up...\n");
                    OC5RS = 1550; // Set servo to 90 degrees
                    ES_Timer_StopTimer(MOTOR_TIMER);
                    ES_Timer_InitTimer(MOTOR_TIMER, 3000); // Wait 100ms
                    CurrentState = RotatingMagnetUp;
                }
                break;

                case EV_MAGNET_DOWN:
                {
                    DB_printf("Magnet moving down...\n");
                    OC5RS = 3500; // Set servo to 180 degrees
                    ES_Timer_StopTimer(MOTOR_TIMER);
                    ES_Timer_InitTimer(MOTOR_TIMER, 2000); // Wait 100ms
                    CurrentState = RotatingMagnetDown;
                }
                break;

                default:
                    break;
            }
        }
        break;
    
        //---------------------------------- Moving Up ----------------------------------
        case MovingUpState:
        {
            if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == MOTOR_TIMER) 
            {
                // Continue stepping through the stepper sequence
                LATBbits.LATB4 = fullStepSequence[currentStep][0];
                LATBbits.LATB5 = fullStepSequence[currentStep][1];
                LATBbits.LATB9 = fullStepSequence[currentStep][2];
                LATBbits.LATB10 = fullStepSequence[currentStep][3];

                if (currentStep == 3) {
                    currentStep = 0;
                } else {
                    currentStep++;
                }

                if (pseudoState == 0) {  
                    // Normal stepping, check ADC threshold
                    if (PORTBbits.RB15 == 0) {  
                        DB_printf("Threshold reached, switching to timeout mode.\n");
                        pseudoState = 1;  // Enter timeout mode
                    }
                } 

                else if (pseudoState == 1) {  
                    // Increment timeout counter while still stepping
                    timeoutCounter++;
                    DB_printf("Timeout counter: %d\n", timeoutCounter);
                    if (timeoutCounter > TIMEOUT_LIMIT_UP) {  
                        DB_printf("Stopping movement (UP).\n");
                        CurrentState = IdleState;
                        ES_Event_t TowerEvent;
                        TowerEvent.EventType = ES_TOWER_FINISHED;
                        PostCommunicationService(TowerEvent);
                    }
                }

                // Continue stepping motor
                ES_Timer_InitTimer(MOTOR_TIMER, STEP_TIME);
            }
        }
        break;

        //---------------------------------- Moving Down ----------------------------------
        case MovingDownState:
        {
            if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == MOTOR_TIMER) 
            {
                ADC_MultiRead(ADC_Results);
                uint16_t sensorValue = ADC_Results[0];  // only read 1 pin...
                DB_printf("ADC Reading: %d\n", sensorValue);

                // Continue stepping through the stepper sequence
                LATBbits.LATB4 = fullStepSequence[currentStep][0];
                LATBbits.LATB5 = fullStepSequence[currentStep][1];
                LATBbits.LATB9 = fullStepSequence[currentStep][2];
                LATBbits.LATB10 = fullStepSequence[currentStep][3];
                
                if (currentStep == 0) {
                    currentStep = 3;
                } else {
                    currentStep--;
                }

                if (pseudoState == 0) {  
                    // Normal stepping, check ADC threshold
                    if (sensorValue < SENSOR_THRESHOLD) {  
                        DB_printf("Threshold reached, switching to timeout mode.\n");
                        pseudoState = 1;  // Enter timeout mode
                    }
                } 

                else if (pseudoState == 1) {  
                    // Increment timeout counter while still stepping
                    timeoutCounter++;
                    DB_printf("Timeout counter: %d\n", timeoutCounter);
                    if (timeoutCounter > TIMEOUT_LIMIT_DOWN) {  
                        DB_printf("Stopping movement (DOWN).\n");
//                        CurrentState = IdleState;
//                        ES_Event_t TowerEvent;
//                        TowerEvent.EventType = ES_TOWER_FINISHED;
//                        PostCommunicationService(TowerEvent);
//                        break;
                    }
                }

                // Continue stepping motor
                ES_Timer_InitTimer(MOTOR_TIMER, STEP_TIME);
            }
        }
        break;
        
        case RotatingMagnetUp:
        {
            if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == MOTOR_TIMER)
            {
                DB_printf("Finished rotating magnet up, turning OC5 off...\n");
                OC5RS = 0; // Turn off PWM output
                CurrentState = IdleState;
                ES_Event_t TowerEvent;
                TowerEvent.EventType = ES_TOWER_FINISHED;
                PostCommunicationService(TowerEvent);
            }
        }
        break;

        //---------------------------------- Rotating Magnet Down ----------------------------------
        case RotatingMagnetDown:
        {
            if (ThisEvent.EventType == ES_TIMEOUT && ThisEvent.EventParam == MOTOR_TIMER)
            {
                DB_printf("Finished rotating magnet down, turning OC5 off...\n");
                OC5RS = 0; // Turn off PWM output
                CurrentState = IdleState;
                ES_Event_t TowerEvent;
                TowerEvent.EventType = ES_TOWER_FINISHED;
                PostCommunicationService(TowerEvent);
            }
        }
        break;
        default:
        {
        }                
    break;// end switch on Current State
    }
//    break;

    return ReturnEvent;
}

/****************************************************************************
 Function
     QueryTemplateSM

 Parameters
     None

 Returns
     TemplateState_t The current state of the Template state machine

 Description
     returns the current state of the Template state machine
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:21
****************************************************************************/
TemplateState_t QueryTemplateFSM(void)
{
  return CurrentState;
}