"""
latlon_utm.py: latitude and longitude to UTM, and the scale error of the projection.

EECE 5554 Robotics Sensing and Navigation, demo for Lecture 03.

UTM cuts the Earth into 60 zones, each 6 degrees of longitude wide, and projects
each zone flat with a Transverse Mercator projection on the WGS 84 ellipsoid.
Distances on the map are exact only on two lines about 180 km either side of the
zone's central meridian. The scale factor k says how much the map stretches:
k = 0.9996 on the central meridian (0.04 percent too short) and above 1 near the
zone edges.

The formulas are the series from Snyder, "Map Projections: A Working Manual"
(USGS Professional Paper 1395, 1987), equations 8-9 to 8-11. This script
reproduces Snyder's worked example exactly. Compare its output with the
Python `utm` package you used in Lab 1.

Run it from a terminal:
    python3 latlon_utm.py 47.6265 -122.3381
"""
import json
import math
import sys

# WGS 84 ellipsoid
A_WGS84 = 6378137.0
F_WGS84 = 1 / 298.257223563
K0 = 0.9996                     # scale on the central meridian
FALSE_EASTING = 500000.0
FALSE_NORTHING_SOUTH = 10000000.0
BAND_LETTERS = "CDEFGHJKLMNPQRSTUVWX"   # 8-degree bands from 80 S; no I or O


def zone_number(lat, lon):
    """UTM zone 1-60, including the Norway and Svalbard exceptions."""
    if 56 <= lat < 64 and 3 <= lon < 12:
        return 32
    if 72 <= lat <= 84 and lon >= 0:
        if lon < 9:
            return 31
        if lon < 21:
            return 33
        if lon < 33:
            return 35
        if lon < 42:
            return 37
    lon = (lon + 180) % 360 - 180
    return int((lon + 180) // 6) % 60 + 1


def zone_letter(lat):
    """Latitude band letter, C (80 S) to X (72 N to 84 N)."""
    if not -80 <= lat <= 84:
        return None
    return BAND_LETTERS[min(int((lat + 80) // 8), 19)]


def central_meridian(zone):
    return -183.0 + 6.0 * zone


def project(lat, lon, lon0, a=A_WGS84, f=F_WGS84, k0=K0):
    """Transverse Mercator forward projection. Returns x, y (meters) and the point scale k."""
    e2 = f * (2 - f)
    ep2 = e2 / (1 - e2)
    phi = math.radians(lat)
    lam = math.radians(lon - lon0)
    n = a / math.sqrt(1 - e2 * math.sin(phi) ** 2)
    t = math.tan(phi) ** 2
    c = ep2 * math.cos(phi) ** 2
    aa = lam * math.cos(phi)
    e4, e6 = e2 * e2, e2 * e2 * e2
    m = a * ((1 - e2 / 4 - 3 * e4 / 64 - 5 * e6 / 256) * phi
             - (3 * e2 / 8 + 3 * e4 / 32 + 45 * e6 / 1024) * math.sin(2 * phi)
             + (15 * e4 / 256 + 45 * e6 / 1024) * math.sin(4 * phi)
             - (35 * e6 / 3072) * math.sin(6 * phi))
    x = k0 * n * (aa + (1 - t + c) * aa ** 3 / 6
                  + (5 - 18 * t + t * t + 72 * c - 58 * ep2) * aa ** 5 / 120)
    y = k0 * (m + n * math.tan(phi) * (aa ** 2 / 2
                                       + (5 - t + 9 * c + 4 * c * c) * aa ** 4 / 24
                                       + (61 - 58 * t + t * t + 600 * c - 330 * ep2) * aa ** 6 / 720))
    k = k0 * (1 + (1 + c) * aa ** 2 / 2
              + (5 - 4 * t + 42 * c + 13 * c * c - 28 * ep2) * aa ** 4 / 24
              + (61 - 148 * t + 16 * t * t) * aa ** 6 / 720)
    return x, y, k


def latlon_to_utm(lat, lon, zone=None):
    """Return easting, northing, zone, letter, scale k, and the central meridian."""
    if zone is None:
        zone = zone_number(lat, lon)
    lon0 = central_meridian(zone)
    x, y, k = project(lat, lon, lon0)
    easting = x + FALSE_EASTING
    northing = y + (FALSE_NORTHING_SOUTH if lat < 0 else 0.0)
    return {"easting": easting, "northing": northing, "zone": zone, "letter": zone_letter(lat),
            "k": k, "lon0": lon0, "dist_from_cm_km": x / 1000.0}


def scale_profile(lat, zone, n=121):
    """Scale factor across the whole zone at this latitude, edge to edge."""
    lon0 = central_meridian(zone)
    out = []
    for i in range(n):
        dlon = -3.0 + 6.0 * i / (n - 1)
        x, _, k = project(lat, lon0 + dlon, lon0)
        out.append({"dlon": dlon, "x_km": x / 1000.0, "k": k})
    return out


def demo_api(params_json):
    """Web page entry point. Input: {"lat": .., "lon": ..}."""
    p = json.loads(params_json or "{}")
    lat = max(-80.0, min(84.0, float(p.get("lat", 47.6265))))
    lon = float(p.get("lon", -122.3381))
    lon = (lon + 180) % 360 - 180
    r = latlon_to_utm(lat, lon)
    k = r["k"]
    r["scale_ppm"] = (k - 1) * 1e6
    r["mm_per_km"] = (k - 1) * 1e6          # 1 ppm = 1 mm per km
    r["profile"] = scale_profile(lat, r["zone"])
    r["lat"], r["lon"] = lat, lon
    # the same point written in the neighboring zone: same place, very different easting
    side = 1 if lon >= r["lon0"] else -1
    nz = (r["zone"] - 1 + side) % 60 + 1
    nb = latlon_to_utm(lat, lon, zone=nz)
    r["neighbor"] = {"zone": nz, "easting": nb["easting"], "northing": nb["northing"], "k": nb["k"]}
    # where k = 1 exactly (the two lines of true scale)
    prof = r["profile"]
    r["true_scale_km"] = [a["x_km"] + (q["x_km"] - a["x_km"]) * (1 - a["k"]) / (q["k"] - a["k"])
                          for a, q in zip(prof, prof[1:]) if (a["k"] - 1) * (q["k"] - 1) < 0]
    return json.dumps(r)


def main(argv):
    lat = float(argv[1]) if len(argv) > 1 else 47.6265
    lon = float(argv[2]) if len(argv) > 2 else -122.3381
    r = latlon_to_utm(lat, lon)
    print("lat {:.6f}, lon {:.6f}".format(lat, lon))
    print("zone {}{}  easting {:.2f} m  northing {:.2f} m".format(r["zone"], r["letter"], r["easting"], r["northing"]))
    print("scale k = {:.7f}  ({:+.0f} ppm, {:+.3f} m per km)".format(r["k"], (r["k"] - 1) * 1e6, (r["k"] - 1) * 1e3))


if __name__ == "__main__":
    main(sys.argv)