/****************************************************************************
 Module
   T_NormalOp.c

 Description
   This is a sub state machine (3rd level) called NormalOp within the
   Mining level within the Master module for ME 218B project for Team 5.
   Implements a hierarchical state machine under the Gen2 Events and
   Services Framework.

 Notes

 Private Definitions
   ENTRY_STATE
   CKH
   GHI
   TargetPeriodA
   TargetPeriodB
   TargetPeriodC
   TargetPeriodD
   CKHMLOC1
   CKHMLOC2
   GHIMLOC1
   GHIMLOC2
   CKHMLOC1
   CKHMLOC2
   GHIMLOC1
   GHIMLOC2
   PUR1
   PUR2
   REVERSE
   FORWARD
   WAIT_TIME
   CurrentState
   DesiredPeriod
   xcoordinate[16]
   ycoordinate[16]
   PrevTargetPeriod

 Private Functions
   static ES_Event_t DuringSearching(ES_Event_t Event)
   static ES_Event_t DuringTransport(ES_Event_t Event)
   static void calcNearestMiner(void)
   static bool isMinerPlaced(uint8_t MLoc)

ES_Event_t RunNormalOp(ES_Event_t CurrentEvent) ----------------------------
  // initialize NextState to CurrentState
  // set MakeTransition false

  // based on our CurrentState:
  // if our CurrentState is Searching
    // call the during function of Searching - DuringSearching
    // process any events
    // if a COLLISION_DETECTED event occurs
      // re-map this event for Mining SM with a parameter of REVERSE
    // if a ES_TIMEOUT event occurs with SERVO_TIMER as a parameter
      // a miner has been successfully picked up, set reposition to false
      // set NextState to Transporting
      // consume this event
    // if a ES_TIMEOUT event occurs with SEARCH_TIMER as a parameter
      // we don't have a miner, set reposition to true
      // set NextState to Transporting
      // consume this event

  // if our CurrentState is Transporting
    // call the during function of Transporting - DuringTransporting
    // process any events
    // if a COLLISION_DETECTED event occurs
      // re-map this event for Mining SM
      // set MakeTransition to true
      // check previous state of TransportingSM
      // if previous state of TransportingSM was Reversing
        // set event parameter to FORWARD
      // otherwise
        // set event parameter to REVERSE
    // if a ES_TIMEOUT event occurs
      // if the parameter is REVERSE_TIMER
        // if 2 miners have been placed
          // Start DISPLAY_TIMER
          // set MakeTransition to true
          // set NextState to Waiting
          // consume this event
      // if the parameter is DROP_OFF_TIMER
        // set NextState to Searching
        // set MakeTransition to true
        // consume this event
      // for any other parameter
        // set NextState to Searching
        // set MakeTransition to true
        // consume this event

  // if our CurrentState is Waitng
    // if an ES_TIMEOUT event occurs with DISPLAY_TIMER as a parameter
      // if less than 2 miners have been placed (active regions have changed)
        // set NextState to Searching
        // set MakeTransition to true
        // consume this event
      // otherwise
        // start DISPLAY_TIMER

  // if MakeTransition is true
    // execute the exit function for current state
    // set CurrentState to NextState
    // execute entry function for next state
    // return ReturnEvent

void StartNormalOp (ES_Event_t CurrentEvent) ------------------------------
  // call the entry function for NormalOp (RunNormalOp)

NormalOpState_t QueryNormalOp(void) ---------------------------------------
  // return CurrentState

void UpdatePrevTargetPeriod(void) -----------------------------------------
  // set PrevTargetPeriod to DesiredPeriod

static ES_Event_t DuringSearching(ES_Event_t Event) -----------------------
  // set ReturnEvent to Event
  // based on Event:
    // process ES_ENTRY or ES_ENTRY_HISTORY events
      // calculate nearest miner to be picked up
      // call StartSearchingSM with Event
    // process ES_EXIT events
      // call RunSearchingSM with Event
    // otherwise
      // call RunSearchingSM with Event

static ES_Event_t DuringTransport(ES_Event_t Event) -----------------------
  // set ReturnEvent to Event
  // based on Event:
    // process ES_ENTRY or ES_ENTRY_HISTORY events
      // calculate current position of robot
      // call StartSearchingSM with Event
    // process ES_EXIT events
      // call RunTransportingSM with Event
    // otherwise
      // call RunTransportingSM with Event

static void calcNearestMiner(void) ---------------------------------------
 // check what team we are on
 // if we are team CKH (red)
   // save miner locations based on correct registers from SPUD
   // calculate distance from each miner
   // if Miner 1 location is good, it is closer than Miner 2 and Miner1 has
   // not been placed in that location already
     // set DesiredPeriod to period of Miner 1, team CKH
   // otherwise
     // set DesiredPeriod to period of Miner 2, team CKH
   // 
     // set DesiredPeriod to period of Miner 2 
   // if DesiredPeriod is the same as PrevTargetPeriod
      // if DesiredPeriod is period of Miner 1 CKH
        // set DesiredPeriod to period of Miner 2
      // if DesiredPeriod is period of Miner 2 CKH
        // set DesiredPeriod to period of Miner 1
 // if we are team GHI (blue)
   // save miner locations based on correct registers from SPUD
   // calculate distance from each miner
   // if Miner 1 location is good, it is closer than Miner 2 and Miner1 has
   // not been placed in that location already
     // set DesiredPeriod to period of Miner 1, team GHI
   // otherwise
     // set DesiredPeriod to period of Miner 2, team GHI
   // 
     // set DesiredPeriod to period of Miner 2 
   // if DesiredPeriod is the same as PrevTargetPeriod
      // if DesiredPeriod is period of Miner 1 GHI
        // set DesiredPeriod to period of Miner 2
      // if DesiredPeriod is period of Miner 2 GHI
        // set DesiredPeriod to period of Miner 1

static bool isMinerPlaced(uint8_t MLoc) --------------------------------
  // save neutral permit locations from SPUD
  // check if MLoc is a good location
  // check what team we are on
    // if we are team CKH (red)
      // save exclusive CKH company location
      // if MLoc is a good location
        // if the miner is in a valid mining region
          // return true
    // if we are team GHI (blue)
      // if MLoc is a good location
        // if the miner is in a valid mining region
          // return true
    // otherwise
      // return false

uint16_t calcDistance(uint8_t MLoc) -------------------------------------
  // get robot's current x coordinate
  // get robot's current y coordinate
  // get miner's current x coordinate
  // get miner's current y coordinate
  // calculate and return L2-norm distance between miner and robot's coordinates

uint16_t calcL1Distance(uint8_t MLoc) -----------------------------------
  // get robot's current x coordinate
  // get robot's current y coordinate
  // get miner's current x coordinate
  // get miner's current y coordinate
  // calculate and return L1-norm distance between miner and robot's coordinates

uint16_t getDesiredPeriod(void) -----------------------------------------
  // return DesiredPeriod

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