import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from rcl_interfaces.srv import GetParameters
import time
from cart_to_angles import ik_sol
import roboticstoolbox as rtb

class ScaledJointTrajectoryPublisher(Node):
    def __init__(self):
        super().__init__('scaled_joint_trajectory_publisher')
        
        # Create a parameter client to request the parameter from /robot_state_publisher
        self.client = self.create_client(GetParameters, '/robot_state_publisher/get_parameters')

        while not self.client.wait_for_service(timeout_sec=3.0):
            self.get_logger().warn("Waiting for /robot_state_publisher parameter service...")

        # Create and send a request to get 'robot_description'
        self.request = GetParameters.Request()
        self.request.names = ["robot_description"]

        self.future = self.client.call_async(self.request)
        rclpy.spin_until_future_complete(self, self.future)

        # Extract the URDF
        if self.future.result():
            self.urdf_string = self.future.result().values[0].string_value
            if self.urdf_string:
                self.get_logger().info("Successfully retrieved URDF from /robot_state_publisher!")
            else:
                self.get_logger().error("URDF is empty")
        else:
            self.get_logger().error("Failed to get URDF from /robot_state_publisher.")

        
        self.publisher = self.create_publisher(
            JointTrajectory, 
            '/scaled_joint_trajectory_controller/joint_trajectory', 
            10
        )
        time.sleep(1)  # Allow publisher to initialize
        self.get_logger().info("Scaled Joint Trajectory Publisher Ready")

    def send_trajectory(self, joint_positions, duration=5.0):
        """Publish a joint trajectory command to move the UR10e."""
        msg = JointTrajectory()
        msg.joint_names = [
            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
            "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
        ]

        point = JointTrajectoryPoint()
        point.positions = joint_positions  # Target joint positions
        point.time_from_start.sec = int(duration)  # Duration in seconds

        msg.points.append(point)

        self.publisher.publish(msg)


def main():
    rclpy.init()
    node = ScaledJointTrajectoryPublisher()

    urdf_string = node.urdf_string

    # Save to a file
    urdf_path = "/tmp/robot.urdf"
    with open(urdf_path, "w") as file:
        file.write(urdf_string)

    print(f"URDF saved to {urdf_path}")

    robot = rtb.ERobot.URDF(urdf_path) # Load into Robotic Toolbox 

    target_position = [0.4, 0.4, 0.4, 0, 0, 0]
    sol = ik_sol(robot,target_position)
    print("Winkel:")
    print([val * (180/3.141) for val in sol])
    print(sol)
    # Move to a target position
    node.send_trajectory(sol)

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
