#include <Streaming.h>

//
// Typdefinition für Millis
//
using Millis_t = decltype(millis());

//
// Globale Konstanten
//
// Definition Hallpins und Hallstatus
namespace gc {
constexpr uint8_t HallSensorPinA {2};
constexpr uint8_t HallSensorPinB {3};
constexpr uint8_t RelayPinA {11};
constexpr uint8_t RelayPinB {12};

constexpr uint8_t LedPinA {A4};   // Kontroll_Leds für Richtungsanzeige
constexpr uint8_t LedPinB {A5};
constexpr uint8_t BlinkLedPin {4};

constexpr Millis_t SpeedDelay {150};    // Wartezeit zwischen den Brems- / Beschleunigungsschritten
constexpr Millis_t RelayDelay {1000};   // Wartezeit für Relaisschaltung
constexpr Millis_t LongDelay {3000};    // Wartezeit vor Wechsel in andere Richtung
constexpr Millis_t BlinkDelay {500};    // Blinkinterval
}   // namespace gc

//
// Klassendefintionen
//
class Timer {
public:
  Timer() : TimeStamp(0), Reached(true) {}

  void start() {
    TimeStamp = millis();
    Reached = false;
  }
  void reset() { Reached = true; }

  bool operator()(const Millis_t Duration) {
    if (!Reached) { Reached = millis() - TimeStamp >= Duration; }
    return Reached;
  }

private:
  Millis_t TimeStamp;
  bool Reached;
};

enum class EngineState : uint8_t { Neutral, Wait, Accelerate, SlowDown };
enum class ControlState : uint8_t {
  Neutral,
  InitSlowDown,
  EngineSlowDown,
  SwitchRelay,
  LongDelay,
  InitAccelerate,
  EngineAccelerate
};

//
// Datentyp Engine_t bauen
//
struct Engine_t {
  uint8_t PwmPin;
  uint8_t HBridgePinA;
  uint8_t HBridgePinB;
  EngineState State;
  EngineState LastState;
  int MaxSpeed;       // Maximale Geschwindigkeit
  int CurrentSpeed;   // Aktuelle Geschwindigkeit
  int Step;           // Schrittweite für Geschwindigkeitsänderung
  char Name[8];
};

//
// Globale Variablen/Objekte
//
Print& cout {Serial};                                                                     // Streaming Ausgabe
Engine_t EngineTrainA {10, 9, 8, EngineState::Neutral, EngineState::Neutral, 80, 0, 2, "Zug A"};   // Steht (Speed 0)
Engine_t EngineTrainB {5, 7, 6, EngineState::Neutral, EngineState::Neutral, 80, 80, 2, "Zug B"};   // Fährt (Speed 80)
Timer WaitTimer;
Timer BlinkTimer;

// H-Brücke Schalten
void switchHBridge(uint8_t OutPinA, uint8_t OutPinB, uint8_t PwmPin, uint8_t LevelA, uint8_t LevelB, int PWMValue) {
  cout << PWMValue << "\r\n";
  digitalWrite(OutPinA, LevelA);
  digitalWrite(OutPinB, LevelB);
  analogWrite(PwmPin, PWMValue);
}

//
// Motor abbremsen oder beschleunigen. Nicht blockierende Wartezeit zwischen den Brems-/Beschleunigungsschritten
//
bool engineControl(Engine_t& Engine, uint8_t LevelOutA, uint8_t LevelOutB, Timer& Wait, Millis_t Wait_ms) {
  switch (Engine.State) {
    case EngineState::SlowDown:
      Engine.LastState = EngineState::SlowDown;
      if (Engine.CurrentSpeed >= Engine.Step) {
        cout << F("Bremse ") << Engine.Name << F(" ab. Speed: ");
        Engine.CurrentSpeed -= Engine.Step;
        // H-Brücke schalten - Motor bremsen
        switchHBridge(Engine.HBridgePinA, Engine.HBridgePinB, Engine.PwmPin, LevelOutA, LevelOutB, Engine.CurrentSpeed);
        Engine.State = EngineState::Wait;
      } else {
        Engine.State = EngineState::Neutral;   // Bremsvorgang abgeschlossen
        return true;
      }
      break;
    case EngineState::Accelerate:
      Engine.LastState = EngineState::Accelerate;
      if (Engine.CurrentSpeed < Engine.MaxSpeed) {
        Engine.CurrentSpeed += Engine.Step;
        cout << F("Beschleunige ") << Engine.Name << F(". Speed: ");
        // H-Brücke schalten - Motor beschleunigt
        switchHBridge(Engine.HBridgePinA, Engine.HBridgePinB, Engine.PwmPin, LevelOutA, LevelOutB, Engine.CurrentSpeed);
        Engine.State = EngineState::Wait;
      } else {
        Engine.State = EngineState::Neutral;   // Beschleunigung abgeschlossen
        return true;
      }
      break;
    case EngineState::Wait:
      if (Wait(Wait_ms)) { 
        Wait.start();
        Engine.State = Engine.LastState; 
      }
      break;
    default: break;
  }
  return false;
}

//
// Löst der angegebene Hallsensor aus, wird ein Zug B abgebremst und Zug A beschleunigt
// Für die umgekehrte Reihenfolge müssen die Engine_t Parameter vertauscht und die zum Hallsensor
// passende ControlState-Variable übergeben werden.
//
void hall(Engine_t& EngineA, Engine_t& EngineB, ControlState& State) {
  switch (State) {
    case ControlState::InitSlowDown:
      if (EngineB.CurrentSpeed < EngineB.Step) {
        State = ControlState::Neutral;
        break;
      }
      digitalWrite(gc::LedPinB, LOW);
      EngineB.State = EngineState::SlowDown;
      cout << F("Leite Bremsen ein\r\n");
      State = ControlState::EngineSlowDown;
      [[fallthrough]];
    case ControlState::EngineSlowDown:
      if (engineControl(EngineB, LOW, HIGH, WaitTimer, gc::SpeedDelay)) {   // Zug B abbremsen
        WaitTimer.start();
        cout << F("Öffne Relais 1\r\n");
        digitalWrite(gc::RelayPinA, LOW);   // Relaiskontakt 1 kurz schließen wenn Bremsen abgeschlossen
        State = ControlState::SwitchRelay;
      }
      break;
    case ControlState::SwitchRelay:
      if (WaitTimer(gc::RelayDelay)) {
        WaitTimer.start();
        cout << F("Schließe Relais 1\r\n");
        digitalWrite(gc::RelayPinA, HIGH);
        State = ControlState::LongDelay;
      }
      break;
    case ControlState::LongDelay:
      if (WaitTimer(gc::LongDelay)) { State = ControlState::InitAccelerate; }
      break;
    case ControlState::InitAccelerate:
      if (EngineA.CurrentSpeed >= EngineA.MaxSpeed) {
        State = ControlState::Neutral;
        break;
      }
      cout << F("Initialisiere Beschleunigen des anderen Zuges\r\n");
      digitalWrite(gc::LedPinA, HIGH);
      EngineA.State = EngineState::Accelerate;
      State = ControlState::EngineAccelerate;
      [[fallthrough]];
    case ControlState::EngineAccelerate:
      if (engineControl(EngineA, HIGH, LOW, WaitTimer, gc::SpeedDelay)) {   // Zug A beschleunigen
        State = ControlState::Neutral;
      }
      break;
    default: break;
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(EngineTrainA.PwmPin, OUTPUT);   // Definition der Pins als Aus-/eingabe
  pinMode(EngineTrainB.PwmPin, OUTPUT);
  pinMode(EngineTrainA.HBridgePinA, OUTPUT);
  pinMode(EngineTrainA.HBridgePinB, OUTPUT);
  pinMode(EngineTrainB.HBridgePinA, OUTPUT);
  pinMode(EngineTrainB.HBridgePinB, OUTPUT);
  pinMode(gc::RelayPinA, OUTPUT);
  pinMode(gc::RelayPinB, OUTPUT);
  pinMode(gc::LedPinA, OUTPUT);
  pinMode(gc::LedPinB, OUTPUT);
  pinMode(gc::BlinkLedPin, OUTPUT);
  pinMode(gc::HallSensorPinA, INPUT);
  pinMode(gc::HallSensorPinB, INPUT);

  // delay(gc::LongDelay);   // Pause vor erstem Start nach links
  // digitalWrite(gc::LedPinA, HIGH);
  // EngineTrainA.State = EngineState::Accelerate;
  // while(!engineControl(EngineTrainA, HIGH, LOW, WaitTimer, 50));   // Zug A beschleunigen
}

//=============================
void loop() {
  static ControlState HallState1A {ControlState::Neutral};
  static ControlState HallState1B {ControlState::Neutral};

  if (HallState1A == ControlState::Neutral && HallState1B == ControlState::Neutral) {
    if (digitalRead(gc::HallSensorPinA) == LOW) {
      HallState1A = ControlState::InitSlowDown;
    } else if (digitalRead(gc::HallSensorPinB) == LOW) {
      HallState1B = ControlState::InitSlowDown;
    }
  }
  hall(EngineTrainA, EngineTrainB, HallState1A);
  hall(EngineTrainB, EngineTrainA, HallState1B);

  if (BlinkTimer(gc::BlinkDelay)) {
    BlinkTimer.start();
    digitalWrite(gc::BlinkLedPin, !digitalRead(gc::BlinkLedPin));
  }
}