#include "hoverboard.h"

namespace {

// XOR of every 16-bit field except the checksum itself, matching
// usart_process_command() and the feedback packer in the firmware.
uint16_t checksumOf(const HoverCommand& c) {
  return (uint16_t)(c.start ^ (uint16_t)c.left ^ (uint16_t)c.right ^ c.buzzer);
}

uint16_t checksumOf(const HoverFeedback& f) {
  return (uint16_t)(f.start ^ (uint16_t)f.cmd1 ^ (uint16_t)f.cmd2 ^
                    (uint16_t)f.speedR_meas ^ (uint16_t)f.speedL_meas ^
                    (uint16_t)f.wheelR_cnt ^ (uint16_t)f.wheelL_cnt ^
                    (uint16_t)f.batVoltage ^ (uint16_t)f.boardTemp ^ f.cmdLed);
}

int16_t clampCmd(int32_t v) {
  if (v >  CMD_MAX) return  CMD_MAX;
  if (v < -CMD_MAX) return -CMD_MAX;
  return (int16_t)v;
}

}  // namespace

void HoverboardLink::begin(HardwareSerial& port) {
  port_ = &port;
  port_->begin(HOVER_BAUD, SERIAL_8N1, PIN_HOVER_RX, PIN_HOVER_TX);
}

void HoverboardLink::send(int16_t left, int16_t right) {
  if (!port_) return;

  // Expire a finished beep here rather than on a timer, so the field is always
  // consistent with the frame actually going out.
  if (buzzer_ != 0 && (int32_t)(millis() - beepUntilMs_) >= 0) buzzer_ = 0;

  HoverCommand c;
  c.start    = HOVER_START_FRAME;
  c.left     = clampCmd(INVERT_LEFT  ? -left  : left);
  c.right    = clampCmd(INVERT_RIGHT ? -right : right);
  c.buzzer   = buzzer_;
  c.checksum = checksumOf(c);

  port_->write((const uint8_t*)&c, sizeof(c));
}

void HoverboardLink::dropByte() {
  // Resync: discard the oldest byte and let the next poll re-test the window.
  if (have_ > 0) {
    memmove(buf_, buf_ + 1, have_ - 1);
    have_--;
  }
}

bool HoverboardLink::poll() {
  if (!port_) return false;
  bool got = false;

  while (port_->available() > 0) {
    if (have_ < sizeof(buf_)) {
      buf_[have_++] = (uint8_t)port_->read();
      rxBytes_++;
    }

    // Keep the window anchored on a start frame before judging the checksum.
    while (have_ >= 2) {
      uint16_t start;
      memcpy(&start, buf_, sizeof(start));
      if (start == HOVER_START_FRAME) break;
      dropByte();
    }

    if (have_ == sizeof(buf_)) {
      HoverFeedback candidate;
      memcpy(&candidate, buf_, sizeof(candidate));

      if (candidate.checksum == checksumOf(candidate)) {
        fb_          = candidate;
        lastFrameMs_ = millis();
        framesOk_++;
        have_ = 0;
        got   = true;
      } else {
        // A corrupt or misaligned frame: step one byte and hunt again rather
        // than throwing away bytes that may hold the real start marker.
        framesBad_++;
        dropByte();
      }
    }
  }

  return got;
}

void HoverboardLink::beep(uint8_t pitch, uint8_t pattern, uint16_t ms) {
  buzzer_      = (uint16_t)pitch | ((uint16_t)pattern << 8);
  beepUntilMs_ = millis() + ms;
}

void HoverboardLink::beepOff() {
  buzzer_ = 0;
}

bool HoverboardLink::linkUp() const {
  return framesOk_ > 0 && (millis() - lastFrameMs_) < FEEDBACK_TIMEOUT_MS;
}

int16_t hoverTickDelta(int16_t prev, int16_t curr) {
  int32_t d = (int32_t)curr - (int32_t)prev;
  d = ((d % ODOM_MODULO) + ODOM_MODULO) % ODOM_MODULO;   // -> [0, ODOM_MODULO)
  if (d >= ODOM_MODULO / 2) d -= ODOM_MODULO;            // -> shortest signed path
  return (int16_t)d;
}