from pymavlink import mavutil
import math
import time
from rich.console import Console
from rich.text import Text
from drone import Drone
import sys


cs = Console()
INFO = Text("INFO")
INFO.stylize("bold magenta")
ACTION = Text("ACTION")
ACTION.stylize("bold green")
ABORT = Text("ABORT")
ABORT.stylize("bold RED")
ENTER = Text("ENTER")
ENTER.stylize("bold RED")
ALERT = Text("ALERT")
ALERT.stylize("bold RED")

cs.print("**", ENTER, " the drone 1 ID : ", end = "")
kk1 = int(input())
cs.print("**", ENTER, " the drone 2 ID : ", end = "")
kk2 = int(input())
cs.print("**", ENTER, " the drone 3 ID : ", end = "")
kk3 = int(input())

cs.print("**", ENTER, " the Altitude and Speed of the RTL respectively for drone 1 (rA, rS) : ", end = "")
r1A, r1S = map(int, input().split(","))
cs.print("**", ENTER, " the Altitude and Speed of the RTL respectively for drone 2 (rA, rS) : ", end = "")
r2A, r2S = map(int, input().split(","))
cs.print("**", ENTER, " the Altitude and Speed of the RTL respectively for drone 3 (rA, rS) : ", end = "")
r3A, r3S = map(int, input().split(","))

cs.print("**", ENTER, " the take off height for drone 1 : ", end = "")
take1 = int(input())
cs.print("**", ENTER, " the take off height for drone 2 : ", end = "")
take2 = int(input())
cs.print("**", ENTER, " the take off height for drone 3 : ", end = "")
take3 = int(input())

cs.print("**", ENTER, " the waypoint mission speed for drone 1: ", end = "")
speed1=int(input())
cs.print("**", ENTER, " the waypoint mission speed for drone 2: ", end = "")
speed2=int(input())
cs.print("**", ENTER, " the waypoint mission speed for drone 3: ", end = "")
speed3=int(input())

waypoints = [
    (-35.3628588,149.1648891, 12),
    (-35.3626331, 149.1653596, 12),
]
# waypoints= [
#     (-35.3629705,149.1649417,12),
#     (-35.3628578,149.1651791,12),
# ]
# for setting up the connection 
cs.print("**", INFO, ": START Connection")
the_connection = mavutil.mavlink_connection('udpin:localhost:14551', baud=115200)
the_connection.wait_heartbeat()
cs.print("**", ACTION, " : telemetry connection  DONE")

# for Connecting the drone 1
cs.print("**", INFO, ": Connecting to Drone 1")
drone1 = Drone(kk1, the_connection)
cs.print("**", ACTION, ": Drone 1 is Connected")

# for connecting the drone 2
cs.print("**", INFO, ": Connecting to Drone 2")
drone2 = Drone(kk2, the_connection)
cs.print("**", ACTION, ": Drone 2 is Connected")

# for connecting the drone 2
cs.print("**", INFO, ": Connecting to Drone 3")
drone3 = Drone(kk3, the_connection)
cs.print("**", ACTION, ": Drone 3 is Connected")

time.sleep(5)

rtl_params = {
    kk1: {'RTL_ALT': r1A * 100, 'RTL_SPEED': r1S * 100},
    kk2: {'RTL_ALT': r2A * 100, 'RTL_SPEED': r2S * 100},
    kk3: {'RTL_ALT': r3A * 100, 'RTL_SPEED': r3S * 100}    
}

for drone_id, params in rtl_params.items():
    for param_name, param_value in params.items():
        the_connection.param_set_send(param_name, param_value,drone_id)

#for setting the mode to guided 
cs.print("**", INFO, ": Seting the mode to GUIDED for drone 1")
drone1.guided()
cs.print("**", ACTION, ": Set the mode to GUIDED for drone 1")

cs.print("**", INFO, ": Seting the mode to GUIDED for drone 2")
drone2.guided()
cs.print("**", ACTION, ": Set the mode to GUIDED for drone 2")

cs.print("**", INFO, ": Seting the mode to GUIDED for drone 3")
drone3.guided()
cs.print("**", ACTION, ": Set the mode to GUIDED for drone 3")

# to arm the drone
cs.print("**", INFO, ": Arming the Drone 1")
drone1.arm()
cs.print("**", ACTION, ": Arm the drone 1")

cs.print("**", INFO, ": Arming the Drone 2")
drone2.arm()
cs.print("**", ACTION, ": Arm the drone 2")

cs.print("**", INFO, ": Arming the Drone 3")
drone3.arm()
cs.print("**", ACTION, ": Arm the drone 3")

time.sleep(5)


#for takeoff
cs.print("**", INFO, ": takeoff for drone 1")
drone1.takeoff(take1)
cs.print("**", ACTION, ": takeoff for drone 1")

cs.print("**", INFO, ": takeoff for drone 2")
drone2.takeoff(take2)
cs.print("**", ACTION, ": takeoff for drone 2")

cs.print("**", INFO, ": takeoff for drone 3")
drone3.takeoff(take3)
cs.print("**", ACTION, ": takeoff for drone 3")

time.sleep(15)


# Initialize the previous_mode variable to None
previous_mode = None
ang=0
while True:
    msg = the_connection.recv_match(type='HEARTBEAT', blocking=False)
    if msg and msg.get_srcSystem() == kk1:
        sysid = msg.get_srcSystem()
        mode = mavutil.mode_string_v10(msg)
        if mode != previous_mode: # check if the mode has changed
            previous_mode = mode # update the previous_mode variable

            print(f"System ID: {sysid}, Mode: {mode}")
            if mode == 'GUIDED': # check if the mode is GUIDED
                cs.print("**",INFO,"Entering GUIDED mode")
                for wp in waypoints:
                    # time.sleep()
                    cs.print("**", INFO, "the master waypoints are : ", wp[0],wp[1], take1)
                    drone1.send_waypoints(wp[0],wp[1],take1,speed1)
                    ang=int(drone1.calculate_angle_offset())
                    print(ang)
                    a1=230+ang
                    print(a1)
                    f1x,f1y= drone2.calculate_follower_coordinate(wp[0],wp[1],a1)
                    cs.print("**", INFO, "the follower 1 waypoints are : ", f1x, f1y, take2)
                    drone2.send_waypoints(f1x,f1y,take2,speed2)
                    a2=130+ang
                    print(a2)
                    f2x,f2y = drone3.calculate_follower_coordinate(wp[0],wp[1],a2)
                    cs.print("**", INFO, "the follower 2 waypoints are : ", f2x, f2y, take3)
                    drone3.send_waypoints(f2x,f2y,take3,speed3)
                    if abort():
                        exit()
            else:
                cs.print("**", ALERT ,": mode changed")
                the_connection.close()
                exit()

            time.sleep(1)

            cs.print("**",INFO," : Set mode to Return to Launch")
            the_connection.mav.set_mode_send(
                kk1,   # Target system ID
                mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,   # Mode flag
                6  # RTL
            )
            print("mode set to RTL for drone 1")
            the_connection.mav.set_mode_send(
                kk2,   # Target system ID
                mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,   # Mode flag
                6  # RTL
            )
            print("mode set to RTL for drone 2")
            the_connection.mav.set_mode_send(
                kk3,   # Target system ID
                mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,   # Mode flag
                6  # RTL
            )
            print("mode set to RTL for drone 3")

            time.sleep(1)
            cs.print("**",INFO," : disarming the drone")
            the_connection.mav.command_long_send(
                the_connection.target_system,   # Target system ID
                the_connection.target_component,   # Target component ID
                mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,   # Command ID
                0,   
                0,   # Arm (0) or disarm (1)
                0,   
                0,
                0,
                0,
                0,
                0
            )
            break

            


# to close the connnection 
cs.print("**", INFO, ": closing the connection")
the_connection.close()
cs.print("**", ACTION, ": Connection Closed")



integrate the above code into the below code, keep in mind that the threading and queue logic should not be changed



def run_drone_check():
    # Get the drone ID
    drone_id = int(input("Enter the drone ID: "))
    # master = mavutil.mavlink_connection('udpin:localhost:14551', source_system=drone_id, baud=57600)
    master = mavutil.mavlink_connection('udpin:localhost:14551', source_system=drone_id, baud=57600)

    # Establish the drone connection
    drone = Drone(drone_id, master)
    drone.wait_heart()

    cs.print("**", ENTER, " the Altitude and Speed of the RTL respectively (rA, rS) : ", end = "")
    rA, rS = map(int, input().split(","))

    cs.print("**", ENTER, " the take off height: ", end = "")
    take = int(input())

    cs.print("**", ENTER, " the waypoint mission speed: ", end = "")
    speed=int(input())
    waypoints = [
        (-35.3625479, 149.1646759, 12),
        (-35.3621984, 149.1659014, 12), 
        (-35.3628811,149.1651413,12),
    ]

    time.sleep(1)
    # Create a Queue and Event for sharing data and controlling the thread
    result_queue = Queue()
    stop_event = threading.Event()

    # Create a thread to run the check_conditions_and_set_mode function in the background
    check_thread = threading.Thread(target=check_conditions_and_set_mode, args=(drone, stop_event, result_queue,master))
    check_thread.start()
    # Execute other commands while check_conditions_and_set_mode is running
    drone.mode(4)
    time.sleep(5)

    # Check the condition before arming the drone
    if not result_queue.get():
        drone.arm()
    time.sleep(5)
    if not result_queue.get():
        drone.takeoff(10)
    time.sleep(10)
    rtl_params = {
        drone_id: {'RTL_ALT': rA * 100, 'RTL_SPEED': rS * 100}
    }

    for drone_id, params in rtl_params.items():
        for param_name, param_value in params.items():
            master.param_set_send(param_name, param_value, drone_id)

        # Initialize the previous_mode variable to None
        previous_mode = None

        while True:
            msg = master.recv_match(type='HEARTBEAT', blocking=True)
            if msg and msg.get_srcSystem() == drone_id:
                sysid = msg.get_srcSystem()
                mode = mavutil.mode_string_v10(msg)
                if mode != previous_mode: # check if the mode has changed
                    previous_mode = mode # update the previous_mode variable

                    print(f"System ID: {sysid}, Mode: {mode}")
                    time.sleep(5)
                    if mode == 'GUIDED': # check if the mode is GUIDED
                        cs.print("**",INFO,"Entering GUIDED mode")
                        abort_flag = False
                        for wp in waypoints:
                            time.sleep(1)
                            drone.send_waypoints(wp[0],wp[1],take,speed)
                            if abort(drone,master):
                                abort_flag = True
                                break
                        if abort_flag:
                            break
                    else:
                        cs.print("**", ALERT ,": mode changed")
                        master.close()
                        exit()

                    time.sleep(1)

                    cs.print("**",INFO," : Set mode to Return to Launch")
                    drone.mode(6)

                    time.sleep(1)
                    cs.print("**",INFO," : disarming the drone")
                    drone.disarm()
                    break

    # Stop the thread
    stop_event.set()
    check_thread.join()

    # Close the drone connection
    master.close()
    print("Connection closed.")
    exit()


if __name__ == "__main__":
    threading.Thread(target=run_drone_check, args=()).start()