// gyro_cal — M2 gate 2: gyro scale verification (closes S1 open item).
//
// The Tundra config carries NO gyro_scale (acc_scale present, gyro absent on
// LHR-599F3B91), so the scale must be measured: rotate the headset by an
// exactly-known angle about one axis (table-edge jig), integrate raw gyro
// over the move, solve scale = angle / integral per axis.
//
// Candidate priors (S1): 1/1024 ~ 0.000976563 and 0.001 rad/s/LSB.
// Gate: integrated angle within ~2% of physical rotation on all axes.
//
// Protocol per run (the tool walks you through it):
//   1. Rest the headset against the jig, still. 2 s bias capture.
//   2. Press Enter to arm, rotate exactly the stated angle (settle ~1 s
//      still before and after the move — rest integrates to ~0 once
//      bias-corrected), press Enter to finish.
//   3. Repeat per axis / angle. 'q' quits and prints the per-axis summary.
//
// Usage: gyro_cal [--imu-serial LHR-XXXXXXXX]

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

#include "device/tundra_imu.h"

using namespace sauna;

namespace {

constexpr double kTimecodeHz = 48e6;  // S1: 48 MHz device clock, uint32 wrap

enum class Mode { Idle, Bias, Integrate };

struct Accum {
  std::mutex mu;
  Mode mode = Mode::Idle;

  // Bias phase
  double biasSum[3] = {};
  double biasSqSum[3] = {};
  uint64_t biasN = 0;

  // Integration phase
  double bias[3] = {};
  double integral[3] = {};  // LSB·s, bias-corrected
  uint32_t lastTc = 0;
  bool haveTc = false;
  uint64_t n = 0;
  double maxDt = 0.0;
  bool gapFault = false;  // dt > 0.5 s — run invalid (device gap/reconnect)

  void onSample(const ImuSample& s) {
    std::lock_guard<std::mutex> lk(mu);
    switch (mode) {
      case Mode::Idle:
        break;
      case Mode::Bias:
        for (int a = 0; a < 3; a++) {
          biasSum[a] += s.gyro[a];
          biasSqSum[a] += (double)s.gyro[a] * s.gyro[a];
        }
        biasN++;
        break;
      case Mode::Integrate:
        if (haveTc) {
          // uint32 subtraction is wrap-safe across the ~89.5 s rollover.
          double dt = (uint32_t)(s.timecode - lastTc) / kTimecodeHz;
          if (dt > 0.5) gapFault = true;
          if (dt > maxDt) maxDt = dt;
          for (int a = 0; a < 3; a++)
            integral[a] += (s.gyro[a] - bias[a]) * dt;
          n++;
        }
        lastTc = s.timecode;
        haveTc = true;
        break;
    }
  }
};

void readLine(std::string* out) {
  out->clear();
  int c;
  while ((c = getchar()) != EOF && c != '\n') out->push_back((char)c);
}

}  // namespace

int main(int argc, char** argv) {
  const char* serial = "";
  for (int i = 1; i < argc; i++) {
    if (!strcmp(argv[i], "--imu-serial") && i + 1 < argc) serial = argv[++i];
    else {
      fprintf(stderr, "usage: gyro_cal [--imu-serial LHR-...]\n");
      return 2;
    }
  }

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

  // Wait for the stream.
  for (int i = 0; i < 50; i++) {
    ImuSample s;
    if (imu.latest(&s)) break;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  {
    ImuSample s;
    if (!imu.latest(&s)) {
      fprintf(stderr, "no IMU samples after 5 s\n");
      return 1;
    }
  }
  printf("IMU streaming from %s\n\n", imu.connectedSerial().c_str());

  struct Run { int axis; double angleDeg; double scale; double offAxisFrac; };
  std::vector<Run> runs;
  static const char axisName[3] = {'X', 'Y', 'Z'};

  for (;;) {
    printf("rotation angle in degrees for this run (90/180/360, q=quit): ");
    fflush(stdout);
    std::string line;
    readLine(&line);
    if (line == "q" || line == "Q") break;
    const double angleDeg = atof(line.c_str());
    if (angleDeg <= 0) {
      printf("  bad angle\n");
      continue;
    }

    printf("  hold STILL against the jig — 2 s bias capture...");
    fflush(stdout);
    {
      std::lock_guard<std::mutex> lk(acc.mu);
      acc.biasSum[0] = acc.biasSum[1] = acc.biasSum[2] = 0;
      acc.biasSqSum[0] = acc.biasSqSum[1] = acc.biasSqSum[2] = 0;
      acc.biasN = 0;
      acc.mode = Mode::Bias;
    }
    std::this_thread::sleep_for(std::chrono::seconds(2));
    double bias[3], noise[3];
    uint64_t biasN;
    {
      std::lock_guard<std::mutex> lk(acc.mu);
      acc.mode = Mode::Idle;
      biasN = acc.biasN;
      for (int a = 0; a < 3; a++) {
        bias[a] = acc.biasN ? acc.biasSum[a] / acc.biasN : 0.0;
        noise[a] = acc.biasN
                       ? std::sqrt(acc.biasSqSum[a] / acc.biasN - bias[a] * bias[a])
                       : 0.0;
      }
    }
    if (biasN < 1000) {
      printf(" only %llu samples — stream unhealthy, run aborted\n",
             (unsigned long long)biasN);
      continue;
    }
    printf(" bias [%.2f %.2f %.2f] LSB  noise sd [%.1f %.1f %.1f] (%llu samples)\n",
           bias[0], bias[1], bias[2], noise[0], noise[1], noise[2],
           (unsigned long long)biasN);

    printf("  press Enter to ARM, rotate exactly %g deg (settle ~1 s still "
           "before/after), Enter again to finish: ",
           angleDeg);
    fflush(stdout);
    readLine(&line);
    {
      std::lock_guard<std::mutex> lk(acc.mu);
      memcpy(acc.bias, bias, sizeof(bias));
      acc.integral[0] = acc.integral[1] = acc.integral[2] = 0;
      acc.haveTc = false;
      acc.n = 0;
      acc.maxDt = 0;
      acc.gapFault = false;
      acc.mode = Mode::Integrate;
    }
    printf("  ...integrating, finish with Enter: ");
    fflush(stdout);
    readLine(&line);

    double integral[3], maxDt;
    uint64_t n;
    bool fault;
    {
      std::lock_guard<std::mutex> lk(acc.mu);
      acc.mode = Mode::Idle;
      memcpy(integral, acc.integral, sizeof(integral));
      n = acc.n;
      maxDt = acc.maxDt;
      fault = acc.gapFault;
    }
    if (fault) {
      printf("  RUN INVALID: stream gap > 0.5 s during integration "
             "(max dt %.3f s)\n",
             maxDt);
      continue;
    }

    int dom = 0;
    for (int a = 1; a < 3; a++)
      if (std::fabs(integral[a]) > std::fabs(integral[dom])) dom = a;
    // 90 deg at ~0.001 rad/s/LSB integrates to ~1571 LSB*s; rest noise over
    // several seconds is single digits. Below 100 there was no real rotation.
    if (n < 500 || std::fabs(integral[dom]) < 100.0) {
      printf("  RUN INVALID: no rotation detected (%llu samples, dominant "
             "integral %.1f LSB*s)\n\n",
             (unsigned long long)n, integral[dom]);
      continue;
    }
    const double angleRad = angleDeg * M_PI / 180.0;
    const double scale = angleRad / std::fabs(integral[dom]);
    double off = 0.0;
    for (int a = 0; a < 3; a++)
      if (a != dom)
        off = std::fmax(off, std::fabs(integral[a] / integral[dom]));

    printf("  integral [%.1f %.1f %.1f] LSB*s  (%llu samples, max dt %.4f s)\n",
           integral[0], integral[1], integral[2], (unsigned long long)n, maxDt);
    printf("  dominant axis %c (sign %c)  off-axis leakage %.1f%%\n",
           axisName[dom], integral[dom] >= 0 ? '+' : '-', off * 100.0);
    printf("  scale = %.9f rad/s/LSB   vs 1/1024: %+.2f%%   vs 0.001: %+.2f%%\n\n",
           scale, (scale / (1.0 / 1024.0) - 1.0) * 100.0,
           (scale / 0.001 - 1.0) * 100.0);
    runs.push_back({dom, angleDeg, scale, off});
  }

  if (!runs.empty()) {
    printf("\n==== summary (%zu runs) ====\n", runs.size());
    for (int a = 0; a < 3; a++) {
      double sum = 0, mn = 1e9, mx = 0;
      int cnt = 0;
      for (const Run& r : runs)
        if (r.axis == a) {
          sum += r.scale;
          mn = std::fmin(mn, r.scale);
          mx = std::fmax(mx, r.scale);
          cnt++;
        }
      if (!cnt) {
        printf("%c: NO RUNS — gate requires all axes\n", axisName[a]);
        continue;
      }
      const double mean = sum / cnt;
      printf("%c: %d runs  mean %.9f  spread %.2f%%  vs 1/1024 %+.2f%%  vs 0.001 %+.2f%%\n",
             axisName[a], cnt, mean, (mx - mn) / mean * 100.0,
             (mean / (1.0 / 1024.0) - 1.0) * 100.0,
             (mean / 0.001 - 1.0) * 100.0);
    }
    printf("gate: per-axis mean must sit within ~2%% of one consistent value "
           "on all three axes\n");
  }

  imu.stop();
  return 0;
}