#include "InputMapper.h"
#include <Arduino.h>

static float normalize(int16_t raw) { return constrain(raw, -1000, 1000) / 1000.0f; }

static float wrapDeg(float d) {
  while (d > 180) d -= 360;
  while (d < -180) d += 360;
  return d;
}

MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
  (void)dt;
  MapperOutput out{};
  out.armed               = p.flags & FLAG_ARMED;
  out.stand_request        = p.flags & FLAG_STAND;
  out.walk_active           = p.flags & FLAG_WALK;
  out.mode_b                = p.flags & FLAG_MODE_B;
  out.gait_wave              = p.flags & FLAG_GAIT_WAVE;
  out.remote_gyro_enabled     = p.flags & FLAG_REMOTE_GYRO;
  out.robot_gyro_enabled       = p.flags & FLAG_ROBOT_GYRO;

  float speed = p.speed_scale / 255.0f;
  float raw_vx = normalize(p.vx) * speed;
  float raw_vy = normalize(p.vy) * speed;
  float raw_yaw_in = normalize(p.yaw_rate);

  if (!out.mode_b) {
    // Mode A: direct twist. Heading and travel direction are fully independent.
    out.twist.vx = raw_vx * MAX_LINEAR_MM_S;
    out.twist.vy = raw_vy * MAX_LINEAR_MM_S;
    out.twist.omega = radians(raw_yaw_in * MAX_TURN_RATE_DEG_S);
  } else {
    // Mode B: heading-hold. Stick angle becomes a target heading; forward
    // speed comes from stick magnitude. yaw_rate is unused here.
    float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
    if (mag > 0.05f) {
      heading_target_deg_ = degrees(atan2(raw_vy, raw_vx));
    }
    float heading_error = wrapDeg(heading_target_deg_ - heading_deg);
    float omega_deg_s = constrain(HEADING_KP * heading_error, -MAX_TURN_RATE_DEG_S, MAX_TURN_RATE_DEG_S);
    out.twist.omega = radians(omega_deg_s);
    out.twist.vx = mag * MAX_LINEAR_MM_S;   // forward-only in the robot's current facing
    out.twist.vy = 0;
  }

  // Right stick: rotation sub-mode (default) or translation sub-mode.
  float rx = normalize(p.rx), ry = normalize(p.ry), rz = normalize(p.rz);
  if (p.flags & FLAG_RSTICK_XLATE) {
    out.pose.x_mm = rx * MAX_POSE_OFFSET_MM;
    out.pose.y_mm = ry * MAX_POSE_OFFSET_MM;
    out.pose.z_mm = rz * MAX_POSE_OFFSET_MM;
  } else {
    out.pose.roll_deg  = rx * MAX_TILT_DEG;
    out.pose.pitch_deg = ry * MAX_TILT_DEG;
    out.pose.yaw_deg   = rz * MAX_TILT_DEG;
  }

  // Remote handset tilt feedforward: its own packet fields, gated only by
  // FLAG_REMOTE_GYRO, added on top of whatever the right stick commands (so
  // it works in both right-stick sub-modes). Total tilt stays within MAX_TILT_DEG.
  if (out.remote_gyro_enabled) {
    out.pose.roll_deg  = constrain(out.pose.roll_deg  + normalize(p.remote_roll)  * MAX_TILT_DEG,
                                   -MAX_TILT_DEG, MAX_TILT_DEG);
    out.pose.pitch_deg = constrain(out.pose.pitch_deg + normalize(p.remote_pitch) * MAX_TILT_DEG,
                                   -MAX_TILT_DEG, MAX_TILT_DEG);
  }

  return out;
}