#pragma once

#include <string>
#include <vector>

namespace fgc {

// Fitting the yaw map from operator sightings of known landmarks.
//
// The gimbal is aimed at something whose true bearing is known (a mast, a peak, a
// road junction on the map), and the encoder position is recorded against that
// bearing. Two sightings determine the line exactly; more are least-squares
// fitted. Nothing here needs an IMU, which is the point: not every tower has one,
// and IMU yaw drifts badly enough that it was never a trustworthy heading source.
//
// Kept I/O-free in fgc_core so the fit is unit-testable without hardware.

struct HeadingSighting {
    long   counts      = 0;    // yaw encoder position when the landmark was centred
    double heading_deg = 0.0;  // its true bearing, degrees (0 = north)
};

struct HeadingFitResult {
    bool        ok = false;
    std::string error;           // operator-facing reason when !ok
    double      counts_per_deg = 0.0;
    double      deg_at_center  = 0.0;  // bearing at center_count, normalised to [0,360)
    double      r2             = 0.0;
    int         n              = 0;
    double      span_deg       = 0.0;  // angular baseline actually used
};

// Shortest baseline we will fit. Two sightings a few degrees apart put the whole
// scale on a handful of counts, so a small aiming error swings counts_per_deg
// wildly - and the result looks perfectly healthy (two points always fit exactly,
// R^2 = 1). Refusing is the only way the operator finds out.
inline constexpr double kMinHeadingBaselineDeg = 10.0;

// Fit counts_per_deg and the bearing at `center_count`. Sightings may be given in
// any order and may span north.
HeadingFitResult fitHeading(const std::vector<HeadingSighting>& sightings, long center_count);

// Parse "<counts>:<deg>" tokens (the form the TUI wizard emits). Returns false and
// sets `error` on the first malformed token.
bool parseHeadingSightings(const std::vector<std::string>& tokens,
                           std::vector<HeadingSighting>& out, std::string& error);

}  // namespace fgc