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

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

namespace gc {
constexpr uint8_t EchoPin {13};
constexpr uint8_t TrigPin {12};
constexpr uint8_t ServoPin {10};

constexpr uint8_t ServoPosMin {15};
constexpr uint8_t ServoPosMax {165};
constexpr uint8_t ServoStepSize {5};
constexpr MillisType ServoDelay_ms {100};   // in ms
}   // namespace gc

//
// Class for controlling a servo (Sonar)
//
class SonarInterface {
public:
  virtual byte operator()() = 0;
  virtual void stop() = 0;
  virtual void start() = 0;
  virtual bool isStopped() const = 0;
  virtual byte pos() const = 0;
  virtual byte prevPos() const = 0;
  virtual void end() = 0;
};

template <byte PosMin, byte PosMax, byte StartPos, MillisType ServoDelay_ms> class SonarServo : public SonarInterface {
  static_assert(PosMin < PosMax, "Min. Position must be smaller than Max. Position!");
  static_assert(PosMin < 181 || PosMax < 181 || StartPos < 181, "Max. Position is 180 Degree!");

public:
  SonarServo(Servo& Sv, int Step) : Sv {Sv}, Step {Step}, Pos {StartPos} {
    auto StepMax = PosMax - PosMin;
    if (StepMax < abs(Step)) { this->Step = (Step < 0) ? -StepMax : StepMax; }
  }

  void begin(byte Pin) {
    Sv.attach(Pin);
    Sv.write(Pos);
    Timestamp = millis();
  }

  virtual byte operator()() override {
    if (!Stopped && millis() - Timestamp >= ServoDelay_ms) {
      Timestamp = millis();
      if (Pos <= PosMin || Pos >= PosMax) { Step = (Pos <= PosMin) ? abs(Step) : -Step; }
      PrevPos = Pos;
      Pos += Step;
      Sv.write(Pos);
    }
    return Pos;
  }

  void stop() override {
    Stopped = true;
    PrevPos = Pos;
  };
  void start() override { Stopped = false; };
  bool isStopped() const override { return Stopped; };
  byte pos() const override { return Sv.read(); }
  byte prevPos() const override { return PrevPos; }
  void end() override { Sv.detach(); }

private:
  Servo& Sv;
  int Step;
  MillisType Timestamp;
  byte Pos;
  byte PrevPos;
  bool Stopped {true};
};

// 1,3 Zoll SH1106
using DisplayType = U8G2_SH1106_128X64_NONAME_1_HW_I2C;

DisplayType Oled(U8G2_R0);
USDSensor DistanceSensor(gc::TrigPin, gc::EchoPin);
Servo ServoMotor;
SonarServo<gc::ServoPosMin, gc::ServoPosMax, gc::ServoPosMin, gc::ServoDelay_ms> SonarCtrl(ServoMotor,
                                                                                           gc::ServoStepSize);

void drawRadar(DisplayType& disp, const char* text) {
  disp.firstPage();
  do {
    disp.setFont(u8g2_font_helvB12_tf);   // 12 px
    disp.drawRFrame(0, 0, 126, 63, 3);    // Rahmen
    disp.drawStr(2, 15, "Sonar:");
    disp.drawLine(3, 18, 51, 18);         // Unterstrich von "Sonar"
    disp.drawFrame(75, 3, 46, 15);        // Rechteck cm anzeige
    disp.setFont(u8g2_font_ncenB08_tr);
    disp.drawStr(102, 15, "cm");
    disp.drawStr(99 - disp.getStrWidth(text), 15, text);

    constexpr int radius[] {14, 24, 34};
    for (const auto& r : radius) {
      disp.drawCircle(65, 62, r, U8G2_DRAW_UPPER_RIGHT | U8G2_DRAW_UPPER_LEFT);   // Halbkreise
    }
    disp.drawLine(64, 62, 64, 25);                                                // Mittlere Linie
    disp.drawLine(64, 62, 36, 36);                                                // Linke Linie
    disp.drawLine(64, 62, 92, 36);                                                // Rechte Linie

  } while (disp.nextPage());
}

void checkDistance(SonarInterface& SonarSv) {
  static int PrevDistance {0};

  SonarSv();
  int Distance = DistanceSensor.measureDistanceCm();
  if (PrevDistance != Distance) {
    PrevDistance = Distance;
    char buffer[7] {"0"};
    snprintf(buffer, sizeof(buffer), "%3d", Distance);
    drawRadar(Oled, buffer);
  }
}

void setup() {
  Serial.begin(115200);

  Oled.begin();
  Oled.clearBuffer();
  Oled.setDrawColor(1);   // Farbe weiß
  drawRadar(Oled, "0");

  SonarCtrl.begin(gc::ServoPin);
  SonarCtrl.start();
}

void loop() { checkDistance(SonarCtrl); }