/* 
 * File:   MeleeStats.c
 * Author: Alex
 *
 * Created on April 27, 2017, 11:05 PM
 */

#include <xc.h>
#include "./configbits-16ex8.h"
#include "SDlib16.h"
#include "SDlib16_delay.h"
#include <inttypes.h>
#include "SPICon.h"
#include "MeleeStatData.h"
#include <math.h>

void resetDataCapture(void);
int waitForEvent(void);
void SPI4init(void);
void SPI2init (void);
void SPI2send (unsigned char c);
void sendFloat(unsigned char indicator1, unsigned char indicator2, float statistic);
void sendInt(unsigned char indicator1, unsigned char indicator2, uint32_t statistic);
void sendHalf(unsigned char indicator1, unsigned char indicator2, uint16_t statistic);
void sendByte(unsigned char indicator1, unsigned char indicator2, uint32_t statistic);
uint8_t readByte(unsigned char * data);
uint16_t readHalf(unsigned char * data);
uint32_t readWord(unsigned char * data);
float readFloat(unsigned char * data);
void readFrameData(void);
void initializePlayers(void);
void publishData(void);
void computeStatistics(void);
int numberOfSetBits(uint16_t x);

void DMAinit(unsigned int source, unsigned int dest);

unsigned int sourceAddr;
unsigned int destAddr;
unsigned int sourceAddr1;
unsigned int destAddr1;

#define DATA_LENGTH 123
#define TRANSFER_WAIT_TIME 500 //30 ms was lowest we got to consistently work

unsigned char currentData[DATA_LENGTH] = {0};
unsigned char previousData[DATA_LENGTH] = {0};
int pos = 0;

Game currentGame;

int main(int argc, char** argv)
{ 
    SPI2init();
    serial_init(57600);
    set_output_device(1);
    sourceAddr = (unsigned int) &SPI4BUF & 0x1FFFFFFF;
    destAddr = (unsigned int) &currentData[0] & 0x1FFFFFFF; 
while(1)
{
    initializePlayers();
    
    publishData();
    
    int check = 0;
  
    printf("Starting \n\r");
    
    SPI4init();   
    
    DMAinit(sourceAddr, destAddr);
    
    while(check!=1)
    {
        check=waitForEvent();
    }

    delay_us(18);
    
    pos=1;
    currentGame.stage = readHalf(currentData);
    pos++;
    currentGame.players[0].characterId = readByte(currentData);
    pos+=3;
    currentGame.players[1].characterId = readByte(currentData);

    
    resetDataCapture();
    check=waitForEvent();
    
    int i=0;
    while(1)
    {
        while(!DCH0INTbits.CHBCIF);
        DCH0INTbits.CHBCIF = 0;
        SPI4CONbits.ON = 0;
        /*
               SPI2_CHIPSELECT = 0;
            int o = 0;
            for(o; o<123; o++)
            {
                SPI2send(currentData[o]);
            }
            SPI2_CHIPSELECT = 1;
        */
        readFrameData();
        
        if(currentGame.frameCounter>1)
            computeStatistics();
        
        resetDataCapture();
        DCH0CONbits.CHEN = 1;
        check = waitForEvent();
        if(check==3)
            break;
    }

    
    publishData();

    printf("Done \n\r");
}
    while(1);
    
    return(EXIT_SUCCESS);
} 

void resetDataCapture(void)
{
    SPI4CONbits.ON = 0;
    DMACONbits.ON = 0;
    memset(currentData, 0x00, DATA_LENGTH);
    delay_us(2);
    SPI4CONbits.ON = 1;
    DMACONbits.ON = 1;
}

int waitForEvent(void)
{
    while(1)
    {
        while(!DCH0INTbits.CHCCIF);
        if(currentData[0]==0x38)
        {
            return 2;
        }
        else if(currentData[0]==0x37)
        {
            return 1;
        }
        else if (currentData[0]==0x39)
        {
            return 3;
        }
        else
        {
            resetDataCapture();
        }
        DCH0INTbits.CHCCIF = 0;
    }
}

void SPI2send (unsigned char c){
    
    char r; // make a variable to hold the data coming in that you are throwing out
    SPI2BUF = c;
    while (!SPI2_RBF){}
    r = SPI2BUF; // Read the data out of the receive buffer and trash
    
}

void DMAinit(unsigned int source, unsigned int dest)
{
    DMACONbits.ON = 1;            // dma module on
    DCRCCON = 0;                // crc module off
    DCH0CON=0x3; // channel off, priority 3, no chaining
    DCH0ECON=0; // no start or stop IRQs, no pattern match
    DCH0INT = 0;                // interrupts disabled
    DCH0SSA = sourceAddr;         // source start address
    DCH0DSA = destAddr;        // destination start address
    DCH0SSIZ = 1;               // source size - 1 bytes
    DCH0DSIZ = DATA_LENGTH;               // destination size - 1 byte
    DCH0CSIZ = 1;               // cell size - 1 bytes
    DCH0ECONbits.CHSIRQ = 0b00101001;          // dma transfer triggered by SPI4RX
    DCH0ECONbits.SIRQEN = 1;
    DCH0INTbits.CHBCIE = 1;
    DCH0INTbits.CHCCIE = 1;
    
    DCH0CONbits.CHEN = 1;
    DCH0CONbits.CHPRI = 0b11;
    
}

void sendFloat(unsigned char indicator1, unsigned char indicator2, float statistic)
{
    unsigned char * floatBytes = (unsigned char *)&statistic;
    unsigned char byte0 = floatBytes[0];
    unsigned char byte1 = floatBytes[1];
    unsigned char byte2 = floatBytes[2];
    unsigned char byte3 = floatBytes[3];
    
    SPI2_CHIPSELECT = 0;
    SPI2send(0x02);
    SPI2send(0x00);
    SPI2send(indicator1);
    SPI2send(indicator2);
    SPI2send(byte3);
    SPI2send(byte2);
    SPI2send(byte1);
    SPI2send(byte0);
    SPI2_CHIPSELECT = 1;
    delay_ms(TRANSFER_WAIT_TIME);
}

void sendInt(unsigned char indicator1, unsigned char indicator2, uint32_t statistic)
{
    SPI2_CHIPSELECT = 0;
    SPI2send(0x02);
    SPI2send(0x00);
    SPI2send(indicator1);
    SPI2send(indicator2);
    SPI2send(statistic & 0xFF);
    SPI2send(statistic >> 8 & 0xFF);
    SPI2send(statistic >> 16 & 0xFF);
    SPI2send(statistic >> 24 & 0xFF);
    SPI2_CHIPSELECT = 1;
    delay_ms(TRANSFER_WAIT_TIME);
}

void sendHalf(unsigned char indicator1, unsigned char indicator2, uint16_t statistic)
{
    SPI2_CHIPSELECT = 0;
    SPI2send(0x02);
    SPI2send(0x00);
    SPI2send(indicator1);
    SPI2send(indicator2);
    SPI2send(statistic & 0xFF);
    SPI2send(statistic >> 8 & 0xFF);
    SPI2send(0);
    SPI2send(0);
    SPI2_CHIPSELECT = 1;
    delay_ms(TRANSFER_WAIT_TIME);
}

void sendByte(unsigned char indicator1, unsigned char indicator2, uint32_t statistic)
{
    SPI2_CHIPSELECT = 0;
    SPI2send(0x02);
    SPI2send(0x00);
    SPI2send(indicator1);
    SPI2send(indicator2);
    SPI2send(statistic);
    SPI2send(0);
    SPI2send(0);
    SPI2send(0);
    SPI2_CHIPSELECT = 1;
    delay_ms(TRANSFER_WAIT_TIME);
}

uint16_t readHalf(unsigned char * data) {
  uint16_t value = data[pos] << 8 | data[pos + 1];
  pos += 2;
  return value;
}

uint8_t readByte(unsigned char * data) {
  uint8_t value = data[pos];
  pos += 1;
  return value;
}

uint32_t readWord(unsigned char * data) {
  uint32_t value = data[pos] << 24 | data[pos+1] << 16 | data[pos+2] << 8 | data[pos + 3];
  pos += 4;
  return value;
}

float readFloat(unsigned char * data) {
  uint32_t value = data[pos] << 24 | data[pos+1] << 16 | data[pos+2] << 8 | data[pos + 3];
  pos += 4;
  return *(float*)(&value);
}

void readFrameData(void)
{
    pos = 1;
    currentGame.frameCounter = readWord(currentData);
    currentGame.randomSeed = readWord(currentData);
        
    int i;
    for (i = 0; i < PLAYER_COUNT; i++)
    {        
        currentGame.players[i].previousFrameData = currentGame.players[i].currentFrameData;
        PlayerFrameData playerData = currentGame.players[i].currentFrameData;

        playerData.internalCharacterId = readByte(currentData);
        playerData.animation = readHalf(currentData);
        playerData.locationX = readFloat(currentData);
        playerData.locationY = readFloat(currentData);

        //Controller information
        playerData.joystickX = readFloat(currentData);
        playerData.joystickY = readFloat(currentData);
        playerData.cstickX = readFloat(currentData);
        playerData.cstickY = readFloat(currentData);
        playerData.trigger = readFloat(currentData);
        playerData.buttons = readWord(currentData);

        //More data
        playerData.percent = readFloat(currentData);
        playerData.shieldSize = readFloat(currentData);
        playerData.lastMoveHitId = readByte(currentData);
        playerData.comboCount = readByte(currentData);
        playerData.lastHitBy = readByte(currentData);
        playerData.stocks = readByte(currentData);

        //Raw controller information
        playerData.physicalButtons = readHalf(currentData);
        playerData.lTrigger = readFloat(currentData);
        playerData.rTrigger = readFloat(currentData);
        
        currentGame.players[i].currentFrameData = playerData;
    }
}

void initializePlayers(void)
{
    int i;
    for (i = 0; i < PLAYER_COUNT; i++)
    {
        Player player = currentGame.players[i];
        player.stats.comboStringIndex = 0;
        player.stats.recoveryIndex = 0;
        player.stats.edgeguardEfficiency = 0;
        player.stats.punishIndex = 0;
        player.stats.framesAboveOthers = 0;
        player.stats.framesClosestCenter = 0;
        player.stats.averageDistanceFromCenter = 0;
        player.stats.framesInShield = 0;
        player.stats.mostFramesWithoutDamage = 0;
        player.stats.rollCount = 0;
        player.stats.spotDodgeCount = 0;
        player.stats.airDodgeCount = 0;
        player.stats.techMissCount = 0;
        player.stats.techLeftCount = 0;
        player.stats.techRightCount = 0;
        player.stats.techPlaceCount = 0;
        player.stats.actionCount = 0;
        player.stats.actionsPerMinute = 0;
        player.stats.numberOfOpenings = 0;
        player.stats.avgComboDmg = 0;
        player.stats.avgHitsPerCombo = 0;
        player.stats.avgDeathPercent = 0;

        player.flags.recoveryStartPercent = 0;
        player.flags.recoveryStartFrame = 0;
        player.flags.isRecovering = 0;
        player.flags.isHitOffStage = 0;
        player.flags.isLandedOnStage = 0;
        player.flags.stringStartPercent = 0;
        player.flags.stringStartFrame = 0;
        player.flags.stringCount = 0;
        player.flags.stringResetCounter = 0;
        player.flags.isPunishing = 0;
        player.flags.punishStartPercent = 0;
        player.flags.punishStartFrame = 0;
        player.flags.punishHitCount = 0;
        
        currentGame.players[i] = player;
    }
}

void publishData(void)
{
    
    sendHalf(0x03, 0x01, currentGame.stage);
    sendInt(0x03, 0x02, currentGame.frameCounter);
    sendByte(0x01, 0x01, currentGame.players[0].characterId);
    sendByte(0x02, 0x01, currentGame.players[1].characterId);
    sendInt(0x01, 0x03, currentGame.players[0].stats.framesClosestCenter);
    sendInt(0x02, 0x03, currentGame.players[1].stats.framesClosestCenter);
    sendInt(0x01, 0x04, currentGame.players[0].stats.rollCount);
    sendInt(0x02, 0x04, currentGame.players[1].stats.rollCount);
    sendInt(0x01, 0x05, currentGame.players[0].stats.spotDodgeCount);
    sendInt(0x02, 0x05, currentGame.players[1].stats.spotDodgeCount);
    sendFloat(0x01, 0x06, currentGame.players[0].stats.actionsPerMinute);
    sendFloat(0x02, 0x06, currentGame.players[1].stats.actionsPerMinute);
    sendFloat(0x01, 0x07, currentGame.players[0].stats.avgComboDmg);
    sendFloat(0x02, 0x07, currentGame.players[1].stats.avgComboDmg);
    sendFloat(0x01, 0x08, currentGame.players[0].stats.avgHitsPerCombo);
    sendFloat(0x02, 0x08, currentGame.players[1].stats.avgHitsPerCombo);
    sendFloat(0x01, 0x0a, currentGame.players[0].stats.edgeguardEfficiency);
    sendFloat(0x02, 0x0a, currentGame.players[1].stats.edgeguardEfficiency);
    sendFloat(0x01, 0x0c, currentGame.players[0].stats.avgDeathPercent);
    sendFloat(0x02, 0x0c, currentGame.players[1].stats.avgDeathPercent);
    
    //printf("Player 1 x: %f \n\r", currentGame.players[0].currentFrameData.locationX);
    //printf("Player 1 y: %f \n\r", currentGame.players[0].currentFrameData.locationY);
    //printf("Player 2 x: %f \n\r", currentGame.players[1].currentFrameData.locationX);
    //printf("Player 2 y: %f \n\r", currentGame.players[1].currentFrameData.locationY);
    //printf("Player 1 avg dist from center: %1.2f \n\r", currentGame.players[0].stats.averageDistanceFromCenter);
    //printf("Player 2 avg dist from center: %1.2f \n\r", currentGame.players[1].stats.averageDistanceFromCenter);
    //printf("Player 1 center control: %i \n\r", currentGame.players[0].stats.framesClosestCenter);
    //printf("Player 2 center control: %i \n\r", currentGame.players[1].stats.framesClosestCenter);
    //printf("Player 1 higher: %i \n\r", currentGame.players[0].stats.framesAboveOthers);
    //printf("Player 2 higher: %i \n\r", currentGame.players[1].stats.framesAboveOthers);
    
    //printf("P1 frames In Shield: %i \n\r", currentGame.players[0].stats.framesInShield);
    //printf("P2 frames In Shield: %i \n\r", currentGame.players[1].stats.framesInShield);
    //printf("P1 roll count: %i \n\r", currentGame.players[0].stats.rollCount);
    //printf("P2 roll count: %i \n\r", currentGame.players[1].stats.rollCount);
    //printf("P1 spot dodge count: %i \n\r", currentGame.players[0].stats.spotDodgeCount);
    //printf("P2 spot dodge count: %i \n\r", currentGame.players[1].stats.spotDodgeCount);
    //printf("P1 air dodge count: %i \n\r", currentGame.players[0].stats.airDodgeCount);
    //printf("P2 air dodge count: %i \n\r", currentGame.players[1].stats.airDodgeCount);
    
    //printf("P1 avg death percent: %3.2f \n\r", currentGame.players[0].stats.avgDeathPercent);
    //printf("P2 avg death percent: %3.2f \n\r", currentGame.players[1].stats.avgDeathPercent);
    //printf("P2 most frames without damage: %i \n\r", currentGame.players[1].stats.mostFramesWithoutDamage);
    //printf("P1 avg combo dmg: %f \n\r", currentGame.players[0].stats.avgComboDmg);
    //printf("P2 avg combo dmg: %f \n\r", currentGame.players[1].stats.avgComboDmg);
    //printf("P1 avg combo hits: %f \n\r", currentGame.players[0].stats.avgHitsPerCombo);
    //printf("P2 avg combo hits: %f \n\r", currentGame.players[1].stats.avgHitsPerCombo);
    //printf("P1 action count: %i \n\r", currentGame.players[0].stats.actionCount);
    //printf("P2 action count: %i \n\r", currentGame.players[1].stats.actionCount);
    //printf("P1 apm: %f \n\r", currentGame.players[0].stats.actionsPerMinute);
    //printf("P2 apm: %f \n\r", currentGame.players[1].stats.actionsPerMinute);
    //printf("P1 recoveries: %i \n\r", currentGame.players[0].stats.recoveryIndex);
    //printf("P2 recoveries: %i \n\r", currentGame.players[1].stats.recoveryIndex);
    //printf("P1 edgeguard eff: %f \n\r", currentGame.players[0].stats.edgeguardEfficiency);
    //printf("P2 edgeguard eff: %f \n\r", currentGame.players[1].stats.edgeguardEfficiency);
    //printf("P1 openings per kill: %f \n\r", currentGame.players[0].stats.openingsPerKill);
    //printf("P2 openings per kill: %f \n\r", currentGame.players[1].stats.openingsPerKill);
    //printf("P1 neutral wins: %f \n\r", currentGame.players[0].stats.neutralWins);
    //printf("P2 neutral wins: %f \n\r", currentGame.players[1].stats.neutralWins);
    //printf("P1 neutral wins: %f \n\r", currentGame.players[0].stats.punishIndex);
    //printf("P2 neutral wins: %f \n\r", currentGame.players[1].stats.punishIndex);
}

void computeStatistics(void)
{
    uint32_t framesSinceStart = currentGame.frameCounter - 1;

    Player* players = currentGame.players;

    float p1CenterDistance = sqrt(pow(players[0].currentFrameData.locationX, 2) + pow(players[0].currentFrameData.locationY, 2));
    float p2CenterDistance = sqrt(pow(players[1].currentFrameData.locationX, 2) + pow(players[1].currentFrameData.locationY, 2));

    players[0].stats.averageDistanceFromCenter = ((framesSinceStart-1)*players[0].stats.averageDistanceFromCenter + p1CenterDistance) / (framesSinceStart);
    players[1].stats.averageDistanceFromCenter = ((framesSinceStart-1)*players[1].stats.averageDistanceFromCenter + p2CenterDistance) / (framesSinceStart);

    //Increment frame counter of person who is closest to center. If the players are even distances from the center, do not increment
    if (p1CenterDistance < p2CenterDistance) 
        players[0].stats.framesClosestCenter++;
    else if(p2CenterDistance < p1CenterDistance) 
        players[1].stats.framesClosestCenter++;
    
    //Increment frame counter of person who is highest;
    if (players[0].currentFrameData.locationY > players[1].currentFrameData.locationY) 
        players[0].stats.framesAboveOthers++;
    else if (players[1].currentFrameData.locationY > players[0].currentFrameData.locationY) 
        players[1].stats.framesAboveOthers++;
    
    int i = 0;
    for (i; i < PLAYER_COUNT; i++) 
    {
        Player cp = players[i];
        Player op = players[!i];
        
        int lostStock = (cp.previousFrameData.stocks - cp.currentFrameData.stocks) > 0;
        int opntLostStock = (op.previousFrameData.stocks - op.currentFrameData.stocks) > 0;

        //Check current action states, although many of these conditions check previous frame data, it shouldn't matter for frame = 1 that there is no previous
        if (cp.currentFrameData.animation >= GUARD_START && cp.currentFrameData.animation <= GUARD_END) 
            cp.stats.framesInShield++;
        else if ((cp.currentFrameData.animation == ROLL_FORWARD && cp.previousFrameData.animation != ROLL_FORWARD) || (cp.currentFrameData.animation == ROLL_BACKWARD && cp.previousFrameData.animation != ROLL_BACKWARD)) 
            cp.stats.rollCount++;
        else if (cp.currentFrameData.animation == SPOT_DODGE && cp.previousFrameData.animation != SPOT_DODGE) 
            cp.stats.spotDodgeCount++;
        else if (cp.currentFrameData.animation == AIR_DODGE && cp.previousFrameData.animation != AIR_DODGE) 
            cp.stats.airDodgeCount++;
        
        //Check if we are getting damaged
        int tookPercent = cp.currentFrameData.percent - cp.previousFrameData.percent > 0;
        if (tookPercent) {
          cp.flags.framesWithoutDamage = 0;
        } 
        else 
        {
          cp.flags.framesWithoutDamage++; //Increment count of frames without taking damage

          //If frames without being hit is greater than previous, set new record
          if (cp.flags.framesWithoutDamage > cp.stats.mostFramesWithoutDamage) 
              cp.stats.mostFramesWithoutDamage = cp.flags.framesWithoutDamage; 
        }

            //------------------------------- Monitor Combo Strings -----------------------------------------
        int opntTookDamage = op.currentFrameData.percent - op.previousFrameData.percent > 0;
        int opntDamagedState = op.currentFrameData.animation >= DAMAGE_START && op.currentFrameData.animation <= DAMAGE_END;
        int opntGrabbedState = op.currentFrameData.animation >= CAPTURE_START && op.currentFrameData.animation <= CAPTURE_END;
        int opntTechState = (op.currentFrameData.animation >= TECH_START && op.currentFrameData.animation <= TECH_END) || op.currentFrameData.animation == TECH_MISS_UP || op.currentFrameData.animation == TECH_MISS_DOWN;

        //By looking for percent changes we can increment counter even when a player gets true combo'd
        //The damage state requirement makes it so things like fox's lasers, grab pummels, pichu damaging self, etc don't increment count
        if (opntTookDamage && (opntDamagedState || opntGrabbedState)) {
          if (cp.flags.stringCount == 0) 
          {
            cp.flags.stringStartPercent = op.previousFrameData.percent;
            cp.flags.stringStartFrame = currentGame.frameCounter;
          }

          cp.flags.stringCount++; //increment number of hits
        }

        //Reset combo string counter when somebody dies or doesn't get hit for too long
        if (opntDamagedState || opntGrabbedState || opntTechState) 
            cp.flags.stringResetCounter = 0;
        else if (cp.flags.stringCount > 0) 
            cp.flags.stringResetCounter++;

        //Mark combo completed if opponent lost his stock or if the counter is greater than threshold frames
        if (cp.flags.stringCount > 0 && (opntLostStock || lostStock || cp.flags.stringResetCounter > COMBO_STRING_TIMEOUT)) {
          ComboString cs = cp.stats.comboStrings[cp.stats.comboStringIndex];
          cs.frameStart = cp.flags.stringStartFrame;
          cs.frameEnd = currentGame.frameCounter;
          cs.percentStart = cp.flags.stringStartPercent;
          cs.percentEnd = op.previousFrameData.percent;
          cs.hitCount = cp.flags.stringCount;

          cp.stats.comboStrings[cp.stats.comboStringIndex] = cs;
          
          cp.stats.avgComboDmg = ((cp.stats.avgComboDmg * cp.stats.comboStringIndex) + (cs.percentEnd - cs.percentStart))/(cp.stats.comboStringIndex+1);
          cp.stats.avgHitsPerCombo = ((cp.stats.avgHitsPerCombo * cp.stats.comboStringIndex) + (cs.hitCount))/(cp.stats.comboStringIndex+1);
                  
          if (cp.stats.comboStringIndex < COMBO_STRING_BUFFER_SIZE - 1) 
          {
             cp.stats.comboStringIndex++;
          }

          //Reset string count
          cp.flags.stringCount = 0;
        }
        
            //------------------- Increment Action Count for APM Calculation --------------------------------
        //First count the number of buttons that go from 0 to 1
        uint16_t buttonChanges = (~cp.previousFrameData.physicalButtons & cp.currentFrameData.physicalButtons) & 0xFFF;
        cp.stats.actionCount += numberOfSetBits(buttonChanges); //Increment action count by amount of button presses

        //Increment action count when sticks change from one region to another. Don't increment when stick returns to deadzone
        uint8_t prevAnalogRegion = getJoystickRegion(cp.previousFrameData.joystickX, cp.previousFrameData.joystickY);
        uint8_t currentAnalogRegion = getJoystickRegion(cp.currentFrameData.joystickX, cp.currentFrameData.joystickY);
        if ((prevAnalogRegion != currentAnalogRegion) && (currentAnalogRegion != 0)) 
            cp.stats.actionCount++;

        //Do the same for c-stick
        uint8_t prevCstickRegion = getJoystickRegion(cp.previousFrameData.cstickX, cp.previousFrameData.cstickY);
        uint8_t currentCstickRegion = getJoystickRegion(cp.currentFrameData.cstickX, cp.currentFrameData.cstickY);
        if ((prevCstickRegion != currentCstickRegion) && (currentCstickRegion != 0)) 
            cp.stats.actionCount++;

        //Increment action on analog trigger... I'm not sure when. This needs revision
        if (cp.previousFrameData.lTrigger < 0.3 && cp.currentFrameData.lTrigger >= 0.3) 
            cp.stats.actionCount++;
        if (cp.previousFrameData.rTrigger < 0.3 && cp.currentFrameData.rTrigger >= 0.3) 
            cp.stats.actionCount++;
        
        cp.stats.actionsPerMinute = ((float)cp.stats.actionCount / (float)framesSinceStart)*3600;
        
                //--------------------------- Recovery detection --------------------------------------------------
        int isOffStage = checkIfOffStage(currentGame.stage, cp.currentFrameData.locationX, cp.currentFrameData.locationY);
        int isInControl = cp.currentFrameData.animation >= GROUNDED_CONTROL_START && cp.currentFrameData.animation <= GROUNDED_CONTROL_END;
        int beingDamaged = cp.currentFrameData.animation >= DAMAGE_START && cp.currentFrameData.animation <= DAMAGE_END;
        int beingGrabbed = cp.currentFrameData.animation >= CAPTURE_START && cp.currentFrameData.animation <= CAPTURE_END;
        int isDying = cp.currentFrameData.animation >= DYING_START && cp.currentFrameData.animation <= DYING_END;

        if (!cp.flags.isRecovering && !cp.flags.isHitOffStage && beingDamaged && isOffStage) {
          //If player took a hit off stage
          cp.flags.isHitOffStage = 1;
        }
        else if (!cp.flags.isRecovering && cp.flags.isHitOffStage && !beingDamaged && !isDying && isOffStage) {
          //If player exited damage state off stage
          cp.flags.isRecovering = 1;
          cp.flags.recoveryStartPercent = cp.currentFrameData.percent;
          cp.flags.recoveryStartFrame = currentGame.frameCounter;
        }
        else if (!cp.flags.isLandedOnStage && (cp.flags.isRecovering || cp.flags.isHitOffStage) && isInControl && !isOffStage) {
          //If a player is in control of his character after recovering flag as landed
          cp.flags.isLandedOnStage = 1;
        }
        else if (cp.flags.isLandedOnStage && isOffStage) {
          //If player landed but is sent back off stage, continue recovery process
          cp.flags.framesSinceLanding = 0;
          cp.flags.isLandedOnStage = 0;
        }
        else if (cp.flags.isLandedOnStage && !isOffStage && !beingDamaged && !beingGrabbed) {
          //If player landed, is still on stage, is not being hit, and is not grabbed, increment frame counter
          cp.flags.framesSinceLanding++;

          //If frame counter while on stage passes threshold, consider it a successful recovery
          if (cp.flags.framesSinceLanding > FRAMES_LANDED_RECOVERY) {
            cp = appendRecovery(1, cp, currentGame.frameCounter);
            cp.flags = resetRecoveryFlags(cp.flags);
          }
        }

        if ((cp.flags.isRecovering || cp.flags.isHitOffStage) && lostStock) {
          //If player dies while recovering, consider it a failed recovery
          if (cp.flags.isRecovering) {
            cp = appendRecovery(0, cp, currentGame.frameCounter);
            op.stats.edgeguardEfficiency = ((op.stats.edgeguardEfficiency*(float)cp.stats.recoveryIndex) + 1) / (float)cp.stats.recoveryIndex;
          }

          cp.flags = resetRecoveryFlags(cp.flags);
        }
        
            //----------------------------- Punish detection --------------------------------------------------
        int opntInControl = op.currentFrameData.animation >= GROUNDED_CONTROL_START && op.currentFrameData.animation <= GROUNDED_CONTROL_END;

        if (opntTookDamage && (opntDamagedState || opntGrabbedState)) {
          // Successfully hit opponent, check if we already have a punish going
          if (!cp.flags.isPunishing) {
            // If we didn't have a punish going, start a new one 
            cp.flags.punishStartPercent = op.previousFrameData.percent;
            cp.flags.punishStartFrame = currentGame.frameCounter;
            cp.stats.numberOfOpenings++;
            cp.stats.neutralWins = (float)cp.stats.numberOfOpenings/(float)(cp.stats.numberOfOpenings+op.stats.numberOfOpenings);
            op.stats.neutralWins = (float)op.stats.numberOfOpenings/(float)(cp.stats.numberOfOpenings+op.stats.numberOfOpenings);
            cp.flags.isPunishing = 1;
          }

          cp.flags.punishHitCount++; //increment number of hits
        }

        if (opntDamagedState || opntGrabbedState) {
          // If opponent got grabbed or damaged, reset the punish reset counter
          cp.flags.framesSincePunishReset = 0; //reset the punish reset timer
        }

        int startNeutralCount = cp.flags.framesSincePunishReset == 0 && opntInControl;
        int continueNeutralCount = cp.flags.framesSincePunishReset > 0;
        if (startNeutralCount || continueNeutralCount) {
          // This will increment the reset timer under the following conditions:
          // 1) if we were punishing opponent but they have now entered an actionable state
          // 2) if counter has already started counting meaning opponent has entered actionable state
          cp.flags.framesSincePunishReset++;
        }

        // Termination condition 1 - we kill our opponent
        if (cp.flags.isPunishing && opntLostStock) {
          cp = appendPunish(1, cp, op, currentGame.frameCounter);
          //op.stats.neutralWins = 1-cp.stats.neutralWins;
          cp.flags = resetPunishFlags(cp.flags);
        }

        // Termination condition 2 - we have not re-hit our opponent in buffer amount
        if (cp.flags.isPunishing && cp.flags.framesSincePunishReset > FRAMES_LANDED_PUNISH) {
          cp = appendPunish(0, cp, op, currentGame.frameCounter);
          //op.stats.neutralWins = 1-cp.stats.neutralWins;
          cp.flags = resetPunishFlags(cp.flags);
        }
        
                //-------------------------- Stock specific stuff -------------------------------------------------
        int prevStockIndex = STOCK_COUNT - cp.previousFrameData.stocks;
        if (prevStockIndex >= 0 && prevStockIndex < STOCK_COUNT) 
        {
          StockStatistics stock = cp.stats.stocks[prevStockIndex];

          if (stock.frameStart == 0) 
              stock.frameStart = currentGame.frameCounter;
          stock.percent = cp.currentFrameData.percent;
          stock.lastHitBy = op.currentFrameData.lastMoveHitId; //This will indicate what this player was killed by
          stock.lastAnimation = cp.currentFrameData.animation; //What was character doing before death
        }

        //Mark last stock as lost if lostStock is true
        if (lostStock && prevStockIndex >= 0 && prevStockIndex < STOCK_COUNT) 
        {
            cp.stats.avgDeathPercent = (cp.stats.avgDeathPercent*prevStockIndex + cp.currentFrameData.percent)/(prevStockIndex + 1);
          
            uint16_t prevOpenings = 0;
            int i = prevStockIndex - 1;
            for (i; i >= 0; i--) 
                prevOpenings += cp.stats.stocks[i].killedInOpenings;

            cp.stats.stocks[prevStockIndex].killedInOpenings = op.stats.numberOfOpenings - prevOpenings;
            op.stats.openingsPerKill = (op.stats.openingsPerKill*prevStockIndex + cp.stats.stocks[prevStockIndex].killedInOpenings)/(prevStockIndex + 1);
            cp.stats.stocks[prevStockIndex].frameEnd = currentGame.frameCounter;
          
        }
        
        players[i] = cp;
        players[!i] = op;
    }
}

int numberOfSetBits(uint16_t x) {
  //This function solves the Hamming Weight problem. Effectively it counts the number of bits in the input that are set to 1
  //This implementation is supposedly very efficient when most bits are zero. Found: https://en.wikipedia.org/wiki/Hamming_weight#Efficient_implementation
  int count = 0;
  for (count; x; count++) x &= x-1;
  return count;
}