// UTM.cpp

// Original Javascript by Chuck Taylor
// Port to C++ by Alex Hajnal
// Adapted for HDmaps usage by Juan Zaratiegui
//
// (Port license follows)
// This is a simple port of the code on the Geographic/UTM Coordinate Converter
// (1) page from Javascript to C++. Using this you can easily convert between
// UTM and WGS84 (latitude and longitude). Accuracy seems to be around 50cm (I
// suspect rounding errors are limiting precision). This code is provided as-is
// and has been minimally tested; enjoy but use at your own risk! The license
// for UTM.cpp and UTM.h is the same as the original Javascript: "The C++ source
// code in UTM.cpp and UTM.h may be copied and reused without restriction."
//
// 1) http://home.hiwaay.net/~taylorc/toolbox/geography/geoutm.html

#include "geo/private/coord/utm.h"
#include "geo/private/util/angle_util.h"

namespace ns
{
namespace geo
{

namespace
{
// WGS84 ellipsoid model constants
constexpr double semiMajorAxis = 6378137.0;
constexpr double semiMinorAxis = 6356752.31424518;

// metric adjusments
constexpr double x_adjust = 500000.0;
constexpr double y_sh_adjust = 10000000.0;

// Precomputed
constexpr auto semiMinorAxis2 = semiMinorAxis * semiMinorAxis;
constexpr auto semiMajorAxis2 = semiMajorAxis * semiMajorAxis;
constexpr auto ep2 = (semiMajorAxis2 - semiMinorAxis2) / semiMinorAxis2;
constexpr auto semiAxisDiff = semiMajorAxis - semiMinorAxis;
constexpr auto semiAxisSum = semiMajorAxis + semiMinorAxis;
constexpr auto n = semiAxisDiff / semiAxisSum;
constexpr auto n2 = n * n;
constexpr auto n3 = n2 * n;
constexpr auto n4 = n2 * n2;
constexpr auto n5 = n3 * n2;

constexpr auto alpha = (semiAxisSum / 2.0) * (1.0 + (n2 / 4.0) + (n4 / 64.0));
constexpr auto beta = (-3.0 * n / 2.0) + (9.0 * n3 / 16.0) + (-3.0 * n5 / 32.0);
constexpr auto gamma = (15.0 * n2 / 16.0) + (-15.0 * n4 / 32.0);
constexpr auto delta = (-35.0 * n3 / 48.0) + (105.0 * n5 / 256.0);
constexpr auto epsilon = (315.0 * n4 / 512.0);

constexpr auto alpha_ = alpha;
constexpr auto beta_ =
    (3.0 * n / 2.0) + (-27.0 * n3 / 32.0) + (269.0 * n5 / 512.0);
constexpr auto gamma_ = (21.0 * n2 / 16.0) + (-55.0 * n4 / 32.0);
constexpr auto delta_ = (151.0 * n3 / 96.0) + (-417.0 * n5 / 128.0);
constexpr auto epsilon_ = (1097.0 * n4 / 512.0);

constexpr double UTMScaleFactor = 0.9996;

constexpr double DegToRad(const double deg)
{
    return (deg / 180.0) * c_pi();
}

constexpr double RadToDeg(const double rad)
{
    return (rad / c_pi()) * 180.0;
}

// UTMCentralMeridian
// Determines the central meridian for the given UTM zone.
//
// Inputs:
//     zone - An integer value designating the UTM zone, range [1,60].
//
// Returns:
//   The central meridian for the given UTM zone, in radians
//   Range of the central meridian is the radian equivalent of [-177,+177].
constexpr double UTMCentralMeridian(const unsigned zone)
{
    return DegToRad(-183.0 + (static_cast<double>(zone) * 6.0));
}

// ArcLengthOfMeridian
// Computes the ellipsoidal distance from the equator to a point at a
// given latitude.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//     phi - Latitude of the point, in radians.
//
// Globals:
//     semiMajorAxis - Ellipsoid model major axis.
//     semiMinorAxis - Ellipsoid model minor axis.
//
// Returns:
//     The ellipsoidal distance of the point from the equator, in meters.
double ArcLengthOfMeridian(const double phi)
{
    /* Now calculate the sum of the series and return */
    return alpha *
           (phi + (beta * std::sin(2.0 * phi)) + (gamma * std::sin(4.0 * phi)) +
            (delta * std::sin(6.0 * phi)) + (epsilon * std::sin(8.0 * phi)));
}

// MapLatLonToXY
// Converts a latitude/longitude pair to x and y coordinates in the
// Transverse Mercator projection.  Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
// GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//    phi - Latitude of the point, in radians.
//    lambda - Longitude of the point, in radians.
//    lambda0 - Longitude of the central meridian to be used, in radians.
//
// Returns a tuple of the values:
//    x - The x coordinate of the computed point.
//    y - The y coordinate of the computed point.
//
std::tuple<double, double> MapLatLonToXY(const double phi, const double lambda,
                                         const double lambda0)
{
    const double cosphi = std::cos(phi);
    const double cosphi2 = cosphi * cosphi;
    const double cosphi4 = cosphi2 * cosphi2;
    const double cosphi6 = cosphi4 * cosphi2;

    /* Precalculate nu2 */
    const double nu2 = ep2 * cosphi2;
    const double nu2_2 = nu2 * nu2;

    /* Precalculate N */
    const double N = semiMajorAxis2 / (semiMinorAxis * std::sqrt(1 + nu2));

    /* Precalculate t */
    const double t = std::tan(phi);
    const double t2 = t * t;
    const double t4 = t2 * t2;
    const double t6 = t4 * t2;

    const double t2nu2 = t2 * nu2;

    /* Precalculate l */
    const double l = lambda - lambda0;
    const double l2 = l * l;
    const double l4 = l2 * l2;
    const double l6 = l2 * l4;

    /* Precalculate coefficients for l**n in the equations below
       so a normal human being can read the expressions for easting
       and northing
       -- l**1 and l**2 have coefficients of 1.0 */
    const double l3coef = 1.0 - t2 + nu2;
    const double l4coef = 5.0 - t2 + 9 * nu2 + 4.0 * nu2_2;
    const double l5coef = 5.0 - 18.0 * t2 + t4 + 14.0 * nu2 - 58.0 * t2nu2;
    const double l6coef = 61.0 - 58.0 * t2 + t4 + 270.0 * nu2 - 330.0 * t2nu2;
    const double l7coef = 61.0 - 479.0 * t2 + 179.0 * t4 - t6;
    const double l8coef = 1385.0 - 3111.0 * t2 + 543.0 * t4 - t6;

    /* Calculate easting (x) */
    const double x =
        N * cosphi * l *
        (1.0 + (cosphi2 * l3coef * l2 / 6.0) + (cosphi4 * l5coef * l4 / 120.0) +
         (cosphi6 * l7coef * l6 / 5040.0));

    /* Calculate northing (y) */
    const double y =
        ArcLengthOfMeridian(phi) +
        (t * N * cosphi2 * l2 / 2.0) * (1.0 + (cosphi2 * l4coef * l2 / 12.0) +
                                        (cosphi4 * l6coef * l4 / 360.0) +
                                        (cosphi6 * l8coef * l6 / 20160.0));

    return std::make_tuple(x, y);
}

// FootpointLatitude
//
// Computes the footpoint latitude for use in converting transverse
// Mercator coordinates to ellipsoidal coordinates.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
//   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//   y - The UTM northing coordinate, in meters.
//
// Returns:
//   The footpoint latitude, in radians.
double FootpointLatitude(const double y)
{
    /* Precalculate y_ (Eq. 10.23) */
    const auto y_ = y / alpha_;

    /* Now calculate the sum of the series (Eq. 10.21) */
    return y_ + (beta_ * std::sin(2.0 * y_)) + (gamma_ * std::sin(4.0 * y_)) +
           (delta_ * std::sin(6.0 * y_)) + (epsilon_ * std::sin(8.0 * y_));
}

// MapXYToLatLon
// Converts x and y coordinates in the Transverse Mercator projection to
// a latitude/longitude pair.  Note that Transverse Mercator is not
// the same as UTM; a scale factor is required to convert between them.
//
// Reference: Hoffmann-Wellenhof, B., Lichtenegger, H., and Collins, J.,
//   GPS: Theory and Practice, 3rd ed.  New York: Springer-Verlag Wien, 1994.
//
// Inputs:
//   x - The easting of the point, in meters.
//   y - The northing of the point, in meters.
//   lambda0 - Longitude of the central meridian to be used, in radians.
//
// Returns a tuple of the values:
//   phi    - Latitude in radians.
//   lambda - Longitude in radians.
//
std::tuple<double, double> MapXYToLatLon(const double x, const double y,
                                         const double lambda0)
{
    /* Get the value of phif, the footpoint latitude. */
    const auto phif = FootpointLatitude(y);

    const double cosphi = std::cos(phif);
    const double cosphi2 = cosphi * cosphi;

    /* Precalculate nu2 */
    const double nuf2 = ep2 * cosphi2;
    const double nuf22 = nuf2 * nuf2;

    /* Precalculate Nf and initialize Nfpow */
    const double Nf = semiMajorAxis2 / (semiMinorAxis * std::sqrt(1 + nuf2));
    double Nfpow = Nf;

    const double tanphi = std::tan(phif);
    const double tanphi2 = tanphi * tanphi;
    const double tanphi4 = tanphi2 * tanphi2;
    const double tanphi6 = tanphi4 * tanphi2;

    /* Precalculate fractional coefficients for x**n in the equations
       below to simplify the expressions for latitude and longitude. */
    const auto x1frac = 1.0 / (Nfpow * cosphi);

    Nfpow *= Nf; /* now equals Nf**2) */
    const auto x2frac = tanphi / (2.0 * Nfpow);

    Nfpow *= Nf; /* now equals Nf**3) */
    const auto x3frac = 1.0 / (6.0 * Nfpow * cosphi);

    Nfpow *= Nf; /* now equals Nf**4) */
    const auto x4frac = tanphi / (24.0 * Nfpow);

    Nfpow *= Nf; /* now equals Nf**5) */
    const auto x5frac = 1.0 / (120.0 * Nfpow * cosphi);

    Nfpow *= Nf; /* now equals Nf**6) */
    const auto x6frac = tanphi / (720.0 * Nfpow);

    Nfpow *= Nf; /* now equals Nf**7) */
    const auto x7frac = 1.0 / (5040.0 * Nfpow * cosphi);

    Nfpow *= Nf; /* now equals Nf**8) */
    const auto x8frac = tanphi / (40320.0 * Nfpow);

    /* Precalculate polynomial coefficients for x**n.
       -- x**1 does not have a polynomial coefficient. */
    const auto x2poly = -1.0 - nuf2;

    const auto x3poly = -1.0 - 2 * tanphi2 - nuf2;

    const auto x4poly = 5.0 + 3.0 * tanphi2 + 6.0 * nuf2 -
                        6.0 * tanphi2 * nuf2 - 3.0 * nuf22 -
                        9.0 * tanphi2 * nuf22;

    const auto x5poly = 5.0 + 28.0 * tanphi2 + 24.0 * tanphi4 + 6.0 * nuf2 +
                        8.0 * tanphi2 * nuf2;

    const auto x6poly = -61.0 - 90.0 * tanphi2 - 45.0 * tanphi4 - 107.0 * nuf2 +
                        162.0 * tanphi2 * nuf2;

    const auto x7poly =
        -61.0 - 662.0 * tanphi2 - 1320.0 * tanphi4 - 720.0 * tanphi6;

    const auto x8poly =
        1385.0 + 3633.0 * tanphi2 + 4095.0 * tanphi4 + 1575 * tanphi6;

    const auto x2 = x * x;
    const auto x3 = x2 * x;
    const auto x4 = x2 * x2;
    const auto x5 = x3 * x2;
    const auto x6 = x3 * x3;
    const auto x7 = x4 * x3;
    const auto x8 = x4 * x4;

    /* Calculate latitude */
    const auto phi = phif + x2frac * x2poly * x2 + x4frac * x4poly * x4 +
                     x6frac * x6poly * x6 + x8frac * x8poly * x8;

    /* Calculate longitude */
    const auto lambda = lambda0 + x1frac * x + x3frac * x3poly * x3 +
                        x5frac * x5poly * x5 + x7frac * x7poly * x7;
    return std::make_tuple(phi, lambda);
}

} // namespace

std::tuple<unsigned, double, double> LonLatToUTMXY(const double lon,
                                                   const double lat,
                                                   unsigned zone)
{
    if (zone == 0)
    {
        zone = static_cast<unsigned>(std::floor((lon + 180.0) / 6.0)) + 1;
    }

    double x, y;
    std::tie(x, y) =
        MapLatLonToXY(DegToRad(lat), DegToRad(lon), UTMCentralMeridian(zone));

    // Adjust easting and northing for UTM system.
    x *= UTMScaleFactor;
    x += x_adjust;
    y *= UTMScaleFactor;
    if (y < 0.0)
    {
        y = y + y_sh_adjust;
    }

    return std::make_tuple(zone, x, y);
}
std::tuple<double, double> UTMXYToLonLat(const unsigned zone,
                                         const bool southen_hemisphere,
                                         double x, double y)
{
    x -= x_adjust;
    x /= UTMScaleFactor;
    if (southen_hemisphere)
    {
        y -= y_sh_adjust;
    }
    y /= UTMScaleFactor;
    const double central_meridian = UTMCentralMeridian(zone);
    const auto& latlon = MapXYToLatLon(x, y, central_meridian);
    return std::make_tuple(RadToDeg(std::get<1>(latlon)),
                           RadToDeg(std::get<0>(latlon)));
}

std::tuple<bool, double, double> getUTMZoneBorderLongitude(unsigned zone)
{
    if (zone >= 1 && zone <= 60)
    {
        double left = 180.0 + (zone - 1) * 6.0;
        if (left > 360)
            left -= 360;
        double right = 180 + (zone)*6.0;
        if (right > 360)
            right -= 360;

        return std::make_tuple(true, left, right);
    }

    return std::make_tuple(false, 0, 0);
}

} // namespace geo
} // namespace ns