"""
    Streaming 6Dof from QTM
"""

import asyncio
import xml.etree.ElementTree as ET
import math
import qtm_rt

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

import time

from csv import DictWriter
import csv

global field_names

field_names = ['Time', 'x', 'y', 'Q', 'u']

global start_time

start_time = time.time()

import socket

UDP_IP_ADDRESS = "192.168.0.105"  # Replace with your server IP address
UDP_PORT_NO = 3001

serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

serverSock.bind((UDP_IP_ADDRESS, UDP_PORT_NO))





def create_body_index(xml_string):
    """ Extract a name to index dictionary from 6dof settings xml """
    xml = ET.fromstring(xml_string)

    body_to_index = {}
    for index, body in enumerate(xml.findall("*/Body/Name")):
        body_to_index[body.text.strip()] = index

    return body_to_index

def body_enabled_count(xml_string):
    xml = ET.fromstring(xml_string)
    return sum(enabled.text == "true" for enabled in xml.findall("*/Body/Enabled"))

async def main():

    # Connect to qtm
    connection = await qtm_rt.connect("192.168.0.111")

    # Connection failed?
    if connection is None:
        print("Failed to connect")
        return

    # Take control of qtm, context manager will automatically release control after scope end
    async with qtm_rt.TakeControl(connection, "password"):

        realtime = True

        if realtime:
            # Start new realtime
            await connection.new()
        else:
            print("make real time = true")

    # Get 6dof settings from qtm
    xml_string = await connection.get_parameters(parameters=["6d"])
    body_index = create_body_index(xml_string)

    print("{} of {} 6DoF bodies enabled".format(body_enabled_count(xml_string), len(body_index)))
    global counter
    counter = time.time()
    
    wanted_body_1 = "Khepera101"

    def on_packet(packet):
        info, bodies = packet.get_6d()
        # print(
        #     "Framenumber: {} - Body count: {}".format(
        #         packet.framenumber, info.body_count
        #     )
        # )



        if wanted_body_1 is not None and wanted_body_1 in body_index:
         
            wanted_index_1 = 6
            
            position_1, rotation_1= bodies[wanted_index_1]
            

            x_2=position_1[0]/1000
            y_2=position_1[1]/1000
            vk = np.array([0.3])
            
            #second
            rotation_1 = np.array(rotation_1)
            rotation_1 = rotation_1.reshape(3,3)
            r_1 = R.from_matrix(rotation_1)
            rot_1 = r_1.as_euler('zyx', degrees=True)

            #print(x,y,theta)

          
            #second
            theta_1=-rot_1[0]
            #print(x,y,theta)

            theta_r1=math.radians(theta_1)
            #print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rot))
            # print(rot)
            # data, addr = serverSock.recvfrom(1024)  # buffer size is 1024 bytes
            #beta = 2
            #Q=math.radians(theta)
            #Control Gain
            k1=0.004
            #accleration control
            k2=0.75

             #synchronization and dpv

            #print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rot))
            # print(rot)
            # data, addr = serverSock.recvfrom(1024)  # buffer size is 1024 bytes
            # k_theta = 0.5
            # k_p = 0.5

            # x_d = -1000  # values given in mm
            # y_d = -1000

            # # V = k_p*e_p
            # # w = k_theta*e_theta

        
            # vel = 0.5
            # alpha = 0.5
            # sigma = 2
           
            # k = 0.03
    

            # Send the move command
            global counter
            global start_time
            elapsed_time = time.time() - counter
    
            if elapsed_time >= 0.05:
                counter = time.time()

                #Defining Positions
                x = np.array([x_2])
                #x=[x(1) x(2) x(3)];
                #y=[y(1) y(2) y(3)];
                y = np.array([y_2])
                #Defining Heading Angles
                theta=np.array([theta_r1])

                #velocity
            

                #desired velocity
                sd=0.02

                #Parameters
                alpha=(11-math.sqrt(105))/4
                delta = ((0.5 + alpha)/2)-alpha
                #sigma
                sigma=alpha



                phi = np.zeros(2)
                rk = np.zeros(2)

             # Calculate phi and rk individually
                
                phi[0] = np.arctan2(y[0], x[0])
                #phi[2] = np.arctan2(y[2], x[2])

                rk[0] = np.sqrt(x[0]**2 + y[0]**2)
               
                #rk[2] = np.sqrt(x[2]**2 + y[2]**2)

    
             # error
                tau_a = alpha**2 + alpha * rk * np.cos(phi) - alpha * (np.sin(theta) + alpha * rk * np.sin(theta - phi))
                tau_b = alpha * rk * np.sin(phi) + alpha * (np.cos(theta) + alpha * rk * np.cos(theta - phi))
                e_k = np.sqrt(tau_a**2 + tau_b**2) / np.sqrt(1 + (alpha * rk)**2 + 2 * alpha * rk * np.cos(phi))

             #      inner product
                inpr = alpha * ((1 + alpha**2) * rk * np.cos(theta - phi) + alpha * np.cos(theta) + alpha * rk**2 * np.cos(theta - 2 * phi)) / (1 + (alpha * rk)**2 + 2 * alpha * rk * np.cos(phi))

             # a_k
                a_k = inpr / (delta**2 - e_k**2)

            # acceleration control
                term1 = 1 + (alpha * rk)**2 + 2 * alpha * rk * np.cos(phi)
                u1 = -k2 * (vk + ((sd * term1) / (alpha * (alpha**2 - 1))))
                u2 = ((k1 * term1**2) / (alpha * (alpha**2 - 1))) * a_k
                u3 = (2 * alpha * vk**2 * (np.cos(theta) + alpha * rk * np.cos(theta - phi))) / term1

                u = u1 + u2 + u3

            # chi
                f1 = 1 + 2 * alpha * rk * np.cos(phi) + alpha**2 * rk**2 * np.cos(2 * phi)
                f2 = 2 * alpha * rk * np.sin(phi) + alpha**2 * rk**2 * np.sin(2 * phi)
                chi = np.arctan2(f2, f1)


           

            # etta
            #etta = (theta % (2 * np.pi) - chi) * (180 / np.pi)
            #newtheta = theta % (2 * np.pi)

            # curvature control
                omega1 = (1 / alpha) * (sd + k1 * a_k )
                omega2 = (2 * alpha * vk) * (np.sin(theta) + alpha * rk * np.sin(theta - phi)) / term1
                omega = omega1 + omega2

           # Update positions and velocities
                vk[0] = vk[0] + u[0]*10 * elapsed_time
               
                command_1 = f"{-float(omega[0])}x{float(u[0]*1000)}".encode()
                serverSock.sendto(command_1, addr)
                print(u[0])
                #print(float(vk[0]*1000), float(omega[0]), float(vk[1]*1000), float(omega[1]))



                # print(u1)


                t = time.time() - start_time

                # dict = {'Time': t, 'x' : x, 'y' : y, 'Q' : Q, 'u' : -u1}

                # with open('mobius_1.csv', 'a') as f_object:

                #     dictwriter_object = DictWriter(f_object, fieldnames=field_names) 
                #     dictwriter_object.writerow(dict)

                # f_object.close()

            #time.sleep(0.05)


            
        else:
            # Print all bodies
            for position, rotation in bodies:
                print("Pos: {} - Rot: {}".format(position, rotation))

    # Start streaming frames
    await connection.stream_frames(components=["6d"], on_packet=on_packet)
    counter = time.time()
    # Wait asynchronously 5 seconds
    await asyncio.sleep(300)

    # Stop streaming
    await connection.stream_frames_stop()


if __name__ == "__main__":
    # Run our asynchronous function until complete
    with open('mobius_1.csv', 'w') as f_object:
        writer = csv.writer(f_object) 
        writer.writerow(field_names)

        f_object.close()

    print("memed")
    data, addr = serverSock.recvfrom(1024)
    print("memed2")
    asyncio.get_event_loop().run_until_complete(main())
    print("stop")
    
    # Stop the robot
    stop_command = "0x0".encode()
    serverSock.sendto(stop_command, addr)
    above code write in below code way import asyncio
import math
import time
import xml.etree.cElementTree as ET
from threading import Thread
import qtm

# The name of the rigid body in QTM
rigid_body_name = 'turtlebot121'

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(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]
            x = temp_cf_pos[0][0] / 1000
            y = temp_cf_pos[0][1] / 1000
            z = temp_cf_pos[0][2] / 1000

            r = temp_cf_pos[1].matrix
            rot = [
                [r[0], r[3], r[6]],
                [r[1], r[4], r[7]],
                [r[2], r[5], r[8]],
            ]

            if self.on_pose:
                # Make sure we got a position
                if math.isnan(x):
                    return

                self.on_pose([x, y, z, rot])

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


if __name__ == '__main__':
    # Specify the IP address of the QTM
    qtm_ip_address = '192.168.0.111'  # Replace with your QTM IP address
    qtm_wrapper = QtmWrapper(rigid_body_name, qtm_ip_address)
    qtm_wrapper.on_pose = lambda pose: print(pose[0], pose[1], pose[2])
    time.sleep(5)
    qtm_wrapper.close()