"""
Equipment positioning optimization for cave surveys.
Determines optimal locations for sensor deployment and emergency caches.

Author: Samir Baladi
"""

import numpy as np
from typing import Dict, List, Tuple, Optional, Union
from dataclasses import dataclass
from scipy.spatial import KDTree
from scipy.optimize import minimize
import logging

logger = logging.getLogger(__name__)

@dataclass
class EquipmentLocation:
    """Represents an equipment location."""
    x: float
    y: float
    z: float
    equipment_type: str  # 'sensor', 'cache', 'base'
    coverage_radius: float  # m
    priority: float  # 0-1 priority score


class EquipmentPositioning:
    """
    Optimizes equipment placement in cave systems.
    """
    
    EQUIPMENT_TYPES = {
        'sensor': {
            'coverage_radius': 50,
            'weight': 1.0,
            'cost': 100
        },
        'emergency_cache': {
            'coverage_radius': 200,
            'weight': 2.0,
            'cost': 500
        },
        'base_station': {
            'coverage_radius': 500,
            'weight': 3.0,
            'cost': 1000
        },
        'communication_relay': {
            'coverage_radius': 300,
            'weight': 1.5,
            'cost': 300
        }
    }
    
    def __init__(self, budget: float = 5000):
        """
        Initialize equipment positioning.
        
        Args:
            budget: Total equipment budget
        """
        self.budget = budget
        self.locations = []
        self.passage_points = []
        self.risk_zones = []
        logger.info(f"Initialized EquipmentPositioning with budget={budget}")

    def add_passage_point(self, x: float, y: float, z: float, importance: float = 1.0) -> None:
        """
        Add a passage point that needs coverage.
        
        Args:
            x, y, z: Coordinates
            importance: Importance of covering this point (0-1)
        """
        self.passage_points.append({
            'position': (x, y, z),
            'importance': importance
        })

    def add_risk_zone(self,
                     x: float, y: float, z: float,
                     radius: float,
                     risk_level: float) -> None:
        """
        Add a high-risk zone requiring monitoring.
        
        Args:
            x, y, z: Zone center
            radius: Zone radius
            risk_level: Risk level (0-1)
        """
        self.risk_zones.append({
            'position': (x, y, z),
            'radius': radius,
            'risk_level': risk_level
        })

    def optimize_sensor_placement(self,
                                  n_sensors: int = 5,
                                  coverage_requirement: float = 0.8) -> List[Dict]:
        """
        Optimize placement of sensors for maximum coverage.
        
        Args:
            n_sensors: Number of sensors to place
            coverage_requirement: Minimum fraction of passage to cover
            
        Returns:
            List of optimal sensor locations
        """
        if not self.passage_points:
            return []
        
        # Convert passage points to array
        points = np.array([p['position'] for p in self.passage_points])
        importance = np.array([p['importance'] for p in self.passage_points])
        
        # Greedy algorithm for sensor placement
        selected = []
        covered = np.zeros(len(points), dtype=bool)
        
        for _ in range(n_sensors):
            best_coverage = 0
            best_point = None
            
            # Try each passage point as potential sensor location
            for i, point in enumerate(points):
                if any(np.array_equal(point, s['position']) for s in selected):
                    continue
                
                # Calculate coverage from this point
                sensor_coverage = np.linalg.norm(points - point, axis=1) < 50
                new_coverage = sensor_coverage & ~covered
                
                # Weighted coverage score
                coverage_score = np.sum(importance[new_coverage])
                
                if coverage_score > best_coverage:
                    best_coverage = coverage_score
                    best_point = point
            
            if best_point is not None:
                selected.append({
                    'position': tuple(best_point),
                    'equipment_type': 'sensor',
                    'coverage_radius': 50,
                    'coverage_score': round(best_coverage, 2)
                })
                
                # Update covered points
                dist = np.linalg.norm(points - best_point, axis=1)
                covered = covered | (dist < 50)
        
        # Check coverage requirement
        coverage_fraction = np.sum(covered) / len(points)
        if coverage_fraction < coverage_requirement:
            logger.warning(f"Coverage {coverage_fraction:.0%} below requirement {coverage_requirement:.0%}")
        
        self.locations.extend(selected)
        return selected

    def optimize_emergency_caches(self,
                                  n_caches: int = 2,
                                  max_distance: float = 500) -> List[Dict]:
        """
        Optimize placement of emergency caches.
        
        Args:
            n_caches: Number of caches to place
            max_distance: Maximum distance to any point (m)
            
        Returns:
            List of optimal cache locations
        """
        if not self.passage_points:
            return []
        
        points = np.array([p['position'] for p in self.passage_points])
        
        # Use k-means clustering to find cache locations
        from sklearn.cluster import KMeans
        
        n_caches = min(n_caches, len(points))
        kmeans = KMeans(n_clusters=n_caches, random_state=42, n_init=10)
        clusters = kmeans.fit_predict(points)
        
        caches = []
        for i in range(n_caches):
            cluster_points = points[clusters == i]
            if len(cluster_points) > 0:
                # Cache at cluster center
                center = kmeans.cluster_centers_[i]
                
                # Calculate coverage
                dist_to_cache = np.linalg.norm(points - center, axis=1)
                coverage = np.sum(dist_to_cache < max_distance)
                coverage_pct = coverage / len(points) * 100
                
                caches.append({
                    'position': tuple(center),
                    'equipment_type': 'emergency_cache',
                    'coverage_radius': max_distance,
                    'points_covered': coverage,
                    'coverage_percent': round(coverage_pct, 1)
                })
        
        self.locations.extend(caches)
        return caches

    def optimize_communication_relays(self,
                                     start_point: Tuple[float, float, float],
                                     end_point: Tuple[float, float, float],
                                     max_relay_distance: float = 300) -> List[Dict]:
        """
        Optimize placement of communication relays along a route.
        
        Args:
            start_point: Start position
            end_point: End position
            max_relay_distance: Maximum distance between relays
            
        Returns:
            List of relay locations
        """
        # Calculate direct distance
        direct_dist = np.sqrt(
            (end_point[0] - start_point[0])**2 +
            (end_point[1] - start_point[1])**2 +
            (end_point[2] - start_point[2])**2
        )
        
        if direct_dist <= max_relay_distance:
            # No relays needed
            return []
        
        # Number of relays needed
        n_relays = int(np.ceil(direct_dist / max_relay_distance)) - 1
        
        # Linear interpolation for relay positions
        relays = []
        for i in range(1, n_relays + 1):
            t = i / (n_relays + 1)
            relay_pos = (
                start_point[0] + t * (end_point[0] - start_point[0]),
                start_point[1] + t * (end_point[1] - start_point[1]),
                start_point[2] + t * (end_point[2] - start_point[2])
            )
            
            relays.append({
                'position': relay_pos,
                'equipment_type': 'communication_relay',
                'coverage_radius': max_relay_distance,
                'segment': i,
                'distance_from_start': round(direct_dist * t, 1)
            })
        
        self.locations.extend(relays)
        return relays

    def optimize_base_station(self,
                             coverage_radius: float = 500) -> Dict:
        """
        Find optimal base station location.
        
        Args:
            coverage_radius: Base station coverage radius
            
        Returns:
            Optimal base station location
        """
        if not self.passage_points:
            return {}
        
        points = np.array([p['position'] for p in self.passage_points])
        importance = np.array([p['importance'] for p in self.passage_points])
        
        # Define objective function (negative coverage)
        def objective(pos):
            dist = np.linalg.norm(points - pos, axis=1)
            coverage = np.sum(importance[dist < coverage_radius])
            return -coverage  # Minimize negative coverage
        
        # Initial guess at centroid
        initial_pos = np.mean(points, axis=0)
        
        # Optimize
        result = minimize(
            objective,
            initial_pos,
            method='Nelder-Mead'
        )
        
        if result.success:
            opt_pos = result.x
            dist = np.linalg.norm(points - opt_pos, axis=1)
            coverage = np.sum(dist < coverage_radius)
            coverage_pct = coverage / len(points) * 100
            
            base = {
                'position': tuple(opt_pos),
                'equipment_type': 'base_station',
                'coverage_radius': coverage_radius,
                'points_covered': coverage,
                'coverage_percent': round(coverage_pct, 1)
            }
            
            self.locations.append(base)
            return base
        
        return {}

    def calculate_coverage(self, location: Dict) -> float:
        """
        Calculate coverage provided by an equipment location.
        
        Args:
            location: Equipment location dictionary
            
        Returns:
            Coverage score
        """
        if not self.passage_points:
            return 0
        
        points = np.array([p['position'] for p in self.passage_points])
        importance = np.array([p['importance'] for p in self.passage_points])
        
        pos = np.array(location['position'])
        radius = location.get('coverage_radius', 50)
        
        dist = np.linalg.norm(points - pos, axis=1)
        covered = importance[dist < radius]
        
        return float(np.sum(covered))

    def get_coverage_map(self, resolution: float = 10) -> Dict[str, np.ndarray]:
        """
        Generate coverage map for the cave system.
        
        Args:
            resolution: Grid resolution (m)
            
        Returns:
            Dictionary with coverage map
        """
        if not self.passage_points:
            return {}
        
        # Get bounds
        points = np.array([p['position'] for p in self.passage_points])
        x_min, x_max = points[:, 0].min(), points[:, 0].max()
        y_min, y_max = points[:, 1].min(), points[:, 1].max()
        
        # Create grid
        x = np.arange(x_min - 100, x_max + 100, resolution)
        y = np.arange(y_min - 100, y_max + 100, resolution)
        X, Y = np.meshgrid(x, y)
        
        # Calculate coverage at each grid point
        coverage = np.zeros_like(X)
        
        for loc in self.locations:
            pos = np.array(loc['position'])
            radius = loc['coverage_radius']
            
            dist = np.sqrt((X - pos[0])**2 + (Y - pos[1])**2)
            coverage += (dist < radius).astype(float)
        
        return {
            'x': x,
            'y': y,
            'X': X,
            'Y': Y,
            'coverage': coverage
        }

    def calculate_budget_usage(self) -> Dict[str, float]:
        """
        Calculate budget usage for current equipment.
        
        Returns:
            Dictionary with budget statistics
        """
        total_cost = 0
        cost_by_type = {}
        
        for loc in self.locations:
            eq_type = loc['equipment_type']
            cost = self.EQUIPMENT_TYPES[eq_type]['cost']
            total_cost += cost
            cost_by_type[eq_type] = cost_by_type.get(eq_type, 0) + cost
        
        return {
            'total_cost': total_cost,
            'budget': self.budget,
            'remaining': self.budget - total_cost,
            'over_budget': total_cost > self.budget,
            'cost_by_type': cost_by_type
        }

    def summary(self) -> Dict[str, Union[int, float, str, List]]:
        """Get summary of equipment positioning."""
        if not self.locations:
            return {'status': 'No equipment positioned'}
        
        budget = self.calculate_budget_usage()
        
        # Group by type
        by_type = {}
        for loc in self.locations:
            eq_type = loc['equipment_type']
            if eq_type not in by_type:
                by_type[eq_type] = 0
            by_type[eq_type] += 1
        
        return {
            'n_equipment': len(self.locations),
            'by_type': by_type,
            'budget': budget,
            'n_passage_points': len(self.passage_points),
            'n_risk_zones': len(self.risk_zones)
        }


# Example usage
if __name__ == "__main__":
    logging.basicConfig(level=logging.INFO)
    
    print("="*60)
    print("CAVORA - Equipment Positioning")
    print("="*60)
    
    ep = EquipmentPositioning(budget=5000)
    
    # Add passage points
    for i in range(20):
        x = np.random.uniform(0, 500)
        y = np.random.uniform(0, 500)
        z = np.random.uniform(-200, 0)
        ep.add_passage_point(x, y, z, importance=np.random.uniform(0.5, 1.0))
    
    # Optimize sensor placement
    sensors = ep.optimize_sensor_placement(n_sensors=5)
    print(f"\nOptimal Sensor Locations: {len(sensors)}")
    
    # Optimize emergency caches
    caches = ep.optimize_emergency_caches(n_caches=2)
    print(f"Emergency Caches: {len(caches)}")
    
    # Calculate budget
    budget = ep.calculate_budget_usage()
    print(f"\nBudget Usage:")
    print(f"  Total: ${budget['total_cost']}")
    print(f"  Remaining: ${budget['remaining']}")
    print(f"  Over Budget: {budget['over_budget']}")
    
    print("\n" + "="*60)
