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

struct PoseCommand {
  float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
  float x_mm = 0, y_mm = 0, z_mm = 0;
};

class BodyPose {
public:
  void begin();
  void setCommand(const PoseCommand& c) { command_ = c; }
  // From RobotIMU stabilization; pass 0,0 when the robot-gyro toggle is off.
  void setCorrection(float roll_deg, float pitch_deg) {
    correction_roll_ = roll_deg;
    correction_pitch_ = pitch_deg;
  }

  void update(float dt);

  // Scales the whole pose down (one factor for all six legs) until every set
  // of six feet in `sets` is still physically possible with the pose applied
  // (reachable, no colliding legs). Pass the stride envelope while walking,
  // so the pose fits the whole stride rather than this tick's feet, or the
  // stand targets otherwise. Call after update() and before apply().
  void fitToReach(const FootTarget sets[][6], int count, float dt);
  float reachScale() const { return reach_scale_; }   // 1 = full commanded pose

  // The operator's tilt as actually applied (smoothed and reach-scaled),
  // without the gyro correction - what self-leveling should keep.
  float commandRollDeg() const  { return s_roll_.current * reach_scale_; }
  float commandPitchDeg() const { return s_pitch_.current * reach_scale_; }

  // Applies the current (smoothed, reach-scaled) pose transform to a body-frame foot target.
  FootTarget apply(const FootTarget& in) const;

  // True if these (un-posed) feet are physically possible with the current
  // pose applied - see Limits::feetFeasible.
  bool feasible(const FootTarget feet[6]) const;

private:
  PoseCommand command_;
  float correction_roll_ = 0, correction_pitch_ = 0;
  float reach_scale_ = 1.0f;
  static constexpr float REACH_RECOVER_PER_SEC = 1.0f;
  static constexpr float REACH_DROP_PER_SEC = 3.0f;

  FootTarget applyScaled(const FootTarget& in, float scale) const;
  bool reachable(const FootTarget sets[][6], int count, float scale) const;

  SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
  SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};
  SmoothedValue s_yaw_{POSE_TILT_MAX_RATE_DEG};
  SmoothedValue s_x_{POSE_OFFSET_MAX_RATE_MM};
  SmoothedValue s_y_{POSE_OFFSET_MAX_RATE_MM};
  SmoothedValue s_z_{POSE_OFFSET_MAX_RATE_MM};
};