#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);
    // Hold the current heading, so switching to mode B doesn't turn the
    // robot toward some old target.
    heading_target_deg_ = heading_deg;
    stick_active_ = false;
  } else {
    // Mode B: steering, relative to where the body faces right now. The
    // stick's forward/back component sets the walking direction, its sideways
    // component steers: forward walks straight on, back walks straight back,
    // forward+left or back+left walk while turning left, left alone turns on
    // the spot (for as long as it's held). Let go and the robot holds the
    // heading it has at that moment. yaw_rate is unused here.
    float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
    float direction = raw_vx < 0 ? -1.0f : 1.0f;
    if (mag > 0.05f) {
      stick_active_ = true;
      heading_target_deg_ = wrapDeg(heading_deg + degrees(atan2(raw_vy, fabs(raw_vx))));
    } else if (stick_active_) {
      stick_active_ = false;
      heading_target_deg_ = heading_deg;   // released: hold this heading
    }
    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);
    // Along the robot's current facing (forward or back), scaled by how well
    // it already faces the target: a hard turn happens mostly on the spot.
    float facing = max(0.0f, cos(radians(heading_error)));
    out.twist.vx = direction * mag * facing * MAX_LINEAR_MM_S;
    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);
  }

  out.stance_height_mm = STANCE_HEIGHT_MM + normalize(p.body_height) * STANCE_HEIGHT_RANGE_MM;

  return out;
}