import rclpy
from rclpy.node import Node
from shape_msgs.msg import Mesh, MeshTriangle
from geometry_msgs.msg import Point
import numpy as np
import open3d as o3d
from rq_controller.common import PyProjection

from rq_hardware.hardware_client import HardwareClient


rclpy.init()

client = HardwareClient()

projection = PyProjection.dummy()
projection.focal_spot_mm = 1000. * (np.random.random(3) - 0.5)
projection.detector_postion_mm = 1000. * (np.random.random(3) - 0.5)
future = client.check_reachability(projection)

rclpy.spin_until_future_complete(client, future)

reachable, cost, projetion_2 = client.reachability_response_2_py(future.result())

print(reachable, cost)

future = client.aquire_projection(projetion_2)

rclpy.spin_until_future_complete(client, future)

projection_simu = client.projection_response_2_py(future.result())

projection.focal_spot_mm = 100. * (np.random.random(3) - 0.5)
future = client.check_reachability(projection)

rclpy.spin_until_future_complete(client, future)

reachable, cost, projetion_2 = client.reachability_response_2_py(future.result())

print(reachable, cost)