Haxmod updated

/*
    Original code by WatchingTime
    https://pastebin.com/YvB9BTp9
    https://twitter.com/WatchingTime_

    Modified and expanded upon by TimpZ
    https://twitter.com/timpz91

    Recommended hardware guide:
    https://docs.google.com/document/d/1KZrORDtJBuovVAHRZjteRitBKkwibZOc7VW0PdGedEk/edit
*/

// Setup and public variables
#include "Nintendo.h"
CGamecubeController controller(2); //sets D2 on arduino to read data from controller
CGamecubeConsole console(3);       //sets D3 on arduino to write data to console
Gamecube_Report_t gcc;             //structure for controller state
bool shield, tilt, dolphin = 0, off = 0, switchPresetR = 0, switchPresetL = 0;
byte axm, aym, cxm, cym, cycles;
char ax, ay, cx, cy, buf;
float swang, seang;
word mode, toggle;
unsigned long nx, ny, nz;
int preset = 1;

// This defines the edges on the controller.
//you can check these either with Serial.print or the 20XX Hack Pack
#define sw_notch_x_value -.7000
#define sw_notch_y_value -.7000
#define se_notch_x_value  .7000
#define se_notch_y_value -.7000


/*
    Available mods with description
WatchingTime
    perfectangles();            - reduces deadzone of cardinals and gives steepest/shallowest angles when on or near the gate
    maxvectors();                   - snaps sufficiently high cardinal inputs to vectors of 1.0 magnitude of analog stick and c stick
    shielddrops();                  - allows shield drops down and gives a 6 degree range of shield dropping centered on SW and SE gates
    backdash();                     - fixes dashback by imposing a 1 frame buffer upon tilt turn values
    dolphinfix();                   - ensures close to 0 values are reported as 0 on the sticks to fix dolphin calibration and allows user to switch to dolphin mode for backdash

TimpZ
    gcc.left = 0;               - disables right lightshield
    gcc.right = 0;              - disables right lightshield
    remap();                    - remaps the A and B buttons. Can be modified for any digital input
    zFrame();                   - disables the z button press after ~1.5 frames
    xFrame();                   - disables the x button press after ~1 frames
    yFrame();                   - disables the y button press after ~1 frames
    lLimit();                   - disables light shield values between 1 and 70 on the L button
    rLimit();                   - disables light shield values between 1 and 70 on the R button
*/


//These functions define what mods the presets should use
//Simply add more if you want
void preset1(){
  perfectangles();
  maxvectors();
  shielddrops();
  backdash();
  dolphinfix();
}

void preset2(){
  backdash();
  shielddrops();
  maxvectors();
  lLimit();
  gcc.right = 0;
  zFrame();
  yFrame();
  //Serial.print(gcc.y);
}

void preset3(){
  remap();

}

void preset4(){
  //no changes
}

void lLimit(){
  if(gcc.left<70) gcc.left = 0;
}

void rLimit(){
  if(gcc.right<70) gcc.right = 0;
}

void zFrame(){
  if(gcc.z){
    if(nz == 0) nz = millis();
    if(millis()-nz>4) gcc.z = 0;
  }else nz = 0;
}

void xFrame(){
  if(gcc.x){
    if(nx == 0) nx = millis();
    if(millis()-nx>3) gcc.x = 0;
  }else nx = 0;
}

void yFrame(){
  if(gcc.y){
    if(ny == 0) ny = millis();
    if(millis()-ny>3) gcc.y = 0;
  }else ny = 0;
}

void remap(){
  bool buff;
  buff = gcc.a;
  gcc.a = gcc.b;
  gcc.b = buff;
}

void perfectangles(){
  if(axm>75){gcc.xAxis = (ax>0)?204:52; if(aym<23) gcc.yAxis = (ay>0)?151:105;}
  if(aym>75){gcc.yAxis = (ay>0)?204:52; if(axm<23) gcc.xAxis = (ax>0)?151:105;}
}

void maxvectors(){
  if(axm>75&&aym< 9){gcc.xAxis  = (ax>0)?255:1; gcc.yAxis  = 128;}
  if(aym>75&&axm< 9){gcc.yAxis  = (ay>0)?255:1; gcc.xAxis  = 128;}
  if(cxm>75&&cym<23){gcc.cxAxis = (cx>0)?255:1; gcc.cyAxis = 128;}
  if(cym>75&&cxm<23){gcc.cyAxis = (cy>0)?255:1; gcc.cxAxis = 128;}
}

void shielddrops(){
  shield = gcc.l||gcc.r||gcc.left>74||gcc.right>74||gcc.z;
  if(shield){
    if(ay<0&&mag(ax,ay)>75){
      if(ax<0&&abs(ang(axm,aym)-swang)<4){gcc.yAxis = 73; gcc.xAxis =  73;}
      if(ax>0&&abs(ang(axm,aym)-seang)<4){gcc.yAxis = 73; gcc.xAxis = 183;}
    }else if(abs(ay+39)<17&&axm<23) gcc.yAxis = 73;
  }
}

void backdash(){
  if(aym<23){
    if(axm<23)buf = cycles;
    if(buf>0){buf--; if(axm<64) gcc.xAxis = 128+ax*(axm<23);}
  }else buf = 0;
}

void dolphinfix(){
  if(axm<5&&aym<5){gcc.xAxis  = 128; gcc.yAxis  = 128;}
  if(cxm<5&&cym<5){gcc.cxAxis = 128; gcc.cyAxis = 128;}
  if(gcc.dright&&mode<2500) dolphin = dolphin||(mode++>2000);
  else mode = 0;
  cycles = 3 + (6*dolphin);
}

float ang(float xval, float yval){return atan(yval/xval)*57.2958;} //function to return angle in degrees when given x and y components
float mag(char  xval, char  yval){return sqrt(sq(xval)+sq(yval));} //function to return vector magnitude when given x and y components

void setup(){
  gcc.origin=0; gcc.errlatch=0; gcc.high1=0; gcc.errstat=0;  //init values
  swang = ang(abs(sw_notch_x_value), abs(sw_notch_y_value)); //calculates angle of SW gate based on user inputted data
  seang = ang(abs(se_notch_x_value), abs(se_notch_y_value)); //calculates angle of SE gate based on user inputted data


  // Start debug serial
  //Serial.begin(115200);
}

void loop(){

    controller.read();
    gcc = controller.getReport();

    ax = gcc.xAxis -128; ay = gcc.yAxis -128; //offsets from neutral position of analog stick
    cx = gcc.cxAxis-128; cy = gcc.cyAxis-128; //offsets from neutral position of c stick
    axm = abs(ax); aym = abs(ay);             //magnitude of analog stick offsets
    cxm = abs(cx); cym = abs(cy);             //magnitude of c stick offsets

    //This code makes you able to switch between the presets.
    //It currently works by holding down d-pad down and scrolling with L and R
    if(gcc.r == 0) switchPresetR = 0;
    if(gcc.ddown && gcc.r && switchPresetR == 0){
        preset +=1;
        switchPresetR = 1;
    if(preset > 4) preset = 4; //change both these numbers to increase the number of presets available
    }
    if(gcc.l == 0) switchPresetL = 0;
    if(gcc.ddown && gcc.l && switchPresetL == 0){
        preset -=1;
        switchPresetL = 1;
    if(preset < 1) preset = 1;
    }

    //This defines how many presets there are
    //If you want more, simply make them
    //Also remember to update the scrolling code above
    if(preset == 1) preset1();                 //Original haxmod
    if(preset == 2) preset2();
    if(preset == 3) preset3();
    if(preset == 4) preset4();                 //Unmodified


    console.write(gcc);                       //sends controller data to the console
}