import asyncio import json import math import random import websockets FREQ_COLORS = { 170.0: "red", 170.1: "blue", 170.2: "green", 170.3: "orange", 170.4: "purple", 170.5: "brown", 170.6: "cyan", 170.7: "magenta", 170.8: "lime", 170.9: "teal", 171.0: "pink", 171.1: "navy", 171.2: "gold", 171.3: "salmon", 171.4: "olive", 171.5: "maroon", 171.6: "coral", 171.7: "indigo", 171.8: "violet", 171.9: "gray" } clients = set() commands = asyncio.Queue() center_lat, center_lon = -41.5917, 145.9344 current_lat, current_lon = center_lat, center_lon active_orbit_target = None orbit_path = [] orbit_index = 0 orbit_active = False orbit_queue = [] completed_orbits = set() active_freq = None # Targets targets = [ {"lat": -41.1568, "lon": 146.4257, "freq": 170.0}, {"lat": -41.3400, "lon": 146.2625, "freq": 170.1}, {"lat": -41.8190, "lon": 146.4390, "freq": 170.2}, {"lat": -42.0139, "lon": 145.6077, "freq": 170.3}, {"lat": -41.6302, "lon": 146.3656, "freq": 170.4}, {"lat": -41.6880, "lon": 146.1030, "freq": 170.5}, {"lat": -41.3899, "lon": 146.3310, "freq": 170.6}, {"lat": -41.4410, "lon": 145.5380, "freq": 170.7}, {"lat": -42.0270, "lon": 146.3340, "freq": 170.8}, {"lat": -41.4960, "lon": 146.4300, "freq": 170.9}, {"lat": -41.7510, "lon": 146.2030, "freq": 171.0}, {"lat": -41.8950, "lon": 146.4310, "freq": 171.1}, {"lat": -41.2390, "lon": 145.7600, "freq": 171.2}, {"lat": -41.3730, "lon": 145.5870, "freq": 171.3}, {"lat": -41.2900, "lon": 145.8420, "freq": 171.4}, {"lat": -41.8770, "lon": 145.7510, "freq": 171.5}, {"lat": -42.0130, "lon": 145.9500, "freq": 171.6}, {"lat": -41.5917, "lon": 145.9344, "freq": 171.7}, {"lat": -41.8000, "lon": 145.5000, "freq": 171.8}, {"lat": -41.5000, "lon": 146.0000, "freq": 171.9} ] def haversine(lat1, lon1, lat2, lon2): R = 6371 dlat = math.radians(lat2 - lat1) dlon = math.radians(lon2 - lon1) a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2 return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) def calculate_bearing(lat1, lon1, lat2, lon2): phi1 = math.radians(lat1) phi2 = math.radians(lat2) delta_lon = math.radians(lon2 - lon1) x = math.sin(delta_lon) * math.cos(phi2) y = math.cos(phi1)*math.sin(phi2) - math.sin(phi1)*math.cos(phi2)*math.cos(delta_lon) return (math.degrees(math.atan2(x, y)) + 360) % 360 def noisy_bearing(bearing, distance_km, min_noise=0.5, max_noise=10.0, max_effective_km=10.0): distance_km = min(distance_km, max_effective_km) noise = max_noise - (distance_km / max_effective_km) * (max_noise - min_noise) return (bearing + random.uniform(-noise, noise)) % 360 def generate_converging_spiral(center_lat, center_lon, num_turns=10, points_per_turn=100, max_radius_km=50): path = [] total_points = num_turns * points_per_turn for i in range(total_points): angle = 2 * math.pi * (i / points_per_turn) radius_km = max_radius_km * (1 - i / total_points) lat_offset = (radius_km * math.sin(angle)) / 111 lon_offset = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat))) path.append((center_lat + lat_offset, center_lon + lon_offset)) return path def generate_orbit(center_lat, center_lon, radius_km=0.1, num_points=100): path = [] for i in range(num_points): angle = 2 * math.pi * i / num_points dlat = (radius_km * math.sin(angle)) / 111 dlon = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat))) path.append((center_lat + dlat, center_lon + dlon)) return path search_path = generate_converging_spiral(center_lat, center_lon, points_per_turn=1000) async def simulate(): global orbit_active, orbit_path, orbit_index, current_lat, current_lon, active_orbit_target, active_freq, completed_orbits point_index = 0 while True: try: while not commands.empty(): cmd = await commands.get() if cmd.get("type") == "command" and cmd.get("action") == "orbit_target": cmd_freq = cmd.get("freq") if cmd_freq in completed_orbits: print(f"๐Ÿšซ Skipping orbit for {cmd_freq} โ€” already completed.") continue if active_freq is None: orbit_queue.append(cmd) active_freq = cmd_freq elif not orbit_active and cmd_freq == active_freq: print(f"๐Ÿ” Updating active orbit target: {cmd}") active_orbit_target["lat"] = cmd["lat"] active_orbit_target["lon"] = cmd["lon"] active_orbit_target["radius"] = cmd.get("radius", 0.1) else: print(f"โณ Queuing orbit for later: freq={cmd_freq}") orbit_queue.append(cmd) except asyncio.QueueEmpty: pass step_km = haversine(*search_path[point_index % len(search_path)], *search_path[(point_index + 1) % len(search_path)]) if not orbit_active and active_orbit_target is None: # Look for next valid orbit target while orbit_queue: next_cmd = orbit_queue.pop(0) freq = next_cmd.get("freq") if freq in completed_orbits: print(f"๐Ÿšซ Skipping orbit for {freq} โ€” already completed.") continue print(f"๐Ÿ›ฐ๏ธ Starting new orbit for {freq}") active_orbit_target = next_cmd active_freq = freq orbit_index = 0 orbit_path = [] break # found a valid one if active_orbit_target: orbit_center_lat = active_orbit_target["lat"] orbit_center_lon = active_orbit_target["lon"] radius = active_orbit_target.get("radius", 0.1) dist = haversine(current_lat, current_lon, orbit_center_lat, orbit_center_lon) if dist < 0.5: if not orbit_active: print(f"๐Ÿ›ฐ๏ธ Starting orbit around ({orbit_center_lat:.4f}, {orbit_center_lon:.4f})") orbit_center_lat = active_orbit_target["lat"] orbit_center_lon = active_orbit_target["lon"] radius = active_orbit_target.get("radius", 0.1) orbit_path = generate_orbit(orbit_center_lat, orbit_center_lon, radius_km=radius) orbit_index = 0 orbit_active = True last_sent_orbit = {"lat": orbit_center_lat, "lon": orbit_center_lon} else: bearing_to_target = calculate_bearing(current_lat, current_lon, orbit_center_lat, orbit_center_lon) dlat = (step_km * math.cos(math.radians(bearing_to_target))) / 111 dlon = (step_km * math.sin(math.radians(bearing_to_target))) / (111 * math.cos(math.radians(current_lat))) current_lat += dlat current_lon += dlon if orbit_active and orbit_path: target_lat, target_lon = orbit_path[orbit_index % len(orbit_path)] orbit_index += 1 current_lat = target_lat current_lon = target_lon if orbit_index >= len(orbit_path): orbit_active = False print(f"โœ… Finished orbit for {active_freq}") completed_orbits.add(active_freq) active_freq = None active_orbit_target = None elif not active_orbit_target: current_lat, current_lon = search_path[point_index % len(search_path)] point_index += 1 await broadcast(json.dumps({"type": "drone", "lat": current_lat, "lon": current_lon, "alt": 100})) for target in targets: await asyncio.sleep(0.01) distance_km = haversine(current_lat, current_lon, target["lat"], target["lon"]) if distance_km > 10: continue signal_strength = max(0.0, 1.0 - (distance_km / 10)) bearing = calculate_bearing(current_lat, current_lon, target["lat"], target["lon"]) noisy = noisy_bearing(bearing, distance_km) mirror = (noisy + 180) % 360 await broadcast(json.dumps({ "type": "bearing", "freq": target["freq"], "lat": current_lat, "lon": current_lon, "bearing": noisy, "mirror": mirror, "alt": 100, "weight": signal_strength, "color": FREQ_COLORS[target["freq"]] })) async def websocket_handler(websocket): clients.add(websocket) print("๐Ÿงช Client connected") try: async for message in websocket: print("๐Ÿ“ฅ Received:", message) try: data = json.loads(message) if data.get("type") == "command" and data.get("action") == "orbit_target": print("๐Ÿ›ฐ๏ธ Queuing orbit command:", data) await commands.put(data) except Exception as e: print("โŒ Invalid message:", e) finally: clients.remove(websocket) async def broadcast(message): if clients: await asyncio.gather(*(client.send(message) for client in clients)) async def main(): print("๐Ÿ“ก Simulation server running at ws://localhost:8765") server = websockets.serve(websocket_handler, "localhost", 8765) await asyncio.gather(server, simulate()) if __name__ == "__main__": asyncio.run(main())