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

Module
TVS.c

Revision
1.0.1

Description
This is a template file for implementing flat state machines under the
Gen2 Events and Services Framework.

Notes

History
When Who What/Why
-------------- --- --------
01/15/12 11:12 jec revisions for Gen2 framework
11/07/11 11:26 jec made the queue static
10/30/11 17:59 jec fixed references to CurrentEvent in RunTemplateSM()
10/23/11 18:20 jec began conversion from SMTemplate.c (02/20/07 rev)
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "TVS.h"
#include "Inputs.h"
#include "UART.h"
#include "OPAMP.h"
#include<math.h>
/*----------------------------- Module Defines ----------------------------*/
#define WEAP_CAP_MAX 200
#define TVS_UPDATE_TIME 50
#define ONE_SEC 1000
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine.They should be functions
relevant to the behavior of this state machine
*/

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

// everybody needs a state variable, you may need others as well.
// type of state variable should match htat of enum in header file
static TvsState_t CurrentState;

static WeaponCoord_t lastWeapLoc;

static Joystick weaponsAngle;
static uint8_t weaponsRange;
static uint8_t weaponsCapacity;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;

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

/****************************************************************************
Function
InitTemplateFSM
 Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
J. Edward Carryer, 10/23/11, 18:55
****************************************************************************/
bool InitTvs(uint8_t Priority)
{
ES_Event_t ThisEvent;

MyPriority = Priority;
// put us into the Initial PseudoState
CurrentState = TvsInitP;
// post the initial transition event
ThisEvent.EventType = ES_INIT;
if (ES_PostToService(MyPriority, ThisEvent) == true)
{
return true;
}
else
{
return false;
}
}

/****************************************************************************
Function
PostTvs

Parameters
EF_Event_t ThisEvent , the event to post to the queue

Returns
boolean 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 PostTvs(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
Function
RunTvs
Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event_t RunTvs(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (CurrentState)
{
case TvsInitP: // If current state is initial Psedudo State
{
if (ThisEvent.EventType == ES_INIT) // only respond to ES_Init
{
// this is where you would put any actions associated with the
// transition from the initial pseudo-state into the actual
// initial state
weaponsCapacity = WEAP_CAP_MAX;
weaponsRange = 0;
weaponsAngle.X = 0;
weaponsAngle.Y = 0;
lastWeapLoc.XCoord = 0;
lastWeapLoc.YCoord = 0;
// now put the machine into the actual initial state
if(ThisEvent.EventParam == 1){
CurrentState = TvsActive;
ES_Timer_InitTimer(TVS_TIMER, TVS_UPDATE_TIME);
}else{
CurrentState = TvsInactive;
}
}
}
break;

case TvsInactive: // If current state is state one

{
switch (ThisEvent.EventType)
{
case ES_TVS_SWITCH: //If event is event one
{ // Execute action function for state one : event one
if(ThisEvent.EventParam == 1){
CurrentState = TvsActive; //Decide what the next state will be
ES_Timer_InitTimer(TVS_TIMER, TVS_UPDATE_TIME);

}
}break;
case ES_OUR_WEAPON_FIRED:
{
 lastWeapLoc = getOurWeaponCoords();
printf("\n\rweapon fired!!\n\r");
}break;
case ES_WEAPONSCAP_UP:
{
weaponsCapacity = ThisEvent.EventParam;
}break;
case ES_DEAD:
{
CurrentState = TvsDead;
}break;
default:
;
} // end switch on CurrentEvent
}
break;
case TvsActive: // If current state is state one
{
switch (ThisEvent.EventType)
{
case ES_TVS_SWITCH: //If event is event one
{ // Execute action function for state one : event one
if(ThisEvent.EventParam == 0){
CurrentState = TvsInactive; //Decide what the next state will be
}
}break;
case ES_WEAPONFIRED_BUTTON_PRESS:
{
printf("\n\rweapon fired!!\n\r");
uint16_t shotPwr = weaponsRange*200/128;
int32_t jsX = getJoystick().X - 512;
int32_t jsY = getJoystick().Y - 512;
int16_t rawRange = sqrt((jsX*jsX)+(jsY*jsY));
if(weaponsCapacity > shotPwr){
//full range used
weaponsCapacity = weaponsCapacity - shotPwr;
lastWeapLoc.XCoord = (float)(jsX)*shotPwr*1000*128/(rawRange*7.8125*200);
//math here
lastWeapLoc.YCoord = (float)(jsY)*shotPwr*1000*128*(-1)/
(rawRange*7.8125*200); //math here
}else {
//full capacity used
lastWeapLoc.XCoord = (float)(jsX)*weaponsCapacity*1000*128/
(rawRange*7.8125*200); //math here
lastWeapLoc.YCoord = (float)(jsY)*weaponsCapacity*1000*128*(-1)/
(rawRange*7.8125*200); //math here
weaponsCapacity = 0;
}
//range^2 = x^2+y^2

ES_Event_t fireEvent;
fireEvent.EventType = ES_OUR_WEAPON_FIRED;
ES_PostAll(fireEvent);

}break;
case ES_TIMEOUT:
{
 //UPDATE Joystick angle and weapons range
if(getAimSwitchState()){
weaponsAngle = getJoystick();
}
weaponsRange = getWeaponsRangeInput();
weaponsAngle = getJoystick();
uint8_t wPwr = getPowerLevels().weapons;
//set shieldLevels
uint16_t weapInc = wPwr*TVS_UPDATE_TIME/ONE_SEC;
//printf("\n\rwcap:%u, winc:%u\n\r", weaponsCapacity, weapInc);
if(weaponsCapacity + weapInc < WEAP_CAP_MAX){
weaponsCapacity += weapInc; // & 255;
}else{
weaponsCapacity = WEAP_CAP_MAX;
}
ES_Timer_InitTimer(TVS_TIMER, TVS_UPDATE_TIME);
}break;
case ES_DEAD:
{
CurrentState = TvsDead;
}break;
// repeat cases as required for relevant events
default:
;
} // end switch on CurrentEvent
}
break;
// repeat state pattern as required for other states
default:
;
} // end switch on Current State
return ReturnEvent;
}

/****************************************************************************
Function
QueryTvs

Parameters
None

Returns
TemplateState_t The current state of the Template state machine

Description
returns the current state of the Template state machine
Notes

Author
J. Edward Carryer, 10/23/11, 19:21
****************************************************************************/
TvsState_t QueryTvs(void)
{
return CurrentState;
}

uint8_t getWeaponsRange(void){
 if(getGameMode().TVS == 1){
return getWeaponsRangeInput();
}else{
return weaponsRange;
}
}

Joystick getWeaponsAngle(void){
if(getGameMode().TVS == 1){
return getJoystick();
}else{
return weaponsAngle;
}
}

WeaponCoord_t getWeaponLocation(void){
return lastWeapLoc;
}

uint8_t getWeaponsCapacity(void){
return weaponsCapacity;
}

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