#include "NextionUI.h"
#include <stdarg.h>

// RGB565 colors as Nextion expects them.
static constexpr uint16_t COL_ON     = 2016;    // green
static constexpr uint16_t COL_OFF    = 50712;   // Nextion default light gray
static constexpr uint16_t COL_DANGER = 63488;   // red
static constexpr uint16_t COL_WARN   = 64512;   // orange

// Serial2's default 64-byte TX buffer would make a full redraw (~300 bytes
// at 115200 baud) block the TX loop for ~25 ms. Give it room to queue.
static uint8_t tx_extra[1024];
static uint8_t rx_extra[128];

void NextionUI::begin() {
  NEXTION_SERIAL.begin(NEXTION_BAUD);
  NEXTION_SERIAL.addMemoryForWrite(tx_extra, sizeof(tx_extra));
  NEXTION_SERIAL.addMemoryForRead(rx_extra, sizeof(rx_extra));
  cmd("bkcmd=0");   // no success/fail replies - we never need them
  force_full_ = true;
}

void NextionUI::cmd(const char* fmt, ...) {
  char buf[64];
  va_list ap;
  va_start(ap, fmt);
  vsnprintf(buf, sizeof(buf), fmt, ap);
  va_end(ap);
  NEXTION_SERIAL.print(buf);
  NEXTION_SERIAL.write(0xFF);
  NEXTION_SERIAL.write(0xFF);
  NEXTION_SERIAL.write(0xFF);
}

// ---- Receive ----

bool NextionUI::decodeCustom(UiEvent& out) {
  out.value = body_len_ > 1 ? body_[1] : 0;
  switch (body_[0]) {
    case 'A': out.cmd = UiCmd::ToggleArm;         break;
    case 'K': out.cmd = UiCmd::Disarm;            break;
    case 'S': out.cmd = UiCmd::ToggleStand;       break;
    case 'G': out.cmd = UiCmd::ToggleGait;        break;
    case 'M': out.cmd = UiCmd::ToggleWalkMode;    break;
    case 'X': out.cmd = UiCmd::ToggleRStickXlate; break;
    case 'R': out.cmd = UiCmd::ToggleRemoteGyro;  break;
    case 'B': out.cmd = UiCmd::ToggleRobotGyro;   break;
    case 'V': out.cmd = UiCmd::SetSpeed;          break;
    case 'H': out.cmd = UiCmd::SetHeight;         break;
    case 'Z': out.cmd = UiCmd::ZeroGyro;          break;
    case 'C': out.cmd = UiCmd::CalStartSave;      break;
    case 'Q': out.cmd = UiCmd::CalCancel;         break;
    default:  return false;
  }
  return true;
}

bool NextionUI::poll(UiEvent& out) {
  while (NEXTION_SERIAL.available()) {
    uint8_t b = NEXTION_SERIAL.read();
    switch (rx_) {
      case Rx::Idle:
        if (b == '#') {
          rx_ = Rx::CustomLen;
        } else {
          rx_ = Rx::Native;
          native_first_ = b;
          native_len_ = 1;
          ff_run_ = (b == 0xFF) ? 1 : 0;
        }
        break;

      case Rx::CustomLen:
        if (b >= 1 && b <= sizeof(body_)) {
          body_len_ = b;
          body_got_ = 0;
          rx_ = Rx::CustomBody;
        } else {
          rx_ = Rx::Idle;   // garbage - resync on the next '#'
        }
        break;

      case Rx::CustomBody:
        body_[body_got_++] = b;
        if (body_got_ == body_len_) {
          rx_ = Rx::Idle;
          if (decodeCustom(out)) return true;
        }
        break;

      case Rx::Native:
        native_len_++;
        ff_run_ = (b == 0xFF) ? ff_run_ + 1 : 0;
        if (ff_run_ == 3) {
          // 0x88 = "Nextion ready" after power-on/reset, 0x00 = startup.
          if (native_first_ == 0x88 || native_first_ == 0x00) force_full_ = true;
          rx_ = Rx::Idle;
        } else if (native_len_ > 16) {
          rx_ = Rx::Idle;   // not a frame we understand
        }
        break;
    }
  }
  return false;
}

// ---- Render ----

void NextionUI::pushState(const UiView& v, bool full) {
  const UiView& s = shown_;
  if (full || v.armed != s.armed) {
    cmd("bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED");
    cmd("bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF);
  }
  if (full || v.stand != s.stand) {
    cmd("bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
    cmd("bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
  }
  if (full || v.gait_wave != s.gait_wave)       cmd("bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
  if (full || v.mode_b != s.mode_b)             cmd("bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
  if (full || v.rstick_xlate != s.rstick_xlate) cmd("bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
  if (full || v.remote_gyro != s.remote_gyro)   cmd("bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
  if (full || v.robot_gyro != s.robot_gyro)     cmd("bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
  if (full || v.speed_target != s.speed_target) {
    cmd("hSpeed.val=%u", v.speed_target);
    cmd("nSpeed.val=%u", (v.speed_target * 100 + 127) / 255);
  }
  if (full || v.height != s.height) {
    cmd("hHeight.val=%d", v.height / 10 + 100);   // slider 0..200, 100 = default
    cmd("nHeight.val=%d", v.height / 10);         // -100..100 %
  }
  if (full || v.calibrating != s.calibrating) {
    cmd("bCal.txt=\"%s\"", v.calibrating ? "SAVE CAL" : "CAL");
    cmd("bCal.bco=%u", v.calibrating ? COL_WARN : COL_OFF);
  }
  if (full || v.message != s.message) cmd("tMsg.txt=\"%s\"", v.message);
}

void NextionUI::pushLink(const UiView& v) {
  if (v.link_ok) cmd("tLink.txt=\"LINK %u%%\"", v.link_pct);
  else           cmd("tLink.txt=\"NO LINK\"");
  cmd("tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER);
}

void NextionUI::render(const UiView& v, uint32_t now_ms) {
  bool full = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS;
  if (full) {
    force_full_ = false;
    last_full_ms_ = now_ms;
  }

  pushState(v, full);

  // Link quality changes nearly every packet; repaint it at a readable rate,
  // but immediately when the link comes or goes.
  if (full || v.link_ok != shown_.link_ok || (now_ms - last_link_ms_) >= NEXTION_LINK_REFRESH_MS) {
    pushLink(v);
    last_link_ms_ = now_ms;
  }

  shown_ = v;
}