#include "fgc/Geometry.h"

#include <algorithm>
#include <cmath>

namespace fgc {

double wrap360(double deg) {
    double w = std::fmod(deg, 360.0);
    if (w < 0.0) w += 360.0;
    // fmod of a tiny negative can round to exactly 360.0 after the += above.
    if (w >= 360.0) w = 0.0;
    return w;
}

double wrap180(double deg) {
    double w = wrap360(deg);
    return w >= 180.0 ? w - 360.0 : w;
}

long AxisMap::toCounts(double deg) const {
    const double lo      = std::min(min_deg, max_deg);
    const double hi      = std::max(min_deg, max_deg);
    const double clamped = std::clamp(deg, lo, hi);
    return std::lround(static_cast<double>(center_count) +
                       (clamped - deg_at_center) * counts_per_deg);
}

double AxisMap::toDeg(long counts) const {
    if (std::abs(counts_per_deg) < 1e-9) return deg_at_center;
    return deg_at_center +
           (static_cast<double>(counts) - static_cast<double>(center_count)) / counts_per_deg;
}

double AxisMap::headingDeg(long counts) const { return wrap360(toDeg(counts)); }

std::optional<long> AxisMap::headingToCounts(double heading_deg) const {
    if (std::abs(counts_per_deg) < 1e-9) return std::nullopt;
    // The travel is under a full turn, so wrapping the offset into [-180, 180)
    // yields the one candidate that could be reachable - there is never a second
    // solution to choose between.
    const double deg = deg_at_center + wrap180(heading_deg - deg_at_center);
    const double lo  = std::min(min_deg, max_deg);
    const double hi  = std::max(min_deg, max_deg);
    if (deg < lo || deg > hi) return std::nullopt;
    return toCounts(deg);
}

}  // namespace fgc