#pragma once

#include <utility>
#include <vector>

namespace fgc {

// Pure math helpers for `gimbal calib`. Kept I/O-free in fgc_core so the fit can
// be unit-tested independently of the threaded routine that drives the hardware.

struct LinearFit {
    bool   ok = false;     // false if < 2 points or x has no spread
    double slope = 0;      // y = slope*x + intercept
    double intercept = 0;
    double r2 = 0;         // coefficient of determination (1 = perfect)
    int    n = 0;
};

// Least-squares fit of y over x. For calibration: x = measured degrees,
// y = motor encoder counts ⇒ slope = counts_per_deg, intercept = zero_count.
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy);

// Circular mean of angles in degrees, robust to ±180° wrap (e.g. IMU heading).
// Returns a value in (-180, 180]. Empty input returns 0.
double circularMeanDeg(const std::vector<double>& degs);

// Phase-unwrap `deg` relative to the previous (already-unwrapped) sample: shift it
// by whole turns so it lands within ±180° of `prev`. Feeding a smoothly-swept
// angle through this in sequence removes the 0/360 (or ±180) discontinuity, so the
// calibration sees a continuous curve for the linear fit.
double unwrapNear(double prev, double deg);

}  // namespace fgc