"""Synthetic 1D translation evaluation; identity rotations, exact timestamp matches.
Reference x=t m, 101 samples at 0.1 s. No SE(3)/Sim(3) fit or real SLAM benchmark.
"""
import math
from pathlib import Path
import time
from slam_kernel import translation_metrics


def reference():
    local=Path(__file__).with_name('slam-reference.tum')
    path=local if local.exists() else Path(__file__).parent.parent/'slam-reference.tum'
    rows=[[float(v) for v in line.split()] for line in path.read_text().splitlines() if line.strip() and not line.startswith('#')]
    if len(rows)!=101 or any(len(r)!=8 or not all(math.isfinite(v) for v in r) for r in rows):
        raise ValueError('Unexpected reference trajectory')
    for i,r in enumerate(rows):
        if abs(r[0]-i/10)>1e-12 or r[1]!=r[0] or r[2:]!=[0,0,0,0,0,1]:
            raise ValueError('Reference must use x=t, 0.1 s and identity rotations')
    return [r[0] for r in rows],[r[1] for r in rows]


def align_translation(estimate,truth,mode):
    if mode=='none':shift=0.
    elif mode=='first_pose':shift=truth[0]-estimate[0]
    elif mode=='mean_position':shift=sum(q-p for p,q in zip(estimate,truth))/len(truth)
    else:raise ValueError('Unknown alignment')
    return [p+shift for p in estimate],shift


def run(name,p):
    if name!='slam' or set(p)!={'offset','drift','interval','alignment'}:
        raise ValueError('Unknown experiment or parameters')
    for key,lo,hi in [('offset',-.5,.5),('drift',-.1,.1),('interval',.1,5)]:
        v=p[key]
        if type(v) not in (int,float) or not math.isfinite(v) or not lo<=v<=hi:
            raise ValueError('Parameter out of range: '+key)
    delta=round(p['interval']*10)
    if abs(delta-p['interval']*10)>1e-8:raise ValueError('Interval must use 0.1 s steps')
    start=time.perf_counter()
    times,truth=reference()
    estimate=[x+p['offset']+p['drift']*t for x,t in zip(truth,times)]
    aligned,shift=align_translation(estimate,truth,p['alignment'])
    raw_ape,rpe=translation_metrics(estimate,truth,delta)
    aligned_ape,_=translation_metrics(aligned,truth,delta)
    position_error=[x-y for x,y in zip(aligned,truth)]
    relative_error=[(estimate[i+delta]-estimate[i])-(truth[i+delta]-truth[i]) for i in range(len(truth)-delta)]
    def series(ident,x,y):return dict(id=ident,x=x,y=y,x_unit='s',y_unit='m')
    return dict(
        metrics=[dict(id='raw_ape',value=raw_ape,unit='m'),dict(id='aligned_ape',value=aligned_ape,unit='m'),
                 dict(id='translation_rpe',value=rpe,unit='m'),dict(id='alignment_shift',value=shift,unit='m'),
                 dict(id='matched_poses',value=len(truth),unit='count'),dict(id='rpe_pairs',value=len(truth)-delta,unit='count')],
        series=[series('reference_position',times,truth),series('estimated_position',times,estimate),
                series('aligned_position',times,aligned),series('position_error',times,position_error),
                series('relative_error',times[:-delta],relative_error)],
        stdout='',provenance=dict(data_type='synthetic',compute_ms=(time.perf_counter()-start)*1000))


if __name__=='__main__':
    import argparse,json
    parser=argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--parameters',default='{"offset":0.1,"drift":0,"interval":1,"alignment":"none"}')
    args=parser.parse_args();print(json.dumps(run('slam',json.loads(args.parameters)),allow_nan=False))