#include "motion_planner.h"

XYMotionPlanner::XYMotionPlanner(FastAccelStepper* stepperX, FastAccelStepper* stepperY)
    : m_x(stepperX), m_y(stepperY) {}

void XYMotionPlanner::setBaseLimits(uint32_t maxSpeedHz, uint32_t maxAccelStepsPerS2) {
  m_baseSpeedHz = max<uint32_t>(1, maxSpeedHz);
  m_baseAccel = max<uint32_t>(1, maxAccelStepsPerS2);
}

void XYMotionPlanner::setMinimums(uint32_t minSpeedHz, uint32_t minAccelStepsPerS2) {
  m_minSpeedHz = minSpeedHz;
  m_minAccel = minAccelStepsPerS2;
}

void XYMotionPlanner::setCurrentPosition(int32_t x, int32_t y) {
  m_currentX = x;
  m_currentY = y;
  m_targetX = x;
  m_targetY = y;
  if (m_x) {
    m_x->setCurrentPosition(x);
  }
  if (m_y) {
    m_y->setCurrentPosition(y);
  }
}

int32_t XYMotionPlanner::currentX() const { return m_currentX; }

int32_t XYMotionPlanner::currentY() const { return m_currentY; }

bool XYMotionPlanner::isRunning() const {
  bool rx = m_x ? m_x->isRunning() : false;
  bool ry = m_y ? m_y->isRunning() : false;
  return rx || ry;
}

void XYMotionPlanner::waitUntilFinished() {
  while (isRunning()) {
    delay(1);
  }
  m_currentX = m_targetX;
  m_currentY = m_targetY;
}

bool XYMotionPlanner::moveTo(int32_t targetX, int32_t targetY) {
  if ((m_x == nullptr) || (m_y == nullptr)) {
    return false;
  }

  // Keep planner state in sync when the previous move already ended,
  // but update() has not been called by the caller yet.
  update();

  m_targetX = targetX;
  m_targetY = targetY;

  int32_t dxSigned = m_targetX - m_currentX;
  int32_t dySigned = m_targetY - m_currentY;

  uint32_t dx = (dxSigned >= 0) ? dxSigned : -dxSigned;
  uint32_t dy = (dySigned >= 0) ? dySigned : -dySigned;

  if ((dx == 0) && (dy == 0)) {
    return false;
  }

  uint32_t dmax = max(dx, dy);

  // Skalierungsfaktoren relativ zur dominanten Achse
  float sx = (float)dx / (float)dmax;
  float sy = (float)dy / (float)dmax;

  // Dominante Achse bekommt die Basiswerte
  uint32_t speedX = scaledValue(m_baseSpeedHz, sx, dx);
  uint32_t speedY = scaledValue(m_baseSpeedHz, sy, dy);
  uint32_t accelX = scaledValue(m_baseAccel, sx, dx);
  uint32_t accelY = scaledValue(m_baseAccel, sy, dy);

  // Kurze Achsen mit Bewegung sollen nicht auf 0 fallen
  if (dx > 0) {
    speedX = max(speedX, m_minSpeedHz);
    accelX = max(accelX, m_minAccel);
  }
  if (dy > 0) {
    speedY = max(speedY, m_minSpeedHz);
    accelY = max(accelY, m_minAccel);
  }

  // FastAccelStepper: speed in steps/s, accel in steps/s²
  if (dx > 0) {
    m_x->setSpeedInHz(speedX);
    m_x->setAcceleration(accelX);
  }
  if (dy > 0) {
    m_y->setSpeedInHz(speedY);
    m_y->setAcceleration(accelY);
  }

  // Bewegungen anstoßen
  // Reihenfolge ist hier meist ausreichend.
  // Für "noch synchroner" müsste man tiefer in die Queue-API gehen.
  if (dx > 0) {
    m_x->moveTo(m_targetX);
  }
  if (dy > 0) {
    m_y->moveTo(m_targetY);
  }

  return true;
}

bool XYMotionPlanner::moveBy(int32_t deltaX, int32_t deltaY) {
  update();

  // Chain relative moves reliably:
  // - if running, append relative to the current planned target
  // - if idle, use the current position
  const int32_t baseX = isRunning() ? m_targetX : m_currentX;
  const int32_t baseY = isRunning() ? m_targetY : m_currentY;
  return moveTo(baseX + deltaX, baseY + deltaY);
}

void XYMotionPlanner::update() {
  if (!isRunning()) {
    m_currentX = m_targetX;
    m_currentY = m_targetY;
  }
}

uint32_t XYMotionPlanner::scaledValue(uint32_t base, float scale, uint32_t distance) {
  if (distance == 0) {
    return 0;
  }
  float v = (float)base * scale;
  if (v < 1.0f) {
    v = 1.0f;
  }
  return (uint32_t)(v + 0.5f);
}
