"""Toy: sim -> demonstrations -> behavior cloning, with MuJoCo + NumPy only.

A 2-joint planar arm must move its fingertip to a random target.
1) An "expert" (a hand-written PD controller in joint space, standing in for a
   human teleoperator) produces demonstrations in simulation.
2) We fit a policy from (observation -> action) pairs by least squares
   (behavior cloning, the simplest form of imitation learning).
3) We roll out the cloned policy on new targets and compare.
This is NOT LeRobot/ACT/VLA; it only shows the data flow of imitation learning.
"""
import numpy as np
import mujoco

XML = """
<mujoco>
  <option timestep="0.01" gravity="0 0 0"/>
  <worldbody>
    <body name="link1">
      <joint name="j1" type="hinge" axis="0 0 1" damping="0.5"/>
      <geom type="capsule" fromto="0 0 0 0.1 0 0" size="0.01" mass="0.1"/>
      <body name="link2" pos="0.1 0 0">
        <joint name="j2" type="hinge" axis="0 0 1" damping="0.5"/>
        <geom type="capsule" fromto="0 0 0 0.1 0 0" size="0.01" mass="0.1"/>
        <site name="tip" pos="0.1 0 0"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <motor joint="j1" gear="1" ctrlrange="-1 1" ctrllimited="true"/>
    <motor joint="j2" gear="1" ctrlrange="-1 1" ctrllimited="true"/>
  </actuator>
</mujoco>
"""
L1 = L2 = 0.1
rng = np.random.default_rng(0)
model = mujoco.MjModel.from_xml_string(XML)
data = mujoco.MjData(model)

def ik(target):
    x, y = target
    c2 = np.clip((x * x + y * y - L1**2 - L2**2) / (2 * L1 * L2), -1, 1)
    q2 = np.arccos(c2)
    q1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(q2), L1 + L2 * np.cos(q2))
    return np.array([q1, q2])

def wrap(a):
    return (a + np.pi) % (2 * np.pi) - np.pi

def obs(target):
    q, dq = data.qpos.copy(), data.qvel.copy()
    err = wrap(ik(target) - q)          # joint-space error to goal
    return np.concatenate([err, dq])     # 4-dim observation

def expert(o):
    return np.clip(2.0 * o[:2] - 0.3 * o[2:], -1, 1)  # PD controller

def sample_target():
    r = rng.uniform(0.06, 0.19); th = rng.uniform(-np.pi, np.pi)
    return np.array([r * np.cos(th), r * np.sin(th)])

def rollout(policy, target, steps=300, record=None):
    mujoco.mj_resetData(model, data)
    data.qpos[:] = rng.uniform(-np.pi, np.pi, 2)
    for _ in range(steps):
        o = obs(target); a = policy(o)
        if record is not None: record.append((o, a))
        data.ctrl[:] = a
        mujoco.mj_step(model, data)
    mujoco.mj_forward(model, data)
    return np.linalg.norm(data.site("tip").xpos[:2] - target)

# 1) collect demos
demos = []
exp_err = [rollout(expert, sample_target(), record=demos) for _ in range(20)]
O = np.array([d[0] for d in demos]); A = np.array([d[1] for d in demos])
# 2) behavior cloning by least squares (linear policy + bias)
X = np.hstack([O, np.ones((len(O), 1))])
W, *_ = np.linalg.lstsq(X, A, rcond=None)
bc = lambda o: np.clip(np.append(o, 1.0) @ W, -1, 1)
# 3) evaluate on new targets
bc_err = [rollout(bc, sample_target()) for _ in range(50)]
zero_err = [rollout(lambda o: np.zeros(2), sample_target()) for _ in range(50)]
print(f"mujoco {mujoco.__version__}, demos: 20 episodes, {len(O)} (obs, action) pairs")
print(f"expert   median tip error: {np.median(exp_err)*1000:.1f} mm")
print(f"cloned   median tip error: {np.median(bc_err)*1000:.1f} mm, success(<5mm) {np.mean(np.array(bc_err)<0.005):.0%}")
print(f"no-op    median tip error: {np.median(zero_err)*1000:.1f} mm")
