#include "fgc/HeadingFit.h"

#include "fgc/Calibration.h"
#include "fgc/Geometry.h"

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <stdexcept>
#include <string>

namespace fgc {

HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count) {
    HeadingFitResult r;
    r.n = static_cast<int>(sightings.size());

    if (sightings.size() < 2) {
        r.error = "need at least two sightings (aim at a second landmark)";
        return r;
    }

    // Sort by encoder position so the bearings can be unwrapped along the axis.
    // Sighting order is whatever the operator found convenient; the axis is the
    // only thing that varies monotonically.
    std::vector<HeadingSighting> pts = sightings;
    std::sort(pts.begin(), pts.end(),
              [](const HeadingSighting& a, const HeadingSighting& b) { return a.counts < b.counts; });

    // Unwrap so a set spanning north fits as a straight line instead of folding
    // at 360 (e.g. 350 and 10 become 350 and 370, not 350 and 10).
    std::vector<std::pair<double, double>> xy;  // x = bearing, y = counts
    xy.reserve(pts.size());
    double prev = wrap180(pts.front().heading_deg);
    for (std::size_t i = 0; i < pts.size(); ++i) {
        const double deg = (i == 0) ? prev : unwrapNear(prev, pts[i].heading_deg);
        prev             = deg;
        xy.emplace_back(deg, static_cast<double>(pts[i].counts));
    }

    auto [lo, hi] = std::minmax_element(
        xy.begin(), xy.end(), [](const auto& a, const auto& b) { return a.first < b.first; });
    r.span_deg = hi->first - lo->first;
    if (r.span_deg < kMinHeadingBaselineDeg) {
        r.error = "sightings are only " + std::to_string(static_cast<int>(r.span_deg)) +
                  " deg apart; use landmarks at least " +
                  std::to_string(static_cast<int>(kMinHeadingBaselineDeg)) +
                  " deg apart (further is better)";
        return r;
    }

    const LinearFit fit = linearFit(xy);
    if (!fit.ok) {
        r.error = "degenerate sightings (all at the same bearing)";
        return r;
    }
    // Distinct bearings at the same encoder position: the axis did not move
    // between sightings, so one of the bearings is wrong.
    if (std::abs(fit.slope) < 1e-6) {
        r.error = "sightings give no encoder travel between bearings; did the gimbal move?";
        return r;
    }

    r.counts_per_deg = fit.slope;
    // The map is anchored on the homed centre, so what gets stored is the bearing
    // that centre points at - solve the fitted line for it.
    r.deg_at_center = wrap360((static_cast<double>(center_count) - fit.intercept) / fit.slope);
    r.r2            = fit.r2;
    r.ok            = true;
    return r;
}

bool parseHeadingSightings(const std::vector<std::string>& tokens,
                           std::vector<HeadingSighting>& out, std::string& error) {
    out.clear();
    for (const std::string& t : tokens) {
        const auto colon = t.find(':');
        if (colon == std::string::npos) {
            error = "expected <counts>:<deg>, got '" + t + "'";
            return false;
        }
        HeadingSighting s;
        try {
            std::size_t used = 0;
            s.counts         = std::stol(t.substr(0, colon), &used);
            if (used != colon) throw std::invalid_argument("trailing");
            const std::string deg = t.substr(colon + 1);
            s.heading_deg         = std::stod(deg, &used);
            if (used != deg.size()) throw std::invalid_argument("trailing");
        } catch (const std::exception&) {
            error = "bad sighting '" + t + "' (expected <counts>:<deg>)";
            return false;
        }
        out.push_back(s);
    }
    return true;
}

}  // namespace fgc