"""Ideal 2D rigid point transform, world A <- local B; metres and degrees.
Column-vector convention: p_A = R_AB p_B + t_AB; positive rotation is CCW.
The rotation function is extracted unchanged from engineering_labs.py.
"""
import math
import time
import numpy as np
from transform_kernel import rotation


def run(name, p):
    if name != 'transforms' or set(p) != {'angle', 'tx', 'ty', 'px', 'py'}:
        raise ValueError('Unknown experiment or parameters')
    for key, value in p.items():
        bound = 180 if key == 'angle' else 5
        if type(value) not in (int, float) or not math.isfinite(value) or not -bound <= value <= bound:
            raise ValueError('Parameter out of range: ' + key)
    start = time.perf_counter()
    R = rotation(np.deg2rad(p['angle']))
    local = np.array([p['px'], p['py']])
    origin = np.array([p['tx'], p['ty']])
    world = R @ local + origin
    recovered = R.T @ (world - origin)
    def series(ident, points):
        points = np.array(points, dtype=float)
        return dict(id=ident, x=points[:, 0].tolist(), y=points[:, 1].tolist(), x_unit='m', y_unit='m')
    return dict(
        metrics=[dict(id='world_point_x', value=float(world[0]), unit='m'),
                 dict(id='world_point_y', value=float(world[1]), unit='m'),
                 dict(id='recovered_x', value=float(recovered[0]), unit='m'),
                 dict(id='recovered_y', value=float(recovered[1]), unit='m'),
                 dict(id='round_trip_error', value=float(np.linalg.norm(recovered-local)), unit='m')],
        series=[series('world_x', [[0, 0], [1, 0]]), series('world_y', [[0, 0], [0, 1]]),
                series('local_x', [origin, origin+R[:, 0]]), series('local_y', [origin, origin+R[:, 1]]),
                series('point_ray', [origin, world]), series('world_point', [world])],
        stdout='', provenance=dict(data_type='synthetic', compute_ms=(time.perf_counter()-start)*1000))


if __name__ == '__main__':
    import argparse
    import json
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--parameters', default='{"angle":90,"tx":1,"ty":2,"px":2,"py":0}')
    args = parser.parse_args()
    print(json.dumps(run('transforms', json.loads(args.parameters)), allow_nan=False))