#pragma once
#include <Arduino.h>

// Remote -> robot, sent at a fixed rate. Kept as the single source of truth
// here so a future remote sketch just has to fill this same struct and
// radio.write() it - nothing on the robot side needs to change.
struct CmdPacket {
  int16_t vx = 0, vy = 0;           // left stick: strafe / forward-back, -1000..1000
  int16_t yaw_rate = 0;             // left stick twist, -1000..1000
  int16_t rx = 0, ry = 0, rz = 0;   // right stick: roll/pitch/yaw OR X/Y/height, per flags
  uint8_t speed_scale = 128;        // 0-255
  uint8_t flags = 0;                // bit-packed, see FLAG_* below
  uint8_t seq = 0;
};

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;   // 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

class Comms {
public:
  bool begin();
  void update();                       // call every loop; polls the radio if present
  bool linkOk() const { return link_ok_; }
  const CmdPacket& latest() const { return packet_; }

  // Lets the serial test harness (or, later, real radio) inject a packet directly.
  void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); }

private:
  CmdPacket packet_;
  uint32_t last_rx_ms_ = 0;
  bool link_ok_ = false;
  bool radio_present_ = false;
};