#include "geo_format.h"

#include <cmath>
#include <cstdio>

namespace roro::gnss {

namespace {
const char* hemisphere(double degrees, bool latitude) {
    return latitude ? (degrees < 0 ? "S" : "N") : (degrees < 0 ? "W" : "E");
}
}  // namespace

std::string formatDecimal(double degrees, bool latitude) {
    char buf[32];
    std::snprintf(buf, sizeof buf, "%.5f\xC2\xB0 %s", std::fabs(degrees), hemisphere(degrees, latitude));
    return buf;
}

std::string formatDms(double degrees, bool latitude) {
    // Work in tenths of a second, so rounding carries into minutes and degrees.
    long tenths = std::lround(std::fabs(degrees) * 36000.0);
    long d = tenths / 36000, m = tenths / 600 % 60, s10 = tenths % 600;
    char buf[40];
    std::snprintf(buf, sizeof buf, "%ld\xC2\xB0 %02ld' %02ld.%ld\" %s", d, m, s10 / 10, s10 % 10,
                  hemisphere(degrees, latitude));
    return buf;
}

std::string maidenhead(double latitude, double longitude) {
    double lon = std::fmin(std::fmax(longitude + 180.0, 0.0), 359.999999);
    double lat = std::fmin(std::fmax(latitude + 90.0, 0.0), 179.999999);
    std::string out;
    out += static_cast<char>('A' + static_cast<int>(lon / 20));
    out += static_cast<char>('A' + static_cast<int>(lat / 10));
    out += static_cast<char>('0' + static_cast<int>(std::fmod(lon, 20) / 2));
    out += static_cast<char>('0' + static_cast<int>(std::fmod(lat, 10)));
    out += static_cast<char>('a' + static_cast<int>(std::fmod(lon, 2) * 12));
    out += static_cast<char>('a' + static_cast<int>(std::fmod(lat, 1) * 24));
    return out;
}

SkyPoint skyPosition(int azimuthDeg, int elevationDeg, int cx, int cy, int radius) {
    int elevation = elevationDeg < 0 ? 0 : elevationDeg > 90 ? 90 : elevationDeg;
    double r = radius * (90 - elevation) / 90.0;
    double az = azimuthDeg * M_PI / 180.0;
    return {cx + static_cast<int>(std::lround(r * std::sin(az))), cy - static_cast<int>(std::lround(r * std::cos(az)))};
}

}  // namespace roro::gnss