// https://forum.arduino.cc/t/sketch-fur-servo-mit-3-potis-drehgeber-lcd-verbesserungsvorschlage/1385507

// This code is orientated to the llvm coding standard
// https://llvm.org/docs/CodingStandards.html

#include <Servo.h>               // https://github.com/arduino-libraries/Servo
#include <LiquidCrystal_I2C.h>   // https://github.com/johnrickman/LiquidCrystal_I2C
#include <RotaryEncoder.h>       // https://github.com/mathertel/RotaryEncoder
#include <Button_SL.hpp>         // https://github.com/DoImant/Button_SL

//
// Global constants (gc)
//
namespace gc {
constexpr uint8_t LcdCols {16};
constexpr uint8_t LcdRows {2};

constexpr uint8_t EncPinClk {6};
constexpr uint8_t EncPinDt {3};
constexpr uint8_t EncPinSw {7};   // Button of encoder
constexpr uint8_t ServoPin {8};

constexpr int CounterStart {5};
constexpr int CounterMax {99};

constexpr int HomePosition {90};
constexpr int LimitLowest {50};
constexpr int LimitHighest {50};
constexpr int LimitDelay_ms {50};                 // Maximum delay in milliceconds
constexpr int DelayMin_ms {5};                    // Minimum Delay for servo movement in milliseconds;

constexpr int AnalogResolution {(1 << 10) - 1};   // 10 Bit
}   // namespace gc

using MillisType = decltype(millis());

//
// Class definitions
//
//////////////////////////////////////////////////////////////////////////////
/// \brief Helperclass for a non blocking delay
///
//////////////////////////////////////////////////////////////////////////////
class NbDelay {
public:
  void start() { timestamp = millis(); }
  boolean operator()(const MillisType duration) { return millis() - timestamp >= duration; }

private:
  MillisType timestamp {0};
};

//
// Data definitions
//
enum class PotiName : uint8_t { Lowest, Highest, Delay };
enum class State : uint8_t { Input, Run };
enum class ServoState : uint8_t { InitN, InitP, RunN, RunP };

// Return enumerators of class <T> as number
template <typename T> constexpr uint8_t toIndex(T Enumerator) noexcept { return static_cast<uint8_t>(Enumerator); }

struct PotiData {
  const uint8_t Pin;
  const int MaxValue;
  int Value;
};

//
// Global objects / variables
//

// {PinNr, Max. possible Value, measured Value}
PotiData Potis[] {
    {A1, gc::LimitLowest,   0}, // lowest
    {A0, gc::LimitHighest,  0}, // highest
    {A2, gc::LimitDelay_ms, 0}  // speed delay
};

LiquidCrystal_I2C Lcd = LiquidCrystal_I2C(0x27, gc::LcdCols, gc::LcdRows);
RotaryEncoder Encoder {gc::EncPinClk, gc::EncPinDt, RotaryEncoder::LatchMode::FOUR3};
Servo MyServoDev;
Btn::ButtonSL Button {gc::EncPinSw};
NbDelay Wait;

//
// Functions
//

//////////////////////////////////////////////////////////////////////////////
/// \brief Get the Encoder Value object
///
/// \param Enc        Reference to Encoder Object
/// \param Value      Value to be set using the encoder (reference)
/// \param ValueMax   Maximum value that can be set
/// \return true      Value has been changed
/// \return false     No change
//////////////////////////////////////////////////////////////////////////////
bool getEncoderValue(RotaryEncoder& Enc, int& Value, const int ValueMax) {
  int SaveValue = Value;
  Enc.tick();
  switch (Enc.getDirection()) {
    case RotaryEncoder::Direction::CLOCKWISE: Value = (Value + 1) % ValueMax; break;
    case RotaryEncoder::Direction::COUNTERCLOCKWISE: Value = (Value != 0) ? Value - 1 : ValueMax; break;
    case RotaryEncoder::Direction::NOROTATION: break;
  }
  return (Value != SaveValue);   // true if the Value has been changed.
}

//////////////////////////////////////////////////////////////////////////////
/// \brief Display data
///
/// \param Disp Reference to Display Object
/// \param PD   Pointer to Array of Datasructure (analog values)
/// \param Cnt  Value of a counter to be displayed
//////////////////////////////////////////////////////////////////////////////
void dispPrint(LiquidCrystal_I2C& Disp, PotiData* PD, int Cnt) {
  char buffer[gc::LcdCols + 1];
  snprintf(buffer, sizeof(buffer), "L: %2d  R: %2d", PD[toIndex(PotiName::Lowest)].Value,
           PD[toIndex(PotiName::Highest)].Value);
  Disp.setCursor(0, 0);
  Disp.print(buffer);
  snprintf(buffer, sizeof(buffer), "S: %2d  C: %2d", gc::LimitDelay_ms - PD[toIndex(PotiName::Delay)].Value, Cnt);
  Disp.setCursor(0, 1);
  Disp.print(buffer);
}

//////////////////////////////////////////////////////////////////////////////
/// \brief Reading out the analog values of a set of potentiometers.
///
/// \tparam (&PD)[N]  Reference to Structure array and number of elements [N]
/// \return true      At least one value has changed
/// \return false     No changes
//////////////////////////////////////////////////////////////////////////////
template <size_t N> bool getPotiValues(PotiData (&PD)[N]) {
  decltype(PD[0].Value) SavedValue[N];
  for (size_t i = 0; i < N; ++i) {
    SavedValue[i] = PD[i].Value;
    PD[i].Value = map(analogRead(PD[i].Pin), 0, gc::AnalogResolution, 0, PD[i].MaxValue);
  }
  // Check whether one of the read analog values has changed.
  for (size_t i = 0; i < N; ++i) {
    if (SavedValue[i] != PD[i].Value) { return true; }   // Yes. At leased one value has been changed
  }
  return false;
}

//////////////////////////////////////////////////////////////////////////////
/// \brief Non blocking servo move
///
/// \param PD       Pointer to Array of Datasructure (analog values)
/// \param MyServo  Reference to servo object
/// \param State    Reference to status variable for the servo movement.
/// \return true    when movement sequence is finished
/// \return false   not finnished yet
//////////////////////////////////////////////////////////////////////////////
bool runMyServo(PotiData* PD, Servo& MyServo, ServoState& State) {
  constexpr uint8_t Highest {toIndex(PotiName::Highest)};
  constexpr uint8_t Lowest {toIndex(PotiName::Lowest)};
  constexpr uint8_t Delay {toIndex(PotiName::Delay)};

  static int Pos {0};
  static int Threshold {0};

  switch (State) {
    case ServoState::InitN:
      digitalWrite(LED_BUILTIN, HIGH);
      Pos = (gc::HomePosition - PD[Lowest].Value);
      Threshold = gc::HomePosition + PD[Highest].Value;
      State = ServoState::RunN;
      [[fallthrough]];
    case ServoState::RunN:
      if (Wait(PD[Delay].Value + gc::DelayMin_ms)) {
        if (Pos < Threshold) {
          Wait.start();
          ++Pos;
          MyServo.write(Pos);
        } else {
          State = ServoState::InitP;
        }
      }
      break;
    case ServoState::InitP:
      digitalWrite(LED_BUILTIN, LOW);
      Threshold = gc::HomePosition - Potis[Lowest].Value;
      State = ServoState::RunP;
      [[fallthrough]];
    case ServoState::RunP:
      if (Wait(PD[Delay].Value + gc::DelayMin_ms)) {
        if (Pos > Threshold) {
          Wait.start();
          --Pos;
          MyServo.write(Pos);
        } else {
          State = ServoState::InitN;
          return true;
        }
      }
      break;
  }
  return false;
}

//
// Main Program
//
void setup() {
  // Serial.begin(115200);
  MyServoDev.attach(gc::ServoPin);   // attaches the servo on pin 8 to the servo object
  pinMode(LED_BUILTIN, OUTPUT);

  Button.begin();
  Button.setDebounceTime_ms(30);
  Lcd.init();
  Lcd.backlight();

  getPotiValues(Potis);
  dispPrint(Lcd, Potis, gc::CounterStart);
}

void loop() {
  static State CaseState {State::Input};
  static ServoState MyServoState {ServoState::InitN};

  static int Counter {gc::CounterStart};   // Run Counter (Set via rotary encoder)
  static int SaveCounter {Counter};

  switch (CaseState) {
    case State::Input:
      // Returns true if at least one analog value or counter has changed. If so display new Value(s).
      if (getPotiValues(Potis) || getEncoderValue(Encoder, Counter, gc::CounterMax)) { dispPrint(Lcd, Potis, Counter); }
      if (Button.tick() != Btn::ButtonState::notPressed) {
        // Do only run if both values aren't zero.
        if ((Potis[toIndex(PotiName::Lowest)].Value + Potis[toIndex(PotiName::Highest)].Value) && Counter > 0) {
          CaseState = State::Run;
          SaveCounter = Counter;
        }
      }
      break;
    case State::Run:
      if (runMyServo(Potis, MyServoDev, MyServoState)) {
        --Counter;
        dispPrint(Lcd, Potis, Counter);
      }
      if (Counter < 1 || Button.tick() != Btn::ButtonState::notPressed) {   // leave State::Run if true
        MyServoDev.write(gc::HomePosition);
        Counter = SaveCounter;
        CaseState = State::Input;
        MyServoState = ServoState::InitN;
        dispPrint(Lcd, Potis, Counter);
      }
      break;
  }
}