//=======================================================
//
// 2 track vehicle controller
//
// by DedGzus
// Started on 01/27/2024 right after the Royal Rumble
//=======================================================

// libraries
#include "ProcessManager.h"

// input pin definitions
#define LThrottlePin A0
#define RThrottlePin A1
#define ThrottleGainPin A2

// led pin definitions
#define LThrottleFwdPin 2
#define LThrottleIdlePin 3
#define LThrottleRevPin 4
#define RThrottleFwdPin 5
#define RThrottleIdlePin 6
#define RThrottleRevPin 7

// constants
#define ThrottleMid 512
#define IdleGap 64

// process manager ptr
ProcessManager* pManager;

// global read variables
volatile unsigned short leftThrottleRead;
volatile unsigned short rightThrottleRead;
volatile unsigned short throttleGainRead;

void setup()
{
  // set LED pins for output
  pinMode(LThrottleFwdPin, OUTPUT);
  pinMode(LThrottleIdlePin, OUTPUT);
  pinMode(LThrottleRevPin, OUTPUT);
  pinMode(RThrottleFwdPin, OUTPUT);
  pinMode(RThrottleIdlePin, OUTPUT);
  pinMode(RThrottleRevPin, OUTPUT);

  // acquire the process manager
  pManager = new ProcessManager();

  // attach the read processes
  pManager->attach( new analogReadProcess(LThrottlePin, &leftThrottleRead));
  pManager->attach( new analogReadProcess(RThrottlePin, &rightThrottleRead));
  pManager->attach( new analogReadProcess(ThrottleGainPin, &throttleGainRead));

//  Serial.begin(9600);
}

void loop()
{
  static byte LThrottleFwdLED(LOW);
  static byte LThrottleIdleLED(LOW);
  static byte LThrottleRevLED(LOW);
  static byte RThrottleFwdLED(LOW);
  static byte RThrottleIdleLED(LOW);
  static byte RThrottleRevLED(LOW);

  // run the process manager
  pManager->onLoop();

  signed short mappedLThrottle = mapThrottle(leftThrottleRead);
  signed short mappedRThrottle = mapThrottle(rightThrottleRead);
  float gain(float(map(throttleGainRead, 0, 1023, 0, 1000)) / 1000);

  // set left throttle LED
  if (mappedLThrottle > 0)
  {
    LThrottleFwdLED = HIGH;
    LThrottleIdleLED = LOW;
    LThrottleRevLED = LOW;
  }
  else if (mappedLThrottle < 0)
  {
    LThrottleFwdLED = LOW;
    LThrottleIdleLED = LOW;
    LThrottleRevLED = HIGH;
  }
  else
  {
    LThrottleFwdLED = LOW;
    LThrottleIdleLED = HIGH;
    LThrottleRevLED = LOW;
  }

  // set right throttle LED
  if (mappedRThrottle > 0)
  {
    RThrottleFwdLED = HIGH;
    RThrottleIdleLED = LOW;
    RThrottleRevLED = LOW;
  }
  else if (mappedRThrottle < 0)
  {
    RThrottleFwdLED = LOW;
    RThrottleIdleLED = LOW;
    RThrottleRevLED = HIGH;
  }
  else
  {
    RThrottleFwdLED = LOW;
    RThrottleIdleLED = HIGH;
    RThrottleRevLED = LOW;
  }

  // write to all LEDs
  digitalWrite(LThrottleFwdPin, LThrottleFwdLED);
  digitalWrite(LThrottleIdlePin, LThrottleIdleLED);
  digitalWrite(LThrottleRevPin, LThrottleRevLED);
  digitalWrite(RThrottleFwdPin, RThrottleFwdLED);
  digitalWrite(RThrottleIdlePin, RThrottleIdleLED);
  digitalWrite(RThrottleRevPin, RThrottleRevLED);

  // for debugging
/*  Serial.print("Left[");
  Serial.print(mappedLThrottle * gain);
  Serial.print("%] Right[");
  Serial.print(mappedRThrottle * gain);
  Serial.print("%] Gain Coefficient[");
  Serial.print(gain);
  Serial.println("]");*/
}

signed short mapThrottle(const int throttle)
{
  // negative throttle
  if (throttle < ThrottleMid - IdleGap)
  {
    return map(throttle, 0, (ThrottleMid - IdleGap), -100, 1);
  }
  // positive throttle
  else if (throttle > ThrottleMid + IdleGap)
  {
    return map(throttle, (ThrottleMid + IdleGap), 1023, 1, 100);
  }

  // throttle within idle gap
  return 0;
}