#include <Arduino.h>
#include <FastAccelStepper.h>
#include "config.h"
#include "Machine.h"
#include "Commands.h"

FastAccelStepperEngine engine;
Machine machine(engine);
Commands commands(machine);

static String rxLine;
static uint32_t lastRxMs = 0;

void setup() {
  Serial.begin(PC_BAUD);
  delay(200);

  machine.begin();

  Serial.println();
  Serial.printf("# Stepper-test klar - %u akser\n", NUM_AXES);
  uint8_t found = 0;
  for (uint8_t i = 0; i < NUM_AXES; i++) {
    Axis& a = machine.axis(i);
    found += a.uartOk();
    Serial.printf("# a%u (bus %c, adr %u, STEP %u, DIR %u): %s\n", i, 'A' + a.config().bus,
                  a.config().address, a.config().stepPin, a.config().dirPin,
                  a.uartOk() ? "TMC2209 fundet" : "ingen driver");
  }
  if (!found) Serial.println("# Ingen drivere fundet - er VM (12/24 V) tændt? Prøver igen løbende.");
  Serial.printf("# DIAG: %s på GPIO%u\n",
                DIAG_MODE == DiagMode::DiodeOr ? "diode-OR (digital)" : "modstande + ADC", PIN_DIAG);
  Serial.println("# Skriv 'help' for kommandoer");
  Serial.println(commands.handle("status"));
}

void loop() {
  // ---- Læs kommandoer fra PC'en (ikke-blokerende) ----
  while (Serial.available()) {
    char c = (char)Serial.read();
    lastRxMs = millis();
    if (c == '\n' || c == '\r') {
      if (rxLine.length()) {
        String reply = commands.handle(rxLine);
        if (reply.length()) Serial.println(reply);
        rxLine = "";
      }
    } else if (rxLine.length() < 80) {
      rxLine += c;
    }
  }

  // ---- Stall, homing, driver-tjek og live-status ----
  machine.poll(Serial);

  // ---- Watchdog: kun aktiv når PC-programmet sender "hb" ----
  uint32_t now = millis();
  if (commands.heartbeatArmed() && now - lastRxMs > WATCHDOG_TIMEOUT_MS && machine.anyRunning()) {
    machine.stopAll();
    Serial.println("# watchdog: ingen heartbeat fra PC - blød stop af alle akser");
    lastRxMs = now;  // undgå at gentage beskeden hver loop
  }
}