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

Module
Zener.c

Revision
1.0

Description
This module handles the ZENER station operation

Notes

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Hardware
#include <xc.h>
#include <proc/p32mx170f256b.h>
#include <sys/attribs.h> // for ISR macors
#include <math.h>

// Event & Services Framework

#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "ES_ShortTimer.h"
#include "ES_Port.h"

//Project files
#include "Zener.h"
#include "PIC32_AD_Lib.h"
#include <stdio.h>
#include <string.h>
#include "OLEDMapSM.h"

// TRIAC-wide helper functions

#include "TRIACService.h"
/*----------------------------- Module Defines ----------------------------*/
// these times assume a 10.000mS/tick timing
#define ENTER_POST ((MyPriority<<3)|0)
#define ENTER_RUN ((MyPriority<<3)|1)
#define ENTER_TIMEOUT ((MyPriority<<3)|2)

#define POSITION_UPDATE_RATE 15.625 //1000/64 = 15.625

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
// add a deferral queue for up to 3 pending deferrals +1 to allow for overhead
static ES_Event_t DeferralQueue[3 + 1];
static uint32_t adcResults[2];

static float LastVelocity[2] = {0, 0}; //module array for velocity

static float LastPosition[2] = {0, 0}; //module array for position
static uint8_t ShieldStr; //module variable for shield strength
static uint8_t HullStr = 100; //module variable for hull strength
static int16_t BeamDx; //module variable for beamDx VW: changed to signed number. TODO: if in
kilometer. if in scaled value, we're good. these may need to be bigger than int16? TS: there
is some weird beam bending that occurs, maybe this is due to rollover?
static int16_t BeamDy; //module variable for beamDy
static uint8_t WeaponFiredFlag;
static uint8_t ZenerHitFlag;

static uint8_t LocalAssumeCommand;

static uint8_t LocalTransmissionType;
static uint8_t LocalWeaponFired;
static uint8_t LocalShipHit;
static uint8_t LocalWeaponsPower;
static uint8_t LocalNavPower = 10;
static uint8_t LocalWeaponsCap;
static uint8_t LocalShieldCap;

static GlobalPacket_t CurrentGlobal;

static GlobalPacket_t* pCurrentGlobal = &CurrentGlobal;

static LocalPacket_t CurrentLocal;

static LocalPacket_t* pCurrentLocal = &CurrentLocal;

static uint8_t RxPacket[20];

static uint8_t* pRxPacket = RxPacket;
static uint8_t RxPacketType; //1 if global packet, 0 if local

/*---------------------------- Module Functions ---------------------------*/

/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/
static void FillRxPacket(void);
void FillLocalPacket(LocalPacket_t* pInputPacket, uint16_t DestinationAddress);
void FillGlobalPacket(GlobalPacket_t* pInputPacket);
static void DecodePacket(uint8_t* inputPacket, uint8_t PacketType);
static float GetDist2Beam(float x, float y, float beamDx, float beamDy, float shipX, float
shipY);

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

/****************************************************************************
Function
InitZener

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

****************************************************************************/
bool InitZener(uint8_t Priority)
{
ES_Event_t ThisEvent;
 MyPriority = Priority;

//VW: don't initialize anything. initialize in pseudo state under run function.

// post the initial transition event

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

/****************************************************************************
Function
PostZener

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
Notes

****************************************************************************/
bool PostZener(ES_Event_t ThisEvent)
{
// printf("\rEvent received at service ZENER\r\n");
return ES_PostToService(MyPriority, ThisEvent);
}

/****************************************************************************
Function
RunZener

Parameters
ES_Event : the event to process

Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

Description
add your description here
Notes

****************************************************************************/
ES_Event_t RunZener(ES_Event_t ThisEvent)
{
ES_Event_t ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
static char DeferredChar = '1';
switch (ThisEvent.EventType)
{
case ES_INIT_ZENER:
{
/********************************************
in here you write your initialization code
*******************************************/
// initialize deferral queue for testing Deferal function
ES_InitDeferralQueueWith(DeferralQueue, ARRAY_SIZE(DeferralQueue));
initUART(); //Init UART rx and tx
initJoystick(); //Init pins for Joystick
initTimer5(); //Init timer 5 for position calculations
}
break;

case ES_NEWGLOBAL:
{
//Init global packet with static values
InitGlobalPacket(pCurrentGlobal);
//Fill global packet with module level variables
FillGlobalPacket(pCurrentGlobal);
//Complete packet with checksum and length
CompleteGlobalPacket(pCurrentGlobal);

/***** VW: draw our location on OLED*****/

SetSelfXY(LastPosition[1], LastPosition[0]);
ES_Event_t OLEDEvent = {ES_DRAW_OLED,};
PostOLEDMapSM(OLEDEvent);
/**************************************************/
//Use void pointer to write packet to tx pin
void* buffPtr = &CurrentGlobal;
for (uint8_t i = 0; i < sizeof(GlobalPacket_t); i++)
{
WriteByte(*((uint8_t *) buffPtr++));
}
//After global packet has been sent post ES_NEWLOCAL event
ES_Event_t LocalEvent;
LocalEvent.EventType = ES_NEWLOCAL;
PostZener(LocalEvent);
}
break;

case ES_NEWLOCAL:
{
//Initialize first local packet with static values
InitLocalPacket(pCurrentLocal);
//Fill packet with data from module level variables
FillLocalPacket(pCurrentLocal, 0x218B);//Vivian's MEICM address
//Complete packet with checksum
CompleteLocalPacket(pCurrentLocal);
//Use void pointer to write packet to tx pin
void* buffPtr = &CurrentLocal;
for (uint8_t i = 0; i < sizeof(LocalPacket_t); i++)
{
WriteByte(*((uint8_t *) buffPtr++));
}
 //Send second local packet repeating the same steps but with a
//different address
InitLocalPacket(pCurrentLocal);
FillLocalPacket(pCurrentLocal, 0x2192); //Mei's MEICM address
CompleteLocalPacket(pCurrentLocal);
buffPtr = &CurrentLocal;
for (uint8_t i = 0; i < sizeof(LocalPacket_t); i++)
{
WriteByte(*((uint8_t *) buffPtr++));
}
//After sending the local packets reset the ZenerHitFlag
ZenerHitFlag = 0; //Reset ZenerHitFlag after sending out local packets
}
break;

case ES_RXPACKET:
{
//Copies received packet into RxPacket and gets packet type
RxPacketType = GetRxPacket(pRxPacket);
//Print statement for debugging
//for (uint8_t i = 0; i < 19;i++)
//{
//printf("%x,",RxPacket[i]);
//}
//printf("\r\n");
//Call the decode packet function to retrieve data from packet
DecodePacket(pRxPacket, RxPacketType);
}
break;
default:
{}
break;
}
return ReturnEvent;
}
/*---------------------------- Packet Functions ----------------------------*/
/****************************************************************************
Function
FillGlobalPacket

Parameters
GlobalPacket_t*: Pointer to a global packet struct

Returns
nothing

Description
Fills the global packet with data from module level variables
Notes

****************************************************************************/
void FillGlobalPacket(GlobalPacket_t* pInputPacket)
{
//Fill remaining frame data with module level variables
pInputPacket->Status = 0b11000010; //team 6, alive, weap not fired
//If Hull Strength is zero mark our team dead
if (HullStr == 0){
 pInputPacket->Status = pInputPacket->Status & 0b11111101;
}
//If weapon is fired set weapon fired flag
if (WeaponFiredFlag){
pInputPacket->Status += 1;
WeaponFiredFlag = 0;
}
pInputPacket->HullStrength = HullStr;
pInputPacket->ShipX = LastPosition[1]/(7.8125); // VW: this will divide then floor it to
the int.
pInputPacket->ShipY = LastPosition[0]/(7.8125);
pInputPacket->BeamDx = BeamDx;
pInputPacket->BeamDy = BeamDy;
//pInputPacket->ShieldStrength = ShieldStr;
pInputPacket->ShieldStrength = LocalShieldCap;

}
/****************************************************************************
Function
FillLocalPacket

Parameters
LocalPacket_t*: Pointer to a global packet struct

Returns
nothing

Description
Fills the local packet with data from module level variables
Notes

****************************************************************************/
void FillLocalPacket(LocalPacket_t* pInputPacket, uint16_t DestinationAddress)
{
//Fill remaining frame data with module level variables
pInputPacket->DestinationAddressMSB = (DestinationAddress >> 8);
pInputPacket->DestinationAddressLSB = DestinationAddress & 0x00FF; //get lower 8
pInputPacket->Status = 0b11010010 + ZenerHitFlag;
pInputPacket->HullStr = HullStr;
pInputPacket->BeamDx = BeamDx;
pInputPacket->BeamDy = BeamDy;
pInputPacket->WeaponsPower = LocalWeaponsPower;
pInputPacket->NavPower = LocalNavPower;
pInputPacket->WeaponsCap = LocalWeaponsCap;
pInputPacket->ShieldCap = LocalShieldCap;
}

/***************************************************************************
private functions
***************************************************************************/
/**********************************************************************
* GetDist2Beam
Returns the shortest distance between our ship and enemy's beam envelope
x: our x coordinate in km
y: our y coordinae in km
beamDx: beam vector x
beamDy: beam vector y
 shipX: enemy coordinate x
shipY: enemy coordinate y in km
********************************************************************/
static float GetDist2Beam(float x, float y, float beamDx, float beamDy, float shipX, float shipY)
{
// wrap around calcs: a point (x,y)in the 2D map (x in [0,512000], y in [0,256000]
// can actually be any of the 9 points in the donut,
// which is the set {(mH + x, nW + y)} where H=512,000, W=256,000. m = -1,0,1, n = -1,0,1
float newDist;
beamDy += 1;
beamDx += 1;
float m = beamDy/beamDx;
float b = shipY - m*shipX;
// case 1: m,n = 0
float x0 = x;
float y0 = y;
float dist2beam = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
// case 2: m=-1,n=-1
x0 = x - REGION_WIDTH;
y0 = y - REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 3: m=-1,n=0
x0 = x - REGION_WIDTH;
y0 = y;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 4: m=-1,n=1
x0 = x - REGION_WIDTH;
y0 = y + REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 5: m=0,n=-1
x0 = x;
y0 = y - REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 6: m=0,n=1
x0 = x;
y0 = y + REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 7: m=1,n=-1
x0 = x + REGION_WIDTH;
y0 = y - REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
 dist2beam = newDist; // return shortest distance we've seen
}
// case 8: m=1,n=0
x0 = x + REGION_WIDTH;
y0 = y;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}
// case 9: m=1,n=1
x0 = x + REGION_WIDTH;
y0 = y + REGION_HEIGHT;
newDist = (abs(y0 - (m*x0+b)) / sqrt(1+m*m));
if (newDist < dist2beam) {
dist2beam = newDist; // return shortest distance we've seen
}

return dist2beam;
}

/****************************************************************************
Function
DecodePacket
Parameters
uint8_t*: inputPacket array
Returns
nothing
Description
Decodes the packet and stores relevant information
Notes

****************************************************************************/
static void DecodePacket(uint8_t* inputPacket, uint8_t PacketType)
{
if (PacketType == 0) //If packet is LocalPacket
{
//Byte 0 -> Start Delimiter
//Byte 1 -> Length MSB
//Byte 2 -> Length LSB
//Byte 3 -> API identifier
//Byte 4 -> Source Address MSB
//Byte 5 -> Source Address LSB
uint16_t Source = (*(inputPacket + 4) << 8) | (*(inputPacket + 5));
//Byte 6 -> RSSI
//Byte 7 -> Options
//Byte 8 -> Status
uint8_t Status = *(inputPacket + 8);
LocalAssumeCommand = (Status >> 5) & 1;
LocalTransmissionType = (Status >> 2) & 7;
LocalWeaponFired = (Status >> 1) & 1;
LocalShipHit = Status & 1;
//Byte 9 -> Hull Strength
//HullStr = *(inputPacket + 9);
//Byte 10 -> Beam Dx LSB
//Byte 11 -> Beam Dx MSB
BeamDx = (*(inputPacket + 11) << 8) | (*(inputPacket + 10));
//Byte 12 -> Beam Dy LSB
 //Byte 13 -> Beam Dy MSB
BeamDy = (*(inputPacket + 13) << 8) | (*(inputPacket + 12));
//Byte 14 -> Weapons Power
LocalWeaponsPower = (*(inputPacket +14));
//Byte 15 -> Navigation Power
//LocalNavPower = (*(inputPacket + 15)); moved to below
//Byte 16 -> Weapons Capacity
LocalWeaponsCap = (*(inputPacket + 16)); //dont care
//Byte 17 -> Shield Capacity
//LocalShieldCap = (*(inputPacket + 17));//dont care
//Byte 18 -> Checksum

if (LocalTransmissionType & 1){ //TVS

float BeamDxf = BeamDx*7.8125; //scale beam calcs
float BeamDyf = BeamDy*7.8125;
SetWeaponStat(LocalWeaponFired, BeamDxf, BeamDyf);
if(LocalWeaponFired){
WeaponFiredFlag = true; //Set local fired flag high
printf("%f,",BeamDx*7.8125);
printf("%f,",BeamDy*7.8125);
printf("WeaponFired\r\n");
}
}
else if ((LocalTransmissionType >> 1) & 1){ //OPAMP
//Store navpower, hull strength and shield strength from OPAMP
LocalNavPower = (*(inputPacket + 15));
HullStr = *(inputPacket + 9);
LocalShieldCap = (*(inputPacket + 17));
}
else if ((LocalTransmissionType >> 2) & 1){ //Zener
//This is Zener so do nothing
}
else{
//error?
}

}
else if (PacketType == 1) //GlobalPacket
{
//Byte 0 -> Start Delimiter
//Byte 1 -> Length MSB
//Byte 2 -> Length LSB
//Byte 3 -> API identifier
//Byte 4 -> Source Address MSB
//Byte 5 -> Source Address LSB
//Byte 6 -> RSSI
//Byte 7 -> Options
//Byte 8 -> Status
uint8_t StatusByte = *(inputPacket + 8);
uint8_t TeamNum = (StatusByte >> 5); //Shift by 5 to go from 765 to 210
uint8_t AliveDead = (StatusByte >> 1) & 1;
uint8_t WeaponFired = StatusByte & 1;
//Byte 9 -> Hull Strength //do we care??
//Byte 10 -> Ship X pos LSB
//Byte 11 -> Ship X pos MSB
float ShipX = (*(inputPacket + 11) << 8) | (*(inputPacket + 10));
ShipX *= 7.8125;//scale
 //Byte 12 -> Ship Y pos LSB
//Byte 13 -> Ship Y pos MSB
float ShipY = (*(inputPacket + 13) << 8) | (*(inputPacket +12));
ShipY *= 7.8125;//scale
//Byte 14 -> Beam dx
//Byte 15 -> Beam dx
float BeamDxO = (*(inputPacket + 15) << 8) | (*(inputPacket + 14));
BeamDxO *= 7.8125; // scale
//Byte 16 -> Beam dy
//Byte 17 -> Beam dy
float BeamDyO = (*(inputPacket + 17) << 8) | (*(inputPacket + 16));
BeamDxO *= 7.8125; // scale
//Byte 18 -> Shield Str //do we care??
//Byte 19 -> CheckSum
if (WeaponFired){
printf("Team: %d, BeamDx: %f, BeamDy: %f\r\n", TeamNum, BeamDxO, BeamDyO);
}
if (TeamNum != 6)
{
if (AliveDead){
MarkTeamActive(TeamNum);
}
else{
MarkTeamDead(TeamNum);
}
SetTeamXY(TeamNum, ShipX, ShipY); //Set the team position on screen
}
//Get distance to the beam with dist2beam
float Dist2Beam = GetDist2Beam(LastPosition[1], LastPosition[0], BeamDxO, BeamDyO,
ShipX, ShipY);
if (WeaponFired == 1 && Dist2Beam <= 1250) //if weapon was fired and we're within beam
{
//if (BeamDxO > 0 && BeamDyO > 0){
if(TeamNum != 3)
{
ZenerHitFlag = 1; //set zener hit flag high
LATAbits.LATA4 = 1; //turn on LED
printf ("BeamDx: %f\r\n", BeamDxO);
printf("BeamDy: %f\r\n", BeamDyO);
printf("HIT by Team%d\r\n",TeamNum);
}
//}
}
else
{
LATAbits.LATA4 = 0; //turn LED off
}
//For debug
//printf("S:%x,%x,%x,%x,",StatusByte,TeamNum,AliveDead,WeaponFired);
//printf("P:%x,%x",ShipX,ShipY);
//printf("B:%x,%x\r\n",BeamDyO,BeamDyO);
}
else
{
//error??
}
//Post OLEDEvent to the OLED sm
 ES_Event_t OLEDEvent = {ES_DRAW_OLED,};
PostOLEDMapSM(OLEDEvent);
}
/*------------------------------- Timer/ISR -------------------------------*/
/****************************************************************************
Function
Timer5ResponseFunc
Parameters
nothing
Returns
nothing
Description
Calculates position and velocity
Notes

****************************************************************************/
void __ISR(_TIMER_5_VECTOR,IPL6SOFT) Timer5ResponseFunc(void)
{
IFS0CLR = _IFS0_T5IF_MASK; // Clear the timer5 interrupt flag
static float newPosition[2];
static float newVelocity[2];
ADC_MultiRead(adcResults); //Read value
//For deadzone creation
for (uint8_t i = 0; i < 2; i++)
{
//If in deadzone set adc results to 0
if (adcResults[i] > 350 && adcResults[i] < 650){
adcResults[i] = 0;
}
else{ //else scale from 0-1023 to -511-512
adcResults[i] = (adcResults[i] - 511);
}
}
if (PORTBbits.RB8 == 0) //Joystick button is pressed, brake engaged
{
//Calculate the norm and normalize the x any y velocity direction
float LastVelNorm = sqrt((LastVelocity[0]*LastVelocity[0])+
(LastVelocity[1]*LastVelocity[1]));
float LastVelX = LastVelocity[1]/LastVelNorm;
float LastVelY = LastVelocity[0]/LastVelNorm;
//Calculate new velocity with accceleration vector pointing the opposite of last
velocity
newVelocity[0] = LastVelocity[0] - LocalNavPower*1000*LastVelY;
newVelocity[1] = LastVelocity[1] - LocalNavPower*1000*LastVelX;
//set last velocity to the new velocity calculated
LastVelocity[0] = newVelocity[0];
LastVelocity[1] = newVelocity[1];
//calculate new position
newPosition[0] = LastPosition[0] + newVelocity[0]/POSITION_UPDATE_RATE;
newPosition[1] = LastPosition[1] + newVelocity[1]/POSITION_UPDATE_RATE;
}
//In joystick deadzone, no thrust, update with last velocity
else if (adcResults[0] == 0 && adcResults[1] == 0)
{
newPosition[0] = LastPosition[0] + LastVelocity[0]/POSITION_UPDATE_RATE;
newPosition[1] = LastPosition[1] + LastVelocity[1]/POSITION_UPDATE_RATE;
}
 else //Changing acceleration
{
//New values are stored in floatResults for negative operations/float math
float floatResults[2];
memcpy(floatResults, adcResults, sizeof(floatResults));
float accNorm = sqrt(adcResults[0]*adcResults[0] +
adcResults[1]*adcResults[1]);
/*takes normalized vector, multiplies by read val/max val,
* multiplied by available nav power in GW,
* multiplied by 1000 bc of 0.001 GW/km/s^2*/
double CurrentAcceleration[2] = {0,0};
CurrentAcceleration[0]=
((adcResults[0]*adcResults[0])/(accNorm*512))*LocalNavPower*1000;
CurrentAcceleration[1]=
((adcResults[1]*adcResults[1])/(accNorm*512))*LocalNavPower*1000;

for (uint8_t i = 0; i < 2; i++) //Calculate new velocity

{
if (floatResults[i] < 0){ //if negative subtract
newVelocity[i] = newVelocity[i] - CurrentAcceleration[i];
}
else{ //else add
newVelocity[i] = newVelocity[i] + CurrentAcceleration[i];
}
}
//Light speed compensation
float TotalVelocity = sqrt(newVelocity[0]*newVelocity[0] +
newVelocity[1]*newVelocity[1]);
float alpha = (TotalVelocity / (300*1000));
if (alpha > 1) //If above light speed -> squash to light speed
{
newVelocity[0] = newVelocity[0]/alpha;
newVelocity[1] = newVelocity[1]/alpha;
TotalVelocity = TotalVelocity/alpha;
}
//Calculate the new position and update LastVelocitys
newPosition[0] = LastPosition[0] + newVelocity[0]/POSITION_UPDATE_RATE;
newPosition[1] = LastPosition[1] + newVelocity[1]/POSITION_UPDATE_RATE;
LastVelocity[0] = newVelocity[0];
LastVelocity[1] = newVelocity[1];
}
//Out of bounds compensation
for (uint8_t i = 0; i < 2; i++)
{
uint16_t size;
if (i == 0){
size = 256; //y direction
}
else{
size = 512; //x direction
}
//if out of bounds
if (newPosition[i] > (size*1000) || newPosition[i] < 0){
if (newPosition[i] > size*1000){ //if over -> subtract
LastPosition[i] = newPosition[i] - (size*1000);
}
if(newPosition[i] < 0){ //if under -> add
 LastPosition[i] = (size*1000) + newPosition[i];
}
}
else{ //Update lastposition
LastPosition[i] = newPosition[i];
}
}
}