"""
sat_dop.py: satellite geometry and dilution of precision (DOP).

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

Each satellite in view gives one pseudorange. Linearizing the navigation
equations gives one row of the geometry matrix H per satellite:

    H_i = [ -e_i,  -n_i,  -u_i,  1 ]

where (e_i, n_i, u_i) is the unit vector from the receiver to satellite i in
local East, North, Up, and the 1 is for the receiver clock error. This is the
same kind of matrix as the design matrix X in linear regression.

    Q = (H^T H)^-1

DOP values are square roots of sums on the diagonal of Q. They have no units:
they multiply the range error (sigma_UERE) into a position error. Satellites
spread across the sky give small DOP; satellites bunched together give large DOP.

Run it from a terminal:
    python3 sat_dop.py
"""
import json
import math
import sys

import numpy as np


def unit_vectors(sats):
    """Azimuth and elevation in degrees -> unit vectors (east, north, up)."""
    az = np.radians([s["az"] for s in sats])
    el = np.radians([s["el"] for s in sats])
    return np.column_stack([np.cos(el) * np.sin(az), np.cos(el) * np.cos(az), np.sin(el)])


def geometry_matrix(sats):
    u = unit_vectors(sats)
    return np.column_stack([-u, np.ones(len(sats))])


def dop(sats, sigma_uere=1.0):
    """DOP values and the 1-sigma horizontal error ellipse for these satellites."""
    if len(sats) < 4:
        return {"ok": False, "reason": "Need at least 4 satellites: 3 position unknowns plus the clock."}
    h = geometry_matrix(sats)
    hth = h.T @ h
    if np.linalg.cond(hth) > 1e12:
        return {"ok": False, "reason": "The geometry is degenerate: the satellites are in a line or on one cone."}
    q = np.linalg.inv(hth)
    d = np.diag(q)
    out = {
        "ok": True,
        "n": len(sats),
        "GDOP": float(math.sqrt(d.sum())),
        "PDOP": float(math.sqrt(d[0] + d[1] + d[2])),
        "HDOP": float(math.sqrt(d[0] + d[1])),
        "VDOP": float(math.sqrt(d[2])),
        "TDOP": float(math.sqrt(d[3])),
        "Q": q.tolist(),
        "H": h.tolist(),
    }
    # horizontal (east, north) covariance in m^2, and its 1-sigma ellipse
    cov = sigma_uere ** 2 * q[:2, :2]
    vals, vecs = np.linalg.eigh(cov)
    major = vecs[:, 1]
    out["ellipse"] = {
        "a": float(math.sqrt(vals[1])),         # semi-major axis, m
        "b": float(math.sqrt(max(vals[0], 0))),  # semi-minor axis, m
        "angle_from_north_deg": float(math.degrees(math.atan2(major[0], major[1])) % 180),
    }
    out["sigma_h"] = float(sigma_uere * out["HDOP"])
    out["sigma_v"] = float(sigma_uere * out["VDOP"])
    return out


def preset(name, n=7, seed=1):
    """A few sky patterns to compare."""
    rng = np.random.default_rng(seed)
    if name == "spread":
        sats = [{"az": 0.0, "el": 85.0}]
        for i in range(n - 1):
            sats.append({"az": (360.0 * i / (n - 1) + 15) % 360, "el": 25.0 + 12.0 * (i % 2)})
    elif name == "clustered":
        sats = [{"az": 20 + 70 * rng.random(), "el": 30 + 45 * rng.random()} for _ in range(n)]
    elif name == "high":
        sats = [{"az": 360.0 * i / n, "el": 72 + 12 * rng.random()} for i in range(n)]
    elif name == "canyon":   # a street running north-south: only satellites along the street are visible
        sats = []
        for i in range(n):
            az = (0 if i % 2 == 0 else 180) + rng.uniform(-18, 18)
            sats.append({"az": az % 360, "el": 30 + 50 * rng.random()})
    else:                    # random
        sats = [{"az": 360 * rng.random(), "el": 10 + 75 * rng.random()} for _ in range(n)]
    return [{"az": round(float(s["az"]), 1), "el": round(float(s["el"]), 1)} for s in sats]


def demo_api(params_json):
    """Web page entry point. Input: {"sats": [{"az":..,"el":..}], "sigma": m, "mask": deg}
    or {"preset": name, "n": count} to get a new sky."""
    p = json.loads(params_json or "{}")
    sats = p.get("sats")
    if p.get("preset") or not sats:
        sats = preset(p.get("preset", "spread"), int(p.get("n", 7)), int(p.get("seed", 1)))
    mask = float(p.get("mask", 10.0))
    used = [s for s in sats if s["el"] >= mask]
    r = dop(used, float(p.get("sigma", 3.0)))
    r["sats"] = sats
    r["used"] = [s["el"] >= mask for s in sats]
    return json.dumps(r)


def main(argv):
    for name in ("spread", "high", "clustered", "canyon"):
        r = dop(preset(name), 3.0)
        print("{:10s} HDOP {:5.2f}  VDOP {:5.2f}  PDOP {:5.2f}   1-sigma horizontal ellipse {:.1f} m x {:.1f} m".format(
            name, r["HDOP"], r["VDOP"], r["PDOP"], r["ellipse"]["a"], r["ellipse"]["b"]))


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