#pragma once
#include "Comms.h"
#include "GaitEngine.h"
#include "BodyPose.h"

struct MapperOutput {
  Twist twist;
  PoseCommand pose;
  bool armed = false;
  bool stand_request = false;
  bool walk_active = false;
  bool mode_b = false;
  bool gait_wave = false;
  bool remote_gyro_enabled = false;
  bool robot_gyro_enabled = false;
  float stance_height_mm = STANCE_HEIGHT_MM;   // requested standing height (feet below hips)
};

class InputMapper {
public:
  // heading_deg is the robot's current tracked heading estimate; only mode B uses it.
  MapperOutput map(const CmdPacket& p, float heading_deg, float dt);

private:
  float heading_target_deg_ = 0;
  bool stick_active_ = false;          // mode B: stick currently pushed
  static constexpr float MAX_TURN_RATE_DEG_S = 90.0f;
  static constexpr float HEADING_KP = 2.0f;
  static constexpr float MAX_LINEAR_MM_S = 150.0f;   // tune to your leg workspace / servo speed
  static constexpr float MAX_POSE_OFFSET_MM = 40.0f;
  static constexpr float MAX_TILT_DEG = 20.0f;
};