/****************************************************************************
 Module
   ColorDetection_EventChecker.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 "ColorDetection_EventChecker.h"

// Hardware
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.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 Stuff
#include "I2CService.h"
#include "BallSortingService.h"
#include "CompassService.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 Servo positions
#define SERVO_CENTER
#define SERVO_RECYCLE
#define SERVO_LANDFILL

// #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
*/

static uint8_t CategorizeColor(uint16_t Clr_Value, uint16_t Red_Value, uint16_t Blu_Value, uint16_t Grn_Value);
/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
// add a deferral queue for up to 3 pending deferrals +1 to allow for ovehead

//Define color
static uint8_t  CurrentColor = 0;
static uint16_t LastColor = 0;
static bool     NewBall = false;
static uint16_t ColorCounter = 0;
static bool     LegalBall = true;

/****************************************************************************
 Function
     CheckNewBall

 Parameters
     N/A

 Returns
     bool, false if there is no ball detected, true if so

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

 Author
     Pablo Martinez Alanis, 01/16/12, 10:00
****************************************************************************/
bool CheckNewBall(void)
{
  ES_Event_t  ThisEvent;
  bool        ReturnVal = false;
  uint8_t     CompassColor;
  uint16_t    ClearValue;
  uint16_t    RedValue;
  uint16_t    BluValue;
  uint16_t    GrnValue;

//  printf("Event Checker");
//Reading current color value from Compass Service

  //Read I2C Clear value
  ClearValue = I2C_GetClearValue();
  RedValue = I2C_GetRedValue();
  BluValue = I2C_GetBlueValue();
  GrnValue = I2C_GetGreenValue();
  //obtain color from private module function
  CurrentColor = CategorizeColor(ClearValue, RedValue, BluValue, GrnValue);
  //printf("C:%d R:%d, B:%d, G:%d\n\r",ClearValue,RedValue,BluValue,GrnValue);
  if ((ClearValue > 4500) && (CurrentColor < 7) && (LegalBall == true))
  {
    if (ColorCounter == 100)
    {
			
			//Obtain value of Accepted Compass Color
			CompassColor=GetColorAssignment();
      //Check if it is the assigned color or a blue ball
      if ((CurrentColor == CompassColor) || (CurrentColor == 4))
      {
        //Recycling ball recieved, send event to Ball Sorting Service
        ThisEvent.EventType = BALL_AT_COLOR_SENSOR;
        ThisEvent.EventParam = 1;
        PostBallSortingService(ThisEvent);
      }
      else
      { //Landfill ball recieved, send event to Ball Sorting Service
        ThisEvent.EventType = BALL_AT_COLOR_SENSOR;
        ThisEvent.EventParam = 0;
        PostBallSortingService(ThisEvent);
      }
      LegalBall = false;
      ReturnVal = true;
    }
    else
    {
      ColorCounter++;
    }
  }
  else if (ClearValue < 2000)
  {
    ColorCounter = 0;
    LegalBall = true;
  }
  return ReturnVal;
}

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

static uint8_t CategorizeColor(uint16_t Clr_Value, uint16_t Red_Value, uint16_t Blu_Value, uint16_t Grn_Value)
{
  //Variable to return color
  uint8_t   BallColor;
  //Obtain percentages of values
  uint16_t  Red_Per = Red_Value * 100 / Clr_Value;
  uint16_t  Grn_Per = Grn_Value * 100 / Clr_Value;
  uint16_t  Blu_Per = Blu_Value * 100 / Clr_Value;
  //printf("C: %d,R: %d, B: %d, G: %d\n\r",Clr_Value,Red_Value,Blu_Value,Grn_Value);
  //printf("R: %d, B: %d, G: %d\n\r",Red_Per,Blu_Per,Grn_Per);
  //Check individual tresholds for each color from percentages
  if ((Red_Per > 65) && (Red_Per < 71) && (Blu_Per > 12) && (Blu_Per < 18) && (Grn_Per > 13) && (Grn_Per < 19))
  {
    //Red ball detected
    BallColor = 0;
    //printf("Red");
  }
  else if ((Red_Per > 59) && (Red_Per < 65) && (Blu_Per > 10) && (Blu_Per < 16) && (Grn_Per > 18) && (Grn_Per < 24))
  {
    //Orange ball detected
    BallColor = 1;
    //printf("Orange");
  }
  else if ((Red_Per > 40) && (Red_Per < 52) && (Blu_Per > 10) && (Blu_Per < 16) && (Grn_Per > 30) && (Grn_Per < 40))
  {
    //Yellow ball detected
    BallColor = 2;
    //printf("Yellow");
  }
  else if ((Red_Per > 30) && (Red_Per < 36) && (Blu_Per > 14) && (Blu_Per < 20) && (Grn_Per > 40) && (Grn_Per < 46))
  {
    //Green ball detected
    BallColor = 3;
    //printf("Green");
  }
  else if ((Red_Per > 8) && (Red_Per < 16) && (Blu_Per > 45) && (Blu_Per < 58) && (Grn_Per > 29) && (Grn_Per < 35))
  {
    //Blue ball detected
    BallColor = 4;
    //printf("Blue");
  }
  else if ((Red_Per > 52) && (Red_Per < 58) && (Blu_Per > 21) && (Blu_Per < 27) && (Grn_Per > 17) && (Grn_Per < 23))
  {
    //Pink ball detected
    BallColor = 5;
    //printf("Pink");
  }
  else
  {
    //No correct color detected, return trash byte
    BallColor = 8;
  }
  return BallColor;
}

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