#include "Config.h"
#include "Sticks.h"
#include "RemoteIMU.h"
#include "RemoteState.h"
#include "PacketBuilder.h"
#include "RadioLink.h"
#include "NextionUI.h"
#include "SerialConsole.h"

// Hexapod remote (Teensy 4.1): reads two 3-axis sticks and the handset
// MPU6050, takes operator commands from the Nextion touchscreen (and the USB
// serial console), and transmits a CmdPacket to the robot at TX_HZ.
// See README.md for wiring, calibration and the serial key map.

Sticks sticks;
RemoteIMU imu;
RemoteState state;
PacketBuilder builder;
RadioLink radio_link;
NextionUI nextion;
SerialConsole console;

uint32_t last_tx_us = 0;
bool dump_packets = false;
uint32_t last_dump_ms = 0;
CmdPacket last_packet;

static void printStatus() {
  static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TW", "R_X", "R_Y", "R_TW" };
  Serial.println("---- remote status ----");
  for (int i = 0; i < NUM_AXES; i++)
    Serial.printf("%-5s raw %4d -> %5d\n", names[i], sticks.raw(i), sticks.frame().v[i]);
  Serial.printf("IMU %s  roll %6.1f  pitch %6.1f (deg, vs zeroed level)\n",
                imu.ok() ? "ok" : "MISSING", imu.rollDeg(), imu.pitchDeg());
  Serial.printf("radio %s  link %s  %u%%\n", radio_link.present() ? "ok" : "MISSING",
                radio_link.linkOk() ? "OK" : "DOWN", radio_link.qualityPct());
  Serial.printf("armed %d stand %d gait %s mode %s rstick %s remote_gyro %d robot_gyro %d speed %u->%u height %d\n",
                state.armed(), state.standRequested(), state.gaitWave() ? "wave" : "tripod",
                state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot",
                state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent(), state.height());
  sticks.printCalibration(Serial);
}

static void dumpPacket(const CmdPacket& p) {
  Serial.printf("#%3u vx %5d vy %5d yaw %5d | r %5d %5d %5d | tilt %5d %5d | h %5d spd %3u flags %02X\n",
                p.seq, p.vx, p.vy, p.yaw_rate, p.rx, p.ry, p.rz,
                p.remote_roll, p.remote_pitch, p.body_height, p.speed_scale, p.flags);
}

// Nextion Editor upload through this USB port (the old remote's b2 button).
// Never returns: the TX loop stops, so the robot loses the link and
// disarms itself after LINK_TIMEOUT_SEC. Power-cycle the remote to leave.
static void enterNextionUpload() {
  state.handle(UiEvent{ UiCmd::Disarm, 0 }, sticks, imu, millis());
  Serial.println("Nextion upload mode: connect Nextion Editor to this USB port and upload.");
  Serial.println("Power-cycle the remote to leave.");
  Serial.flush();
  nextion.runUploadPassthrough();
}

static void pollInputs(uint32_t now_ms) {
  UiEvent ev;
  while (nextion.poll(ev)) {
    if (ev.cmd == UiCmd::NextionUpload) enterNextionUpload();
    state.handle(ev, sticks, imu, now_ms);
  }

  SerialConsole::Local local;
  while (console.poll(ev, local)) {
    if (ev.cmd == UiCmd::NextionUpload) enterNextionUpload();
    if (ev.cmd != UiCmd::None) {
      bool ok = state.handle(ev, sticks, imu, now_ms);
      if (!ok && state.message(now_ms)[0]) Serial.println(state.message(now_ms));
    }
    switch (local) {
      case SerialConsole::Local::PrintStatus: printStatus(); break;
      case SerialConsole::Local::ToggleDump:  dump_packets = !dump_packets; break;
      case SerialConsole::Local::ResetCal:
        if (state.armed()) { Serial.println("disarm first"); break; }
        sticks.resetCalibrationToDefaults();
        Serial.println("stick calibration reset to Config.h defaults");
        break;
      case SerialConsole::Local::None: break;
    }
  }
}

void setup() {
  Serial.begin(115200);
  nextion.begin();

  // Keep the remote still and both sticks released during power-on: the
  // IMU captures its gyro bias and the sticks capture their centers here.
  if (!sticks.begin())     Serial.println("stick center capture skipped on some axes - was a stick held at power-on?");
  if (!imu.begin())        Serial.println("remote IMU not responding - remote-gyro tilt unavailable");
  if (!radio_link.begin()) Serial.println("radio not responding - check nRF24 wiring/power");

  state.begin();
  last_tx_us = micros();
}

void loop() {
  uint32_t now_ms = millis();

  // UI input is drained every pass, not just on TX ticks, so no touch
  // events are lost between packets.
  pollInputs(now_ms);

  uint32_t now_us = micros();
  float dt = (now_us - last_tx_us) / 1e6f;
  if (dt < (1.0f / TX_HZ)) return;   // hold to the fixed TX rate
  last_tx_us = now_us;

  sticks.update();
  imu.update(dt);
  radio_link.update(now_ms);
  state.update(dt, radio_link.linkOk(), now_ms);

  last_packet = builder.build(sticks.frame(), imu, state, now_ms);
  radio_link.send(last_packet);

  UiView view;
  state.fillView(view, now_ms);
  view.link_ok = radio_link.linkOk();
  view.link_pct = radio_link.qualityPct();
  nextion.render(view, now_ms);

  if (dump_packets && now_ms - last_dump_ms >= 100) {
    last_dump_ms = now_ms;
    dumpPacket(last_packet);
  }
}