#pragma once
#include "Kinematics.h"
#include "BodyPose.h"
#include "Config.h"

struct GaitPattern {
  float duty_factor;              // fraction of the cycle each leg spends in stance
  float leg_phase_offset[6];      // each leg's phase offset within the cycle, 0..1
};

extern const GaitPattern GAIT_TRIPOD;
extern const GaitPattern GAIT_WAVE;

struct Twist {
  float vx = 0, vy = 0;   // mm/s, body frame
  float omega = 0;        // rad/s, body frame
};

class GaitEngine {
public:
  void begin();
  void setRequestedPattern(const GaitPattern* p) { requested_pattern_ = p; }
  void setCycleDuration(float sec) { cycle_sec_ = sec; }

  // Advances every leg by dt under the current twist and pattern, writing each
  // leg's target foot position (body frame, before the body pose) into out[6].
  // `pose` is the body pose that will be applied on top: planted feet only
  // move as far as stays physically possible with it.
  void update(float dt, const Twist& twist, const BodyPose& pose, FootTarget out[6]);

  // Puts every foot back at its neutral stance position and restarts the
  // cycle to match. Call when walking starts, since the stand pose holds the
  // feet at neutral in between.
  void resetFeet();

  bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; }

  // The extreme foot positions a stride at this twist reaches:
  // [0] even legs at the leading end of their stride, odd legs at the trailing end,
  // [1] the other way round (tripod legs alternate even/odd, so neighbours are
  //     always in opposite phases - this is when they come closest),
  // [2] every foot at neutral, lifted by SWING_LIFT_MM (mid-swing).
  static constexpr int ENVELOPE_SETS = 3;
  void strideEnvelope(const Twist& twist, FootTarget sets[ENVELOPE_SETS][6]) const;

  // Scales the twist down (all components together) until that envelope,
  // with `pose` applied, is physically possible: every foot reachable and no
  // legs colliding. Pass GaitEngine::NO_POSE for the stride alone.
  // twistScale() reports the factor applied (1 = as commanded); with
  // `chained`, this call's factor multiplies onto the previous call's, for a
  // second limit applied to an already-limited twist.
  Twist limitTwist(const Twist& twist, const BodyPose& pose, bool chained = false);
  float twistScale() const { return twist_scale_; }
  static const BodyPose NO_POSE;

  // The twist the planted feet actually carried out in the last update()
  // (it can be less than asked when a stride change runs a foot out of reach).
  const Twist& appliedTwist() const { return applied_twist_; }

  // Current foot targets (body frame, before the body pose).
  const FootTarget* feet() const { return foot_; }

private:
  Twist applied_twist_;
  bool envelopeFeasible(const Twist& twist, const BodyPose& pose) const;
  float twist_scale_ = 1.0f;

  float leg_phase_[6];
  const GaitPattern* legs_pattern_[6];
  FootTarget foot_[6];      // current foot target, body frame (stance feet are integrated)
  FootTarget liftoff_[6];   // where each swing started
  float swing_start_[6];    // phase at which each swing started (duty factor, except right after resetFeet)
  const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
  float cycle_sec_ = DEFAULT_CYCLE_SEC;
};