"""
nmea_gga.py: read GGA sentences the way the Lab 1 driver does.

EECE 5554 Robotics Sensing and Navigation, demo for Lab 1.

A GNSS receiver prints NMEA 0183 text, one sentence per line. The GGA
sentence carries the position fix. This script checks each sentence's
checksum, splits it into fields, and turns the latitude and longitude
from ddmm.mmmm into decimal degrees.

Run it from a terminal:
    python3 nmea_gga.py              decode the built-in sample lines
    python3 nmea_gga.py my_log.txt   decode every GGA line in a file

The demo web page calls demo_api() with the text you paste.
"""
import json
import math
import sys

# GGA fix quality codes (NMEA 0183)
FIX_QUALITY = {
    0: "No fix (invalid)",
    1: "GPS fix (standalone)",
    2: "Differential GPS fix",
    3: "PPS fix",
    4: "RTK fixed",
    5: "RTK float",
    6: "Estimated (dead reckoning)",
    7: "Manual input",
    8: "Simulation",
}

# Names of the 14 data fields after the sentence ID, in order
FIELD_NAMES = [
    "UTC time (hhmmss.ss)",
    "Latitude (ddmm.mmmm)",
    "N or S",
    "Longitude (dddmm.mmmm)",
    "E or W",
    "Fix quality",
    "Satellites in use",
    "HDOP",
    "Altitude above mean sea level",
    "Altitude unit",
    "Geoid separation",
    "Separation unit",
    "Age of differential data (s)",
    "Reference station ID",
]


def nmea_checksum(body):
    """XOR of every character between '$' and '*'."""
    cs = 0
    for ch in body:
        cs ^= ord(ch)
    return cs


def with_checksum(body):
    """Build a full sentence from its body, e.g. 'GPGGA,...' -> '$GPGGA,...*4F'."""
    return "${}*{:02X}".format(body, nmea_checksum(body))


def check_sentence(line):
    """Return (ok, given, computed). given is None if the sentence has no '*hh'."""
    line = line.strip()
    if not line.startswith("$"):
        return False, None, None
    star = line.find("*")
    body = line[1:star] if star >= 0 else line[1:]
    computed = nmea_checksum(body)
    if star < 0 or len(line) < star + 3:
        return False, None, computed
    try:
        given = int(line[star + 1:star + 3], 16)
    except ValueError:
        return False, None, computed
    return given == computed, given, computed


def ddmm_to_deg(value, hemi, deg_digits):
    """'4737.5900', 'N', 2 -> 47.626500.  Degrees are the first 2 (lat) or 3 (lon) digits."""
    deg = float(value[:deg_digits])
    minutes = float(value[deg_digits:])
    out = deg + minutes / 60.0
    return -out if hemi in ("S", "W") else out


def haversine_m(lat1, lon1, lat2, lon2):
    """Great-circle distance in meters on a sphere of radius 6371 km."""
    r = 6371000.0
    p1, p2 = math.radians(lat1), math.radians(lat2)
    dp, dl = p2 - p1, math.radians(lon2 - lon1)
    a = math.sin(dp / 2) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(dl / 2) ** 2
    return 2 * r * math.asin(min(1.0, math.sqrt(a)))


def parse_gga(line):
    """Decode one GGA sentence into a dictionary."""
    line = line.strip()
    out = {"line": line, "ok": False, "errors": [], "fields": []}
    ok, given, computed = check_sentence(line)
    out["checksum"] = {
        "given": None if given is None else "{:02X}".format(given),
        "computed": None if computed is None else "{:02X}".format(computed),
        "ok": ok,
    }
    if not line.startswith("$"):
        out["errors"].append("Does not start with '$'.")
        return out
    if not ok:
        out["errors"].append("Checksum mismatch: the line was corrupted, so the driver should skip it.")
    star = line.find("*")
    parts = (line[1:star] if star >= 0 else line[1:]).split(",")
    sid = parts[0]
    out["talker"] = sid[:2]
    out["type"] = sid[2:]
    if sid[2:] != "GGA":
        out["errors"].append("Not a GGA sentence ({}).".format(sid))
        return out
    data = (parts[1:] + [""] * 14)[:14]
    out["fields"] = [{"index": i + 1, "name": n, "raw": v} for i, (n, v) in enumerate(zip(FIELD_NAMES, data))]
    t = data[0]
    if len(t) >= 6:
        h, m, s = int(t[0:2]), int(t[2:4]), float(t[4:])
        out["time_utc"] = "{:02d}:{:02d}:{:05.2f}".format(h, m, s)
        out["seconds_of_day"] = h * 3600 + m * 60 + s
    q = int(data[5]) if data[5].isdigit() else 0
    out["fix_quality"] = {"code": q, "meaning": FIX_QUALITY.get(q, "Unknown")}
    out["num_sats"] = int(data[6]) if data[6].isdigit() else None
    out["hdop"] = float(data[7]) if data[7] else None
    if q == 0 or not data[1] or not data[3]:
        out["errors"].append("No position in this sentence (fix quality 0). Don't publish it as a fix.")
        return out
    lat = ddmm_to_deg(data[1], data[2], 2)
    lon = ddmm_to_deg(data[3], data[4], 3)
    out["lat_deg"], out["lon_deg"] = lat, lon
    alt = float(data[8]) if data[8] else None
    sep = float(data[10]) if data[10] else None
    out["alt_msl_m"] = alt
    out["geoid_sep_m"] = sep
    out["alt_ellipsoid_m"] = None if (alt is None or sep is None) else alt + sep
    out["age_s"] = float(data[12]) if data[12] else None
    # two classic mistakes, and how far off each one puts you
    naive_lat = float(data[1]) / 100.0 * (-1 if data[2] == "S" else 1)
    naive_lon = float(data[3]) / 100.0 * (-1 if data[4] == "W" else 1)
    no_sign_lon = abs(lon)
    out["mistakes"] = [
        {"what": "Reading ddmm.mmmm as decimal degrees",
         "lat": naive_lat, "lon": naive_lon,
         "error_km": haversine_m(lat, lon, naive_lat, naive_lon) / 1000.0},
        {"what": "Forgetting that W (or S) means negative",
         "lat": lat, "lon": no_sign_lon,
         "error_km": haversine_m(lat, lon, lat, no_sign_lon) / 1000.0},
    ]
    out["ok"] = ok
    return out


# Sample lines near Lake Union Park, Seattle. Checksums are computed, except the last one,
# which was damaged on purpose.
SAMPLES = [
    with_checksum("GPGGA,183012.00,4737.59000,N,12220.28600,W,1,09,0.9,12.3,M,-18.5,M,,"),
    with_checksum("GNGGA,183013.00,4737.59031,N,12220.28575,W,5,16,0.7,12.4,M,-18.5,M,2.0,0000"),
    with_checksum("GNGGA,183014.00,4737.59052,N,12220.28561,W,4,18,0.6,12.1,M,-18.5,M,1.0,0000"),
    with_checksum("GPGGA,183015.00,,,,,0,00,99.99,,,,,,"),
]
SAMPLES.append(SAMPLES[0].replace("12.3,M", "13.3,M"))


def demo_api(params_json):
    """Web page entry point. Input: {"text": "..."}; output: one result per non-empty line."""
    p = json.loads(params_json or "{}")
    text = p.get("text") or "\n".join(SAMPLES)
    lines = [ln for ln in text.splitlines() if ln.strip()]
    results = [parse_gga(ln) for ln in lines[:200]]
    return json.dumps({"results": results, "samples": SAMPLES})


def main(argv):
    if len(argv) > 1:
        with open(argv[1]) as f:
            lines = [ln for ln in f if "GGA" in ln]
    else:
        lines = SAMPLES
    for ln in lines:
        r = parse_gga(ln)
        print(r["line"])
        print("  checksum ok: {}   fix: {}".format(r["checksum"]["ok"], r.get("fix_quality", {}).get("meaning")))
        if "lat_deg" in r:
            print("  lat {:.7f}  lon {:.7f}  sats {}  HDOP {}".format(r["lat_deg"], r["lon_deg"], r["num_sats"], r["hdop"]))
        for e in r["errors"]:
            print("  note:", e)


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