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

Module
Attack.c

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

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_ShortTimer.h"

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"
#include "termio.h"

#include "Attack.h"
#include "LEDRegisterWrite.h"
#include "AudioRegisterWrite.h"
#include "PWM16Tiva.h"
#include "ADMulti.h"
#include "TRex.h"
#include "MusicSequence.h"
#include "Idle.h"

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

// these times assume a 1.000mS/tick timing
#define ONE_SEC 976
#define HALF_SEC (ONE_SEC / 2)
#define TWO_SEC (ONE_SEC * 2)
#define FIVE_SEC (ONE_SEC * 5)

#define RAPTOR_DURATION TWO_SEC

#define LED_DATA BIT0HI

#define LED_SCLK BIT1HI
#define LED_RCLK BIT2HI

#define AUDIO_DATA BIT1HI

#define AUDIO_SCLK BIT2HI
#define AUDIO_RCLK BIT3HI

#define SERVO_GROUP 4
#define RAPTOR_SERVO 8
#define MAX_ATTACKS 3
#define RAPTOR_HOME 0
#define RAPTOR_END 5

#define ADC_CHANNELS 1

#define HIT1_LED 0x0100

#define HIT2_LED 0x0200
#define HIT3_LED 0x0400
#define HAND_UP_LED 0x0800
#define HAND_DOWN_LED 0x1000

/*---------------------------- 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 consts want to control type

static const uint32_t HEIGHT_DISC = 250; // about 0.2V
static const uint32_t LOW_LIMIT = 3000; // about 2.6V 15cm
static const uint32_t HIGH_LIMIT = 1300; // about 0.73V 50cm

static const uint16_t SERVO_PERIOD = 25000; // 20ms in 0.8us ticks

// just static
static uint8_t MyPriority;

static AttackState_t CurrentState = InitializeAttack;

static uint32_t FullHeight[ADC_CHANNELS];

static uint32_t LastHeight[ADC_CHANNELS];

static uint32_t HandHeights[] = {

HIGH_LIMIT, 1400, 1650, 2000, 2500, LOW_LIMIT
};
// 40cm, 30cm, 25cm, 20cm
static uint16_t HitLED[] = {
HIT1_LED, HIT2_LED, HIT3_LED
};

static uint16_t RaptorPositions[] = {

2800, 2640, 2480, 2320, 2160, 2000
}; //one
servo extreme to the other (need to find desire angles)
static uint8_t RaptorIndex = 0;
static uint8_t AttackIndex = 1;
static uint8_t Direction = 0; // 0 down 1 up

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

/****************************************************************************
Function
InitAttack

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

****************************************************************************/
bool InitAttack(uint8_t Priority)
{
ES_Event ThisEvent;

MyPriority = Priority;

//Put us into the initial pseudo-state to set up for the initial transition
 CurrentState = InitializeAttack;

//Initialize a single AtoD pin for range finder input

ADC_MultiInit(ADC_CHANNELS);

//Initialize Module level variables

ADC_MultiRead(FullHeight);
LastHeight[0] = FullHeight[0];

// post the initial transition event

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

/****************************************************************************
Function
PostAttack

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

****************************************************************************/
bool PostAttack(ES_Event ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
Function
RunAttack

Parameters
ES_Event : the event to process

Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

Description
Controls the raptor servo position using analog IR range finder

****************************************************************************/
ES_Event RunAttack(ES_Event ThisEvent)
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
uint16_t LEDValue;

switch (CurrentState)
{
case InitializeAttack:
{
 if (ThisEvent.EventType == ES_INIT)
{
// write raptor servos to home
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
//RaptorIndex = RaptorIndex + 1;
CurrentState = WaitForLastNote;
}
}
break;

case WaitForLastNote:
{
if (ThisEvent.EventType == LAST_NOTE)
{
// write Down LED on
LEDValue = (LED_GetCurrentRegister() | HAND_DOWN_LED);
LED_Write(LEDValue);
RaptorIndex = 0;
Direction = 1;
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);

CurrentState = WaitForDown;
}
else if (ThisEvent.EventType == EARLY_RESET)
{
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
CurrentState = WaitForLastNote;
}
}
break;

case WaitForDown:
{
if (ThisEvent.EventType == MOVE_DOWN)
{
// Write raptors to next position (dont need to change period)
PWM_TIVA_SetPulseWidth(RaptorPositions[RaptorIndex], RAPTOR_SERVO);
PostIdle(ThisEvent);
}
else if ((ThisEvent.EventType == FULLY_DOWN) && (AttackIndex != MAX_ATTACKS))
{
// write next HITLED ON, handdown led OFF and handup led ON
LEDValue = ((LED_GetCurrentRegister() | HitLED[AttackIndex - 1] |
HAND_UP_LED) & ~HAND_DOWN_LED);
if (AttackIndex == 2)
{
LEDValue |= HitLED[0];
}
LED_Write(LEDValue);
// Write raptors to end position (dont need to change period)
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_END], RAPTOR_SERVO);

ES_Event NewEvent;
NewEvent.EventType = ATTACK;
NewEvent.EventParam = AttackIndex;
PostTRex(NewEvent);
PostMusicSequence(NewEvent);

// post the fully up to idle

PostIdle(ThisEvent);

AttackIndex++;
CurrentState = WaitForUp;
}
 else if ((ThisEvent.EventType == FULLY_DOWN) && (AttackIndex == MAX_ATTACKS))
{
// write next HITLED ON, handdown led OFF and handup led OFF
LEDValue = ((LED_GetCurrentRegister() | HitLED[0] | HitLED[1] | HitLED[2])
& ~HAND_DOWN_LED & ~HAND_UP_LED);
LED_Write(LEDValue);
// Write raptors to end position (dont need to change period)
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_END], RAPTOR_SERVO);
// set raptors up timer
ES_Timer_InitTimer(RaptorTimer, RAPTOR_DURATION);

// post victory to all SM

ES_Event NewEvent;
NewEvent.EventType = VICTORY;
ES_PostList00(NewEvent);

PostIdle(ThisEvent);

CurrentState = HoldUp;
}
else if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
MainTimer))
{
// write next HITLEDs OFF, handdown led OFF and handup led OFF
LEDValue = (LED_GetCurrentRegister() & ~HIT1_LED & ~HIT2_LED & ~HIT3_LED &
~HAND_DOWN_LED & ~HAND_UP_LED);
LED_Write(LEDValue);

// Write raptors to home position (dont need to change period)

PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);

CurrentState = WaitForHeadphones;
}
else if (ThisEvent.EventType == EARLY_RESET)
{
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
CurrentState = WaitForLastNote;
}
}
break;

case WaitForUp:
{
if (ThisEvent.EventType == MOVE_UP)
{
// Write raptors to next position (dont need to change period)
PWM_TIVA_SetPeriod(SERVO_PERIOD, SERVO_GROUP);
PWM_TIVA_SetPulseWidth(RaptorPositions[RaptorIndex], RAPTOR_SERVO);

PostIdle(ThisEvent);
}
else if (ThisEvent.EventType == FULLY_UP)
{
// write handdown led ON and handup led OFF
LEDValue = ((LED_GetCurrentRegister() | HAND_DOWN_LED) & ~HAND_UP_LED);
LED_Write(LEDValue);
// Write raptors to next position (dont need to change period)
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);

PostIdle(ThisEvent);

CurrentState = WaitForDown;
}
else if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
MainTimer))
{
// write next HITLEDs OFF, handdown led OFF and handup led OFF
LEDValue = (LED_GetCurrentRegister() & ~HIT1_LED & ~HIT2_LED & ~HIT3_LED &
~HAND_DOWN_LED & ~HAND_UP_LED);
LED_Write(LEDValue);

// Write raptors to home position (dont need to change period)

PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);

CurrentState = WaitForHeadphones;
}
else if (ThisEvent.EventType == EARLY_RESET)
{
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
CurrentState = WaitForLastNote;
}
}
break;

case HoldUp:
{
if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam ==
RaptorTimer))
{
// Write raptors to home position (dont need to change period)
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);

CurrentState = WaitForHeadphones;
}
else if (ThisEvent.EventType == HEADPHONES_RESET)
{
RaptorIndex = 0;
AttackIndex = 1;
LastHeight[0] = FullHeight[0];

PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
CurrentState = WaitForLastNote;
}
else if (ThisEvent.EventType == EARLY_RESET)
{
PWM_TIVA_SetPulseWidth(RaptorPositions[RAPTOR_HOME], RAPTOR_SERVO);
CurrentState = WaitForLastNote;
}
}
break;

case WaitForHeadphones:
{
if (ThisEvent.EventType == HEADPHONES_RESET)
{
RaptorIndex = 0;
AttackIndex = 1;
LastHeight[0] = FullHeight[0];
CurrentState = WaitForLastNote;
}
}
break;
}
return ReturnEvent;
}

/****************************************************************************
Function
 Check4HandMovement

Parameters
void

Returns
True if event is posted

Description
Determines if hand has moved a certain distance
(kind of a SM and E checker in one, I did it this way to reduce the number of events)

****************************************************************************/
// TODO: figure out if these variables can be reduced to uint16_t
bool Check4HandMovement(void)
{
bool ReturnValue = false;
uint32_t CurrentHeight[ADC_CHANNELS];
uint32_t Delta;

// read current height

ADC_MultiRead(CurrentHeight);

Delta = abs(CurrentHeight[0] - LastHeight[0]);

if (Direction) //going down

{
if ((LastHeight[0] < LOW_LIMIT) && (CurrentHeight[0] > LOW_LIMIT))
{
RaptorIndex = RAPTOR_END;

ES_Event NewEvent;
NewEvent.EventType = FULLY_DOWN;
NewEvent.EventParam = CurrentHeight[0];
PostAttack(NewEvent);

Direction = 0; // going up
ReturnValue = true;
}
for (uint8_t i = (RaptorIndex + 1); i <= 4; i++)
{
if (((LastHeight[0] < HandHeights[i]) && (CurrentHeight[0] >
HandHeights[i])))
{
RaptorIndex = i;
ES_Event NewEvent;
NewEvent.EventType = MOVE_DOWN;
NewEvent.EventParam = CurrentHeight[0];
PostAttack(NewEvent);
ReturnValue = true;
}
}
}
else // going up
{
if ((LastHeight[0] > HIGH_LIMIT) && (CurrentHeight[0] < HIGH_LIMIT))
{
RaptorIndex = RAPTOR_HOME;

ES_Event NewEvent;
NewEvent.EventType = FULLY_UP;
NewEvent.EventParam = CurrentHeight[0];
PostAttack(NewEvent);
 Direction = 1; // going down
ReturnValue = true;
}
for (uint8_t i = (RaptorIndex - 1); i >= 1; i--)
{
if (((LastHeight[0] > HandHeights[i]) && (CurrentHeight[0] <
HandHeights[i])))
{
RaptorIndex = i;
ES_Event NewEvent;
NewEvent.EventType = MOVE_UP;
NewEvent.EventParam = CurrentHeight[0];
PostAttack(NewEvent);
ReturnValue = true;
}
}
}

LastHeight[0] = CurrentHeight[0];

return ReturnValue;
}

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