"""Run inside Isaac Sim to test L20 thumb slider jumps; pauses on completion.

Results: builtins._l20_drive_result. This changes the slider target temporarily.
Requires a clean initial scene and a nonlooping timeline long enough for 16 seconds.
"""
import asyncio, builtins

async def drive_test():
    import asyncio, time, builtins, numpy as np, omni.usd, omni.timeline, omni.kit.app, omni.physx
    from isaacsim.core.simulation_manager import SimulationManager
    from pxr import Usd
    s = omni.usd.get_context().get_stage()
    tl = omni.timeline.get_timeline_interface()
    joint = s.GetPrimAtPath('/tn__linkerhand_g20_lefturdf_cZ0/Physics/thumb_cmc_roll')
    target = joint.GetAttribute('drive:angular:physics:targetPosition')
    r = {'status': 'RUNNING', 'steps': 0, 'segments': [], 'max_speed': 0.0}
    builtins._l20_drive_result = r

    def step(dt):
        r['steps'] += 1
    sub = omni.physx.get_physx_interface().subscribe_physics_step_events(step)

    def arr(x):
        return x.numpy() if hasattr(x, 'numpy') else np.asarray(x)
    try:
        tl.play()
        a = None
        deadline = time.monotonic() + 60
        for goal in [0, 30, 60, 80, 0, -20, 100, 0]:
            with Usd.EditContext(s, s.GetRootLayer()):
                target.Set(float(goal))
            start = r['steps']
            while r['steps'] - start < 480:
                await asyncio.wait_for(omni.kit.app.get_app().next_update_async(), 10)
                assert time.monotonic() < deadline, 'Wall timeout'
                if r['steps'] < 4:
                    continue
                if a is None:
                    a = SimulationManager.get_physics_simulation_view().create_articulation_view('/tn__linkerhand_g20_lefturdf_cZ0/Physics/fixed_base_joint')
                    names = list(a.shared_metatype.dof_names)
                    idx = names.index('thumb_cmc_roll')
                q = arr(a.get_dof_positions())
                v = arr(a.get_dof_velocities())
                links = arr(a.get_link_transforms())
                assert all((np.isfinite(x).all() for x in [q, v, links])), 'Nonfinite state'
                r['max_speed'] = max(r['max_speed'], float(np.abs(v).max()))
                assert np.abs(v).max() < 10, 'Excessive speed'
                assert np.abs(links[..., :3]).max() < 0.5, 'Escaped link'
                assert q[0, idx] > -0.05 and q[0, idx] < 1.45, 'Roll limit violation'
            actual = float(np.rad2deg(q[0, idx]))
            r['segments'].append({'target_deg': goal, 'actual_deg': actual})
            assert abs(actual - min(80.214, max(0, goal))) < 3, 'Tracking error ' + str(actual)
        r['status'] = 'PASS'
    except Exception as e:
        r['status'] = 'FAIL'
        r['error'] = str(e)
    finally:
        tl.pause()
        sub = None
builtins._l20_drive_task = asyncio.ensure_future(drive_test())
print('Started bounded thumb target-jump test')