/* Board: Arduino Pro Micro (Leonardo)
 The Board behaves as an XInput device ( pc gamepad ). 
To program it, the Arduino IDE has to be modified with the XInput Board Package for AVR boards https://github.com/dmadison/ArduinoXInput_AVR
*/

#include "config.h"
#include "speedSensorHelpers.h"
#include "inputHelpers.h"
#include "displayHelpers.h"

#ifndef DEBUG
#include <XInput.h>
#endif

//Setup Functions
void setupSensors();
void setupXController();

void updateController();

// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  Serial.begin(9600);
  Serial.println("Hello World");
  
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(CALIBRATE_BUTTON,INPUT_PULLUP);
  setupSensors();
  loadRanges();
  setupSpeedSensor();
  setupXController();
  
  #ifndef DEBUG
  XInput.releaseAll();
  #endif

  initDisplay();
  displayStatusMsg("Ready");
  delay(1000);
  clearDisplay();
}

void loop() {
  updateController();
  updateSpeedSensor();
  if( !digitalRead(CALIBRATE_BUTTON) ){
    calibrate();
    storeRanges();
  }
  
  delay(10);
}

void setupXController(){
  #ifndef DEBUG
  XInput.begin();
  XInput.setRange(TRIGGER_RIGHT, 0, 100);
  XInput.setRange(JOY_LEFT, -100, 100);
  #endif
}

void updateController(){
  int actualSteeringInput = getSteeringInput();
  
  int gtaSteeringInput = getModifiedSteering(actualSteeringInput);
  int brakingInput = getBrakingInput();
  float speed = getSpeedMs();

  #ifdef DEBUG
  Serial.print("actualSteering: ");Serial.print(actualSteeringInput);Serial.print(",");
  Serial.print("gtaSteeringInput: ");Serial.print(gtaSteeringInput);Serial.print(",");
  Serial.print("breaking: ");Serial.print(brakingInput);Serial.print(",");
  Serial.print("speed: "); Serial.println(speed);
  #endif

  #ifndef DEBUG
  XInput.setTrigger(TRIGGER_RIGHT,brakingInput);
  XInput.setJoystickX(JOY_LEFT, gtaSteeringInput, false);
  #endif
  if(speed > 2.0){
    XInput.setButton(BUTTON_RB,true);
  }
  else{
    XInput.setButton(BUTTON_RB,false);
  }
  if(speed > 0.5){
    #ifndef DEBUG
    XInput.setButton(BUTTON_A,true);
    #endif
  }
  else{
    #ifndef DEBUG
    XInput.setButton(BUTTON_RB,false);
    XInput.setButton(BUTTON_A,false);
    #endif
  }
}
