#include <Servo.h>

const int servoPin = 9;            // servo signal pin (yellow/white wire)
const int slowButtonPin = 2;       // pushbutton pin for slow mode
const int mediumButtonPin = 3;     // pushbutton pin for medium mode
const int fastButtonPin = 4;       // pushbutton pin for fast mode
const int onOffButtonPin = 5;      // pushbutton pin for on/off
const int ledPin = 11;             // led pin for indication (using a pwm-capable pin)

int slowButtonState = HIGH;        // current state of the slow mode button
int mediumButtonState = HIGH;      // current state of the medium mode button
int fastButtonState = HIGH;        // current state of the fast mode button
int onOffButtonState = HIGH;       // current state of the on/off button

unsigned long lastSlowPressTime = 0;     // last time the slow button was pressed
unsigned long lastMediumPressTime = 0;   // last time the medium button was pressed
unsigned long lastFastPressTime = 0;     // last time the fast button was pressed
unsigned long lastOnOffPressTime = 0;    // last time the on/off button was pressed

unsigned long debounceDelay = 200;   // debounce time in milliseconds; adjust if needed
Servo wiperServo;                    // create a servo object
int currentPosition = 0;             // current position of the servo
int defaultPosition = 0;             // default position of the servo
int speed = 0;                       // default speed; 0 for stopped, adjust as needed
int increment = 2;                   // increment for each movement
bool isOn = false;                   // variable to store the on/off state
int lastSpeed = 20;                  // variable to store the last speed

void setup() {
  wiperServo.attach(servoPin);          // attach the servo to digital pin 9
  pinMode(slowButtonPin, INPUT_PULLUP); // set slow button pin as input with internal pull-up resistor
  pinMode(mediumButtonPin, INPUT_PULLUP); // set medium button pin as input with internal pull-up resistor
  pinMode(fastButtonPin, INPUT_PULLUP); // set fast button pin as input with internal pull-up resistor
  pinMode(onOffButtonPin, INPUT_PULLUP); // set on/off button pin as input with internal pull-up resistor
  pinMode(ledPin, OUTPUT);               // set led pin as output

  // set default position
  wiperServo.write(defaultPosition);
}

void loop() {
  // read the state of the buttons
  unsigned long currentMillis = millis();
  
  // check if the slow button is pressed and debounce it
  if (currentMillis - lastSlowPressTime >= debounceDelay) {
    int slowButtonReading = digitalRead(slowButtonPin);
    if (slowButtonReading != slowButtonState) {
      lastSlowPressTime = currentMillis;
      slowButtonState = slowButtonReading;

      if (slowButtonState == LOW && isOn) {
        speed = 20;  // set speed for slow mode
        lastSpeed = speed; // store the last speed
        analogWrite(ledPin, 85); // set led to low brightness (85/255 ~ 33% brightness)
      }
    }
  }

  // check if the medium button is pressed and debounce it
  if (currentMillis - lastMediumPressTime >= debounceDelay) {
    int mediumButtonReading = digitalRead(mediumButtonPin);
    if (mediumButtonReading != mediumButtonState) {
      lastMediumPressTime = currentMillis;
      mediumButtonState = mediumButtonReading;

      if (mediumButtonState == LOW && isOn) {
        speed = 15;   // set speed for medium mode
        lastSpeed = speed; // store the last speed
        analogWrite(ledPin, 170); // set led to medium brightness (170/255 ~ 66% brightness)
      }
    }
  }

  // check if the fast button is pressed and debounce it
  if (currentMillis - lastFastPressTime >= debounceDelay) {
    int fastButtonReading = digitalRead(fastButtonPin);
    if (fastButtonReading != fastButtonState) {
      lastFastPressTime = currentMillis;
      fastButtonState = fastButtonReading;

      if (fastButtonState == LOW && isOn) {
        speed = 10;   // set speed for fast mode
        lastSpeed = speed; // store the last speed
        analogWrite(ledPin, 255); // set led to high brightness (255/255 = 100% brightness)
      }
    }
  }

  // check if the on/off button is pressed and debounce it
  if (currentMillis - lastOnOffPressTime >= debounceDelay) {
    int onOffButtonReading = digitalRead(onOffButtonPin);
    if (onOffButtonReading != onOffButtonState) {
      lastOnOffPressTime = currentMillis;
      onOffButtonState = onOffButtonReading;

      if (onOffButtonState == LOW) {
        isOn = !isOn;  // toggle the on/off state
        if (isOn) {
          // resume movement with the last speed and led brightness
          speed = lastSpeed;
          analogWrite(ledPin, speed == 20 ? 85 : speed == 15 ? 170 : 255);
        } else {
          // stop the movement
          speed = 0;
          wiperServo.write(defaultPosition); // set servo position to default
          analogWrite(ledPin, 0); // turn off led
        }
      }
    }
  }

  // move the servo with the current speed
  if (isOn && speed != 0) {
    currentPosition += increment; // increment position
    if (currentPosition >= 180 || currentPosition <= 0) {
      increment = -increment; // change direction if reaching limits
    }
    wiperServo.write(currentPosition); // set servo position
    delay(speed); // delay based on speed
  }
}