#pragma once

#include <optional>

namespace fgc {

// Normalise an angle to [0, 360) / [-180, 180). Free functions because headings
// are compared and differenced in several places outside AxisMap.
double wrap360(double deg);
double wrap180(double deg);

// Affine map between degrees and absolute encoder counts for one axis, anchored
// on the *mechanical centre* found by homing:
//
//   counts = center_count + (deg - deg_at_center) * counts_per_deg
//   deg    = deg_at_center + (counts - center_count) / counts_per_deg
//
// The firmware speaks only in encoder counts. `center_count` is the count the
// firmware assigns to the midpoint of the homed travel (it is told this value,
// see the CENTER command), so it is reproducible across power cycles - unlike the
// free-running encoder origin it replaced. `deg_at_center` is what that midpoint
// points at: a true compass heading for yaw (0 = north), an elevation above level
// for pitch (+ = up).
//
// min_deg/max_deg are DERIVED from the homed soft limits after each home, not
// configured - the firmware's limits are the only place they can be correct.
// counts_per_deg may be negative to flip the direction sense.
struct AxisMap {
    double counts_per_deg = 1.0;        // calibrate on the rig
    long   center_count   = 500000;     // count at the homed mechanical centre
    double deg_at_center  = 0.0;        // heading/elevation there
    double min_deg        = -100000.0;  // soft clamp applied by toCounts (degrees)
    double max_deg        = 100000.0;

    // Degrees -> counts. Clamps `deg` to [min_deg, max_deg] and rounds.
    long toCounts(double deg) const;

    // Counts -> degrees. Returns deg_at_center if counts_per_deg is ~0
    // (misconfigured), i.e. it degrades to "we are at the centre" rather than to
    // an arbitrary zero.
    double toDeg(long counts) const;

    // Yaw only: the same reading as a compass heading in [0, 360).
    double headingDeg(long counts) const;

    // Yaw only: counts for a true heading, taking the short way round. Returns
    // nullopt when the heading lies in the arc the gimbal cannot reach - the
    // travel is under 360 deg, so there is always a dead sector, and pointing at
    // the nearest limit instead would silently image the wrong direction.
    std::optional<long> headingToCounts(double heading_deg) const;
};

// Per-axis maps for the two-axis gimbal.
struct Geometry {
    AxisMap yaw;
    AxisMap pitch;
};

}  // namespace fgc