/*
   move two servos combined


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

   Variant with a HC ultrasonic Sensor as external Trigger for Close

   by noiasca
   2023-02-18
*/

#include <Servo.h>

constexpr uint8_t trigPin = 23;  // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
constexpr uint8_t echoPin = 22;  // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
constexpr uint8_t distanceThreshold = 50; // centimeters

constexpr uint8_t openPin = 12;     // GPIO for a movement
constexpr uint8_t closePin = 14;    // GPIO for another movement
constexpr uint8_t servoAPin = 26;   // GPIO for Servo A
constexpr uint8_t servoBPin = 25;   // GPIO for Servo B

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 {90};              // target angle
    uint16_t current {90};             // 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 smoothServoA;  // create a servo object
SmoothServo smoothServoB;

void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT); // set ESP32 pin to output mode
  pinMode(echoPin, INPUT);  // set ESP32 pin to input mode
  smoothServoA.begin(servoAPin);          // start the servo object
  smoothServoB.begin(servoBPin);
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);

  pinMode(ledRPin, OUTPUT);
  pinMode(ledYPin, OUTPUT);
  pinMode(ledGPin, OUTPUT);
}

void red(){
  digitalWrite(ledRPin, HIGH);
  digitalWrite(ledYPin, LOW);
  digitalWrite(ledGPin, LOW);
}

void yellow(){
  digitalWrite(ledRPin, LOW);
  digitalWrite(ledYPin, HIGH);
  digitalWrite(ledGPin, LOW);
}

void green(){
  digitalWrite(ledRPin, LOW);
  digitalWrite(ledYPin, LOW);
  digitalWrite(ledGPin, HIGH);
}

void doorOpen() {
  Serial.println(F("move open"));
  smoothServoA.set(90, 1000);
  smoothServoB.set(90, 1000);
  //smoothServoA.servo.write(90); // hardcoded write
}

void doorClose() {
  Serial.println(F("move close in a different speed"));
  smoothServoA.set(180, 500);    // angle, milliseconds for movement
  smoothServoB.set(115, 500);
}

float getDistance() {
  // variables will change:
  float duration_us, distance_cm;
  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // measure duration of pulse from ECHO pin
  duration_us = pulseIn(echoPin, HIGH);
  // calculate the distance
  distance_cm = 0.017 * duration_us;

  // read buttons, sensors...
  if (digitalRead(openPin) == LOW) doorOpen();
  if (digitalRead(closePin) == LOW) doorClose();

  // call all servos
  uint32_t currentMillis = millis();
  smoothServoA.update(currentMillis);  // call the update method in loop
  smoothServoB.update(currentMillis);

  // print the value to Serial Monitor
  Serial.print("distance: ");
  Serial.print(distance_cm);
  Serial.println(" cm");
  return distance_cm;
}

// not used - just as an example how to avoid a dirty delay
void handleHC() {
  static uint32_t previousMillis = 0;
  if (millis() - previousMillis > 500) {
    previousMillis = millis();
    float distance = getDistance();
  }
}

// read buttons, sensors... within a finite state machine
void fsm() {
  enum State {OPEN, TRIGGER, CLOSE};
  static State state;
  static uint32_t previousMillis = 0;

  switch (state) {
    case State::OPEN:
      if (millis() - previousMillis > 500) {
        previousMillis = millis();
        float distance = getDistance();
        if (distance < distanceThreshold) {
          state = TRIGGER;
          yellow();
          Serial.println(F("door will close in short time"));
        }
      }
      if (digitalRead(closePin) == LOW) {
        doorClose();
        red();
        Serial.println(F("door will close imidiately"));
        state = CLOSE;
      }
      break;
    case State::TRIGGER:
      if (millis() - previousMillis > 1000) {
        previousMillis = millis();
        state = CLOSE;
        Serial.println(F("door closing"));
        doorClose();
        red();
      }
      break;
    case State::CLOSE:
      if (digitalRead(openPin) == LOW) {
        Serial.println(F("door will open"));
        state = OPEN;
        doorOpen();
        green();
      }
      break;
  }
}

void loop() {
  fsm();
  // call all servos
  uint32_t currentMillis = millis();
  smoothServoA.update(currentMillis);  // call the update method in loop
  smoothServoB.update(currentMillis);
}