#include "fgc/Calibration.h"

#include <cmath>

namespace fgc {

LinearFit linearFit(const std::vector<std::pair<double, double>>& xy) {
    LinearFit f;
    f.n = static_cast<int>(xy.size());
    if (xy.size() < 2) return f;

    double sx = 0, sy = 0;
    for (const auto& p : xy) { sx += p.first; sy += p.second; }
    const double mx = sx / xy.size(), my = sy / xy.size();

    double sxx = 0, sxy = 0, syy = 0;
    for (const auto& p : xy) {
        const double dx = p.first - mx, dy = p.second - my;
        sxx += dx * dx;
        sxy += dx * dy;
        syy += dy * dy;
    }
    if (sxx <= 0.0) return f;  // no spread in x -> undefined slope

    f.slope     = sxy / sxx;
    f.intercept = my - f.slope * mx;
    f.r2        = (syy > 0.0) ? (sxy * sxy) / (sxx * syy) : 1.0;
    f.ok        = true;
    return f;
}

double circularMeanDeg(const std::vector<double>& degs) {
    if (degs.empty()) return 0.0;
    double sc = 0, ss = 0;
    for (double d : degs) {
        const double r = d * M_PI / 180.0;
        sc += std::cos(r);
        ss += std::sin(r);
    }
    return std::atan2(ss, sc) * 180.0 / M_PI;
}

double unwrapNear(double prev, double deg) {
    while (deg - prev > 180.0) deg -= 360.0;
    while (deg - prev < -180.0) deg += 360.0;
    return deg;
}

}  // namespace fgc