/****************************************************************************
 Module
   TestHarnessService0.c

 Revision
   1.0.1

 Description
   This is the first service for the Test Harness under the
   Gen2 Events and Services Framework.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 10/26/17 18:26 jec     moves definition of ALL_BITS to ES_Port.h
 10/19/17 21:28 jec     meaningless change to test updating
 10/19/17 18:42 jec     removed referennces to driverlib and programmed the
                        ports directly
 08/21/17 21:44 jec     modified LED blink routine to only modify bit 3 so that
                        I can test the new new framework debugging lines on PF1-2
 08/16/17 14:13 jec      corrected ONE_SEC constant to match Tiva tick rate
 11/02/13 17:21 jec      added exercise of the event deferral/recall module
 08/05/13 20:33 jec      converted to test harness service
 01/16/12 09:58 jec      began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// This module
#include "TestHarnessService0.h"

// Hardware
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_nvic.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"

// Event & Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_ShortTimer.h"
#include "ES_Port.h"

//Other Includes
#include "GameplayHSM.h"
#include "MotorSM.h"
#include "ServoModule.h"
#include "GameControlService.h"
#include "Gameplay_GameTasksSM.h"
#include "BallSortingService.h"

/*----------------------------- Module Defines ----------------------------*/
// these times assume a 1.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 ALL_BITS (0xff<<2)   Moved to ES_Port.h
/*---------------------------- 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 ovehead
static ES_Event_t DeferralQueue[3 + 1];

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitTestHarnessService0

 Parameters
     uint8_t : the priorty of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, and does any
     other required initialization for this service
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitTestHarnessService0(uint8_t Priority)
{
  ES_Event_t ThisEvent;

  MyPriority = Priority;
  /********************************************
   in here you write your initialization code
   *******************************************/
  // initialize deferral queue for testing Deferal function
  ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));
  // initialize LED drive for testing/debug output
  // initialize the Short timer system for channel A
  ES_ShortTimerInit(MyPriority, SHORT_TIMER_UNUSED);

#ifdef _INCLUDE_BYTE_DEBUG_
  // initialize the byte-wide debugging
  _HW_ByteDebug_Init();
#endif

//  // set up I/O lines for debugging
//  // enable the clock to Port B
//  HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
//  // kill a few cycles to let the peripheral clock get going
//  while ((HWREG(SYSCTL_PRGPIO) & BIT1HI) != BIT1HI)
//  {}
//  // Enable pins for digital I/O
//  HWREG(GPIO_PORTB_BASE + GPIO_O_DEN) |= (BIT2HI);

//  // make pin 2 on Port B into outputs
//  HWREG(GPIO_PORTB_BASE + GPIO_O_DIR) |= (BIT2HI);
//  // start with the lines low
//  HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT2LO;

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

/****************************************************************************
 Function
     PostTestHarnessService0

 Parameters
     EF_Event ThisEvent ,the event to post to the queue

 Returns
     bool 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 PostTestHarnessService0(ES_Event_t ThisEvent)
{
#ifdef _INCLUDE_BYTE_DEBUG_
  _HW_ByteDebug_SetValueWithStrobe( ENTER_POST);
  _HW_ByteDebug_SetValueWithStrobe( END_SERVICE);
#endif
  return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunTestHarnessService0

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes

 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunTestHarnessService0(ES_Event_t ThisEvent)
{
  ES_Event_t  ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  ES_Event_t  NextEvent;

#ifdef _INCLUDE_BYTE_DEBUG_
  _HW_ByteDebug_SetValueWithStrobe(ENTER_RUN);
#endif
  switch (ThisEvent.EventType)
  {
    case ES_INIT:
    {
      ES_Timer_InitTimer(SERVICE0_TIMER, HALF_SEC);
      puts("Key Mapping:");
      //NextEvent.EventType=MOTOR_COMMAND;
      //NextEvent.EventParam=FORWARD;
      //PostMotorSM(NextEvent);
    }
    break;
    case ES_NEW_KEY:   // announce
    {
      printf("\n\rES_NEW_KEY received with -> %c <- in Service 0\r\n",
          (char)ThisEvent.EventParam);
      if ('F' == ThisEvent.EventParam)
      {
        printf("\n\rForward");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = FORWARD;
        PostMotorSM(NextEvent);
      }
      else if ('B' == ThisEvent.EventParam)
      {
        printf("\n\rBackward");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = BACKWARDS;   // This one gets posted normally
        PostMotorSM(NextEvent);
        // but we slide the deferred events under it so it(they) should come out first
      }
      else if ('L' == ThisEvent.EventParam)
      {
        printf("\n\rLeft Stop");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = LEFT_STOP_RIGHT_BACK;
        PostMotorSM(NextEvent);
      }
      else if ('R' == ThisEvent.EventParam)
      {
        printf("\n\rRight Stop");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = RIGHT_STOP_LEFT_BACK;
        PostMotorSM(NextEvent);
      }
      else if ('S' == ThisEvent.EventParam)
      {
        printf("\n\rStop");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = STOP;
        PostMotorSM(NextEvent);
      }
      else if ('Q' == ThisEvent.EventParam)
      {
        printf("\n\rCW");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = CW;
        PostMotorSM(NextEvent);
      }
      else if ('W' == ThisEvent.EventParam)
      {
        printf("\n\rCCW");
        NextEvent.EventType = MOTOR_COMMAND;
        NextEvent.EventParam = CCW;
        PostMotorSM(NextEvent);
      }
      else if ('T' == ThisEvent.EventParam)
      {
        uint8_t LEDStatus = (HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) & BIT1HI);
        printf("Toggling LED %x\n\r", LEDStatus);
        if (LEDStatus == 0)
        {
          printf("Turning OFF LED");
          HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) |= BIT1HI;
        }
        else
        {
          printf("Turning on LED");
          HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ALL_BITS)) &= BIT1LO;
        }
      }
      else if ('P' == ThisEvent.EventParam)
      {
        printf("Send out trash");
        SortTrash();
      }
      else if ('O' == ThisEvent.EventParam)
      {
        printf("Send out recycle");
        SortRecycle();
      }
      else if ('I' == ThisEvent.EventParam)
      {
        printf("Reset Sorter");
        ResetSorter();
      }
      else if ('G' == ThisEvent.EventParam)
      {
        printf("\r\nHARD CODED GAME START\r\n");
        NextEvent.EventType = GAME_STARTED;
        PostGameplayMasterSM(NextEvent);
        PostGameControlService(NextEvent);
      }
      else if ('0' == ThisEvent.EventParam)
      {
        printf("\r\nHARD CODED GAME OVER\r\n");
        NextEvent.EventType = GAME_OVER;
        PostGameplayMasterSM(NextEvent);
        PostGameControlService(NextEvent);
      }
      else if ('1' == ThisEvent.EventParam)
      {
        printf("\r\nHARD CODED START_DUMPING\r\n");
        NextEvent.EventType = START_DUMPING;
        PostGameplayMasterSM(NextEvent);
        PostGameControlService(NextEvent);
      }
      else if ('6' == ThisEvent.EventParam)
      {
        printf("\r\nHARD CODED BEACON_DETECTED with Freq %d\r\n", QueryBeaconRearFreq());
        NextEvent.EventType = BEACON_DETECTED;
        NextEvent.EventParam = REAR;
        PostGameplayMasterSM(NextEvent);
        PostGameControlService(NextEvent);
      }
      else if ('5' == ThisEvent.EventParam)
      {
        printf("\r\nHARD CODED TAPE_DETECTED\r\n");
        NextEvent.EventType = TAPE_DETECTED;
        NextEvent.EventParam = CENTER;
        PostGameplayMasterSM(NextEvent);
        PostGameControlService(NextEvent);
      }
      else if ('3' == ThisEvent.EventParam)
      {
        printf("\r\nOpen Recycle Chute\r\n");
        OpenRecycleChute();
        NextEvent.EventType = RECYCLE_EMPTY;
        PostGameplayMasterSM(NextEvent);
        PostBallSortingService(NextEvent);
      }
      else if ('7' == ThisEvent.EventParam)
      {
        printf("\r\nClose Recycle Chute\r\n");
        CloseRecycleChute();
      }
      else if ('8' == ThisEvent.EventParam)
      {
        printf("\r\nOpen Trash Chute\r\n");
        OpenTrashChute();
        NextEvent.EventType = TRASH_EMPTY;
        PostGameplayMasterSM(NextEvent);
        PostBallSortingService(NextEvent);
      }
      else if ('9' == ThisEvent.EventParam)
      {
        printf("\r\nClose Trash Chute\r\n");
        CloseTrashChute();
      }
    }
    break;
    default:
    {}
    break;
  }
#ifdef _INCLUDE_BYTE_DEBUG_
  _HW_ByteDebug_SetValueWithStrobe(END_SERVICE);
#endif

  return ReturnEvent;
}

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

/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/