from typing import List, Dict, Tuple
from dataclasses import dataclass

@dataclass
class DeliveryPoint:
    id: str
    position: np.ndarray
    priority: int
    package_weight: float
    time_window: Tuple[float, float]

class DeliveryRouter:
    def __init__(self, depot: np.ndarray, drone_specs: Dict):
        self.depot = depot
        self.max_range = drone_specs['range_km'] * 1000  # Convert to meters
        self.max_payload = drone_specs['max_payload_kg']
        self.speed = drone_specs['speed_ms']
        self.battery_rate = drone_specs['battery_rate']  # % per meter

    def optimize_route(self, deliveries: List[DeliveryPoint]) -> List[List[DeliveryPoint]]:
        """Optimize delivery routes using nearest neighbor + 2-opt"""
        # Cluster deliveries by proximity
        clusters = self._cluster_deliveries(deliveries)

        routes = []
        for cluster in clusters:
            route = self._optimize_cluster(cluster)
            routes.append(route)

        return routes

    def _cluster_deliveries(self, deliveries: List[DeliveryPoint]) -> List[List[DeliveryPoint]]:
        """Cluster deliveries based on drone capacity"""
        clusters = []
        current_cluster = []
        current_distance = 0
        current_weight = 0

        sorted_deliveries = sorted(deliveries,
                                  key=lambda d: np.linalg.norm(d.position - self.depot))

        for delivery in sorted_deliveries:
            dist_to_add = np.linalg.norm(delivery.position - self.depot)
            new_distance = current_distance + dist_to_add * 2  # Round trip
            new_weight = current_weight + delivery.package_weight

            if (new_distance <= self.max_range * 0.9 and
                new_weight <= self.max_payload * 0.9):
                current_cluster.append(delivery)
                current_distance = new_distance
                current_weight = new_weight
            else:
                if current_cluster:
                    clusters.append(current_cluster)
                current_cluster = [delivery]
                current_distance = dist_to_add * 2
                current_weight = delivery.package_weight

        if current_cluster:
            clusters.append(current_cluster)

        return clusters

    def _optimize_cluster(self, cluster: List[DeliveryPoint]) -> List[DeliveryPoint]:
        """Optimize single route using 2-opt improvement"""
        if len(cluster) <= 2:
            return cluster

        # Initial route using nearest neighbor
        route = self._nearest_neighbor_route(cluster)

        # 2-opt improvement
        improved = True
        while improved:
            improved = False
            for i in range(1, len(route) - 1):
                for j in range(i + 1, len(route)):
                    new_route = route[:i] + route[i:j+1][::-1] + route[j+1:]
                    if self._route_distance(new_route) < self._route_distance(route):
                        route = new_route
                        improved = True

        return route

    def _nearest_neighbor_route(self, cluster: List[DeliveryPoint]) -> List[DeliveryPoint]:
        """Generate initial route using nearest neighbor"""
        unvisited = cluster.copy()
        route = [unvisited.pop(0)]

        while unvisited:
            current = route[-1]
            nearest = min(unvisited,
                        key=lambda d: np.linalg.norm(d.position - current.position))
            route.append(nearest)
            unvisited.remove(nearest)

        return route

    def _route_distance(self, route: List[DeliveryPoint]) -> float:
        """Calculate total route distance"""
        if not route:
            return 0

        distance = np.linalg.norm(route[0].position - self.depot)
        for i in range(len(route) - 1):
            distance += np.linalg.norm(route[i+1].position - route[i].position)
        distance += np.linalg.norm(route[-1].position - self.depot)

        return distance

    def estimate_flight_time(self, route: List[DeliveryPoint]) -> Dict:
        """Estimate flight time and battery usage"""
        distance = self._route_distance(route)
        flight_time = distance / self.speed / 60  # minutes
        battery_usage = distance * self.battery_rate

        return {
            'distance_m': distance,
            'flight_time_min': flight_time,
            'battery_usage_pct': battery_usage,
            'remaining_battery': 100 - battery_usage
        }

    def check_time_windows(self, route: List[DeliveryPoint]) -> List[Dict]:
        """Check if route meets time windows"""
        violations = []
        current_time = 0

        for i, delivery in enumerate(route):
            travel_time = 0
            if i == 0:
                travel_time = np.linalg.norm(delivery.position - self.depot) / self.speed / 60
            else:
                travel_time = np.linalg.norm(delivery.position - route[i-1].position) / self.speed / 60

            current_time += travel_time

            if current_time < delivery.time_window[0]:
                current_time = delivery.time_window[0]  # Wait
            elif current_time > delivery.time_window[1]:
                violations.append({
                    'delivery_id': delivery.id,
                    'arrival': current_time,
                    'window': delivery.time_window,
                    'delay': current_time - delivery.time_window[1]
                })

        return violations

from typing import List, Dict, Optional
from dataclasses import dataclass
from enum import Enum

class PackageType(Enum):
    STANDARD = "standard"
    FRAGILE = "fragile"
    TEMPERATURE_CONTROLLED = "temp_controlled"
    PERISHABLE = "perishable"

@dataclass
class Package:
    id: str
    weight: float
    dimensions: Tuple[float, float, float]
    package_type: PackageType
    priority: int
    temperature_range: Optional[Tuple[float, float]] = None

class PayloadManager:
    def __init__(self, max_weight: float, max_volume: float,
                 cargo_dimensions: Tuple[float, float, float]):
        self.max_weight = max_weight
        self.max_volume = max_volume
        self.cargo_dims = cargo_dimensions
        self.loaded_packages: List[Package] = []

    def can_load(self, package: Package) -> bool:
        """Check if package can be loaded"""
        current_weight = sum(p.weight for p in self.loaded_packages)
        current_volume = sum(
            p.dimensions[0] * p.dimensions[1] * p.dimensions[2]
            for p in self.loaded_packages
        )

        new_weight = current_weight + package.weight
        new_volume = current_volume + product(package.dimensions)

        return (new_weight <= self.max_weight and
                new_volume <= self.max_volume)

    def load_package(self, package: Package) -> bool:
        """Load a package"""
        if self.can_load(package):
            self.loaded_packages.append(package)
            self._optimize_loading_order()
            return True
        return False

    def unload_package(self, package_id: str) -> Optional[Package]:
        """Unload a package by ID"""
        for i, pkg in enumerate(self.loaded_packages):
            if pkg.id == package_id:
                return self.loaded_packages.pop(i)
        return None

    def _optimize_loading_order(self):
        """Optimize package loading order for efficient delivery"""
        # Sort by delivery priority and position
        self.loaded_packages.sort(key=lambda p: (-p.priority, p.id))

    def get_center_of_mass(self) -> np.ndarray:
        """Calculate center of mass for flight stability"""
        if not self.loaded_packages:
            return np.zeros(3)

        total_weight = sum(p.weight for p in self.loaded_packages)
        weighted_position = np.zeros(3)

        for i, pkg in enumerate(self.loaded_packages):
            # Approximate position in cargo bay
            position = np.array([
                (i % 2 - 0.5) * 0.3,  # x
                (i // 2 - 0.5) * 0.3,  # y
                0.1  # z (height in cargo)
            ])
            weighted_position += position * pkg.weight

        return weighted_position / total_weight

    def get_load_summary(self) -> Dict:
        """Get summary of current load"""
        total_weight = sum(p.weight for p in self.loaded_packages)
        total_volume = sum(
            p.dimensions[0] * p.dimensions[1] * p.dimensions[2]
            for p in self.loaded_packages
        )

        type_counts = {}
        for pkg in self.loaded_packages:
            ptype = pkg.package_type.value
            type_counts[ptype] = type_counts.get(ptype, 0) + 1

        return {
            'package_count': len(self.loaded_packages),
            'total_weight_kg': total_weight,
            'total_volume_m3': total_volume,
            'weight_utilization': total_weight / self.max_weight * 100,
            'volume_utilization': total_volume / self.max_volume * 100,
            'packages_by_type': type_counts,
            'center_of_mass': self.get_center_of_mass().tolist()
        }

    def validate_payload(self) -> List[str]:
        """Validate payload configuration"""
        warnings = []

        total_weight = sum(p.weight for p in self.loaded_packages)
        if total_weight > self.max_weight * 0.9:
            warnings.append("Weight at 90%+ capacity")

        com = self.get_center_of_mass()
        if np.linalg.norm(com[:2]) > 0.2:
            warnings.append("Center of mass significantly off-center")

        temp_packages = [p for p in self.loaded_packages
                        if p.package_type == PackageType.TEMPERATURE_CONTROLLED]
        if len(temp_packages) > 1:
            conflicting = self._check_temperature_conflicts(temp_packages)
            if conflicting:
                warnings.append(f"Temperature conflict: {conflicting}")

        return warnings

    def _check_temperature_conflicts(self, packages: List[Package]) -> str:
        """Check for conflicting temperature requirements"""
        for i, p1 in enumerate(packages):
            for p2 in packages[i+1:]:
                if p1.temperature_range and p2.temperature_range:
                    if (p1.temperature_range[1] < p2.temperature_range[0] or
                        p2.temperature_range[1] < p1.temperature_range[0]):
                        return f"{p1.id} and {p2.id}"
        return ""


def product(tup):
    result = 1
    for x in tup:
        result *= x
    return result

from typing import Tuple, Optional
from dataclasses import dataclass

@dataclass
class LandingPad:
    position: np.ndarray
    size: Tuple[float, float]  # width, length in meters
    qr_code_id: str
    lights_enabled: bool = True

class PrecisionLander:
    def __init__(self, gps_accuracy: float = 0.02,
                 vision_accuracy: float = 0.05):
        self.gps_accuracy = gps_accuracy  # RTK-GPS
        self.vision_accuracy = vision_accuracy
        self.landing_phase = "approach"
        self.max_wind_speed = 8.0  # m/s

    def compute_landing_trajectory(self, current_pos: np.ndarray,
                                   current_vel: np.ndarray,
                                   pad: LandingPad,
                                   wind_vector: np.ndarray) -> List[np.ndarray]:
        """Compute precise landing trajectory"""
        waypoints = []

        # Phase 1: High approach
        high_approach = pad.position + np.array([0, 0, 15])
        waypoints.append(high_approach)

        # Phase 2: Low approach with wind compensation
        approach_offset = self._compensate_wind(wind_vector)
        low_approach = pad.position + approach_offset + np.array([0, 0, 5])
        waypoints.append(low_approach)

        # Phase 3: Hover and align
        hover_point = pad.position + np.array([0, 0, 3])
        waypoints.append(hover_point)

        # Phase 4: Final descent
        waypoints.append(pad.position + np.array([0, 0, 0.5]))

        # Phase 5: Touchdown
        waypoints.append(pad.position)

        return waypoints

    def _compensate_wind(self, wind: np.ndarray) -> np.ndarray:
        """Compute wind compensation offset"""
        # Into-wind offset for stable approach
        wind_speed = np.linalg.norm(wind)
        if wind_speed > 0:
            into_wind = -wind / wind_speed
            offset = into_wind * min(wind_speed * 0.5, 2.0)
            offset[2] = 0  # No vertical offset
            return offset
        return np.zeros(3)

    def vision_guided_landing(self, camera_frame: np.ndarray,
                              target_pad: LandingPad) -> Tuple[float, float, float]:
        """Compute landing correction from camera frame"""
        # Detect QR code / landing marker
        detected, center, size = self._detect_landing_marker(camera_frame)

        if detected:
            # Compute offset from center
            frame_center = np.array([camera_frame.shape[1] / 2,
                                    camera_frame.shape[0] / 2])
            offset_pixels = center - frame_center

            # Convert to meters (approximate)
            scale = target_pad.size[0] / size if size > 0 else 1.0
            offset_meters = offset_pixels * scale * 0.001

            # Return correction: x, y offset and size for altitude estimation
            return (offset_meters[0], offset_meters[1], size)

        return (0, 0, 0)

    def _detect_landing_marker(self, frame: np.ndarray) -> Tuple[bool, np.ndarray, float]:
        """Detect landing marker in camera frame"""
        # Simplified marker detection
        # In production, use ArUco or custom marker detection
        gray = np.mean(frame, axis=2)

        # Simple threshold for dark marker
        threshold = np.percentile(gray, 10)
        marker_mask = gray < threshold

        if np.sum(marker_mask) > 100:
            from scipy import ndimage
            center = ndimage.center_of_mass(marker_mask)
            size = np.sqrt(np.sum(marker_mask))
            return True, np.array(center), size

        return False, np.zeros(2), 0

    def check_landing_conditions(self, wind_speed: float,
                                 visibility: float,
                                 precipitation: bool) -> Dict:
        """Check if landing conditions are safe"""
        conditions = {
            'wind_ok': wind_speed < self.max_wind_speed,
            'visibility_ok': visibility > 1000,  # meters
            'precip_ok': not precipitation,
            'can_land': True,
            'warnings': []
        }

        if wind_speed > self.max_wind_speed * 0.8:
            conditions['warnings'].append(f"Wind approaching limit: {wind_speed:.1f} m/s")

        if visibility < 2000:
            conditions['warnings'].append(f"Reduced visibility: {visibility:.0f}m")

        conditions['can_land'] = all([
            conditions['wind_ok'],
            conditions['visibility_ok'],
            conditions['precip_ok']
        ])

        return conditions

from typing import List, Dict, Tuple
from dataclasses import dataclass
from enum import Enum

class DeliveryStatus(Enum):
    PENDING = "pending"
    IN_TRANSIT = "in_transit"
    APPROACHING = "approaching"
    DELIVERED = "delivered"
    FAILED = "failed"

@dataclass
class DeliveryJob:
    job_id: str
    package_id: str
    recipient_address: str
    delivery_point: np.ndarray
    time_window: Tuple[float, float]
    status: DeliveryStatus
    special_instructions: str = ""

class LastMileOptimizer:
    def __init__(self, delivery_zone: Tuple[np.ndarray, np.ndarray]):
        self.zone_min, self.zone_max = delivery_zone
        self.flight_altitude = 30.0  # meters
        self.min_deliverable_altitude = 3.0  # meters

    def plan_delivery_sequence(self, jobs: List[DeliveryJob]) -> List[DeliveryJob]:
        """Plan optimal delivery sequence"""
        # Sort by priority and time windows
        prioritized = sorted(jobs, key=lambda j: (
            -self._priority_score(j),
            j.time_window[0]
        ))

        # Group by proximity
        sequenced = self._sequence_by_proximity(prioritized)

        return sequenced

    def _priority_score(self, job: DeliveryJob) -> float:
        """Calculate delivery priority score"""
        score = 1.0

        # Time window urgency
        current_time = 0  # Would be actual time
        time_until_deadline = job.time_window[1] - current_time
        if time_until_deadline < 30:  # minutes
            score *= 2.0

        return score

    def _sequence_by_proximity(self, jobs: List[DeliveryJob]) -> List[DeliveryJob]:
        """Sequence jobs by geographic proximity"""
        if not jobs:
            return []

        sequenced = [jobs[0]]
        remaining = jobs[1:]

        while remaining:
            current = sequenced[-1]
            nearest = min(remaining,
                        key=lambda j: np.linalg.norm(
                            j.delivery_point - current.delivery_point))
            sequenced.append(nearest)
            remaining.remove(nearest)

        return sequenced

    def compute_delivery_approach(self, delivery_point: np.ndarray,
                                  obstacles: List[Dict]) -> Dict:
        """Compute safe approach path to delivery point"""
        approach_path = []

        # Find safe approach direction (avoid obstacles)
        approach_angle = self._find_safe_approach(delivery_point, obstacles)

        # Generate waypoints
        approach_start = delivery_point + np.array([
            np.cos(approach_angle) * 20,
            np.sin(approach_angle) * 20,
            self.flight_altitude
        ])

        approach_path.append(approach_start)
        approach_path.append(delivery_point + np.array([0, 0, self.flight_altitude]))
        approach_path.append(delivery_point + np.array([0, 0, self.min_deliverable_altitude]))
        approach_path.append(delivery_point)

        return {
            'waypoints': approach_path,
            'approach_angle': approach_angle,
            'descent_rate': 0.5  # m/s
        }

    def _find_safe_approach(self, target: np.ndarray,
                           obstacles: List[Dict]) -> float:
        """Find safest approach angle"""
        best_angle = 0
        best_clearance = 0

        for angle in np.arange(0, 2 * np.pi, np.pi / 4):
            clearance = self._check_clearance(target, angle, obstacles)
            if clearance > best_clearance:
                best_clearance = clearance
                best_angle = angle

        return best_angle

    def _check_clearance(self, target: np.ndarray, angle: float,
                        obstacles: List[Dict]) -> float:
        """Check clearance for approach angle"""
        approach_dir = np.array([np.cos(angle), np.sin(angle), 0])
        min_clearance = float('inf')

        for obs in obstacles:
            obs_pos = np.array(obs['position'])
            obs_radius = obs.get('radius', 5.0)

            # Vector from target to obstacle
            to_obs = obs_pos - target
            to_obs[2] = 0  # Project to horizontal

            # Distance along approach direction
            along = np.dot(to_obs, approach_dir)
            if along > 0:  # Obstacle is in approach direction
                perpendicular = np.linalg.norm(to_obs - along * approach_dir)
                clearance = perpendicular - obs_radius
                min_clearance = min(min_clearance, clearance)

        return min_clearance if min_clearance != float('inf') else 100.0

    def generate_delivery_confirmation(self, job: DeliveryJob,
                                      photo: np.ndarray) -> Dict:
        """Generate delivery confirmation"""
        return {
            'job_id': job.job_id,
            'package_id': job.package_id,
            'delivery_time': self._get_timestamp(),
            'photo_evidence': photo is not None,
            'gps_coordinates': job.delivery_point.tolist(),
            'recipient': job.recipient_address,
            'status': 'delivered'
        }

    def _get_timestamp(self) -> str:
        from datetime import datetime
        return datetime.now().isoformat()

# delivery_system.py
from typing import List, Dict
from dataclasses import dataclass

@dataclass
class DeliveryRequest:
    request_id: str
    pickup_point: np.ndarray
    delivery_point: np.ndarray
    package_weight: float
    priority: int
    time_window: Tuple[float, float]

class DroneDeliverySystem:
    def __init__(self, depot_location: np.ndarray, fleet_size: int = 3):
        self.depot = depot_location
        self.fleet_size = fleet_size
        self.router = DeliveryRouter(depot_location, {
            'range_km': 15,
            'max_payload_kg': 2.5,
            'speed_ms': 15,
            'battery_rate': 0.005
        })
        self.pending_deliveries: List[DeliveryRequest] = []
        self.active_deliveries: Dict[str, DeliveryJob] = {}

    def add_delivery_request(self, request: DeliveryRequest):
        """Add new delivery request"""
        self.pending_deliveries.append(request)
        self._optimize_pending()

    def _optimize_pending(self):
        """Optimize pending deliveries into efficient routes"""
        if not self.pending_deliveries:
            return

        # Convert to delivery points
        delivery_points = []
        for req in self.pending_deliveries:
            dp = DeliveryPoint(
                id=req.request_id,
                position=req.delivery_point,
                priority=req.priority,
                package_weight=req.package_weight,
                time_window=req.time_window
            )
            delivery_points.append(dp)

        # Get optimized routes
        routes = self.router.optimize_route(delivery_points)

        print(f"Optimized {len(self.pending_deliveries)} deliveries "
              f"into {len(routes)} routes")

        # Assign routes to drones (simplified)
        for i, route in enumerate(routes):
            drone_id = f"DRONE-{i+1:03d}"
            self._assign_route(drone_id, route)

    def _assign_route(self, drone_id: str, route: List[DeliveryPoint]):
        """Assign delivery route to drone"""
        for point in route:
            job = DeliveryJob(
                job_id=f"JOB-{point.id}",
                package_id=point.id,
                recipient_address=f"Location {point.id}",
                delivery_point=point.position,
                time_window=point.time_window,
                status=DeliveryStatus.PENDING
            )
            self.active_deliveries[job.job_id] = job

        print(f"Assigned {len(route)} deliveries to {drone_id}")

    def monitor_delivery(self, job_id: str) -> Dict:
        """Monitor delivery progress"""
        if job_id not in self.active_deliveries:
            return {'error': 'Job not found'}

        job = self.active_deliveries[job_id]

        return {
            'job_id': job.job_id,
            'status': job.status.value,
            'delivery_point': job.delivery_point.tolist(),
            'time_window': job.time_window,
            'estimated_arrival': self._estimate_arrival(job)
        }

    def _estimate_arrival(self, job: DeliveryJob) -> float:
        """Estimate arrival time in minutes"""
        # Simplified estimation
        distance = np.linalg.norm(job.delivery_point - self.depot)
        speed = 15  # m/s
        return distance / speed / 60

    def get_system_status(self) -> Dict:
        """Get overall system status"""
        active = len(self.active_deliveries)
        pending = len(self.pending_deliveries)

        return {
            'fleet_size': self.fleet_size,
            'active_deliveries': active,
            'pending_deliveries': pending,
            'total_deliveries_today': active + pending,
            'average_delivery_time': self._calc_avg_time(),
            'on_time_rate': self._calc_on_time_rate()
        }

    def _calc_avg_time(self) -> float:
        """Calculate average delivery time"""
        # Simplified
        return 18.5  # minutes

    def _calc_on_time_rate(self) -> float:
        """Calculate on-time delivery rate"""
        return 0.97  # 97%


# Usage
depot = np.array([0, 0, 0])
system = DroneDeliverySystem(depot, fleet_size=5)

# Add delivery requests
requests = [
    DeliveryRequest(
        request_id="REQ001",
        pickup_point=depot,
        delivery_point=np.array([1000, 500, 0]),
        package_weight=1.2,
        priority=1,
        time_window=(0, 60)
    ),
    DeliveryRequest(
        request_id="REQ002",
        pickup_point=depot,
        delivery_point=np.array([800, -300, 0]),
        package_weight=0.8,
        priority=2,
        time_window=(0, 90)
    ),
]

for req in requests:
    system.add_delivery_request(req)

print(system.get_system_status())