// ahrs_log — M2 gate 3 verification tool.
//
// Head-locked logging of the Mahony AHRS: euler angles, gravity vector in
// head frame, accel magnitude, drift since mark. Gate criteria (plan):
//   - gravity vector stable at rest
//   - rest drift < ~1°/min pitch/roll (yaw drift accepted)
//   - recenter works
//   - rotation sense matches physical motion on all axes:
//       look LEFT  -> yaw increases?  (verify sign conventions physically)
//       look UP    -> pitch increases?
//       tilt head right (ear to shoulder) -> roll changes sign accordingly
//   The PHYSICAL sense check is mandatory before gate 4 (render).
//
// Keys: r = recenter, b = recapture gyro bias (hold still), m = re-mark
// drift reference, q = quit.
//
// Usage: ahrs_log --config <config.json> [--gyro-scale 0.001]
//                 [--imu-serial LHR-...]

#include <conio.h>

#define _USE_MATH_DEFINES
#include <cmath>
#include <cstdio>
#include <cstring>
#include <string>
#include <thread>

#include "calib/hmd_config.h"
#include "device/tundra_imu.h"
#include "track/ahrs.h"

using namespace sauna;

namespace {

// Euler for q = Ry(yaw)*Rx(pitch)*Rz(roll), world +Y up / -Z forward.
void toEuler(const Quat& q, double* yawDeg, double* pitchDeg, double* rollDeg) {
  const double m02 = 2 * (q.x * q.z + q.w * q.y);
  const double m12 = 2 * (q.y * q.z - q.w * q.x);
  const double m22 = 1 - 2 * (q.x * q.x + q.y * q.y);
  const double m10 = 2 * (q.x * q.y + q.w * q.z);
  const double m11 = 1 - 2 * (q.x * q.x + q.z * q.z);
  const double k = 180.0 / M_PI;
  *yawDeg = std::atan2(m02, m22) * k;
  *pitchDeg = std::asin(std::fmax(-1.0, std::fmin(1.0, -m12))) * k;
  *rollDeg = std::atan2(m10, m11) * k;
}

}  // namespace

int main(int argc, char** argv) {
  std::string configPath;
  const char* serial = "";
  Ahrs::Params params;
  for (int i = 1; i < argc; i++) {
    if (!strcmp(argv[i], "--config") && i + 1 < argc) configPath = argv[++i];
    else if (!strcmp(argv[i], "--gyro-scale") && i + 1 < argc)
      params.gyroScale = atof(argv[++i]);
    else if (!strcmp(argv[i], "--imu-serial") && i + 1 < argc) serial = argv[++i];
    else {
      fprintf(stderr,
              "usage: ahrs_log --config <config.json> [--gyro-scale s] "
              "[--imu-serial LHR-...]\n");
      return 2;
    }
  }
  if (configPath.empty()) {
    fprintf(stderr, "--config is required\n");
    return 2;
  }

  HmdConfig cfg;
  std::string err;
  if (!LoadHmdConfig(configPath, &cfg, &err)) {
    fprintf(stderr, "%s\n", err.c_str());
    return 1;
  }

  Ahrs ahrs;
  ahrs.configure(cfg, params);

  TundraImu imu;
  imu.setSampleSink([&ahrs](const ImuSample& s) { ahrs.update(s); });
  if (!imu.start(serial)) return 1;

  printf("gyro scale %.9f rad/s/LSB  (gate-2 value; default is the unverified prior)\n",
         params.gyroScale);
  printf("keys: r=recenter  b=bias recapture  m=mark drift ref  q=quit\n");
  printf("capturing gyro bias — hold the headset STILL...\n");

  bool sawInit = false;
  bool serialChecked = false;
  double markYaw = 0, markPitch = 0, markRoll = 0;
  auto markAt = std::chrono::steady_clock::now();

  for (;;) {
    std::this_thread::sleep_for(std::chrono::milliseconds(200));

    while (_kbhit()) {
      const int c = _getch();
      if (c == 'q' || c == 'Q') goto done;
      if (c == 'r' || c == 'R') {
        ahrs.recenter();
        sawInit = false;  // re-mark on next tick
        printf("\n[recentered]\n");
      }
      if (c == 'b' || c == 'B') {
        ahrs.requestBiasCapture();
        sawInit = false;
        printf("\n[bias recapture — hold still]\n");
      }
      if (c == 'm' || c == 'M') sawInit = false;
    }

    // Config/unit serial cross-check once the device has latched.
    if (!serialChecked) {
      const std::string unit = imu.connectedSerial();
      if (!unit.empty()) {
        serialChecked = true;
        if (unit != cfg.serial)
          printf("\nWARNING: config %s but attached unit %s — calibration "
                 "does not match this headset!\n",
                 cfg.serial.c_str(), unit.c_str());
      }
    }

    const Ahrs::Status st = ahrs.status();
    if (!st.initialized) continue;

    double yaw, pitch, roll;
    toEuler(ahrs.pose(), &yaw, &pitch, &roll);

    if (!sawInit) {
      sawInit = true;
      markYaw = yaw;
      markPitch = pitch;
      markRoll = roll;
      markAt = std::chrono::steady_clock::now();
      printf("bias [%.2f %.2f %.2f] LSB — drift reference marked\n",
             st.gyroBiasLsb[0], st.gyroBiasLsb[1], st.gyroBiasLsb[2]);
    }

    const double mins =
        std::chrono::duration<double>(std::chrono::steady_clock::now() - markAt)
            .count() /
        60.0;
    auto wrap = [](double d) {
      while (d > 180) d -= 360;
      while (d < -180) d += 360;
      return d;
    };
    const double dy = wrap(yaw - markYaw), dp = wrap(pitch - markPitch),
                 dr = wrap(roll - markRoll);

    printf("\ryaw %+8.2f pitch %+7.2f roll %+7.2f | g_head [%+.3f %+.3f %+.3f] "
           "|a| %.3fg | drift°/min y%+.2f p%+.2f r%+.2f (%.1fm) | n %llu rst %llu   ",
           yaw, pitch, roll, st.gravityHead[0], st.gravityHead[1],
           st.gravityHead[2], st.accelMagG,
           mins > 0.01 ? dy / mins : 0.0, mins > 0.01 ? dp / mins : 0.0,
           mins > 0.01 ? dr / mins : 0.0, mins, (unsigned long long)st.samples,
           (unsigned long long)st.resets);
    fflush(stdout);
  }

done:
  printf("\n");
  imu.stop();
  return 0;
}