"""Run new device nodes on a private ROS domain, with every device mocked."""
import threading
import time
from unittest.mock import patch
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import WrenchStamped
from arm_msgs.msg import TactileStatus
from arm_msgs.srv import GripperExecute
from std_srvs.srv import Trigger
from arm_application.dh_pgea import GripperState
import arm_application.gripper_node as gripper
from arm_application.tactile_flow_node import TactileFlowNode


class FakeGripper:
    writes=[];fail_force=False
    def __init__(self,**kwargs):self.state=GripperState(init_state=1,grip_state=1,position=300)
    def open(self):pass
    def close(self):pass
    def get_run_states(self,**kwargs):return self.state
    def set_target_force(self,v):self.writes.append(('force',v));return not self.fail_force
    def set_target_speed(self,v):self.writes.append(('speed',v));return True
    def set_target_position(self,v):self.writes.append(('position',v));self.state.position=v;return True

rclpy.init(args=['--ros-args','-r','__ns:=/isolated_devices'])
gripper.DhPgeaGripper=FakeGripper
jaw=gripper.GripperNode()
with patch.object(TactileFlowNode,'_capture',return_value=None):sensor=TactileFlowNode()
client_node=Node('device_test_client');statuses=[];frames=[];forces=[]
client_node.create_subscription(TactileStatus,'/tactile/left/status',statuses.append,10)
client_node.create_subscription(WrenchStamped,'/tactile/left/wrench',forces.append,qos_profile_sensor_data)
client_node.create_subscription(CompressedImage,'/tactile/left/flow/compressed',frames.append,qos_profile_sensor_data)
client=client_node.create_client(GripperExecute,'/isolated_devices/execute')
executor=MultiThreadedExecutor(num_threads=2)
for node in (jaw,sensor,client_node):executor.add_node(node)
thread=threading.Thread(target=executor.spin,daemon=True);thread.start()
try:
    assert client.wait_for_service(timeout_sec=3)
    def request(command,pos=0.,force=33.,speed=25.):
        req=GripperExecute.Request();req.command.command=command;req.command.position=pos;req.command.force=force;req.command.speed=speed
        future=client.call_async(req);end=time.monotonic()+3
        while not future.done() and time.monotonic()<end:time.sleep(.01)
        assert future.done();return future.result()
    assert request('connect').success and FakeGripper.writes==[]
    assert not request('init').success and FakeGripper.writes==[]
    assert not request('move',float('nan')).success and FakeGripper.writes==[]
    assert request('move',7.,25.,12.).success
    assert FakeGripper.writes==[('force',25),('speed',12),('position',350)]
    FakeGripper.writes=[];FakeGripper.fail_force=True
    assert not request('move',5.).success
    assert FakeGripper.writes==[('force',33)]
    ok,encoded=cv2.imencode('.jpg',np.zeros((64,64,3),np.uint8));assert ok
    end=time.monotonic()+3
    while time.monotonic()<end and not frames:
        with sensor._lock:
            sensor._sample=(time.monotonic(),dict(fps=30.,fnormal=123.,fshearx=36000.,fsheary=-36000.,contact=True))
            sensor._error='';sensor._jpeg=(time.monotonic(),encoded.tobytes())
        time.sleep(.02)
    assert frames and any(s.connected and s.fnormal==123. for s in statuses)
    end=time.monotonic()+2
    while not forces and time.monotonic()<end:time.sleep(.01)
    assert forces and abs(forces[-1].wrench.force.x-1.19)<1e-6
    assert abs(forces[-1].wrench.force.y+1.19)<1e-6
    assert forces[-1].wrench.force.z==0.
    assert cv2.imdecode(np.frombuffer(frames[-1].data,np.uint8),cv2.IMREAD_COLOR).shape==(64,64,3)
    reset_client=client_node.create_client(Trigger,'/tactile/left/reset_reference')
    assert reset_client.wait_for_service(timeout_sec=2)
    resets=[]
    class FakeSensor:
        def reset(self):resets.append('reset')
    def reset_worker():
        if sensor._reference_reset.wait(1.):sensor._apply_reference_reset(FakeSensor())
    with sensor._lock:sensor._sample=(time.monotonic(),dict(fps=30.,fnormal=0.,fshearx=0.,fsheary=0.,contact=False))
    worker=threading.Thread(target=reset_worker);worker.start()
    future=reset_client.call_async(Trigger.Request());end=time.monotonic()+3
    while not future.done() and time.monotonic()<end:time.sleep(.01)
    worker.join(2)
    assert future.done() and future.result().success and resets==['reset']
    sensor._reference_reset.set();sensor._abort_reference_reset()
    sensor._apply_reference_reset(FakeSensor())
    assert resets==['reset'] and not sensor._reference_result[0]
    with sensor._lock:sensor._sample=None;sensor._jpeg=None;sensor._error='USB offline'
    end=time.monotonic()+2
    while time.monotonic()<end and not any(not s.connected and s.error_msg=='USB offline' for s in statuses):time.sleep(.02)
    assert any(not s.connected and s.error_msg=='USB offline' for s in statuses)
    print('new gripper typed control, no-auto-init, failed-force guard, tactile JPEG and offline status PASS',flush=True)
finally:
    executor.shutdown(timeout_sec=3);thread.join(timeout=3)
    for node in (jaw,sensor,client_node):node.destroy_node()
    rclpy.shutdown()