"""Synthetic translation-only trajectory checks; Python 3.12 standard library.
No alignment, scale correction or time association. Not a SLAM benchmark.
"""
import math
import csv


def main():
    times = [i / 10 for i in range(101)]
    truth = times
    cases = {'constant_bias': [x + .1 for x in truth],
             'linear_drift': [x + .01 * t for x, t in zip(truth, times)]}
    with open('slam-reference.tum', 'w') as out:
        for t, x in zip(times, truth):
            out.write(f'{t:.1f} {x:.8f} 0 0 0 0 0 1\n')
    for name, estimate in cases.items():
        ape = math.sqrt(sum((x-y)**2 for x,y in zip(estimate,truth))/len(truth))
        delta = 10
        rpe = math.sqrt(sum(((estimate[i+delta]-estimate[i])-(truth[i+delta]-truth[i]))**2
                            for i in range(len(truth)-delta))/(len(truth)-delta))
        print(f'{name}: unaligned translation APE RMSE={ape:.6f} m, 1s all-pair translation RPE RMSE={rpe:.6f} m')
        with open(f'slam-{name}.tum', 'w') as out:
            for t,x in zip(times,estimate):
                out.write(f'{t:.1f} {x:.8f} 0 0 0 0 0 1\n')


if __name__ == '__main__':
    main()