// Applique murale à bras articulés et motorisés
// https://forum.arduino.cc/t/applique-murale-a-bras-articules-et-motorises/1301784

#include <Servo.h>

const int8_t servoPin[] = {9, 7, 5, 3};
const size_t ServoNumber = sizeof(servoPin) / sizeof(*servoPin);
#define extendBtnPin  A0
#define stopBtnPin    A1
#define retractBtnPin A2
#define PAUSE_BETWEEN_PULSES 20
#define LED_PACE 200

#define PRESSED  LOW
#define RELEASED HIGH
enum Status : int8_t {STOPPED, EXTENDING, RETRACTING};

Servo servo[ServoNumber];

void setup() {
  Serial.begin(115200);

  for (size_t index = 0; index < ServoNumber; index++) {
    servo[index].attach(servoPin[index]);
  }

  pinMode(extendBtnPin, INPUT_PULLUP);
  pinMode(retractBtnPin, INPUT_PULLUP);
  pinMode(stopBtnPin, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  static int16_t servoTargetPos[ServoNumber] = {10, 170, 10, 170};
  static int16_t servoPos[ServoNumber] = {11, 169, 11, 169};
  static size_t servoIndex = ServoNumber - 2;
  static Status status = RETRACTING;
  static uint32_t nextPulse = millis();
  static uint32_t nextLedBlink = 0;
  uint32_t currentTime = millis();

  if (status && currentTime > nextPulse) {
    nextPulse += PAUSE_BETWEEN_PULSES;
    if (servoPos[servoIndex] != servoTargetPos[servoIndex]) {
      // Slowly reach the target position
      if (servoPos[servoIndex] < servoTargetPos[servoIndex]) {
        servoPos[servoIndex]++;
      }
      else {
        servoPos[servoIndex]--;
      }
      // Set the servo to the corresponding position
      servo[servoIndex].write(servoPos[servoIndex]);

      // Serial.print("servo ");
      // Serial.print(servoIndex);
      // Serial.print(" pulse: ");
      // Serial.println(servoPos[servoIndex]);
    }

    if (servoPos[servoIndex + 1] != servoTargetPos[servoIndex + 1]) {
      // Slowly reach the target position
      if (servoPos[servoIndex + 1] < servoTargetPos[servoIndex + 1]) {
        servoPos[servoIndex + 1]++;
      }
      else {
        servoPos[servoIndex + 1]--;
      }
      // Set the servo to the corresponding position
      servo[servoIndex + 1].write(servoPos[servoIndex + 1]);

      // Serial.print("servo ");
      // Serial.print(servoIndex + 1);
      // Serial.print(" pulse: ");
      // Serial.println(servoPos[servoIndex + 1]);
    }
    else if (servoPos[servoIndex] == servoTargetPos[servoIndex]) { // Make sure both target are reached
      if (status == EXTENDING) {
        if (servoIndex < ServoNumber - 2) {
          // Next servo pair
          servoIndex += 2;
        }
        else {
          status = STOPPED;
          Serial.println("Movement done !");
        }
      }
      else {
        if (servoIndex > 0) {
          // Previous servo pair
          servoIndex -= 2;
        }
        else {
          status = STOPPED;
          Serial.println("Movement done !");
        }
      }
    }
    // Serial.flush();
  }
  // Si l’interrupteur est en position 1
  // Ouverture
  if (status != EXTENDING && digitalRead(extendBtnPin) == PRESSED) {
    status = EXTENDING;
    Serial.println("Extending");
    servoTargetPos[0] = 170;
    servoTargetPos[1] = 10;
    servoTargetPos[2] = 170;
    servoTargetPos[3] = 10;
    nextPulse = currentTime;
  }

  // Si l’interrupteur est en position 2
  // Fermeture
  if (status != RETRACTING && digitalRead(retractBtnPin) == PRESSED) {
    status = RETRACTING;
    Serial.println("Retracting");
    servoTargetPos[0] = 10;
    servoTargetPos[1] = 170;
    servoTargetPos[2] = 10;
    servoTargetPos[3] = 170;
    nextPulse = currentTime;
  }

  // Si l’interrupteur est en position 0
  // Arrêt
  if (status != STOPPED && digitalRead(stopBtnPin) == PRESSED) {
    status = STOPPED;
    Serial.println("Stopped");
    servoTargetPos[0] = servoPos[0];
    servoTargetPos[1] = servoPos[1];
    servoTargetPos[2] = servoPos[2];
    servoTargetPos[3] = servoPos[3];
  }

  if (status) {
    if (currentTime > nextLedBlink) {
      digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
      nextLedBlink = currentTime + LED_PACE;
    }
  }
  else {
    if (digitalRead(LED_BUILTIN)) {
      digitalWrite(LED_BUILTIN, LOW);
    }
  }
}