#include "Commands.h"
#include "config.h"

// Parser et heltal; returnerer false hvis teksten ikke er et gyldigt tal.
static bool parseLong(const String& s, long& out) {
  if (s.length() == 0) return false;
  char* end = nullptr;
  out = strtol(s.c_str(), &end, 10);
  return *end == '\0';
}

static bool parseFloat(const String& s, float& out) {
  if (s.length() == 0) return false;
  char* end = nullptr;
  out = strtof(s.c_str(), &end);
  return *end == '\0';
}

static bool isDigits(const String& s) {
  if (s.length() == 0) return false;
  for (size_t i = 0; i < s.length(); i++) {
    if (!isdigit((unsigned char)s[i])) return false;
  }
  return true;
}

String Commands::handle(const String& raw) {
  String line = raw;
  line.trim();
  if (line.length() == 0) return "";

  // ---- Valgfrit akse-index først ----
  int axisIdx = -1;
  int sp = line.indexOf(' ');
  String first = sp < 0 ? line : line.substring(0, sp);
  if (isDigits(first)) {
    axisIdx = first.toInt();
    if (axisIdx >= NUM_AXES) return "err ukendt akse " + first + " (0-" + String(NUM_AXES - 1) + ")";
    line = sp < 0 ? String() : line.substring(sp + 1);
    line.trim();
    if (line.length() == 0) return "err mangler kommando efter akse-index";
    sp = line.indexOf(' ');
  }

  String cmd = sp < 0 ? line : line.substring(0, sp);
  String arg = sp < 0 ? String() : line.substring(sp + 1);
  cmd.toLowerCase();
  arg.trim();

  // ---- Fælles kommandoer ----
  if (cmd == "hb") {
    _hbArmed = true;
    return "";
  }
  if (cmd == "help") {
    return "ok kommandoer: [akse] <kommando>. Fælles: status estop, og uden akse: en dis stop (alle). "
           "Pr. akse: en dis stop move <steps> goto <steps> rev <omdr> speed <steps/s> accel <steps/s2> "
           "zero current <mA> mode stealth|spread uartdiag "
           "sgt <0-255> sgauto on|off sgoff <+-n> sg on|off sgr home +|-";
  }
  if (cmd == "status") {
    String out = "G " + _m.globalJson();
    for (uint8_t i = 0; i < NUM_AXES; i++) out += "\nS " + String(i) + " " + _m.axis(i).statusJson();
    return out;
  }
  if (cmd == "estop") { _m.estop(); return "ok estop (alle drivere disabled)"; }

  if (axisIdx < 0) {
    if (cmd == "en") {
      const char* err = _m.enableAll();
      return err ? String("err ") + err : String("ok alle fundne akser enabled");
    }
    if (cmd == "dis") { _m.disableAll(); return "ok alle akser disabled"; }
    if (cmd == "stop") { _m.stopAll(); return "ok stop alle"; }
  }

  // ---- Akse-kommandoer (uden index: akse 0) ----
  uint8_t ai = axisIdx < 0 ? 0 : axisIdx;
  Axis& a = _m.axis(ai);
  _m.requestStatus(ai);   // ny S-linje efter kommandoen, så UI'et altid er opdateret
  String pre = "a" + String(ai) + " ";
  auto okOrErr = [&](const char* err, const String& okText) {
    return err ? "err " + pre + err : "ok " + pre + okText;
  };

  if (cmd == "en") return okOrErr(_m.enableAxis(ai), "enabled");
  if (cmd == "dis") { _m.disableAxis(ai); return "ok " + pre + "disabled"; }
  if (cmd == "stop") { a.stop(); return "ok " + pre + "stop"; }
  if (cmd == "zero") return okOrErr(a.setZero(), "pos=0");
  if (cmd == "uartdiag") return a.uartDiag();

  long n;
  float f;
  if (cmd == "move") {
    if (!parseLong(arg, n)) return "err brug: [akse] move <steps>";
    return okOrErr(a.move(n), "move " + String(n));
  }
  if (cmd == "goto") {
    if (!parseLong(arg, n)) return "err brug: [akse] goto <steps>";
    return okOrErr(a.moveTo(n), "goto " + String(n));
  }
  if (cmd == "rev") {
    if (!parseFloat(arg, f)) return "err brug: [akse] rev <omdrejninger>";
    long steps = lroundf(f * STEPS_PER_REV);
    return okOrErr(a.move(steps), "move " + String(steps));
  }
  if (cmd == "speed") {
    if (!parseLong(arg, n) || n <= 0) return "err brug: [akse] speed <steps/s>";
    a.setSpeed(n);
    return "ok " + pre + "speed=" + String(a.speed()) + " sgt=" + String(a.sgThreshold());
  }
  if (cmd == "accel") {
    if (!parseLong(arg, n) || n <= 0) return "err brug: [akse] accel <steps/s2>";
    return "ok " + pre + "accel=" + String(a.setAccel(n));
  }
  if (cmd == "current") {
    if (!parseLong(arg, n) || n <= 0) return "err brug: [akse] current <mA>";
    return "ok " + pre + "current=" + String(a.setCurrent(n > 65535 ? 65535 : n));
  }
  if (cmd == "sgt") {
    if (!parseLong(arg, n) || n < 0 || n > 255) return "err brug: [akse] sgt <0-255>";
    a.setSgThreshold(n);
    return "ok " + pre + "sgt=" + String(n) + " manuel (stall når sg <= " + String(2 * n) + ")";
  }
  if (cmd == "sgauto") {
    arg.toLowerCase();
    if (arg != "on" && arg != "off") return "err brug: [akse] sgauto on|off";
    a.setSgAuto(arg == "on");
    return "ok " + pre + "sgauto=" + arg + " sgt=" + String(a.sgThreshold());
  }
  if (cmd == "sgoff") {
    if (!parseLong(arg, n)) return "err brug: [akse] sgoff <-100..100>";
    int16_t off = a.setSgOffset(constrain(n, -1000L, 1000L));
    return "ok " + pre + "sgoff=" + String(off) + " sgt=" + String(a.sgThreshold());
  }
  if (cmd == "sg") {
    arg.toLowerCase();
    if (arg == "on") return okOrErr(a.setStallStop(true), "stall-stop til");
    if (arg == "off") { a.setStallStop(false); return "ok " + pre + "stall-stop fra"; }
    return "err brug: [akse] sg on|off";
  }
  if (cmd == "sgr") return "ok " + pre + "sg=" + String(a.sgResult());
  if (cmd == "home") {
    if (arg == "+") return okOrErr(_m.home(ai, 1), "homing +");
    if (arg == "-") return okOrErr(_m.home(ai, -1), "homing -");
    return "err brug: [akse] home +|-";
  }
  if (cmd == "mode") {
    arg.toLowerCase();
    if (arg == "stealth") { a.setStealthChop(true); return "ok " + pre + "mode=stealth"; }
    if (arg == "spread") { a.setStealthChop(false); return "ok " + pre + "mode=spread"; }
    return "err brug: [akse] mode stealth|spread";
  }
  return "err ukendt kommando '" + cmd + "' (prøv help)";
}