#pragma once
#include "Kinematics.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) into out[6].
  void update(float dt, const Twist& twist, FootTarget out[6]);

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

private:
  float leg_phase_[6];
  const GaitPattern* legs_pattern_[6];
  const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
  float cycle_sec_ = DEFAULT_CYCLE_SEC;
};