#pragma once
#include <Arduino.h>

// =====================================================================
// Remote -> robot wire protocol. SINGLE SOURCE OF TRUTH.
//
// Both robot/ and remote/ compile against this exact header (PlatformIO
// lib_dir = shared). Any change here changes the over-the-air format:
// rebuild and reflash BOTH boards together. The static payload size is set
// to sizeof(CmdPacket) on both radios, so a remote and robot running
// mismatched versions simply won't hear each other (the robot then fails
// safe on link timeout) instead of misinterpreting fields.
//
// Axis convention (right-handed body frame, viewed from above, like ROS
// REP-103): +X forward, +Y left, +Z up. Positive rotations follow the
// right-hand rule: +roll = right side down, +pitch = nose down,
// +yaw = counterclockwise (turn left).
//
// All analog fields are normalized -1000..1000. The robot owns the physical
// scaling (max mm/s, max tilt deg, max offset mm) in InputMapper, so the
// remote never needs to know robot geometry.
// =====================================================================

struct CmdPacket {
  int16_t vx = 0;                  // left stick: + forward / - backward
  int16_t vy = 0;                  // left stick: + strafe left / - strafe right
  int16_t yaw_rate = 0;            // left stick twist: + turn left (CCW)
  int16_t rx = 0, ry = 0, rz = 0;  // right stick: roll/pitch/yaw, or X/Y/Z offset if FLAG_RSTICK_XLATE
  int16_t remote_roll = 0;         // remote handset tilt (feedforward), only honored with FLAG_REMOTE_GYRO
  int16_t remote_pitch = 0;
  uint8_t speed_scale = 128;       // 0-255, multiplies vx/vy
  uint8_t flags = 0;               // bit-packed, see FLAG_* below
  uint8_t seq = 0;                 // increments every transmitted packet
};

// Both ends are Teensy (Cortex-M7, little-endian, 2-byte int16 alignment):
// 8 x int16 + 3 x uint8 = 19 bytes, padded to 20. If a different MCU family
// ever joins, pack the struct explicitly rather than trusting this.
static_assert(sizeof(CmdPacket) == 20, "CmdPacket layout changed - reflash both boards");
static_assert(sizeof(CmdPacket) <= 32, "nRF24L01 payloads are limited to 32 bytes");

constexpr uint8_t FLAG_ARMED        = 1 << 0;
constexpr uint8_t FLAG_STAND        = 1 << 1;   // 0 = sit requested, 1 = stand requested
constexpr uint8_t FLAG_WALK         = 1 << 2;   // left stick past deadzone -> walking
constexpr uint8_t FLAG_MODE_B       = 1 << 3;   // 0 = mode A direct, 1 = mode B heading-hold
constexpr uint8_t FLAG_RSTICK_XLATE = 1 << 4;   // 0 = right stick = rotation, 1 = translation
constexpr uint8_t FLAG_GAIT_WAVE    = 1 << 5;   // 0 = tripod requested, 1 = wave requested
constexpr uint8_t FLAG_REMOTE_GYRO  = 1 << 6;   // remote tilt feedforward enabled
constexpr uint8_t FLAG_ROBOT_GYRO   = 1 << 7;   // robot stabilization feedback enabled

// ---- Radio link settings (must be identical on both ends) ----
constexpr uint8_t  HXP_RADIO_ADDRESS[6] = "HXPD1";      // 5-byte pipe address + NUL
constexpr uint8_t  HXP_RADIO_CHANNEL    = 96;           // 2400+96 MHz; pick one clear of your local Wi-Fi
constexpr uint8_t  HXP_RADIO_PAYLOAD    = sizeof(CmdPacket);
// Data rate / PA level are RF24 enums; kept as plain ints here so this header
// doesn't depend on RF24.h. 2 == RF24_250KBPS, 1 == RF24_PA_LOW in RF24 1.x;
// both Comms.cpp (robot) and RadioLink.cpp (remote) static_assert this.
constexpr uint8_t  HXP_RADIO_DATA_RATE  = 2;            // RF24_250KBPS
constexpr uint8_t  HXP_RADIO_PA_LEVEL   = 1;            // RF24_PA_LOW