"""Isolated DDS-domain integration fixture. All hardware transports are fake."""
import threading
import time
from pathlib import Path

# The caller assigns a private ROS_DOMAIN_ID before importing rclpy.
import rclpy
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from arm_msgs.srv import GripperExecute
from arm_application.dh_pgea import GripperState
import arm_application.dh_pgea_node as driver


class FakeGripper:
    writes=[]
    def __init__(self,**kwargs):
        self.state=GripperState(init_state=1,grip_state=1,position=300,target_position=300,target_force=33)
    def open(self): pass
    def close(self): pass
    def probe(self): return 1
    def get_run_states(self,**kwargs): return self.state
    def initialize(self): self.writes.append('INIT')
    def move(self,pos,force,speed):
        self.writes.append((pos,force,speed));self.state.position=pos;self.state.target_position=pos;return True


def main():
    driver.DhPgeaGripper=FakeGripper
    rclpy.init(args=['--ros-args','-r','__ns:=/station/grippers/arm_89','-p','auto_init:=false'])
    device=driver.DhPgeaNode()
    assert device.get_namespace()=='/station/grippers/arm_89',device.get_namespace()
    client_node=Node('station_test_client')
    executor=MultiThreadedExecutor(num_threads=2);executor.add_node(device);executor.add_node(client_node)
    thread=threading.Thread(target=executor.spin,daemon=True);thread.start()
    try:
        client=client_node.create_client(GripperExecute,'/station/grippers/arm_89/execute')
        assert client.wait_for_service(timeout_sec=3)
        def command(name,position=0,force=33,speed=25):
            req=GripperExecute.Request();req.command.command=name;req.command.position=float(position)
            req.command.force=float(force);req.command.speed=float(speed)
            future=client.call_async(req);deadline=time.monotonic()+3
            while not future.done() and time.monotonic()<deadline: time.sleep(.01)
            assert future.done(),'service did not respond'
            return future.result()
        assert command('connect').success
        assert FakeGripper.writes==[],FakeGripper.writes
        assert not command('move',1,force=999).success
        assert not command('move',float('nan')).success
        assert not command('rotate_to',90).success
        assert FakeGripper.writes==[]
        assert command('move',10).success
        assert FakeGripper.writes==[(500,33,25)],FakeGripper.writes
        assert command('connect').status.finger_position_per_mille==500.
        print('ROS typed service, namespace, bounds and no-auto-init PASS',flush=True)
    finally:
        executor.shutdown(timeout_sec=3);thread.join(timeout=3)
        device.destroy_node();client_node.destroy_node();rclpy.shutdown()
    # Camera subscriptions must follow consumers, not all configured cameras.
    from station import operator_camera_view as camera
    cams=[{'id':'left','role':'wrist'},{'id':'right','role':'wrist'},{'id':'sim_wrist','role':'wrist'}]
    camera.watch(set(),set());node=camera._open_node(cams)
    try:
        node.sync_streams();assert node._streams.items=={}
        camera.watch({'left_color'},set());node.sync_streams()
        assert set(node._streams.items)=={'left_color'}
        assert all(node._accept('left_color') for _ in range(10)), 'color preview must preserve input cadence'
        from sensor_msgs.msg import Image
        from rclpy.qos import qos_profile_sensor_data
        publisher=node.create_publisher(Image,'/left/color/image_raw',qos_profile_sensor_data)
        message=Image();message.width=4;message.height=4;message.step=12;message.encoding='rgb8';message.data=bytes([90,20,10])*16
        deadline=time.monotonic()+3
        while 'left_color' not in camera.previews() and time.monotonic()<deadline:
            publisher.publish(message);rclpy.spin_once(node,timeout_sec=.05);camera.publish_latest()
        blob,width,height=camera.previews()['left_color']
        assert (width,height)==(4,4) and blob[:3]==bytes([90,20,10])
        import tempfile
        from station.cam_bridge import _export
        shms={}
        try:
            with tempfile.TemporaryDirectory() as directory:
                _export(Path(directory),shms,{})
                assert shms
                first=next(iter(shms.values()))[1]
                assert camera._shm_read(first)[3]==blob
        finally:
            for _,shm in shms.values(): shm.close();shm.unlink()
        camera.watch({'right_depth'},set());node.sync_streams()
        assert set(node._streams.items)=={'right_depth'}
        camera.watch(set(),{'left_color'});node.sync_streams()
        assert set(node._streams.items)=={'left_color'}
        camera.watch(set(),set());node.sync_streams()
        assert not node._streams.items and not node._detections.items
        print('ROS dynamic camera subscriptions PASS',flush=True)
    finally:
        node.destroy_node();rclpy.shutdown()


    # Real-camera UI preview uses a compressed transport; perception stays raw.
    import cv2,numpy as np
    from sensor_msgs.msg import CompressedImage
    camera.watch({'compressed_test_color'},set())
    node=camera._open_node([{'id':'compressed_test','role':'wrist','preview_transport':'compressed'}])
    try:
        node.sync_streams()
        publisher=node.create_publisher(CompressedImage,'/compressed_test/color/preview/compressed',qos_profile_sensor_data)
        ok,jpg=cv2.imencode('.jpg',np.full((720,1280,3),(20,80,150),np.uint8));assert ok
        sample=CompressedImage();sample.format='jpeg';sample.data=jpg.tobytes()
        deadline=time.monotonic()+3
        while 'compressed_test_color' not in camera.previews() and time.monotonic()<deadline:
            publisher.publish(sample);rclpy.spin_once(node,timeout_sec=.05);camera.publish_latest()
        blob,w,h=camera.previews()['compressed_test_color']
        assert (w,h)==(1280,720),(w,h)
        assert len(blob)==w*h*3
        print('ROS compressed preview preserves camera resolution PASS',flush=True)
    finally:
        camera.watch(set(),set());node.destroy_node();rclpy.shutdown()

    # Exercise native Action feedback/cancellation without a robot controller.
    from station.ros_gateway import RosGateway
    from arm_application.site_io import load_site_dict
    from sensor_msgs.msg import JointState
    from controller_manager_msgs.srv import ListControllers, SwitchController
    from controller_manager_msgs.msg import ControllerState
    from rclpy.action import ActionServer, CancelResponse
    from rclpy.callback_groups import ReentrantCallbackGroup
    from control_msgs.action import FollowJointTrajectory
    from station.tasks import Cancelled
    gateway=RosGateway(load_site_dict())
    fake=Node('fake_controller')
    controller_state = ['active']
    switches = []
    def controllers(req,resp):
        state=ControllerState();state.name='rizon_arm_controller';state.state=controller_state[0]
        state.claimed_interfaces=[f"{arm['sn']}_joint{i}/position" for i in range(1,8)] if state.state=='active' else []
        resp.controller=[state];return resp
    fake.create_service(ListControllers,'/controller_manager/list_controllers',controllers)
    def switch(req,resp):
        switches.append((list(req.activate_controllers),list(req.deactivate_controllers)))
        controller_state[0]='inactive' if req.deactivate_controllers else 'active'
        resp.ok=True;return resp
    fake.create_service(SwitchController,'/controller_manager/switch_controller',switch)
    group=ReentrantCallbackGroup()
    def execute(handle):
        end=time.monotonic()+.7
        while time.monotonic()<end:
            if handle.is_cancel_requested:
                handle.canceled();return FollowJointTrajectory.Result()
            time.sleep(.01)
        handle.succeed();return FollowJointTrajectory.Result()
    action=ActionServer(fake,FollowJointTrajectory,'/rizon_arm_controller/follow_joint_trajectory',
                        execute_callback=execute,cancel_callback=lambda request:CancelResponse.ACCEPT,callback_group=group)
    gateway.executor.add_node(fake)
    arm=next(a for a in gateway.arms if a['id']==gateway.active_arm)
    msg=JointState();msg.name=[f"{arm['sn']}_joint{i}" for i in range(1,8)];msg.position=[0.]*7
    try:
        from unittest.mock import patch
        from flexiv_msgs.msg import RobotStates
        from station.operator_arm_state import tcp_display_pose
        import math
        assert tcp_display_pose((0.,0.,0.),(0.,0.,0.,0.)) is None
        yaw_pose=tcp_display_pose((.1,.2,.3),(0.,0.,math.sqrt(.5),math.sqrt(.5)))
        assert yaw_pose[:3]==[100.,200.,300.] and abs(yaw_pose[-1]-90.)<1e-8
        native=RobotStates()
        native.tcp_pose.pose.position.x=.5
        native.tcp_pose.pose.orientation.w=1.
        aid=arm['id']
        with patch('station.ros_gateway.time.monotonic',return_value=100.):
            gateway._joint(msg);native.robot_timestamp.sec=10;gateway._native_state(aid,native)
            assert gateway.states()[0][aid]['enabled'] is None
        with patch('station.ros_gateway.time.monotonic',return_value=100.1):
            gateway._joint(msg);native.robot_timestamp.sec=11;gateway._native_state(aid,native)
            assert gateway.states()[0][aid]['enabled'] is True
            assert gateway.states()[0][aid]['tcp_pose']==[500.,0.,0.,0.,0.,0.]
        with patch('station.ros_gateway.time.monotonic',return_value=102.):
            gateway._joint(msg);gateway._native_state(aid,native)
            assert gateway.states()[0][aid]['pose_live'] is True
            assert gateway.states()[0][aid]['tcp_pose'] is None
            assert gateway.states()[0][aid]['enabled'] is None, 'repeated cached hardware sample must not confirm enable'
        gateway._robot_clock.clear()
        gateway._last_joint_sample=0.
        gateway._joint(msg)
        assert gateway.move_joints(arm,[.01]*7,.15,threading.Event())['ok']
        gateway._joint(msg)
        cancel=threading.Event();timer=threading.Timer(.3,cancel.set);timer.start()
        try:
            gateway.move_joints(arm,[.01]*7,.15,cancel)
            raise AssertionError('expected action cancellation')
        except Cancelled: pass
        timer.join()
        assert not gateway._goals
        joint_stop=threading.Event()
        def joint_feedback():
            while not joint_stop.wait(.03): gateway._joint(msg)
        joint_thread=threading.Thread(target=joint_feedback);joint_thread.start()
        try:
            assert gateway.stop_arm(arm)['ok']
            assert gateway.confirm_stopped(arm)['ok']
            assert controller_state[0]=='inactive'
            assert switches[-1]==([],['rizon_arm_controller'])
            # Enable/readiness checks must not resume a stopped trajectory.
            switch_count = len(switches)
            gateway.require_arm(arm['id'],threading.Event(),resume=False)
            assert controller_state[0]=='inactive'
            assert len(switches)==switch_count
            # Only the next explicit motion command restores the controller.
            gateway.require_arm(arm['id'],threading.Event())
            assert controller_state[0]=='active'
            assert switches[-1]==(['rizon_arm_controller'],[])
            wrong={**arm,'sn':'OtherRobot'}
            try:
                gateway.stop_arm(wrong)
                raise AssertionError('wrong-arm stop accepted')
            except RuntimeError as exc: assert '不属于' in str(exc)
        finally:
            joint_stop.set();joint_thread.join()
        print('ROS stop deactivation, stationary confirmation, explicit resume and arm ownership PASS',flush=True)
        gateway._motion_watch.add(arm['id'])
        gateway._robot_clock[arm['id']] = (123, time.monotonic())
        assert gateway.motion_interruption(arm['id']) is None
        gateway._robot_clock[arm['id']] = (123, time.monotonic()-1.)
        assert gateway.motion_interruption(arm['id'])
        assert gateway.motion_interruption(arm['id']) is None
        # A recovered Enable sample does not recreate the cancelled motion watch/task.
        gateway._robot_clock[arm['id']] = (124, time.monotonic())
        assert arm['id'] not in gateway._motion_watch
        gateway._robot_clock.clear()
        print('ROS operational feedback loss cancels old motion once; no automatic resume PASS',flush=True)
        from std_msgs.msg import Float32MultiArray
        saved_gripper=gateway._grippers.pop(gateway.active_arm,None)
        try:
            legacy=Float32MultiArray();legacy.data=[1.,1.,300.,300.,33.,0.]
            gateway._legacy_grip(legacy)
            assert gateway.states()[1][gateway.active_arm]['connected'] is False
        finally:
            if saved_gripper:gateway._grippers[gateway.active_arm]=saved_gripper
        from arm_msgs.msg import TactileStatus
        from station import site_catalog
        force=TactileStatus();force.connected=True;force.fnormal=1.25;force.fshearx=-.5;force.fsheary=.75
        gateway._tactile('left',force)
        assert site_catalog._TACTILE['left']['fnormal']==1.25
        assert site_catalog._TACTILE['left']['fshearx']==-.5
        from geometry_msgs.msg import WrenchStamped
        calibrated=WrenchStamped();calibrated.wrench.force.x=.2;calibrated.wrench.force.y=-.3;calibrated.wrench.force.z=.4
        gateway._tactile_wrench('left',calibrated);gateway._tactile('left',force)
        assert site_catalog._TACTILE['left']['force_n']=={'fnormal':.4,'fshearx':.2,'fsheary':-.3}
        assert site_catalog._TACTILE['left']['fnormal']==1.25
        assert gateway.tactile_history()['left'][-1]['fshearx']==.2
        packet=gateway.tactile_sample_packet({})
        assert packet['samples']['left'][-1]['fshearx']==.2
        seq=packet['samples']['left'][-1]['seq']
        assert gateway.tactile_sample_packet({'left':seq})['samples']['left']==[]
        gateway.clear_tactile_reference('left')
        assert gateway.tactile_history()['left']==[]
        assert gateway.tactile_sample_packet({})['epochs']['left']>packet['epochs']['left']
        assert site_catalog._TACTILE['left']['force_n'] is None
        gateway.watch_tactile(True)
        gateway._tactile_image('left_thermal',message)
        assert 'left_thermal' in gateway.tactile_frames()
        gateway.watch_tactile(False)
        assert not gateway.tactile_frames() and not gateway._tactile_images.items
        print('ROS tactile driver reuse, unchanged force values and preview lifetime PASS',flush=True)
        print('ROS persistent gateway, arm identity and trajectory cancellation PASS',flush=True)
    finally:
        action.destroy();gateway.executor.remove_node(fake);fake.destroy_node();gateway.close()


if __name__=='__main__': main()