in the below code, implemnet grey work optimization along with pid control for swarming
from pymavlink import mavutil
import math
import time


class Drone:

    def __init__(self, system_id, connection):
        self.system_id = system_id
        self.connection = connection

    def set_mode(self, mode):
        self.connection.mav.set_mode_send(
            self.system_id,
            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
            mode
        )

    def arm(self, arm=True):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, int(arm), 0, 0, 0, 0, 0,
                                              0)

    def takeoff(self, altitude):
        self.connection.mav.command_long_send(self.system_id, self.connection.target_component,
                                              mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude)

    def send_waypoint(self, wp, next_wp, speed=3):
        vx, vy, vz = calculate_velocity_components(wp, next_wp, speed)
        self.connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_global_int_message(
            10,
            self.system_id,
            self.connection.target_component,
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
            int(0b110111111000),
            int(wp[0] * 10 ** 7),
            int(wp[1] * 10 ** 7),
            wp[2],
            vx, vy, vz, 0, 0, 0, 0  # Set vx, vy, and vz from calculated components
        ))

    def get_position(self):
        self.connection.mav.request_data_stream_send(
            self.system_id, self.connection.target_component,
            mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1, 1)

        while True:
            msg = self.connection.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
            if msg.get_srcSystem() == self.system_id:
                return (msg.lat / 10 ** 7, msg.lon / 10 ** 7, msg.alt / 10 ** 3)


class PIDController:

    def __init__(self, kp, ki, kd, limit):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.limit = limit
        self.prev_error = 0
        self.integral = 0

    def update(self, error, dt):
        derivative = (error - self.prev_error) / dt
        self.integral += error * dt
        self.integral = max(min(self.integral, self.limit), -self.limit)  # Clamp the integral term
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.prev_error = error
        return output

distance = 5  # Distance in meters
angle = 60  # Angle in degrees
kp = 0.1
ki = 0.01
kd = 0.05
pid_limit = 0.0001
pid_lat = PIDController(kp, ki, kd, pid_limit)
pid_lon = PIDController(kp, ki, kd, pid_limit)

def calculate_follower_coordinates(wp, distance, angle):
    earth_radius = 6371000.0  # in meters
    latitude_change = (180 * distance * math.cos(math.radians(angle))) / (math.pi * earth_radius)
    longitude_change = (180 * distance * math.sin(math.radians(angle))) / (
            math.pi * earth_radius * math.cos(math.radians(wp[0])))
    new_latitude = wp[0] + latitude_change
    new_longitude = wp[1] + longitude_change
    return (new_latitude, new_longitude, wp[2])

def calculate_velocity_components(current_wp, next_wp, speed):
    dx = next_wp[0] - current_wp[0]
    dy = next_wp[1] - current_wp[1]
    dz = next_wp[2] - current_wp[2]
    dx2 = dx ** 2
    dy2 = dy ** 2
    dz2 = dz ** 2
    distance = math.sqrt(dx2 + dy2 + dz2)
    vx = (dx / distance) * speed
    vy = (dy / distance) * speed
    vz = (dz / distance) * speed
    return vx, vy, vz

waypoints = [
    (28.5861474, 77.3421320, 10),
    (28.5859040, 77.3420736, 10)
]

the_connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600)

master_drone = Drone(3, the_connection)
follower_drone = Drone(2, the_connection)

# Set mode to Guided and arm both drones
for drone in [master_drone, follower_drone]:
    drone.set_mode(4)
    drone.arm()
    drone.takeoff(10)


class AbortTimeoutError(Exception):
    pass

def abort():
    print("Type 'abort' to return to Launch and disarm motors.")
    start_time = time.monotonic()
    while time.monotonic() - start_time < 7:
        user_input = input("Time left: {} seconds \n".format(int(7 - (time.monotonic() - start_time))))
        if user_input.lower() == "abort":
            print("Returning to Launch and disarming motors…")
            for drone in [master_drone, follower_drone]:
                drone.set_mode(6)  # RTL mode
                drone.arm(False)  # Disarm motors
            return True
    print("7 seconds have passed. Proceeding with waypoint task...")
    return False


mode = the_connection.mode_mapping()[the_connection.flightmode]
print(f"the drone is currectly at mode {mode}")

time_start = time.time()
while mode == 4:
    if abort():
        exit()

    if time.time() - time_start >= 1:
        for index, master_wp in enumerate(waypoints[:-1]):
            next_wp = waypoints[index + 1]  
            master_drone.send_waypoint(master_wp, next_wp, speed=3)
            follower_position = master_drone.get_position()
            if follower_position is None:
                break
            follower_wp = calculate_follower_coordinates(follower_position, distance, angle)
            dt = time.time() - time_start
            pid_lat_output = pid_lat.update(follower_wp[0] - follower_position[0], dt)
            pid_lon_output = pid_lon.update(follower_wp[1] - follower_position[1], dt)
            adjusted_follower_wp = (
                follower_wp[0] + pid_lat_output, follower_wp[1] + pid_lon_output, follower_wp[2])
            follower_drone.send_waypoint(adjusted_follower_wp, next_wp, speed=3)
        else:
            mode_mapping = the_connection.mode_mapping()
            current_mode = the_connection.flightmode
            mode = mode_mapping[current_mode]
            time_start = time.time()
            time.sleep(0.1)
            continue
        break

for drone in [master_drone, follower_drone]:
    drone.set_mode(6)
    drone.arm(False)

the_connection.close()