class Button {
  public:
    boolean isPressed = false, wasPressed = false;
    boolean isProperPressed = false, wasProperPressed = false;
    //boolean isProperReleased = false, wasProperReleased = false;
    boolean invertInput = true;
    long timeOfLastRising = 0, timeOfLastFalling = 0;
    long timeOfLastProperRising = 0, timeOfLastProperFalling = 0
    long properPressTime = 0, properReleaseTime = 0;
    int pin;
    
    Button() {}

    Button(int pin_) {
      init(pin_, properPressTime, true, true);
    }

    Button(int pin_, long properPressTime_) {
      init(pin_, properPressTime_, true, true);
    }

    Button(int pin_, long properPressTime_, boolean internalPullUp) {
      init(pin_, properPressTime_, internalPullUp, true);
    }

    Button(int pin_, long properPressTime_, boolean internalPullUp, boolean invertInput_) {
      init(pin_, properPressTime_, internalPullUp, invertInput_);
    }

    void init(int pin_) {
      init(pin_, properPressTime, true, true);
    }

    void init(int pin_, long properPressTime_) {
      init(pin_, properPressTime_, true, true);
    }

    void init(int pin_, long properPressTime_, boolean internalPullUp) {
      init(pin_, properPressTime_, internalPullUp, true);
    }

    void init(int pin_, long properPressTime_, boolean internalPullUp, boolean invertInput_) {
      pin = pin_;
      properPressTime = properPressTime_;
      properReleaseTime = properPressTime;
      pinMode(pin, internalPullUp ? INPUT_PULLUP : INPUT);
      invertInput = invertInput_;
    }

    boolean getState() {
      boolean input = digitalRead(pin);
      return invertInput ? (!input) : input;
    }

    void run() {
      wasPressed = isPressed;
      isPressed = getState();
      if(justPressed()) timeOfLastRising = millis();
      if(justReleased()) timeOfLastFalling = millis();

      wasProperPressed = isProperPressed;
      //wasProperReleased = isProperReleased;
      //isProperPressed = isPressed && millis() - timeOfLastRising > properPressTime;
      //isProperReleased = !isPressed && millis() - timeOfLastFalling > properReleaseTime;
      if(wasProperPressed) {
        if(millis() - timeOfLastFalling > properReleaseTime) isProperPressed = false;
        else isProperPressed = true;
      }
      else {
        if(millis() - timeOfLastRising > properPressTime) isProperPressed = true;
        else isProperPressed = false;
      }
      //isProperPressed = !wasProperReleased && millis() - timeOfLastRising > properPressTime;
      //isProperReleased = !wasProperPressed && millis() - timeOfLastFalling > properReleaseTime;

      //might wanna add timeOfLastProperRising and falling, if reverse turns off randomly
      if(justProperPressed()) timeOfLastProperRising = millis();
      if(justProperReleased()) timeOfLastProperFalling = millis();
    }
    
    boolean justPressed() {
      return !wasPressed && isPressed;
    }
    
    boolean justReleased() {
      return wasPressed && !isPressed;
    }

    boolean pressed() {
      return isPressed;
    }
    
    boolean released() {
      return !isPressed;
    }

    boolean justProperPressed() {
      return !wasProperPressed && isProperPressed;
    }
    
    boolean justProperReleased() {
      //return !wasProperReleased && isProperReleased;
      return wasProperPressed && !isProperPressed;
    }

    boolean properPressed() {
      return isProperPressed;
    }
    
    boolean properReleased() {
      //return isProperReleased;
      return !isProperPressed;
    }

    long isPressedFor() {
      return isPressed ? (millis() - timeOfLastRising) : -1;
    }

    long isReleasedFor() {
      return !isPressed ? (millis() - timeOfLastRising) : -1;
    }

    long isProperPressedFor() {
      return isProperPressed ? (millis() - timeOfLastProperRising) : -1;
    }

    long isProperReleasedFor() {
      return !isProperPressed ? (millis() - timeOfLastProperRising) : -1;
    }
};

#include <Servo.h>

//motor
int const speedPin = 5;
int const directionPin = 4;
int const breakSignalPin = 3;
boolean motorDefaultDirection = true;

//input pins
int const breakPotPin = A0;
int const gasPotPin = A3;
int const keyPin = 6;
int const speed1Pin = 8;
int const speed2Pin = 7;

//LED pins
int const redLEDpin = 11;
int const greenLEDpin = 10;
int const blueLEDpin = 9;

//thresholds
int const gasMin = 144, gasMax = 678;
int const gasMinValue = 20;//0-255
int const lowSpeedMultiplier = 0.5;
int const reverseMultiplier = 0.5;
int const breakMin = 222, breakMax = 700; //real max is 755, 700 is to make sure a full stop, maybe change later
//int const fullBreakThreshold = 700;//probs change later;
long reverseMakeSureTime = 500;
long reverseBlinkingPeriod = 500;

//buttons
long const antidrebezgTime = 200;
Button keyButton, speed1Button, speed2Button;

Servo myServo;

void setup() {
  Serial.begin(9600);
  pinMode(speedPin, OUTPUT);
  pinMode(directionPin, OUTPUT);
  pinMode(breakSignalPin, OUTPUT);
  analogWrite(speedPin, 0);
  digitalWrite(directionPin, HIGH);

  pinMode(redLEDpin, OUTPUT);
  pinMode(greenLEDpin, OUTPUT);
  pinMode(blueLEDpin, OUTPUT);

  keyButton.init(keyPin, antidrebezgTime);
  speed1Button.init(speed1Pin, antidrebezgTime);
  speed2Button.init(speed2Pin, antidrebezgTime);

  myServo.attach(13);
}

void loop() {
  runAllButtons();
  manageMotor();
}

void runAllButtons() {
  keyButton.run();
  speed1Button.run();
  speed2Button.run();
}

boolean blinkingState(long onTime) {
  return blinkingState(onTime, onTime);
}

boolean blinkingState(long onTime, long offTime) {
  return (millis() % (onTime+offTime)) < onTime;
}

void manageMotor() {
  float breakInput = analogRead(breakPotPin);
  breakInput = myCMap(breakInput, breakMin, breakMax, 0, 255);
  if(breakInput < 255) {
    unbreakMotor();
    float gasInput = myCMap(analogRead(gasPotPin), gasMin, gasMax, gasMinValue, 255);
    float speed = max(gasInput - breakInput, 0);
    
    if(speed1Button.isProperReleasedFor() > reverseMakeSureTime 
    && speed2Button.isProperReleasedFor() > reverseMakeSureTime) {
      manageReverse();
    }
    else {
      forwardMotor();
      if(speed1Button.properPressed()) speed *= lowSpeedMultiplier;
    }
    //analogWrite(speedPin, int(speed));
    myServo.write(int(speed/255*180));
  }
  else breakMotor();
}

void reverseMotor() {digitalWrite(directionPin, !motorDefaultDirection);}

void forwardMotor() {digitalWrite(directionPin, motorDefaultDirection);}

void breakMotor() {digitalWrite(breakSignalPin, HIGH);}

void unbreakMotor() {digitalWrite(breakSignalPin, LOW);}

void manageReverse() {
  reverseMotor();
      speed *= reverseMultiplier;
      digitalWrite(redLEDpin, blinkingState(reverseBlinkingPeriod));
}

float myMap(float A, float minA, float maxA, float minB, float maxB) {
  return (A-minA)/(maxA-minA)*(maxB-minB)+minB;
}

float myCMap(float A, float minA, float maxA, float minB, float maxB) {
  return myConstrain(myMap(A, minA, maxA, minB, maxB), minB, maxB);
}

float myConstrain(float A, float minA, float maxA) {
  if(A > maxA) return maxA;
  else if( A < minA) return minA;
  else return A;
}