#include "Servo.h"
#include "HCSR04.h"

using MillisType = decltype(millis());
using USDSensor = UltraSonicDistanceSensor;

namespace gc {
constexpr uint8_t EchoPin {A5};
constexpr uint8_t TrigPin {A4};
constexpr uint8_t ServoPin {10};
constexpr MillisType SensorInterval {2000};
}   // namespace gc

class Timer {
public:
  void start() { timeStamp = millis(); }
  bool operator()(const MillisType duration) const { return (millis() - timeStamp >= duration) ? true : false; }

private:
  MillisType timeStamp {0};
};

class ServoInterface : public Servo {
public:
  virtual void operator()() = 0;
  virtual void operator()(uint8_t) = 0;
  virtual uint8_t getIndex() const = 0;
};

template <uint8_t Pin> class ServoCtrl : public ServoInterface {
public:
  template <size_t N> ServoCtrl(const uint8_t (&PosTable)[N]) : PosTable {PosTable}, TableElements {N} {}
  void begin() { this->attach(Pin); }

  virtual void operator()() override {
    this->write(PosTable[Index]);
    Index = (Index + 1) % TableElements;
  }

  virtual void operator()(uint8_t idx) override {
    if (idx < TableElements) {
      Index = idx;
      this->write(PosTable[Index]);
    }
  }

  virtual uint8_t getIndex() const override { return Index; }

private:
  const uint8_t* PosTable;
  const uint8_t TableElements;
  uint8_t Index {0};
};

// constexpr uint8_t ServoPositions[] {30, 70, 110, 150, 110, 70};
constexpr uint8_t ServoPositions[] {30, 70, 110, 150};

ServoCtrl<gc::ServoPin> SonarServo(ServoPositions);
USDSensor DistanceSensor(gc::TrigPin, gc::EchoPin);
Timer SensorTimer;

unsigned int checkDistance(ServoInterface& SCtrl, USDSensor& DSensor) {
  SCtrl();
  return DSensor.measureDistanceCm();
}

void setup() {
  Serial.begin(115200);
  Serial.println("Start Program and wait a little...");
  SonarServo.begin();
  delay(5000);
  Serial.println("Go...");
}

void loop() {
  if (SensorTimer(gc::SensorInterval)) {
     Serial.print("Index: "); Serial.print(SonarServo.getIndex()); Serial.print(" Distance: ");
     unsigned Distance = checkDistance(SonarServo, DistanceSensor);
    Serial.println(Distance);
    SensorTimer.start();
  }
}