#include <Arduino.h>
#include <FastAccelStepper.h>

class XYMotionPlanner {
public:
  XYMotionPlanner(FastAccelStepper* stepperX, FastAccelStepper* stepperY);

  // Basisparameter der "dominanten" Achse
  void setBaseLimits(uint32_t maxSpeedHz, uint32_t maxAccelStepsPerS2);

  // Minimale Werte vermeiden, dass eine kurze Achse mit 0 endet
  void setMinimums(uint32_t minSpeedHz, uint32_t minAccelStepsPerS2);

  // Aktuelle Positionen der Planner-Sicht setzen, z.B. nach Homing
  void setCurrentPosition(int32_t x, int32_t y);

  int32_t currentX() const;
  int32_t currentY() const;

  bool isRunning() const;

  // Optional blockierend warten
  void waitUntilFinished();

  // Startet eine koordinierte lineare P2P-Bewegung
  // Rückgabe false = nichts zu tun oder ungültig
  bool moveTo(int32_t targetX, int32_t targetY);

  // Convenience: relative Bewegung
  bool moveBy(int32_t deltaX, int32_t deltaY);

  // Muss regelmäßig aufgerufen werden, wenn du die Planner-Position
  // nach abgeschlossener Bewegung sauber nachführen willst
  void update();

private:
  FastAccelStepper* m_x = nullptr;
  FastAccelStepper* m_y = nullptr;

  int32_t m_currentX = 0;
  int32_t m_currentY = 0;
  int32_t m_targetX = 0;
  int32_t m_targetY = 0;

  uint32_t m_baseSpeedHz = 2000;
  uint32_t m_baseAccel = 4000;
  uint32_t m_minSpeedHz = 1;
  uint32_t m_minAccel = 1;

  static uint32_t scaledValue(uint32_t base, float scale, uint32_t distance);
};