/*
   move servos smoothly

   https://forum.arduino.cc/t/servo-ansteuerung-fur-schallplatten-waschmaschine/1109620/4

   based on:
   https://forum.arduino.cc/t/mehrere-servos-synchron-ansteuern/1030870/50

   Variant with a finite state machine / workflow

   by noiasca
   2023-04-01
*/

#include <Servo.h>

constexpr uint8_t startPin = 14;     // GPIO for a movement
constexpr uint8_t stopPin = 12;    // GPIO for another movement
constexpr uint8_t servoAPin = 26;   // GPIO for Servo A

constexpr uint8_t ledRPin = 21;
constexpr uint8_t ledYPin = 19;
constexpr uint8_t ledGPin = 18;

// make your own class for two servos
class SmoothServo {
  protected:
    uint16_t target {0};               // target angle
    uint16_t current {0};              // current angle
    uint8_t interval {1};              // delay time
    uint32_t previousMillis {0};       // last movement
    uint16_t targetTime {500};         // how long has the servo time for the movement
    uint32_t previousMillisStart{0};   // last start with set

  public:
    Servo servo;

    void begin(const byte pin) {
      servo.attach(pin);
      servo.write(target);   // bring the servo to a defined angle
    }

    void set(uint16_t target, uint16_t targetTime = 500) {
      this->target = target;
      this->targetTime = targetTime;
      previousMillisStart = millis();
    }

    void update(uint32_t currentMillis = millis()) {
      if (currentMillis - previousMillis > interval) { // slow down the servos
        previousMillis = currentMillis;
        if (target != current) {
          uint32_t passedTime = currentMillis - previousMillisStart;
          if (passedTime < targetTime) {
            uint32_t remainingTime = targetTime - passedTime;
            int diff = target - current;
            diff = abs(diff);
            interval = remainingTime / diff;
          }
          else {
            interval = 0;
            // could also be used to force the servo in the target position
          }
          if (target < current) {
            current = current - 1;
          }
          else if (target > current) {
            current = current + 1;
          }
          servo.write(current);
        }
      }
    }
};

SmoothServo smoothServo;  // create a servo object

void setup() {
  Serial.begin(115200);
  smoothServo.begin(servoAPin);          // start the servo object
  pinMode(startPin, INPUT_PULLUP);
  pinMode(stopPin, INPUT_PULLUP);
  pinMode(ledRPin, OUTPUT);   // für später mal
  pinMode(ledYPin, OUTPUT);
  pinMode(ledGPin, OUTPUT);

  // bring Servo in initial state
  smoothServo.set(0, 500);    // angle, milliseconds for movement
}

// read buttons, handle workflow  with a finite state machine
void fsm() {
  enum State {INIT, IDLE, STEP_A, PAUSE, STEP_B, END};
  static State state;
  static uint32_t previousMillis = 0;

  switch (state) {
    case State::INIT:
      if (millis() - previousMillis > 500) {
        Serial.println(F("go to IDLE"));
        state = IDLE;
      }
      break;
    case State::IDLE:
      if (digitalRead(startPin) == LOW) {
        previousMillis = millis();  // "reset" timer for next step
        Serial.println(F("STEP_A"));
        state = STEP_A;
        smoothServo.set(20, 4000);  // degrees, milliseconds
      }
      break;
    case State::STEP_A:
      if (millis() - previousMillis  > 4000) {
        previousMillis = millis(); // "reset" timer for next step
        Serial.println(F("PAUSE"));
        state = PAUSE;
      }
      break;
    case State::PAUSE:
      if (millis() - previousMillis  > 5000) {
        previousMillis = millis();  // "reset" timer for next step
        Serial.println(F("STEP_B"));
        state = STEP_B;
        smoothServo.set(45, 5000);
      }
      break;
    case State::STEP_B:
      if (millis() - previousMillis  > 5000) {
        Serial.println(F("END - wait for stop button (home)"));
        state = END;
      }
      break;
    case State::END:
      if (digitalRead(stopPin) == LOW) {
        previousMillis = millis();  // "reset" timer for next step
        Serial.println(F("go back to INIT"));
        state = INIT;
        smoothServo.set(0, 500);
      }
      break;
  }
}

void loop() {
  fsm();
  smoothServo.update();  // call the update method in loop
}