/*
   move two servos combined
   https://forum.arduino.cc/t/mehrere-servos-synchron-ansteuern/1030870/20

   dynamic

   by noiasca
   2022-09-12

   todo: put it on HP
*/

#include <Servo.h>

constexpr uint8_t openPin = 4;     // GPIO for a movement
constexpr uint8_t closePin = 3;    // GPIO for another movement
constexpr uint8_t servoAPin = 8;   // GPIO for Servo A
constexpr uint8_t servoBPin = 9;   // GPIO for Servo B

// 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 as 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, int16_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);
  smoothServoA.begin(servoAPin);          // start the servo object
  smoothServoB.begin(servoBPin);
  pinMode(openPin, INPUT_PULLUP);
  pinMode(closePin, INPUT_PULLUP);
}

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

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

void loop()
{
  // 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);
}