#include "PacketBuilder.h"

// Handset tilt (deg) -> -1000..1000 with a small deadzone for hand tremor,
// scaled by the fade blend so toggling remote gyro ramps instead of steps.
int16_t PacketBuilder::tiltToWire(float deg, float blend) {
  float mag = fabsf(deg) - REMOTE_TILT_DEADZONE_DEG;
  if (mag <= 0 || blend <= 0) return 0;
  float n = constrain(mag / (REMOTE_TILT_FULL_SCALE_DEG - REMOTE_TILT_DEADZONE_DEG), 0.0f, 1.0f);
  return (int16_t)lroundf(copysignf(n, deg) * 1000.0f * blend);
}

CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms) {
  CmdPacket p;
  p.seq = seq_++;
  p.speed_scale = st.speedCurrent();

  uint8_t f = 0;
  if (st.armed())          f |= FLAG_ARMED;
  if (st.standRequested()) f |= FLAG_STAND;
  if (st.modeB())          f |= FLAG_MODE_B;
  if (st.rstickXlate())    f |= FLAG_RSTICK_XLATE;
  if (st.gaitWave())       f |= FLAG_GAIT_WAVE;
  if (st.robotGyro())      f |= FLAG_ROBOT_GYRO;

  // Disarmed: send a neutral command. The robot ignores motion then anyway,
  // but this keeps its pose smoothers from winding up to a stale stick.
  if (!st.armed()) {
    p.flags = f;
    return p;
  }

  // ---- Left stick: walking. Handset stick frame -> body frame:
  //   stick forward (+Y) -> +vx forward, stick right (+X) -> -vy (vy + = left),
  //   twist CCW -> +yaw_rate (turn left).
  p.vx       =  s.v[L_Y];
  p.vy       = -s.v[L_X];
  p.yaw_rate =  s.v[L_TWIST];

  // ---- Right stick: body pose. Each packet field is "about / along" one body
  // axis, so the physical-axis mapping differs per sub-mode:
  if (st.rstickXlate()) {
    p.rx =  s.v[R_Y];       // X offset: stick forward -> body forward
    p.ry = -s.v[R_X];       // Y offset: stick right  -> body right (-Y)
    p.rz =  s.v[R_TWIST];   // Z offset: twist CCW    -> body up
  } else {
    p.rx =  s.v[R_X];       // roll:  stick right   -> right side down
    p.ry =  s.v[R_Y];       // pitch: stick forward -> nose down
    p.rz =  s.v[R_TWIST];   // yaw:   twist CCW     -> body yaws left
  }

  // ---- Walk flag: in mode B the robot ignores yaw_rate, so twist alone
  // shouldn't start stepping. Linger avoids STAND<->WALKING chatter.
  bool walk_input = p.vx != 0 || p.vy != 0 || (!st.modeB() && p.yaw_rate != 0);
  if (walk_input) { walk_seen_ = true; last_walk_input_ms_ = now_ms; }
  bool walking = walk_seen_ && (now_ms - last_walk_input_ms_) < WALK_LINGER_MS;
  if (st.standRequested() && (walk_input || walking)) f |= FLAG_WALK;

  // ---- Remote tilt feedforward: keep the flag up for the whole fade-out so
  // the robot sees the tilt ramp to zero rather than being cut off.
  float blend = st.remoteTiltBlend();
  if (st.remoteGyro() || blend > 0.0f) {
    f |= FLAG_REMOTE_GYRO;
    p.remote_roll  = tiltToWire(imu.rollDeg(),  blend);
    p.remote_pitch = tiltToWire(imu.pitchDeg(), blend);
  }

  p.flags = f;
  return p;
}