// Serial link to a hoverboard mainboard running hoverboard-firmware-hack-FOC.
//
// Frames are the firmware's own structs (Inc/util.h, Src/main.c) — all int16,
// naturally packed, little-endian on both ends, XOR checksum.
#pragma once

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

constexpr uint16_t HOVER_START_FRAME = 0xABCD;   // firmware SERIAL_START_FRAME

// What we send. Under TANK_STEERING the firmware assigns:
//   command.steer -> cmdL (left wheel),  command.speed -> cmdR (right wheel).
// The field names below say what they actually drive.
struct __attribute__((packed)) HoverCommand {
  uint16_t start;
  int16_t  left;
  int16_t  right;
  // Mainboard buzzer: low byte = pitch (0 = silent), high byte = pattern.
  // Level-triggered, so we own the duration; the mainboard's own fault beeps
  // take priority over anything requested here.
  uint16_t buzzer;
  uint16_t checksum;
};

// What we receive. This layout assumes ENABLE_ODOMETRY is defined in the
// firmware; without it the two wheel counters are absent and the frame is four
// bytes shorter, which shows up here as a permanent checksum failure.
struct __attribute__((packed)) HoverFeedback {
  uint16_t start;
  int16_t  cmd1;          // command echo, left
  int16_t  cmd2;          // command echo, right
  int16_t  speedR_meas;   // rpm
  int16_t  speedL_meas;   // rpm
  int16_t  wheelR_cnt;    // hall ticks, wraps at ODOM_MODULO
  int16_t  wheelL_cnt;
  int16_t  batVoltage;    // volts * 100
  int16_t  boardTemp;     // degrees C * 10
  uint16_t cmdLed;
  uint16_t checksum;
};

class HoverboardLink {
public:
  void begin(HardwareSerial& port);

  // Queue a command. Values are clamped to +/-CMD_MAX and wheel inversion from
  // robot_config.h is applied here, so callers work in robot coordinates.
  void send(int16_t left, int16_t right);

  // Sound the mainboard buzzer for `ms`, then clear it automatically. Pitch
  // and pattern use the firmware's own scale: lower pitch value = higher tone,
  // pattern 0 = continuous. Non-blocking.
  void beep(uint8_t pitch, uint8_t pattern = 0, uint16_t ms = 120);
  void beepOff();

  // Drain the receive buffer. Returns true if a valid frame arrived this call.
  bool poll();

  const HoverFeedback& feedback() const { return fb_; }
  uint32_t lastFrameMs()          const { return lastFrameMs_; }
  bool     linkUp()               const;
  uint32_t framesOk()             const { return framesOk_; }
  uint32_t framesBad()            const { return framesBad_; }
  // Raw bytes seen on RX. Separates "the wire is silent" from "bytes are
  // arriving but never frame up" (wrong baud, swapped pins, wrong layout).
  uint32_t rxBytes()              const { return rxBytes_; }

  float batteryVolts() const { return fb_.batVoltage / 100.0f; }
  float boardTempC()   const { return fb_.boardTemp / 10.0f; }

private:
  HardwareSerial* port_ = nullptr;
  HoverFeedback   fb_{};
  uint8_t         buf_[sizeof(HoverFeedback)]{};
  size_t          have_ = 0;
  uint32_t        lastFrameMs_ = 0;
  uint32_t        framesOk_  = 0;
  uint32_t        framesBad_ = 0;
  uint32_t        rxBytes_   = 0;
  uint16_t        buzzer_      = 0;
  uint32_t        beepUntilMs_ = 0;

  void dropByte();
};

// Signed difference between two readings of a counter that wraps at
// ODOM_MODULO, taking the short way round.
int16_t hoverTickDelta(int16_t prev, int16_t curr);