"""Gradients through written code: forward kinematics you wrote, a learned pose.

`Arm` is plain tensor code marked replayable, so a gradient passes through it.
`pose` is a learned network. Supervising the fingertip against the target trains
`pose` to reach, with no joint-angle labels anywhere.
Runs offline on CPU:  pip install 'tensorcode[vec]'  then  python reach.py
"""
import torch
from tensorcode import trace, training
from tensorcode.ops import Operation
from tensorcode.ops.vec import Latent, Space
from tensorcode.ops.vec.decode import Decode

LINKS = (0.5, 0.4, 0.3)  # three link lengths, in metres


class Arm(Operation):
    """Written: joint angles -> fingertip (x, y)."""
    replayable = True  # pure tensor code, so replay can recompute it with gradients

    def forward(self, angles, *, context=None):
        x = y = heading = 0.0
        for L, a in zip(LINKS, angles.unbind(1)):
            heading = heading + a
            x = x + L * torch.cos(heading)
            y = y + L * torch.sin(heading)
        return torch.stack([x, y], -1)


torch.manual_seed(3)
space = Space("target position", 2)
pose = Decode({"architecture": "mlp", "hidden_dimensions": [32], "input_space": space.configuration(),
               "output_dimensions": 3, "output": "joint angles"})
arm = Arm()


def targets(points):
    return Latent(torch.tensor(points), space)


goals = [[0.9, 0.3], [0.6, 0.6], [0.2, 0.9], [-0.3, 0.8], [0.7, -0.4], [0.4, 0.1]]
with trace() as t:
    tip = arm(pose(targets(goals)))
t.supervise(tip, goals, loss="mse", source="the targets themselves")

trainer = training.Trainer.from_ops({"pose": pose, "arm": arm},
                                    optimizer=lambda p: torch.optim.Adam(p, lr=0.02))
losses = trainer.fit([t], epochs=400)
print(f"loss {losses[0]:.3f} -> {losses[-1]:.4f}")
with torch.no_grad():
    unseen = [[0.8, 0.5], [0.0, 1.0], [0.5, -0.2]]  # positions it never trained on
    for goal, xy in zip(unseen, arm(pose(targets(unseen)))):
        print(f"target ({goal[0]:.1f}, {goal[1]:.1f}): fingertip ({xy[0]:.2f}, {xy[1]:.2f})")
