"""Verify the geometric dilution of precision (GDOP) used as a chart diagnostic.

Source: parkinson1996gdop.  Hifuku reports GDOP as a measure-only indicator of how
well the in-plane barycentric position is constrained by the three anchor
distances.  With unit line-of-sight vectors u_i = (X - Pi) / ||X - Pi|| stacked as
the rows of H (shape 3 x 2), the implementation in hifuku.anchor_triangle.gdop
computes

    GDOP = sqrt(trace((H^T H)^{-1})).

This script confirms:
  1. the trace-of-inverse identity for the 2x2 normal matrix H^T H,
     trace((H^T H)^{-1}) = (a + c) / (a c - b^2), which is exactly the
     det-and-cofactor computation in the code,
  2. the closed-form value at the centroid of an equilateral triangle, where the
     three lines of sight are 120 degrees apart, giving H^T H = (3/2) I and
     GDOP = 2 / sqrt(3),
  3. numerically, that the centroid is the minimum and that GDOP grows as the
     query leaves the triangle.

Run: python docs/theory/algebra/parkinson1996gdop-dilution.py
"""
import numpy as np
import sympy as sp

# --- 1. trace((H^T H)^{-1}) = (a + c) / (a c - b^2) for H^T H = [[a, b], [b, c]] ---
a, b, c = sp.symbols("a b c", real=True)
HtH = sp.Matrix([[a, b], [b, c]])
trace_inv = sp.simplify(sp.trace(HtH.inv()))
expected = (a + c) / (a * c - b**2)
assert sp.simplify(trace_inv - expected) == 0, f"trace-of-inverse identity: {trace_inv}"

# --- 2. Equilateral centroid: three unit vectors 120 deg apart give H^T H = (3/2) I ---
theta = sp.symbols("theta", real=True)
# Sum of outer products of unit vectors at angles 0, 120, 240 degrees.
S = sp.zeros(2, 2)
for ang in (0, sp.Rational(2, 3) * sp.pi, sp.Rational(4, 3) * sp.pi):
    u = sp.Matrix([sp.cos(ang), sp.sin(ang)])
    S += u * u.T
S = sp.simplify(S)
assert S == sp.Rational(3, 2) * sp.eye(2), f"centroid H^T H is not (3/2) I: {S}"
gdop_centroid = sp.sqrt(sp.trace(S.inv()))
assert sp.simplify(gdop_centroid - 2 / sp.sqrt(3)) == 0, f"centroid GDOP: {gdop_centroid}"

# --- 3. Numeric: centroid is the minimum; GDOP grows outside the triangle ---
def gdop_num(X, anchors):
    rows = []
    for P in anchors:
        d = X - P
        rows.append(d / np.linalg.norm(d))
    H = np.array(rows)
    return float(np.sqrt(np.trace(np.linalg.inv(H.T @ H))))

P1 = np.array([0.0, 0.0])
P2 = np.array([1.0, 0.0])
P3 = np.array([0.5, np.sqrt(3) / 2.0])
centroid = (P1 + P2 + P3) / 3.0

g_centroid = gdop_num(centroid, (P1, P2, P3))
assert abs(g_centroid - 2.0 / np.sqrt(3.0)) < 1e-9, g_centroid

# The centroid is a local minimum: small perturbations do not lower GDOP.
rng = np.random.default_rng(0)
for _ in range(2000):
    jitter = centroid + 0.05 * rng.standard_normal(2)
    assert gdop_num(jitter, (P1, P2, P3)) >= g_centroid - 1e-9

# GDOP grows without bound as the query leaves the triangle.
assert gdop_num(np.array([5.0, 5.0]), (P1, P2, P3)) > 3.0 * g_centroid

print("All GDOP checks passed.")
print(f"  trace((H^T H)^-1) = (a + c) / (a c - b^2)")
print(f"  centroid GDOP = 2 / sqrt(3) = {float(2 / sp.sqrt(3)):.6f}")
print(f"  numeric centroid GDOP = {g_centroid:.6f}")