#include <Servo.h>

//Button class with debouncer
class Button {                                   
    const byte buttonPin;                        
    static constexpr byte debounceDelay = 20; //debounce delay     
    const bool active;                           
    bool lastButtonState = HIGH;                 
    byte lastDebounceTime = 0;                   
    
  public:
    Button(byte attachTo, bool active = LOW) : buttonPin(attachTo), active(active) {} //class parameters; activates internal pullups by default

    // sets the pinMode and optionally activates the internal pullups
    void begin() {                               
      if (active == LOW)
        pinMode(buttonPin, INPUT_PULLUP);
      else
        pinMode(buttonPin, INPUT);
    }

    bool wasPressed() { 
      bool buttonState = LOW;
      byte reading = LOW;
      if (digitalRead(buttonPin) == active) reading = HIGH;
      if (((millis() & 0xFF ) - lastDebounceTime) > debounceDelay) {
        if (reading != lastButtonState && lastButtonState == LOW) {
          buttonState = HIGH;
        }
        lastDebounceTime = millis() & 0xFF;   
        lastButtonState = reading;
      }
      return buttonState;
    }
};

//change these for the assigned pins
Servo servo;
Button servoButton(A0); //servo down button
Button blueButton(A1);  // time decrement button
Button redButton(A2); // time increment button
Button greenButton(A3); //start button

bool isRunning = false; // is the orbiter running


//SETTINGS:
//release delay, increment delay, decrement delay settings
unsigned long startTime;
float releaseDelay = 0.5 * 1000; // release delay in milliseconds
float launcherDelay = 2 * 1000; // launcher delay in milliseconds
float totalDelay = releaseDelay + launcherDelay; //total time from when green button is pressed to lander release
float incrementDelay = 0.5 * 1000; //time increment
float decrementDelay = 0.1 * 1000; //time decrement

//Servo Settings
float servoAngleIncrement = 2;
float servoAngle = 90; //initial servo jump when white or blue buttons are pressed
float buttonInterval = 200;
int endAngle = 10;
int startAngle = 90;

bool blueLEDState = LOW;
bool redLEDState = LOW;
bool whiteLEDState = LOW;
float previousMillis = 0;




void setup() {
  Serial.begin(115200);
  Serial.println(F("Start"));

  servo.write(startAngle); // Initial position of the servo
  servo.attach(10); //servo attached to pin
  redButton.begin();  // decrement time delay 
  blueButton.begin(); // increment time delay
  greenButton.begin(); // start button
  servoButton.begin(); //servo button
  pinMode(3, OUTPUT); //white LED 
  pinMode(4, OUTPUT); //blue LED
  pinMode(7, OUTPUT);//red LED
  pinMode (6, OUTPUT); //green LED
  
  Serial.println(F("end of setup"));
}


void loop() {
  
  float currentMillis = millis();

  //Blinker for blue LED
  if(currentMillis > previousMillis + 200 && (blueLEDState == HIGH) && !isRunning){
    digitalWrite(4, LOW);
    blueLEDState = LOW;
  }

  //Blinker for red LED
  if(currentMillis > previousMillis + 200 && (redLEDState == HIGH) && !isRunning){
    digitalWrite(7, LOW);
    redLEDState = LOW;
  }

  //Blinker for white LED
  if(currentMillis > previousMillis + 200 && (whiteLEDState == HIGH) && !isRunning){
    digitalWrite(3, LOW);
    blueLEDState = LOW;
  }


   //Blinker for blue LED
  if(currentMillis > previousMillis + 200 && (whiteLEDState == HIGH) && !isRunning){
  digitalWrite(4, LOW);
  whiteLEDState = LOW;
  }
  
  /*
  //Serial print out for testing
  Serial.println(releaseDelay);
  Serial.println(totalDelay);
  */
  


  //Red LED turns off to indicate orbiter launch
  if ((millis() - startTime) > launcherDelay && isRunning){
    digitalWrite(7, LOW);
  }

  //Green LED turns off to indicate lander launch
  if ((millis() - startTime) > totalDelay  && isRunning){
    servo.write(endAngle);
    isRunning = false;
    digitalWrite(6, LOW);
  }

  //Blue button decrements releaseDelay
  if (blueButton.wasPressed()){
    releaseDelay -= decrementDelay;
    totalDelay = releaseDelay + launcherDelay;
    digitalWrite(4, HIGH);
    blueLEDState = HIGH;
    previousMillis = millis();
  }

  /* //Alternate functionality for blueButton moves ther servo arm up
    if (blueButton.wasPressed()){
      servoAngle += servoAngleIncrement;
      servo.write(servoAngle);
      digitalWrite(4, HIGH);
      blueLEDState = HIGH;
      previousMillis = millis();   
    }
  */

  //Red button decrements releaseDelay
  if (redButton.wasPressed()){
    releaseDelay += incrementDelay;
    totalDelay = releaseDelay + launcherDelay;
    digitalWrite(7, HIGH);
    redLEDState = HIGH;
    previousMillis = millis();
  } 

  //Green button starts launch sequence
  if (greenButton.wasPressed()){
    startTime = millis();
    isRunning = true;
    digitalWrite(7, HIGH);
    digitalWrite(6, HIGH);
  } 

  //White servo button moves servo arm down
  if (servoButton.wasPressed()){
    servoAngle -= servoAngleIncrement;
    servo.write(servoAngle);
    digitalWrite(3, HIGH);
    whiteLEDState = HIGH;
    previousMillis = millis();
  } 

}