"""Independent SE(3) APE/RPE; stdin JSON, bounded caller supplies passive data."""
import csv
import io
import json
import sys
from decimal import Decimal
import numpy as np


def parse(text, ground=False):
    rows = list(csv.reader(io.StringIO(text))) if ground else [x.split() for x in text.splitlines()]
    poses = []
    for row in rows:
        if not row or row[0].startswith('#'):
            continue
        if len(row) != 8:
            raise ValueError('Expected eight columns')
        stamp = int(row[0]) if ground else int((Decimal(row[0])*10**9).to_integral_value())
        numbers = np.array([float(x) for x in row[1:]])
        if not np.all(np.isfinite(numbers)):
            raise ValueError('Nonfinite pose')
        q = numbers[[4, 5, 6, 3]] if ground else numbers[3:]
        norm = np.linalg.norm(q)
        if abs(norm-1) > 1e-5:
            raise ValueError('Invalid quaternion')
        if poses and stamp <= poses[-1][0]:
            raise ValueError('Nonmonotonic timestamps')
        poses.append((stamp, numbers[:3], q/norm))
    if len(poses) < 2 or len(poses) > 100000:
        raise ValueError('Invalid pose count')
    return poses


def rotation(q):
    x, y, z, w = q
    return np.array([[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
                     [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
                     [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]])


def slerp(a, b, u):
    d = float(a@b)
    if d < 0:
        b, d = -b, -d
    if d > .9995:
        q = a+u*(b-a)
        return q/np.linalg.norm(q)
    theta = np.arccos(np.clip(d, -1, 1))
    return (np.sin((1-u)*theta)*a+np.sin(u*theta)*b)/np.sin(theta)


def associate(est, gt):
    import bisect
    stamps = [p[0] for p in gt]
    result, gaps = [], []
    for stamp, _, _ in est:
        j = bisect.bisect_left(stamps, stamp)
        if j < len(gt) and stamps[j] == stamp:
            result.append(gt[j]); gaps.append(0); continue
        if j == 0 or j == len(gt):
            raise ValueError('Ground truth does not bracket estimate')
        a, b = gt[j-1], gt[j]
        gap = b[0]-a[0]
        if gap > 20_000_000:
            raise ValueError('Ground truth gap exceeds20ms')
        u = (stamp-a[0])/gap
        result.append((stamp, (1-u)*a[1]+u*b[1], slerp(a[2], b[2], u)))
        gaps.append(gap)
    return result, gaps


def align(est, ref):
    x = np.array([p[1] for p in est]); y = np.array([p[1] for p in ref])
    xc, yc = x-x.mean(0), y-y.mean(0)
    u, s, vt = np.linalg.svd(xc.T@yc/len(x))
    if s[1] <= max(s[0]*1e-10, 1e-15):
        raise ValueError('Degenerate alignment')
    d = np.eye(3); d[2, 2] = np.linalg.det(vt.T@u.T)
    r = vt.T@d@u.T
    return r, y.mean(0)-r@x.mean(0), s


def angle(r):
    return float(np.degrees(np.arccos(np.clip((np.trace(r)-1)/2, -1, 1))))


def stats(a):
    a = np.asarray(a)
    return {'count':len(a), 'rmse':float(np.sqrt(np.mean(a*a))),
            'median':float(np.median(a)), 'max':float(np.max(a))}


def evaluate(est, ref, delta=20):
    r, t, singular = align(est, ref)
    ap, ar, rp, rr, dt = [], [], [], [], []
    for e, g in zip(est, ref):
        ap.append(np.linalg.norm(g[1]-(r@e[1]+t)))
        ar.append(angle(rotation(g[2]).T@r@rotation(e[2])))
    for i in range(len(est)-delta):
        j = i+delta
        e0, e1, g0, g1 = est[i], est[j], ref[i], ref[j]
        re = rotation(e0[2]).T@rotation(e1[2])
        rg = rotation(g0[2]).T@rotation(g1[2])
        pe = rotation(e0[2]).T@(e1[1]-e0[1])
        pg = rotation(g0[2]).T@(g1[1]-g0[1])
        rp.append(np.linalg.norm(rg.T@(pe-pg)))
        rr.append(angle(rg.T@re))
        dt.append((e1[0]-e0[0])*1e-9)
    return {'alignment':{'rotation':r.tolist(),'translation_m':t.tolist(),'scale':1,
                         'singular_values_m2':singular.tolist()},
            'ape_translation_m':stats(ap),'ape_rotation_deg':stats(ar),
            'rpe_translation_m':stats(rp),'rpe_rotation_deg':stats(rr),
            'rpe_delta_frames':delta,'rpe_duration_s':[min(dt),max(dt)],
            'elapsed_s':[(e[0]-est[0][0])*1e-9 for e in est],
            'ape_translation_series_m':[float(v) for v in ap]}


def selftest():
    q = np.array([0.,0.,0.,1.]); p = lambda t,x:(t,np.array(x,dtype=float),q.copy())
    base=[p(i*50_000_000,[np.sin(i*.1),np.cos(i*.1),i*.01]) for i in range(30)]
    rz=rotation(np.array([0.,0.,np.sin(.3),np.cos(.3)]))
    altered=[(v[0],rz@v[1]+[1,2,3],np.array([0.,0.,np.sin(.3),np.cos(.3)])) for v in base]
    result=evaluate(altered,base)
    assert result['ape_translation_m']['max']<1e-12
    assert result['rpe_translation_m']['max']<1e-12
    assert result['ape_rotation_deg']['max']<1e-5
    drift=[p(i*50_000_000,[i*.02,0,0]) for i in range(30)]
    # Relative translation residual has known norm .4m at20 frames.
    moving=[(v[0],v[1]+d[1],v[2]) for v,d in zip(base,drift)]
    assert abs(evaluate(moving,base)['rpe_translation_m']['rmse']-.4)<1e-12
    assert np.allclose(slerp(q,-q,.5),q)
    mid,_=associate([p(5_000_000,[0,0,0])],[p(0,[0,0,0]),p(10_000_000,[2,0,0])])
    assert np.allclose(mid[0][1],[1,0,0])
    for est,gt in [([p(-1,[0,0,0])],base),([p(1,[0,0,0])],base)]:
        try: associate(est,gt)
        except ValueError: pass
        else: raise AssertionError('Coverage/gap rejection missing')
    for raw in ['0 0 0 0 0 0 0 1\n0 0 0 0 0 0 0 1', '0 0 0 0 0 0 0 2\n1 0 0 0 0 0 0 1']:
        try: parse(raw)
        except ValueError: pass
        else: raise AssertionError('Invalid input accepted')
    return {'status':'PASS','checks':['rigid_invariance','known_drift','quaternion_sign',
            'interpolation_midpoint','no_extrapolation','gap_limit','monotonic','quaternion_norm']}


if __name__ == '__main__':
    payload=json.load(sys.stdin)
    tests=selftest()
    est=parse(payload['estimate']); gt=parse(payload['ground_truth'],True)
    if len(est)!=600: raise ValueError('Expected retained600 poses')
    ref,gaps=associate(est,gt)
    result=evaluate(est,ref)
    result.update({'status':'EVALUATED_NOT_CERTIFIED','synthetic_checks':tests,
        'poses':len(est),'ground_truth_rows':len(gt),'max_bracket_gap_ns':max(gaps),
        'numpy':np.__version__,'method':'linear/SLERP; SE3 scale1; overlapping20-frameRPE'})
    print(json.dumps(result,allow_nan=False))