import math
import time
import asyncio
import xml.etree.cElementTree as ET
from threading import Thread

import numpy as np
from scipy.spatial.transform import Rotation as R

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

import qtm  # Ensure that the Qualisys Python SDK is installed

# The name of the rigid body in QTM
RIGID_BODY_NAME = 'turtlebot121'  # Replace with your rigid body name
QTM_IP_ADDRESS = '192.168.0.151'  # Replace with your QTM IP address

class QtmWrapper(Thread):
    def __init__(self, body_name, ip_address):
        Thread.__init__(self)

        self.body_name = body_name
        self.ip_address = ip_address  # Store the IP address
        self.on_pose = None
        self.connection = None
        self.qtm_6DoF_labels = []
        self._stay_open = True

        self.start()

    def close(self):
        self._stay_open = False
        self.join()

    def run(self):
        asyncio.run(self._life_cycle())

    async def _life_cycle(self):
        await self._connect()
        while self._stay_open:
            await asyncio.sleep(0.1)
        await self._close()

    async def _connect(self):
        print('Connecting to QTM on ' + self.ip_address)
        self.connection = await qtm.connect(self.ip_address)

        params = await self.connection.get_parameters(parameters=['6d'])
        xml = ET.fromstring(params)
        self.qtm_6DoF_labels = [label.text.strip() for label in xml.findall('*/Body/Name')]

        await self.connection.stream_frames(
            components=['6D'],
            on_packet=self._on_packet)

    def _on_packet(self, packet):
        header, bodies = packet.get_6d()

        if bodies is None:
            return

        if self.body_name not in self.qtm_6DoF_labels:
            print('Body ' + self.body_name + ' not found.')
        else:
            index = self.qtm_6DoF_labels.index(self.body_name)
            temp_cf_pos = bodies[index]
            position = temp_cf_pos[0]
            rotation_matrix = temp_cf_pos[1].matrix

            if self.on_pose:
                # Make sure we got a position
                if math.isnan(position[0]):
                    return
                # Pass position and rotation matrix to on_pose callback
                self.on_pose(position, rotation_matrix)

    async def _close(self):
        await self.connection.stream_frames_stop()
        self.connection.disconnect()

class TeleopRobotNode(Node):
    def __init__(self):
        super().__init__('teleop_robot')
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.velocity_msg = Twist()

        # Initialize control law parameters
        self.V = 0.6  # Linear velocity (m/s)
        self.k2 = 2.0
        self.k1 = 1.0
        self.ka = 1.0
        self.d = 0.7
        self.z_o = 0.0  # Initial value for z_o
        self.x_d = 0.0  # Desired x position
        self.y_d = 0.0  # Desired y position

        self.counter = time.time()
        self.Prev_time = time.time()

        # Start the QtmWrapper to get pose data
        self.qtm_wrapper = QtmWrapper(RIGID_BODY_NAME, QTM_IP_ADDRESS)
        self.qtm_wrapper.on_pose = self.on_pose_callback

    def on_pose_callback(self, position_raw, rotation_matrix_raw):
        # Process pose data
        x = position_raw[0] / 1000.0  # Convert from mm to meters
        y = position_raw[1] / 1000.0
        z = position_raw[2] / 1000.0

        # Convert rotation matrix to Euler angles
        rotation_matrix = np.array(rotation_matrix_raw).reshape(3, 3)
        r = R.from_matrix(rotation_matrix)
        rot = r.as_euler('zyx', degrees=True)
        theta = -rot[0]  # Assuming yaw is the angle we need (negative sign based on original code)
        theta_r = math.radians(theta)

        # Implement control law
        current_time = time.time()
        elapsed_time = current_time - self.counter

        if elapsed_time >= 0.05:
            h = current_time - self.Prev_time
            # Update z_o
            distance_squared = (x - self.x_d) ** 2 + (y - self.y_d) ** 2
            z_dot = -(self.k2 * self.z_o) + (0.5 * distance_squared)
            self.z_o = self.z_o + z_dot * h
            self.Prev_time = current_time

            numerator = (-self.k2 * self.z_o + 0.5 * distance_squared)
            denominator = math.sqrt(1 + numerator ** 2)
            c = numerator / denominator

            denominator_w = self.ka ** 2 - distance_squared
            if denominator_w == 0.0:
                w = 0.0  # Avoid division by zero
            else:
                w = (self.k1 * self.V * c / denominator_w) + (self.V / self.d)
            
            # Set velocities
            self.velocity_msg.linear.x = self.V
            self.velocity_msg.angular.z = -w  # Negative sign as per original code

            # Publish the Twist message
            self.publisher.publish(self.velocity_msg)

            # Update counter
            self.counter = current_time

            # Debugging information
            self.get_logger().info(f"Position: ({x:.2f}, {y:.2f}), Theta: {theta:.2f} deg")
            self.get_logger().info(f"z_o: {self.z_o:.4f}, h: {h:.4f}, w: {w:.4f}")
            self.get_logger().info(f"Linear Velocity: {self.velocity_msg.linear.x:.3f}, Angular Velocity: {self.velocity_msg.angular.z:.3f}")

    def destroy_node(self):
        # Close the QtmWrapper when shutting down the node
        self.qtm_wrapper.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = TeleopRobotNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
i want to run simple code where the robot simply return to origin implement this in place of control law i have written