"""Exercise the real left ROS callback without DDS or hardware publishing."""
import sys,json
from types import SimpleNamespace
from build import ROOT,OUTPUT,MODEL
sys.path.insert(0,str(ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2'))
import mujoco
from sensor_msgs.msg import JointState
from builtin_interfaces.msg import Time
from linker_hand_mujoco_ros2 import o30_left_plan_bridge as left
class Node:
    def __init__(self):self.subs=[];self.messages=[]
    def get_logger(self):return SimpleNamespace(info=lambda *_:None,warn=lambda *_:None)
    def get_clock(self):return SimpleNamespace(now=lambda:SimpleNamespace(to_msg=lambda:Time()))
    def create_subscription(self,t,topic,cb,q):self.subs.append((topic,cb));return cb
    def create_publisher(self,*args):return SimpleNamespace(publish=self.messages.append)
    def create_timer(self,*args):return None
m=mujoco.MjModel.from_xml_path(str(MODEL/'linker_hand_o30_left/linker_hand_o30_left.xml'));d=mujoco.MjData(m);node=Node()
b=left.O30LeftPlanBridge(node,m,d,nominal_topic='/unused',collision_project=False,mapping='g20_urdf')
assert node.subs[0][0]=='/cb_left_hand_control_cmd'
spec=json.loads((OUTPUT/'commands.json').read_text());order=spec['left_ros_order']
assert order==list(left.o30j.O30_LEFT_JOINT_ORDER)
cases=[(x['sdk_command'],x['left_ros_radians']) for x in spec['pinches'].values()]
cases.append((spec['zero']['sdk_command'],spec['zero']['left_ros_radians']))
for n,a in zip(order,spec['zero']['left_ros_radians']):assert abs(d.qpos[b.joint_qpos[n]]-a)<1e-10
for command in [[0.]*20,[255.]*20,[-10.]*20,[300.]*20,list(range(0,200,10))]:
    angles={}
    for n,v in zip(left.o30j.O30_LEFT_SDK_JOINT_ORDER,command):
        lo,hi=m.jnt_range[m.joint(n).id]
        angles[n]=float(lo+(hi-lo)*min(max(v,0),255)/255)
    cases.append((command,[angles[n] for n in order]))
for command,expected in cases:
    node.subs[0][1](JointState(position=[float(v) for v in command]))
    for _ in range(500):
        with b.lock:b.step_simulation_unlocked()
    for n,a in zip(order,expected):assert abs(d.qpos[b.joint_qpos[n]]-a)<1e-10,(n,a)
    b._publish_state();assert node.messages[-1].name==order
before=dict(b.goal)
for invalid in [[0.]*19,[0.]*21,[float('nan')]*20,[float('inf')]*20]:
    node.subs[0][1](JointState(position=invalid))
    assert b.goal==before
print('PASS: 10 SDK commands (new startup zero, four pinches, endpoints, clipping, mixed order) through left callback; malformed messages rejected.')