/*
  Forum: https://forum.arduino.cc/t/servo-control-with-one-button/1447740
  Wokwi: https://wokwi.com/projects/466365025604980737

  2026/06/09
  ec2021

*/

#include <Servo.h>


constexpr byte pwmPins[] = { 5, 6, 9, 10, 11 };
constexpr int noOfPins = sizeof(pwmPins) / sizeof(pwmPins[0]);
Servo servos[noOfPins];

constexpr byte btPin{ 2 };
constexpr byte redPin{ 4 };
constexpr byte greenPin{ 3 };
constexpr unsigned long duration{ 3000 };
constexpr byte delta{ 10 };
constexpr byte openPosition{ 180 };
constexpr byte closePosition{ 0 };

enum class LOCK { IDLE,
                  CLOSING,
                  OPENING,
                  PREPARE
                };

LOCK state = LOCK::IDLE;
boolean locked = true;
byte lockPosition = 0;
unsigned long lastStep = 0;

void setup() {
  Serial.begin(115200);
  pinMode(btPin, INPUT_PULLUP);
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  for (int i = 0; i < noOfPins; i++) {
    servos[i].attach(pwmPins[i]);
  }
}

void loop() {
  drawerLock();
}

void drawerLock() {
  switch (state) {
    case LOCK::IDLE:
      if (digitalRead(btPin) == LOW) {
        state = locked ? LOCK::OPENING : LOCK::CLOSING;
        delay(100);  //Very simple debouncing
        digitalWrite(redPin, LOW);
        digitalWrite(greenPin, LOW);
      }
      break;
    case LOCK::CLOSING:
      doBlink(redPin);
      if (millis() - lastStep > 100) {
        lastStep = millis();
        if (lockPosition < delta) {
          lockPosition = closePosition;
        } else {
          lockPosition -= delta;
        }
        setServoTo(lockPosition);
      }
      if (lockPosition == closePosition) {
        locked = true;
        state = LOCK::PREPARE;
      }
      break;
    case LOCK::OPENING:
      doBlink(greenPin);
      if (millis() - lastStep > 100) {
        lastStep = millis();
        if (lockPosition + delta >= openPosition) {
          lockPosition = openPosition;
        } else {
          lockPosition += delta;
        }
        setServoTo(lockPosition);
      }
      if (lockPosition == openPosition) {
        locked = false;
        state = LOCK::PREPARE;
      }
      break;
    case LOCK::PREPARE:
      if (locked) {
        setLedsOnOff(redPin, greenPin);
      } else {
        setLedsOnOff(greenPin, redPin);
      }
      state = LOCK::IDLE;
      break;
  }
}


void setServoTo(byte value) {
  for (int i = 0; i < noOfPins; i++) {
    servos[i].write(value);
  }
}

void doBlink(byte pin) {
  static unsigned long lastBlink = 0;
  if (millis() - lastBlink > 500) {
    lastBlink = millis();
    byte newState = !digitalRead(pin);
    digitalWrite(pin, newState);
  }
}

void setLedsOnOff(byte a, byte b) {
  digitalWrite(a, HIGH);
  digitalWrite(b, LOW);
}