"""Run beside ecef-enu-example.json using Python 3. No dependencies."""
import json
import math
from pathlib import Path

def mm(a, b):
    return [[sum(x * y for x, y in zip(row, col)) for col in zip(*b)] for row in a]

d = json.loads(Path(__file__).with_name('ecef-enu-example.json').read_text())
phi, lam = map(math.radians, [d['latitude_deg'], d['longitude_deg']])
R = [[-math.sin(lam), math.cos(lam), 0],
     [-math.sin(phi)*math.cos(lam), -math.sin(phi)*math.sin(lam), math.cos(phi)],
     [math.cos(phi)*math.cos(lam), math.cos(phi)*math.sin(lam), math.sin(phi)]]
delta = [p - origin for p, origin in zip(d['point_ecef_m'], d['origin_ecef_m'])]
enu = [row[0] for row in mm(R, [[v] for v in delta])]
P = mm(mm(R, d['covariance_ecef_m2']), list(map(list, zip(*R))))
for actual, expected in zip(enu, d['offset_enu_m']):
    if not math.isclose(actual, expected, abs_tol=1e-9):
        raise ValueError('Offset mismatch')
for actual, expected in zip(sum(P, []), sum(d['covariance_enu_m2'], [])):
    if not math.isclose(actual, expected, abs_tol=1e-9):
        raise ValueError('Covariance mismatch')
print(json.dumps({'offset_enu_m': enu, 'covariance_enu_m2': P,
                  'formal_sigmas_m': [math.sqrt(P[i][i]) for i in range(3)],
                  'horizontal_rms_m': math.sqrt(P[0][0] + P[1][1]),
                  'trace_before_m2': sum(d['covariance_ecef_m2'][i][i] for i in range(3)),
                  'trace_after_m2': sum(P[i][i] for i in range(3))}, indent=2))