// RC Power Wheels
// By Roy El-Rayes
// http://www.dadinasmarthome.com

// Power Wheels Wire Harness
// Battery + (Wht1)   - (Blk1)

// Pedal
// P1     P2            P3
// Org1   Red1          Wht1

// <- Switch Top
// White Switch 1
// I1    I2    I3
// Wht2  Red4  Blu1/Red3
// I4    I5    I6
// Wht2  Org2  Blk4/Blk3

// <- Switch Top
// Red Switch 2
// I1             I2           I3
// Red3/Red2      Red1         Blk2
// I4             I5           I6
// Blk3/Blk2      Blk1/Org1    Red2

// Motor Driver Side
// Blk4 Red4

// Motor Passenger Side
// Org2 Blu1

// Black4 and Orange2 are combined
// Red4 and Blue1 are combined

// (Black4 + Orange2 + Ground) + (Red4 + Blue1 + Power) = Forward
// (Black4 + Orange2 + Power) + (Red4 + Blue1 + Ground) = Reverse

// Pedal
// P1   P2      P3
// Blk  Yel     Red
// Blk = GND
// Yel = Input (pedalInputPin)
// Red = 5V

// White Switch (I) Off = Reverse
// White Switch (I) On = Forward
// Red Switch (II) On = Fast
// Red Switch (II) Off = Slow

#define FORWARD 1
#define BACKWARD 2

const byte cPedalPin = 2;
const byte cThrottleForwardReversePin = 3;
const byte cThrottleFastSlowPin = 4;
const byte cRPWMPin = 6;
const byte cLPWMPin = 7;
const byte cRENPin = 24;
const byte cLENPin = 22;

const byte cRCForwardBackPin = 8;
const byte cRCSpeedPin = 9;
const byte cRCOverridePin = 10;

bool RC_FULL_CONTROL;
bool hasRCInput;

const byte FULL_MAX_THROTTLE = 255;
byte MAX_THROTTLE = 255;
// -MAX_THROTTLE    <->    0    <->    MAX_THROTTLE
//  REVERSE                0           FORWARD
// When in reverse, we pass in -MAX_THROTTLE for the PWM

int throttleAmount = 0;
int lastThrottleAmount = 0;
byte throttleChangeAmount = 17; // 17, 51

int vehicleState = FORWARD;
int rcForwardBackData;
int rcOverrideData;
int rcSpeedData;

void setup()
{
  // Start with setting all of our pins appropriately
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(cPedalPin, INPUT);
  pinMode(cThrottleForwardReversePin, INPUT);
  pinMode(cThrottleFastSlowPin, INPUT);

  pinMode(cRCForwardBackPin, INPUT);
  pinMode(cRCOverridePin, INPUT);
  pinMode(cRCSpeedPin, INPUT);

  pinMode(cLPWMPin, OUTPUT);
  pinMode(cRPWMPin, OUTPUT);

  pinMode(cLENPin,OUTPUT);
  pinMode(cRENPin,OUTPUT);
  digitalWrite(cLENPin,HIGH);
  digitalWrite(cRENPin,HIGH);

  setForward();

  RC_FULL_CONTROL = false;

  // Start us off in a stopped state
  analogWrite(cRPWMPin, 0);
  analogWrite(cLPWMPin, 0);
  Serial.begin(9600);
}

void loop()
{
  int pedalState = digitalRead(cPedalPin); // read the pedal state
  int forwardState = digitalRead(cThrottleForwardReversePin); // read the forward state

  hasRCInput = readRCInput(); // check if we have RC Input
  if (!RC_FULL_CONTROL && !hasRCInput) // if we don't have RC input and we haven't taken over full control over RC, let the driver control the vehicle
  {
    if (forwardState == HIGH)
    {
      setReverse();
    } else {
      setForward();
    }
    if (pedalState == HIGH) {
      increaseThrottle();
    } else {
      decreaseThrottle();
    }
  }

  sendCommandToMotors(); // command the motors
  delay(50);
}

bool readRCInput()
{
  bool hasInput = false;
  rcOverrideData = pulseIn(cRCOverridePin, HIGH, 15000); // give this one a timeout incase the controller is off

  if (rcOverrideData > 0) // that means we are on
  {
    rcForwardBackData = pulseIn(cRCForwardBackPin, HIGH);
    rcSpeedData = pulseIn(cRCSpeedPin, HIGH);

    if (rcOverrideData < 1100)
    {
      RC_FULL_CONTROL = true;
    } else {
      RC_FULL_CONTROL = false;
    }

    if (rcSpeedData > 1800)
    {
      MAX_THROTTLE = FULL_MAX_THROTTLE;
    } else if (rcSpeedData > 1700) {
      MAX_THROTTLE = FULL_MAX_THROTTLE - 50;
    } else {
      MAX_THROTTLE = FULL_MAX_THROTTLE - 100; // this is about a walking speed
    }

    if (throttleAmount > MAX_THROTTLE)
    {
      throttleAmount = MAX_THROTTLE;
    }

    if (rcForwardBackData > 1600)
    {
      setForward();
      increaseThrottle();
      hasInput = true;
    } else if (rcForwardBackData < 1300) {
      setReverse();
      increaseThrottle();
      hasInput = true;
    } else if (RC_FULL_CONTROL) {
      setForward();
      decreaseThrottle();
      hasInput = true;
    }
  } else {
    RC_FULL_CONTROL = false;
  }
  return hasInput;
}
void sendCommandToMotors()
{
  if (lastThrottleAmount != throttleAmount)
  {
    if (vehicleState == FORWARD)
    {
      if (throttleAmount >= 0) // if we are already moving forward
      {
        digitalWrite(cRPWMPin, 0); // keep moving forward
        analogWrite(cLPWMPin, throttleAmount);
        Serial.print("Forward : ");
        Serial.println(throttleAmount);
      } else {
        int inverseThrottleAmount =  -1 * throttleAmount;
        analogWrite(cRPWMPin, inverseThrottleAmount); // we're not yet throttled back into forward position, so keep slowing our reverse down
        digitalWrite(cLPWMPin, 0);
        Serial.print("Forward-: ");
        Serial.println(inverseThrottleAmount);
      }
    } else { // we want to go reverse
      if (throttleAmount <= 0) // if we are already going in reverse
      {
        int inverseThrottleAmount =  -1 * throttleAmount;
        analogWrite(cRPWMPin, inverseThrottleAmount); // keep going in reverse
        digitalWrite(cLPWMPin, 0);
        Serial.print("Reverse-: ");
        Serial.println(inverseThrottleAmount);
      } else {
        digitalWrite(cRPWMPin, 0); // we're not yet going in reverse, so keep slowing down our forward
        analogWrite(cLPWMPin, throttleAmount);
        Serial.print("Reverse : ");
        Serial.println(throttleAmount);
      }
    }
    lastThrottleAmount = throttleAmount;
  }
}
void setForward()
{
  vehicleState = FORWARD;
}
void setReverse()
{
  vehicleState = BACKWARD;
}
void increaseThrottle()
{
  if (vehicleState == FORWARD) // if we want to go forward
  {
    if (throttleAmount + throttleChangeAmount <= MAX_THROTTLE) // if we're not less than max throttle
    {
      throttleAmount = throttleAmount + throttleChangeAmount; // increase throttle
    }
  } else { // we want to go backward
    if (throttleAmount - throttleChangeAmount >= -MAX_THROTTLE) // if we're not less than -max throttle
    {
      throttleAmount = throttleAmount - throttleChangeAmount; // increase throttle in the opposite direction
    }
  }
}
void decreaseThrottle()
{
  if (throttleAmount > 0)
  {
    if (throttleAmount - throttleChangeAmount >= 0)
    {
      throttleAmount = throttleAmount - throttleChangeAmount;
    } else {
      throttleAmount = 0;
    }
  } else {
    if (throttleAmount + throttleChangeAmount <= 0)
    {
      throttleAmount = throttleAmount + throttleChangeAmount;
    } else {
      throttleAmount = 0;
    }
  }
}