#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);

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

private:
  PoseCommand command_;
  float correction_roll_ = 0, correction_pitch_ = 0;

  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};
};