// Maggi's Shitty Sequential Shifter + Hand Brake handler v0.01a
// Author: Magnus Thor Jonsson
//--------------------------------------------------------------------


// Initializing the Joystick library
#include <Joystick.h>
Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID,JOYSTICK_TYPE_GAMEPAD,
  2, 0,                 // Button Count, Hat Switch Count
  false, false, false,  // X but no Y or Z Axis
  false, false, false,  // No Rx, Ry, or Rz
  false, false,         // No rudder or throttle
  false, true, false);  // No accelerator, 1 brake, and no steering

// Sequential shifter
int inputSequentialUp   = 11;
int inputSequentialDown = 12;
int lastSequentialButtonState[2] = {0,0};

// Hand brake
int inputHandBrake = A5;
unsigned int baselineBrakePressure  = 100;
unsigned int currentBrakePressure   = 0;
unsigned int pressureDeviation      = 5;   // Hard value not percentage

// Iterators
unsigned int index = 0;


void setup()
{
  Joystick.setBrakeRange(0, 1000);

  // Initialize Button Pins
  pinMode(inputSequentialUp, INPUT_PULLUP);
  pinMode(inputSequentialDown, INPUT_PULLUP);

  // Initialize Joystick Library
  Joystick.begin();
  // Start brake on 0
  Joystick.setBrake(0);
}


void loop()
{
  handleShifter();
  handleHandbrake();

  delay(50);
}


/**
 * Handles sequential shifter buttons
 */
void handleShifter()
{
  for (index = 0; index < 2; index++)
  {
    int currentSequentialButtonState = !digitalRead(index + 11);
    if (currentSequentialButtonState != lastSequentialButtonState[index])
    {
      Joystick.setButton(index, currentSequentialButtonState);
      lastSequentialButtonState[index] = currentSequentialButtonState;
    }
  }
}


/**
 * Handles handbrake pressure sensor
 */
void handleHandbrake()
{
  int currentBrakePressure = analogRead(inputHandBrake);

  // Normalize to zero
  if (currentBrakePressure < baselineBrakePressure)
    currentBrakePressure = 0;
  else
    currentBrakePressure = currentBrakePressure - baselineBrakePressure;

  if (currentBrakePressure > pressureDeviation)
  {
    currentBrakePressure = currentBrakePressure * 10; // Turn 100 into 1000

    // Linear conversion from 2000 max to 1000 new_value = ( (old_value - old_min) / (old_max - old_min) ) * (new_max - new_min) + new_min
    currentBrakePressure = (int)(((float)currentBrakePressure / 2000.0) * 1000);
    Joystick.setBrake(currentBrakePressure);
  }
  //Serial.println(currentBrakePressure);
}