// Motion planning: jerk-limited body velocities, a speed-dependent steering
// curve, and closed-loop distance / rotation moves driven by wheel odometry.
#pragma once

#include <Arduino.h>
#include "robot_config.h"

// Wheel travel accumulated from the firmware's hall tick counters.
class Odometry {
public:
  void reset();                                  // zero the accumulators
  void update(int16_t rawLeft, int16_t rawRight);

  double left()  const { return left_m_; }
  double right() const { return right_m_; }
  double distance() const { return 0.5 * (left_m_ + right_m_); }          // metres
  double heading()  const { return (right_m_ - left_m_) / TRACK_WIDTH_M; } // radians, CCW+

private:
  bool    started_ = false;
  int16_t prevL_ = 0, prevR_ = 0;
  double  left_m_ = 0, right_m_ = 0;
};

// One axis of a jerk-limited profile: acceleration itself is rate-limited, so
// the command has no step in its second derivative and the load never lurches.
class JerkAxis {
public:
  JerkAxis(float vmax, float amax, float jmax) : vmax_(vmax), amax_(amax), jmax_(jmax) {}

  void  update(float target, float dt);
  void  hardZero() { v_ = 0; a_ = 0; }
  void  setVMax(float vmax) { vmax_ = vmax; }
  void  setLimits(float amax, float jmax, float amaxDec, float jmaxDec) {
    amax_ = amax; jmax_ = jmax; amaxDec_ = amaxDec; jmaxDec_ = jmaxDec;
  }
  float velocity() const { return v_; }
  float accel()    const { return a_; }

private:
  float v_ = 0, a_ = 0;
  float vmax_, amax_, jmax_;
  float amaxDec_ = 0, jmaxDec_ = 0;   // 0 until setLimits: fall back to accel
};

class Motion {
public:
  enum class Mode { Idle, Drive, Velocity, MoveLinear, MoveAngular, EStop };

  // Pushes the default limits into both axes. Without this the axes start with
  // no braking limits set and fall back to the acceleration ones, while the
  // move planner already assumes the boost — it then brakes too late.
  Motion();

  // Operator-style input, -1..1 each. `steer` is scaled by the speed-dependent
  // curve, so the same stick deflection spins on the spot at a standstill and
  // only leans the robot at speed.
  void setDrive(float throttle, float steer);

  // Programmatic input in physical units, no steering curve applied.
  void setVelocity(float mps, float radps);

  // Closed-loop moves. Both return to Idle on arrival; busy() reports progress.
  void moveDistance(float metres, const Odometry& odo);
  void rotate(float radians, const Odometry& odo);

  // 0 = soft (long hauls), 1 = sharp (indoors). Scales acceleration and jerk
  // on both axes; top speed is a separate setting.
  void  setResponse(float r);
  float response() const { return response_; }

  // How much firmer braking is than acceleration. 1.0 = symmetric.
  void  setDecelBoost(float x);
  float decelBoost() const { return decelBoost_; }

  void stop();            // ramp to zero through the jerk limits
  void emergencyStop();   // zero the command this instant, then latch
  void clearEStop();

  void update(const Odometry& odo, float dt);

  // True once per abandoned move, so the caller can report it.
  bool takeAbort() { const bool a = aborted_; aborted_ = false; return a; }

  int16_t cmdLeft()  const { return cmdL_; }
  int16_t cmdRight() const { return cmdR_; }

  Mode        mode()       const { return mode_; }
  const char* modeName()   const;
  bool        busy()       const { return mode_ == Mode::MoveLinear || mode_ == Mode::MoveAngular; }
  float       linear()     const { return lin_.velocity(); }
  float       angular()    const { return ang_.velocity(); }
  float       steerGain()  const;
  double      remaining()  const { return remaining_; }

private:
  JerkAxis lin_{V_MAX, A_MAX_MPS2, J_MAX_MPS3};
  JerkAxis ang_{W_MAX_RADS, ALPHA_MAX, J_MAX_ALPHA};

  Mode   mode_ = Mode::Idle;
  float  reqThrottle_ = 0, reqSteer_ = 0;   // Drive mode, normalised
  float  reqV_ = 0, reqW_ = 0;              // Velocity mode, physical units
  double moveFrom_ = 0, moveTo_ = 0;        // metres or radians
  double holdRef_  = 0;                     // heading (linear move) or distance (rotation)
  double remaining_ = 0;
  double lastRemaining_ = 0;                // for stall detection
  float  stallTimer_ = 0;
  bool   aborted_ = false;
  int    moveDir_ = 1;                      // sign of the move as commanded

  float  response_    = RESPONSE_DEFAULT;
  float  decelBoost_  = DECEL_BOOST_DEFAULT;
  float  aMax_     = A_MAX_MPS2,  jMax_   = J_MAX_MPS3;
  float  alphaMax_ = ALPHA_MAX,   jAlpha_ = J_MAX_ALPHA;

  int16_t cmdL_ = 0, cmdR_ = 0;

  void mixToWheels();
  bool stalled(float dt);
  bool overshot() const;
};