// calib_check — M2 gate 1 verification tool.
//
// Loads a per-unit config JSON, prints the parsed calibration, and (with
// --dump) replays the S3 OpenVR ComputeDistortion ground-truth grid through
// the C++ evaluator. Gate: max error matches the S3 Python evaluator's
// float32-noise-floor result (~0.0002 px on LHR-599F3B91), i.e. < 0.001 px.
//
// Usage:
//   calib_check --config <config.json> [--dump <distortion_dump.json>]
//               [--expect-serial LHR-XXXXXXXX]

#include <cmath>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <string>

#include "calib/hmd_config.h"
#include "nlohmann/json.hpp"

using nlohmann::json;
using namespace sauna;

static void printConfig(const HmdConfig& cfg) {
  printf("unit %s (%s)  eye %dx%d px  parallel_render_cameras=%d  ipd default %.1f mm\n",
         cfg.serial.c_str(), cfg.model.c_str(), cfg.eye_width_px,
         cfg.eye_height_px, cfg.parallel_render_cameras ? 1 : 0,
         cfg.ipd_default_mm);
  printf("imu: plus_x [%g %g %g]  plus_z [%g %g %g]  pos [%g %g %g] m\n",
         cfg.imu.frame.plus_x[0], cfg.imu.frame.plus_x[1], cfg.imu.frame.plus_x[2],
         cfg.imu.frame.plus_z[0], cfg.imu.frame.plus_z[1], cfg.imu.frame.plus_z[2],
         cfg.imu.frame.position[0], cfg.imu.frame.position[1], cfg.imu.frame.position[2]);
  printf("imu: acc_bias [%g %g %g]  acc_scale [%g %g %g]  gyro_bias [%g %g %g]  gyro_scale %s\n",
         cfg.imu.acc_bias[0], cfg.imu.acc_bias[1], cfg.imu.acc_bias[2],
         cfg.imu.acc_scale[0], cfg.imu.acc_scale[1], cfg.imu.acc_scale[2],
         cfg.imu.gyro_bias[0], cfg.imu.gyro_bias[1], cfg.imu.gyro_bias[2],
         cfg.imu.has_gyro_scale ? "PRESENT" : "ABSENT (gate 2 must measure)");
  static const char* eyeName[2] = {"left ", "right"};
  static const char* chName[3] = {"red  ", "green", "blue "};
  for (int e = 0; e < 2; e++) {
    const EyeCalib& eye = cfg.eye[e];
    printf("%s: grow %.3f  r2_cutoff %.3f (not applied)  fx %.6f fy %.6f  color_mult [%g %g %g]\n",
           eyeName[e], eye.grow_for_undistort, eye.undistort_r2_cutoff,
           eye.intrinsics[0][0], eye.intrinsics[1][1],
           eye.color_mult[0], eye.color_mult[1], eye.color_mult[2]);
    for (int ch = 0; ch < 3; ch++) {
      const DistortionChannel& c = eye.rgb[ch];
      printf("  %s center (%+.10f, %+.10f)  k [%.10f %.10f %.10f]\n",
             chName[ch], c.center_x, c.center_y, c.k[0], c.k[1], c.k[2]);
    }
    // S3 center convention cross-check: center_x = -K[0][2], center_y = +K[1][2].
    double dx = eye.rgb[1].center_x + eye.intrinsics[0][2];
    double dy = eye.rgb[1].center_y - eye.intrinsics[1][2];
    if (std::fabs(dx) > 1e-12 || std::fabs(dy) > 1e-12)
      printf("  WARNING: center/intrinsics sign convention mismatch (dx %g dy %g)\n", dx, dy);
  }
}

static int checkDump(const HmdConfig& cfg, const std::string& dumpPath) {
  std::ifstream f(dumpPath);
  if (!f) {
    fprintf(stderr, "cannot open dump %s\n", dumpPath.c_str());
    return 1;
  }
  json dump = json::parse(f);

  std::string dumpSerial = dump.value("serial", std::string());
  if (!dumpSerial.empty() && dumpSerial != cfg.serial)
    printf("WARNING: dump serial %s != config serial %s\n", dumpSerial.c_str(),
           cfg.serial.c_str());

  const double panelPx = cfg.eye_width_px;  // error metric units (verify.py)
  static const char* chKey[3] = {"r", "g", "b"};
  double overallMax = 0.0;
  double projMax = 0.0;

  for (size_t e = 0; e < dump.at("eyes").size(); e++) {
    const json& eyeDump = dump.at("eyes").at(e);
    const EyeCalib& model = cfg.eye[e];
    for (int ch = 0; ch < 3; ch++) {
      double se = 0.0, mx = 0.0;
      size_t count = 0;
      for (const json& pt : eyeDump.at("points")) {
        if (pt.is_null()) continue;
        const double u = pt.at("uv").at(0).get<double>();
        const double v = pt.at("uv").at(1).get<double>();
        double ours[3][2];
        EvalDistortion(model, u, v, ours);
        const json& theirs = pt.at(chKey[ch]);
        const double dx = (ours[ch][0] - theirs.at(0).get<double>()) * panelPx;
        const double dy = (ours[ch][1] - theirs.at(1).get<double>()) * panelPx;
        const double e2 = dx * dx + dy * dy;
        se += e2;
        if (e2 > mx) mx = e2;
        count++;
      }
      const double rms = count ? std::sqrt(se / count) : NAN;
      const double mxe = std::sqrt(mx);
      printf("%s %s: rms %.6f px  max %.6f px  (%zu pts)\n",
             eyeDump.at("eye").get<std::string>().c_str(), chKey[ch], rms, mxe,
             count);
      if (mxe > overallMax) overallMax = mxe;
    }
    // Projection fold closed form vs dumped GetProjectionRaw.
    const json& pr = eyeDump.at("projection_raw");
    double t[4], d[4] = {pr.at(0).get<double>(), pr.at(1).get<double>(),
                         pr.at(2).get<double>(), pr.at(3).get<double>()};
    ProjectionRawTangents(model, &t[0], &t[1], &t[2], &t[3]);
    double pe = 0.0;
    for (int i = 0; i < 4; i++) pe = std::fmax(pe, std::fabs(t[i] - d[i]));
    printf("%s projection_raw  dump L%+.5f R%+.5f T%+.5f B%+.5f\n",
           eyeDump.at("eye").get<std::string>().c_str(), d[0], d[1], d[2], d[3]);
    printf("%s closed-form     pred L%+.5f R%+.5f T%+.5f B%+.5f  max|diff| %.6f%s\n",
           eyeDump.at("eye").get<std::string>().c_str(), t[0], t[1], t[2], t[3],
           pe, pe < 5e-4 ? "" : "  FOLD MISMATCH");
    if (pe > projMax) projMax = pe;
  }

  const bool pass = overallMax < 0.001 && projMax < 5e-4;
  printf("\ndistortion max %.6f px (gate < 0.001), projection fold max "
         "%.6f tan (gate < 5e-4)  ->  %s\n",
         overallMax, projMax, pass ? "PASS" : "FAIL");
  return pass ? 0 : 1;
}

int main(int argc, char** argv) {
  std::string configPath, dumpPath, expectSerial;
  for (int i = 1; i < argc; i++) {
    if (!strcmp(argv[i], "--config") && i + 1 < argc) configPath = argv[++i];
    else if (!strcmp(argv[i], "--dump") && i + 1 < argc) dumpPath = argv[++i];
    else if (!strcmp(argv[i], "--expect-serial") && i + 1 < argc) expectSerial = argv[++i];
    else {
      fprintf(stderr,
              "usage: calib_check --config <config.json> [--dump <dump.json>] "
              "[--expect-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;
  }
  printConfig(cfg);

  if (!expectSerial.empty() && cfg.serial != expectSerial) {
    fprintf(stderr,
            "FAIL: config serial %s != expected %s (stale cache config? "
            "enlyzeam holds ~47 factory configs)\n",
            cfg.serial.c_str(), expectSerial.c_str());
    return 1;
  }

  if (!dumpPath.empty()) return checkDump(cfg, dumpPath);
  return 0;
}