#include <xc.

h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

#include "ES_Configure.h"
#include "ES_Framework.h"
#include "DrivingService.h"
#include "dbprintf.h"

#include "PIC32PortHAL.h"
#include "PIC32_AD_Lib.h"

#include "LEDService.h"

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

#define TICS_PER_MS 2500
#define FULL_CCW ((uint16_t)(2.25*TICS_PER_MS))
#define FULL_CW ((uint16_t)(0.7*TICS_PER_MS))

typedef enum
{
InitDriving,
Waiting2Begin,
Steering,
Aligned
} ES_DrivingState_t;
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
uint16_t GetServoPos(void);

/*---------------------------- Module Variables ---------------------------*/

// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
static uint8_t CurrentState;
// track where we are in number of PWM ticks
static uint16_t CurrentPosition;
//array for pin values to select from for LEDs
static uint16_t PinArray[5] = {_Pin_4, _Pin_5, _Pin_11, _Pin_9, _Pin_8};
//pins corresponding to PointLEDs
static uint16_t PointLedAll = _Pin_4 | _Pin_5 | _Pin_11 | _Pin_8 | _Pin_9;
//array for angle positions for LEDs
static uint16_t AngleArray[5] = {1750, 2719, 3687, 4656, 5625};
//wiggle room for alignment
static uint16_t Delta = 5;
//index of LED to move to
static uint16_t PointLED;
//index of LED that indicates that target can be shot
static uint16_t TargetLED = _Pin_10;
//array for reading analog to digital output
static uint32_t ConversionResults[2];
//ccw limit on servo
static uint16_t ccwLimit = FULL_CCW;
//cw limit on servo
static uint16_t cwLimit = FULL_CW;
//factor to convert pot values to PWM ticks
const float Potfactor = (0.7*2500 - 2.25*2500)/1023;
//timer for hitting target
static uint16_t TARGET_TIME = 15000;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitTemplateService

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 InitDrivingService(uint8_t Priority)
{
ES_Event_t ThisEvent;

MyPriority = Priority;
/********************************************
in here you write your initialization code
*******************************************/
CurrentState = InitDriving;

// post the initial transition event

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

/****************************************************************************
Function
PostTemplateService

Parameters
EF_Event_t 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 PostDrivingService(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
Function
RunTemplateService

Parameters
ES_Event_t : 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 RunDrivingService(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

ES_DrivingState_t NextState; //variable to keep track of next state

NextState = CurrentState;
/********************************************
in here you write your service code
*******************************************/
//state machine
if (CurrentState == InitDriving)
{
if (ThisEvent.EventType == ES_INIT)
{
DB_printf("\rES_INIT received in Service %d\r\n", MyPriority);
//set up analog for pot
PortSetup_ConfigureAnalogInputs(_Port_B, _Pin_2 | _Pin_12);
ADC_ConfigAutoScan(BIT4HI | BIT12HI, 2);
ADC_MultiRead(ConversionResults);
//set up pins for LEDs
PortSetup_ConfigureDigitalOutputs(_Port_B, PointLedAll | _Pin_10);

NextState = Waiting2Begin;
}
}
if (CurrentState == Waiting2Begin)
{
if (ThisEvent.EventType == ES_BEGIN)
{
//light up initial LED
srand(time(0));
PointLED = rand() % 5;
uint16_t randomLED = PinArray[PointLED];
LATB = randomLED;
 //set Target timer
ES_Timer_InitTimer(TARGET_TIMER, TARGET_TIME);
NextState = Steering; //go to next state

}
}
if (CurrentState == Steering)
{
if (ThisEvent.EventType == ES_ALIGNED)
{
//Turn on Target LED
LATBSET = TargetLED;
NextState = Aligned;
}
if (ThisEvent.EventType == ES_TIMEOUT)
{
//Turn off Point LED and pick new LED
uint8_t NewLED = rand() % 5;
while(NewLED == PointLED)
{
NewLED = rand() % 5;
}
PointLED = NewLED;
LATB = PinArray[PointLED];

//Reset Target timer

ES_Timer_InitTimer(TARGET_TIMER, TARGET_TIME);
printf("TARGET_TIMEOUT\r\n");
}
if (ThisEvent.EventType == ES_ACTIVITY_TIMEOUT) {
//Turn off PointLED
LATBCLR = PointLedAll;
//Disable all timers
ES_Timer_StopTimer(TARGET_TIMER);

NextState = Waiting2Begin;
}
}
if (CurrentState == Aligned)
{
if (ThisEvent.EventType == ES_TARGET_SHOT)
{
printf("TARGET SHOT\r\n");
// uint32_t val = (uint32_t) ThisEvent.EventParam;
// printf("Value: %d", val);
//Turn off Point LED and pick new LED
uint8_t NewLED = rand() % 5;
printf("%d", PointLED);
while(NewLED == PointLED)
{
NewLED = rand() % 5;
}
PointLED = NewLED;
LATB = PinArray[PointLED];

//Turn off TargetLED

LATBCLR = TargetLED;
//reset target timer
ES_Timer_InitTimer(TARGET_TIMER, TARGET_TIME);
 //reset activity timer
ES_Timer_InitTimer(ACTIVITY_TIMER,30000);

//add to score
ThisEvent.EventType = ES_ADD_SCORE;
ThisEvent.EventParam = 10;
PostLEDService(ThisEvent);

NextState = Steering;
}
if (ThisEvent.EventType == ES_TIMEOUT)
{
//Turn off Point LED and pick new LED
uint8_t NewLED = rand() % 5;
while(NewLED == PointLED)
{
NewLED = rand() % 5;
}
PointLED = NewLED;
LATB = PinArray[PointLED];

//Turn off TargetLED

LATBCLR = TargetLED;
//Reset Target timer
ES_Timer_InitTimer(TARGET_TIMER, TARGET_TIME);
printf("TARGET_TIMEOUT\r\n");

NextState = Steering;
}
if (ThisEvent.EventType == ES_ACTIVITY_TIMEOUT) {
//Turn off TargetLED
LATBCLR = TargetLED;
//Turn off PointLED
LATBCLR = PointLedAll;
//Disable all timers
ES_Timer_StopTimer(TARGET_TIMER);

NextState = Waiting2Begin;
}
if (ThisEvent.EventType == ES_NOT_ALIGNED) {
//Turn off TargetLED
LATBCLR = TargetLED;
// printf("NOTALIGNED-");

NextState = Steering;
}

if (ThisEvent.EventType == ES_NEW_KEY)
{
if (ThisEvent.EventParam = 's')
{
ES_Event_t ShotEvent;
ShotEvent.EventType = ES_TARGET_SHOT;
ES_PostToService(MyPriority, ShotEvent);
}
}
}
CurrentState = NextState;
return ReturnEvent;
}

bool CheckLEDAlignment(void)
{
CurrentPosition = GetServoPos();
bool ReturnVal = false;
if ((AngleArray[PointLED] + 400 >= CurrentPosition) && (AngleArray[PointLED] -
400 <= CurrentPosition))
{
if (CurrentState == Steering) {
ES_Event_t AlignedEvent;
AlignedEvent.EventType = ES_ALIGNED;
ES_PostToService(MyPriority, AlignedEvent);
ReturnVal = true;
printf("ALIGNED\r\n");
}
} else {
if (CurrentState == Aligned) {
ES_Event_t AlignedEvent;
AlignedEvent.EventType = ES_NOT_ALIGNED;
ES_PostToService(MyPriority, AlignedEvent);
printf("444\r\n");
}
}
return ReturnVal;
}
/***************************************************************************
private functions
***************************************************************************/
uint16_t GetServoPos(void)
{
ADC_MultiRead(ConversionResults);
uint16_t PotPos = ConversionResults[0];
return ccwLimit + Potfactor*PotPos;
}
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/