/*
   Slow Servo Movements - Programable Servo Sweep
   based on request of 

   by noiasca https://werner.rothschopf.net/microcontroller/202207_millis_slow_servo.htm
   2022-07-15 https://forum.arduino.cc/t/i-want-the-servo-to-be-slower-how-do-i-control-the-speed-of-the-servo-servo-speed-control/1012695/
   2024-10-09 https://forum.arduino.cc/t/potentiometer-als-positionsgeber-mit-einem-mini/1309595
*/

#include <Servo.h>
#include <OneButton.h>

constexpr uint8_t buttonAPin = 4;    //T1
constexpr uint8_t buttonBPin = 3;   // T2
constexpr uint8_t servoPin = 8;   // GPIO for Servo

OneButton btnA;
OneButton btnB;

uint16_t targetLow = 0;
uint16_t targetHigh = 180;

enum class State {SWEEP, PROG_LOW, PROG_HIGH} state;  // either program modus or sweep modus

// make your own servo class
class SlowServo {
  protected:
    uint16_t target = 90;       // target angle
    uint16_t current = 90;      // current angle
    uint8_t interval = 50;      // delay time
    uint32_t previousMillis = 0;
    using CallbackVU = void (*)(uint16_t value);  
    CallbackVU funcPtr;         // callback
  public:
    Servo servo;

    void attachTargetReached(CallbackVU funcPtr) {
      (*this).funcPtr = funcPtr;
    }

    void begin(uint8_t pin) {
      servo.attach(pin);
    }

    void setSpeed(uint8_t newSpeed) {
      interval = newSpeed;
    }

    void set(uint16_t newTarget) {
      target = newTarget;
    }

    uint16_t getCurrent() {
       return current;
    }

    void increase() {
       if (current < 180) target = ++current;
       servo.write(target);
    }

    void decrease() {
       if (current > 0) target = --current;
       servo.write(target);
    }

    void update() {
      if (millis() - previousMillis > interval) {
        previousMillis = millis();
        if (target < current) {
          current--;
          servo.write(current);
          if (funcPtr && target == current) funcPtr(current);
        }
        else if (target > current) {
          current++;
          servo.write(current);
          if (funcPtr && target == current) funcPtr(current);
        }
      }
    }
};

SlowServo myservo;  // create a servo object

void changeDirection(uint16_t current) {
  if (state == State::SWEEP) {
    if (current == targetLow) 
      myservo.set(targetHigh);
    else if (current == targetHigh) 
      myservo.set(targetLow); 
    }
}

void aClick() {
  switch (state) {
     case State::SWEEP: 
       state = State::PROG_LOW;
       myservo.set(targetLow);  // stop servo at the lower end
       Serial.println(F("PROG LOW"));
     break;
     case State::PROG_LOW : 
     case State::PROG_HIGH : 
       myservo.decrease();
       Serial.println(myservo.getCurrent());
     break;
  }
}

void bClick() {
  switch (state) {
     case State::SWEEP: 
       state = State::PROG_LOW;
       myservo.set(targetLow);  // stop servo at the lower end
       Serial.println(F("PROG LOW"));
     break;
     case State::PROG_LOW : 
     case State::PROG_HIGH : 
       myservo.increase();
        Serial.println(myservo.getCurrent());
     break;
  }
}

void aDoubleClick() {
  switch (state) {
     case State::SWEEP: 
       // pause?
     break;
     case State::PROG_LOW : 
       targetLow = myservo.getCurrent();
       myservo.set(targetHigh);     // go to the current end limit
       state = State::PROG_HIGH;
       Serial.print(F("targetLow=")); Serial.println(targetLow);
       Serial.println(F("PROG HIGH"));
     break;
  }
}

void bDoubleClick() {
  switch (state) {
     case State::SWEEP: 
       // pause?
     break;
     case State::PROG_HIGH : 
       targetHigh = myservo.getCurrent();
       myservo.set(targetLow);    // we are at the new higher end, so lets go down to low.
       state = State::SWEEP;
       Serial.print(F("targetHigh=")); Serial.println(targetHigh);
       Serial.println(F("SWEEP"));
     break;
  }
}

void setup() {
  Serial.begin(115200);
  myservo.begin(servoPin);            // start the servo object
  myservo.set(targetHigh);            // set a target at the begin
  myservo.setSpeed(10);
  myservo.attachTargetReached(changeDirection);  // what should be done at the end of a movement
  
  btnA.setup(buttonAPin, INPUT_PULLUP, true);
  btnB.setup(buttonBPin, INPUT_PULLUP, true);
  btnA.attachClick(aClick);
  btnB.attachClick(bClick);
  btnA.attachDoubleClick(aDoubleClick);
  btnB.attachDoubleClick(bDoubleClick);
}

void loop() {
  btnA.tick();
  btnB.tick();
  myservo.update();  // call the update method in loop
  
  
  // just debug print of state
  static int previousState = 0;
  if (previousState != (int)state ) {
    previousState = (int)state;
    Serial.print(F("state=")); Serial.println((int)state);
  }
}
//