"""Synthetic position-only exercise; NOT a VIO implementation. MIT; LICENSE.txt."""
import csv
import json
import math
import tempfile
from pathlib import Path

DRIFT_PER_SECOND = 0.01

def generate(drift=DRIFT_PER_SECOND):
    rows = []
    for i in range(101):
        t = i / 10
        x, y = math.cos(t), math.sin(t)
        rows.append((t, x, y, 0.0, x + drift * t, y, 0.0))
    errors = [sum((r[j + 3] - r[j]) ** 2 for j in (1, 2, 3)) for r in rows]
    relative = [sum(((b[j+3]-a[j+3])-(b[j]-a[j]))**2 for j in (1,2,3)) for a,b in zip(rows, rows[1:])]
    return rows, {'synthetic': True, 'alignment': 'none; identical frames and timestamps',
                  'samples': len(rows), 'units': 'metres, seconds',
                  'position_rmse_m': math.sqrt(sum(errors)/len(errors)),
                  'position_increment_rmse_m': math.sqrt(sum(relative)/len(relative)),
                  'final_position_error_m': math.sqrt(errors[-1])}

if __name__ == '__main__':
    rows, summary = generate()
    output = Path(tempfile.mkdtemp(prefix='trajectory-output-', dir='.'))
    with (output/'trajectory.csv').open('w', newline='', encoding='utf-8') as handle:
        writer = csv.writer(handle)
        writer.writerow(['time_s','ref_x_m','ref_y_m','ref_z_m','est_x_m','est_y_m','est_z_m'])
        writer.writerows(rows)
    (output/'summary.json').write_text(json.dumps(summary, indent=2)+'\n', encoding='utf-8')
    print(output.resolve())
    print(json.dumps(summary, indent=2))