// Neck calibration — M4 step 4 (docs/m4-hardening.md §4).
//
// Guided lever-arm solve: with the torso still, head rotation about the
// neck pivot makes the IMU's accelerometer see the rigid-body terms of its
// offset from the pivot. Per sample, in the head frame:
//
//   f  =  wdot x r  +  w x (w x r)  +  u(q)  +  b
//
// f = measured specific force (m/s^2), w/wdot = head-frame angular
// rate/acceleration, u(q) = world up rotated into the head frame times g
// (the AHRS orientation supplies it), r = pivot->IMU lever arm, b = accel
// bias (nuisance). Linear in (r, b): stacked 6-unknown least squares over
// a guided capture — yaw shakes observe the forward+lateral components,
// pitch nods observe up+forward.
//
// Gates (reject + ask to redo rather than accept garbage):
//  - excitation: each phase must actually contain its rotation;
//  - lateral ~ 0: a head on a neck has no sideways lever arm — a big |x|
//    means torso sway or a bad capture;
//  - residual: torso translation violates the fixed-pivot model and shows
//    up as unexplained acceleration.
//
// The solve yields pivot->IMU; rendering wants pivot->eye-midpoint
// (the neck model translates the eye midpoint). Caller passes the config
// imu position (IMU location in head frame, head origin = eye midpoint)
// and the result reports both.

#pragma once

#include <cstdint>
#include <string>
#include <vector>

#include "track/ahrs.h"  // Quat

namespace sauna {

class NeckCalibrator {
 public:
  enum Phase { kIdle = 0, kYaw = 1, kPitch = 2 };

  // Feed every sample while capturing (AHRS tap, head frame, raw gyro —
  // no Mahony correction mixed in). accHeadG in g, dt in seconds.
  void feed(const double omegaHead[3], const double accHeadG[3],
            const Quat& q, double dt, int phase);

  struct Result {
    bool ok = false;
    std::string message;       // human verdict (gate that failed, or stats)
    double rImuM[3] = {};      // pivot->IMU, head frame (m)
    double rEyeM[3] = {};      // pivot->eye-midpoint = rImu - imuPos
    double neckForwardMm = 0;  // -rEye.z * 1000 (head +z is back)
    double neckUpMm = 0;       // +rEye.y * 1000
    double accBias[3] = {};    // solved nuisance (m/s^2)
    double residualRms = 0;    // m/s^2 after the fit
    double yawRms = 0, pitchRms = 0;  // excitation (rad/s)
    uint64_t samples = 0;
  };

  // imuPosHead = config imu frame position (IMU in head frame, m). Zero =
  // IMU assumed at the head origin (the message notes the assumption).
  Result solve(const double imuPosHead[3]) const;

  void clear();

 private:
  struct Sample {
    double t;         // accumulated time (s)
    double w[3];      // rad/s, head frame
    double f[3];      // specific force, m/s^2, head frame
    double up[3];     // world up in head frame (unit), from q
    int phase;
  };
  std::vector<Sample> samples_;
  double tAcc_ = 0.0;
};

}  // namespace sauna