class Mission:
    def __init__(self):
        self.waypoints = []

    def add_waypoint(self, lat, lon, alt, action="fly_through", speed=5):
        self.waypoints.append({
            "lat": lat, "lon": lon, "alt": alt,
            "action": action, "speed": speed
        })
        return self

    def total_distance(self):
        total = 0
        for i in range(1, len(self.waypoints)):
            w1, w2 = self.waypoints[i-1], self.waypoints[i]
            R = 6371000
            dlat = math.radians(w2["lat"] - w1["lat"])
            dlon = math.radians(w2["lon"] - w1["lon"])
            a = (math.sin(dlat/2)**2 +
                 math.cos(math.radians(w1["lat"])) *
                 math.cos(math.radians(w2["lat"])) *
                 math.sin(dlon/2)**2)
            horizontal = R * 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
            vertical = abs(w2["alt"] - w1["alt"])
            total += math.sqrt(horizontal**2 + vertical**2)
        return total

    def estimated_time(self):
        dist = self.total_distance()
        avg_speed = sum(wp["speed"] for wp in self.waypoints) / len(self.waypoints)
        return dist / avg_speed

    def summary(self):
        print(f"Mission: {len(self.waypoints)} waypoints")
        print(f"Distance: {self.total_distance():.0f}m")
        print(f"Time: {self.estimated_time():.0f}s")

mission = Mission()
mission.add_waypoint(37.774900, -122.419400, 10, "takeoff")
mission.add_waypoint(37.774900, -122.419450, 30, "photo")
mission.add_waypoint(37.774950, -122.419450, 30, "photo")
mission.add_waypoint(37.774950, -122.419400, 30, "photo")
mission.add_waypoint(37.774900, -122.419400, 10, "land")
mission.summary()

class GeoFence:
    def __init__(self, name, max_alt=120, max_dist=5000):
        self.name = name
        self.exclusion_zones = []
        self.max_altitude = max_alt
        self.max_distance = max_dist

    def add_exclusion(self, vertices):
        self.exclusion_zones.append(vertices)

    def point_in_polygon(self, point, polygon):
        x, y = point
        n = len(polygon)
        inside = False
        j = n - 1
        for i in range(n):
            xi, yi = polygon[i]
            xj, yj = polygon[j]
            if ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi):
                inside = not inside
            j = i
        return inside

    def check_position(self, lat, lon, alt):
        violations = []
        if alt > self.max_altitude:
            violations.append(f"Altitude {alt:.0f}m exceeds max {self.max_altitude}m")
        for zone in self.exclusion_zones:
            if self.point_in_polygon((lat, lon), zone):
                violations.append("Inside EXCLUSION zone!")
        return {"safe": len(violations) == 0, "violations": violations}

fence = GeoFence("Campus")
fence.add_exclusion([(37.7745, -122.4195), (37.7745, -122.4190),
                      (37.7743, -122.4190), (37.7743, -122.4195)])

result = fence.check_position(37.7744, -122.4192, 30)
print(f"Safe: {result['safe']}")
print(f"Violations: {result['violations']}")

class ReturnToHome:
    def __init__(self, home_lat, home_lon):
        self.home = (home_lat, home_lon)
        self.rth_altitude = 30

    def initiate(self, current_lat, current_lon, current_alt, battery_pct):
        reason = "Low battery" if battery_pct < 30 else "Manual RTH"
        print(f"RTH: {reason}")
        if current_alt < self.rth_altitude:
            print(f"  Phase 1: Climbing to {self.rth_altitude}m")
        print(f"  Phase 2: Flying to home")
        print(f"  Phase 3: Descending and landing")
        print(f"  RTH Complete!")

rth = ReturnToHome(37.774900, -122.419400)
rth.initiate(37.775500, -122.420000, 15, 25)

class MissionPlanner:
    def __init__(self, home_lat, home_lon):
        self.home = (home_lat, home_lon)
        self.waypoints = []

    def generate_grid(self, start_lat, start_lon, rows, cols, spacing_m, alt):
        dlat = spacing_m / 111320
        dlon = spacing_m / (111320 * math.cos(math.radians(start_lat)))
        for r in range(rows):
            for c in range(cols):
                self.waypoints.append({
                    "lat": start_lat + r * dlat,
                    "lon": start_lon + c * dlon,
                    "alt": alt, "photo": True
                })
        print(f"Generated {len(self.waypoints)} waypoints")

    def validate(self, battery_wh, power_w):
        time_min = battery_wh * 0.8 / power_w * 60
        print(f"Flight time: {time_min:.1f} min | Waypoints: {len(self.waypoints)}")
        return time_min > len(self.waypoints) * 0.5

planner = MissionPlanner(37.7749, -122.4194)
planner.add_waypoint(37.7749, -122.4194, 10)
planner.generate_grid(37.7748, -122.4195, 5, 10, 20, 30)
planner.validate(battery_wh=40, power_w=200)