"""Bounded experimental L20 dynamic diagnostic, NOT a training or hardware entry.

Requires inspected PhysX 110.1.13 + current Isaac Lab ProxyArray/xyzw API. Runtime
coupling is tested from passive follower response, not inferred from schema names.
Use an external timeout <=300s. Do not launch alongside another Kit/GPU job.
"""

import argparse
import hashlib
import json
import random
import traceback
from pathlib import Path


def main():
    from isaaclab.app import AppLauncher

    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("asset", type=Path, help="Experimental prepared overlay, never original fixed preview")
    parser.add_argument("--manifest", type=Path, required=True, help="Original data/source manifest")
    parser.add_argument("--hdf5", type=Path, help="Omit for explicitly synthetic diagnostic reference")
    parser.add_argument("--episode", default="demo_000000")
    parser.add_argument("--steps", type=int, default=480, help="Per repetition at 240Hz; two repetitions")
    parser.add_argument("--execute-experimental", action="store_true", help="Acknowledge uncalibrated controller")
    parser.add_argument("--limits", type=Path, help="JSON fields of control.Limits, SI units; UNCALIBRATED")
    AppLauncher.add_app_launcher_args(parser)
    parser.set_defaults(headless=True, visualizer="none")
    args = parser.parse_args()
    if not args.execute_experimental or not 100 <= args.steps <= 1200:
        parser.error("Require --execute-experimental and 100 <= steps <= 1200 per repetition")

    # Kit must choose/register its USD libraries before any pxr-dependent imports.
    launcher = AppLauncher(args)
    app = launcher.app
    exit_code = 0
    try:
        import numpy as np
        import torch
        from dex_workbench_tracking.cli import synthetic
        from dex_workbench_tracking.control import (
            Limits,
            reference_pose_to_xyzw,
            rotation_error,
            validate_reference,
            wrench,
            xyzw_pose_to_reference,
        )
        from dex_workbench_tracking.prepared import inspect_prepared, require_backend, validate_mimic
        from dex_workbench_tracking.trajectory import load, require, sample

        manifest = json.loads(args.manifest.read_text())
        prepared = inspect_prepared(args.asset, manifest)
        limits = Limits(**json.loads(args.limits.read_text())) if args.limits else Limits()
        data = load(args.hdf5, manifest) if args.hdf5 else synthetic(manifest)
        episode = data.episodes[args.episode]
        validate_reference(episode, limits)
        dt = 1 / 240
        require(args.steps * dt <= episode.time[-1], "Reference shorter than requested steps")
        reference = sample(episode, np.arange(args.steps + 1) * dt)
        # Hold-start reset uses zero velocities; require a genuinely moving diagnostic
        # for every mimic pair, so an ignored/disabled constraint cannot pass at zero.
        names = list(data.joint_names)
        for eq in prepared["mimic"]:
            require(
                np.ptp(reference.joint_position[:, names.index(eq["reference"])]) > 0.002,
                "Each mimic leader must move >0.002rad",
            )
        require(
            np.max(np.linalg.norm(reference.wrist_position - reference.wrist_position[0], axis=1)) > 0.001,
            "Wrist reference must translate >1mm",
        )
        require(
            max(np.linalg.norm(rotation_error(q, reference.wrist_quaternion[0])) for q in reference.wrist_quaternion)
            > 0.005,
            "Wrist reference must rotate >0.005rad",
        )
        from isaaclab_physx.physics import PhysxCfg

        import omni.kit.app
        from pxr import Usd, UsdPhysics

        import isaaclab.sim as sim_utils
        from isaaclab.actuators import ImplicitActuatorCfg
        from isaaclab.assets import Articulation, ArticulationCfg

        manager = omni.kit.app.get_app().get_extension_manager()
        extension = manager.get_enabled_extension_id("omni.physx")
        require(extension is not None, "PhysX extension not enabled")
        version = manager.get_extension_dict(extension)["package"]["version"]
        require_backend(version, bool(Usd.SchemaRegistry().FindAppliedAPIPrimDefinition("NewtonMimicAPI")))
        random.seed(42)
        np.random.seed(42)
        torch.manual_seed(42)
        sim = sim_utils.SimulationContext(
            sim_utils.SimulationCfg(dt=dt, gravity=(0, 0, -9.81), device=args.device, physics=PhysxCfg())
        )
        cfg = ArticulationCfg(
            prim_path="/World/Hand",
            spawn=sim_utils.UsdFileCfg(usd_path=str(args.asset.resolve())),
            init_state=ArticulationCfg.InitialStateCfg(pos=(0, 0, 0.4)),
            actuators={
                "model_independent": ImplicitActuatorCfg(
                    joint_names_expr=prepared["independent_joint_names"],
                    stiffness=limits.finger_stiffness,
                    damping=limits.finger_damping,
                    effort_limit_sim=limits.finger_effort,
                    velocity_limit_sim=limits.finger_velocity,
                )
            },
        )
        hand = Articulation(cfg)
        # Validate remapped USD relationships after reference spawning, before physics.
        remapped = dict(manifest)
        remapped["joints"] = [
            dict(j, path="/World/Hand" + j["path"][len(manifest["default_prim"]) :]) for j in manifest["joints"]
        ]
        validate_mimic(sim.stage, remapped, passive=True)
        anchor_path = "/World/Hand" + manifest["world_fixed_joints"][0]["path"][len(manifest["default_prim"]) :]
        anchor = sim.stage.GetPrimAtPath(anchor_path)
        require(anchor.IsValid() and not anchor.IsActive(), "Obsolete world anchor must be inactive")
        active_joints = [p for p in sim.stage.Traverse() if p.IsA(UsdPhysics.Joint)]
        require(len(active_joints) == len(names), "Unexpected active joint count")
        require(all(p.IsA(UsdPhysics.RevoluteJoint) for p in active_joints), "Unexpected active constraint")
        sim.reset()
        require(not hand.is_fixed_base and hand.num_instances == 1, "Expected one floating PhysX articulation")
        require(
            set(hand.joint_names) == set(names) and len(hand.joint_names) == len(names), "Runtime DOF mapping mismatch"
        )
        require(hand.num_bodies == len(manifest["bodies"]), "Runtime body count mismatch")
        require(hand.body_names[0] == manifest["root_link"], "Unexpected root body ordering")
        runtime_ids = [hand.joint_names.index(n) for n in names]
        master_ids = [hand.joint_names.index(n) for n in prepared["independent_joint_names"]]
        master_columns = [names.index(n) for n in prepared["independent_joint_names"]]
        follower_ids = [hand.joint_names.index(eq["joint"]) for eq in prepared["mimic"]]

        def array(proxy):
            return proxy.torch.detach().cpu().numpy().copy()

        def tensor(values):
            return torch.as_tensor(values, dtype=torch.float32, device=hand.device)

        def state():
            pose = array(hand.data.root_link_pose_w)[0]
            # Installed Lab uses xyzw; HDF5/controller use wxyz. Explicit boundary.
            pose = xyzw_pose_to_reference(pose)
            velocity = array(hand.data.root_link_vel_w)[0]
            q = array(hand.data.joint_pos)[0, runtime_ids]
            return pose, velocity, q

        for gains in (hand.data.joint_stiffness, hand.data.joint_damping):
            require((array(gains)[0, follower_ids] == 0).all(), "Runtime follower gains are nonzero")
        masses = array(hand.data.body_mass)[0]
        require(np.isfinite(masses).all() and (masses > 0).all(), "Invalid runtime mass")
        require(masses.sum() * 9.81 < limits.force * 0.8, "Insufficient bounded gravity support headroom")
        lower = np.array([j["lower_rad"] for j in manifest["joints"]])
        upper = np.array([j["upper_rad"] for j in manifest["joints"]])
        traces, reset_states, summaries = [], [], []
        for repetition in range(2):
            hand.reset()
            hand.permanent_wrench_composer.reset()
            hand.instantaneous_wrench_composer.reset()
            pose0 = reference_pose_to_xyzw(reference.wrist_position[0], reference.wrist_quaternion[0])
            hand.write_root_link_pose_to_sim_index(root_pose=tensor(pose0[None]))
            hand.write_root_link_velocity_to_sim_index(root_velocity=tensor(np.zeros((1, 6))))
            hand.write_joint_position_to_sim_index(
                position=tensor(reference.joint_position[0:1]), joint_ids=runtime_ids
            )
            hand.write_joint_velocity_to_sim_index(velocity=tensor(np.zeros((1, len(names)))), joint_ids=runtime_ids)
            hand.set_joint_position_target_index(
                target=tensor(reference.joint_position[0:1, master_columns]), joint_ids=master_ids
            )
            hand.set_joint_velocity_target_index(target=tensor(np.zeros((1, len(master_ids)))), joint_ids=master_ids)
            hand.update(dt)
            reset_pose, reset_vel, reset_q = state()
            np.testing.assert_allclose(reset_pose[:3], reference.wrist_position[0], atol=1e-6)
            require(
                np.linalg.norm(rotation_error(reference.wrist_quaternion[0], reset_pose[3:])) < 1e-6,
                "Reset orientation mismatch",
            )
            np.testing.assert_allclose(reset_q, reference.joint_position[0], atol=1e-6)
            np.testing.assert_allclose(reset_vel, 0, atol=1e-6)
            np.testing.assert_allclose(array(hand.data.joint_vel), 0, atol=1e-6)
            reset_states.append(np.r_[reset_pose, reset_vel, reset_q])
            trace, errors = [], []
            for step in range(args.steps):
                require(app.is_running(), "Application stopped before finite test completed")
                pose, velocity, q = state()
                force, torque = wrench(
                    reference.wrist_position[step],
                    reference.wrist_quaternion[step],
                    pose,
                    velocity,
                    array(hand.data.root_com_pose_w)[0, :3],
                    array(hand.data.body_com_pose_w)[0, :, :3],
                    masses,
                    limits,
                )
                hand.permanent_wrench_composer.set_forces_and_torques_index(
                    forces=tensor(force[None, None]),
                    torques=tensor(torque[None, None]),
                    body_ids=torch.tensor([0], dtype=torch.int32, device=hand.device),
                    is_global=True,
                )
                hand.set_joint_position_target_index(
                    target=tensor(reference.joint_position[step : step + 1, master_columns]), joint_ids=master_ids
                )
                hand.write_data_to_sim()
                sim.step(render=False)
                hand.update(dt)
                pose, velocity, q = state()
                qdot = array(hand.data.joint_vel)[0]
                require(all(np.isfinite(v).all() for v in (pose, velocity, q, qdot)), "Nonfinite dynamic state")
                require(
                    np.linalg.norm(velocity[:3]) < 0.5
                    and np.linalg.norm(velocity[3:]) < 3
                    and np.max(np.abs(qdot)) < 2,
                    "Measured velocity safety bound exceeded",
                )
                require((q >= lower - 0.01).all() and (q <= upper + 0.01).all(), "Joint limit violation >0.01rad")
                residual = max(
                    abs(
                        q[names.index(eq["joint"])]
                        - eq["multiplier"] * q[names.index(eq["reference"])]
                        - eq["offset_rad"]
                    )
                    for eq in prepared["mimic"]
                )
                position_error = np.linalg.norm(pose[:3] - reference.wrist_position[step + 1])
                angle_error = np.linalg.norm(rotation_error(reference.wrist_quaternion[step + 1], pose[3:]))
                joint_error = np.max(np.abs(q - reference.joint_position[step + 1]))
                require(residual < 0.002, "Mimic runtime residual >0.002rad; parser/constraint not verified")
                require(
                    position_error < 0.05 and angle_error < 0.5 and joint_error < 0.2,
                    "Tracking safety envelope exceeded",
                )
                trace.append(np.r_[pose, q, velocity, qdot])
                errors.append([position_error, angle_error, joint_error, residual])
            trace, errors = np.array(trace), np.array(errors)
            for eq in prepared["mimic"]:
                for name in (eq["joint"], eq["reference"]):
                    require(np.ptp(trace[:, 7 + names.index(name)]) > 0.002, f"No nontrivial runtime motion: {name}")
            require(
                np.max(np.linalg.norm(trace[:, :3] - reset_pose[:3], axis=1)) > 0.0005, "No nontrivial root translation"
            )
            require(
                max(np.linalg.norm(rotation_error(p[3:7], reset_pose[3:])) for p in trace) > 0.002,
                "No nontrivial root rotation",
            )
            traces.append(trace)
            summaries.append(
                {
                    "repetition": repetition,
                    "max_errors_m_rad_rad_rad": errors.max(axis=0).tolist(),
                    "rms_errors_m_rad_rad_rad": np.sqrt((errors**2).mean(axis=0)).tolist(),
                }
            )
        np.testing.assert_allclose(reset_states[0], reset_states[1], atol=1e-6, rtol=0)
        position_end = 7 + len(names)
        np.testing.assert_allclose(traces[0][:, :position_end], traces[1][:, :position_end], atol=1e-3, rtol=0)
        # Root linear/angular and joint velocities: 1e-3 m/s or rad/s absolute.
        np.testing.assert_allclose(traces[0][:, position_end:], traces[1][:, position_end:], atol=1e-3, rtol=0)
        hand.permanent_wrench_composer.reset()
        print(
            json.dumps(
                {
                    "status": "PASS",
                    "check": "bounded_experimental_dynamic_tracking",
                    "runtime_verified_for_this_run_only": True,
                    "provenance": data.metadata["provenance"],
                    "reference_source": "hdf5" if args.hdf5 else "analytic_in_memory",
                    "reference_hdf5_sha256": hashlib.sha256(args.hdf5.read_bytes()).hexdigest() if args.hdf5 else None,
                    "reference_description": data.metadata["source_description"],
                    "world_anchor_inactive": not anchor.IsActive(),
                    "active_state_joint_count": len(active_joints),
                    "runtime_is_fixed_base": hand.is_fixed_base,
                    "seed": 42,
                    "num_envs": 1,
                    "steps_per_repetition": args.steps,
                    "repetitions": 2,
                    "physx_version": version,
                    "prepared": prepared,
                    "limits_uncalibrated": vars(limits),
                    "metrics": summaries,
                    "hardware_and_training_validated": False,
                }
            ),
            flush=True,
        )
    except BaseException:
        # Kit fast shutdown may not return; emit the failure before closing.
        exit_code = 1
        traceback.print_exc()
        raise
    finally:
        app.close(exit_code=exit_code)


if __name__ == "__main__":
    main()