"""GPS coordinate calculations using proper formulas."""
import math
from typing import Dict, List, Tuple


class DroneKnowledgeBase:
    """GPS coordinate calculations from drone-ai-knowledge-base.md."""
    
    def __init__(self):
        self.METERS_PER_DEGREE_LAT = 111000.0
        self.direction_rules = {
            'north': (1, 0), 'south': (-1, 0),
            'east': (0, 1), 'west': (0, -1),
            'northeast': (0.707, 0.707), 'northwest': (0.707, -0.707),
            'southeast': (-0.707, 0.707), 'southwest': (-0.707, -0.707)
        }
        
    def calculate_coordinate_offset(self, lat: float, lon: float, 
                                  distance_m: float, direction: str) -> Tuple[float, float]:
        """Calculate new coordinates from offset."""
        lat_deg_per_m = 1.0 / self.METERS_PER_DEGREE_LAT
        # Avoid division by zero at poles and ensure minimal east/west offset
        cos_lat = max(1e-6, abs(math.cos(math.radians(lat))))
        lon_deg_per_m = 1.0 / (self.METERS_PER_DEGREE_LAT * cos_lat)
        
        if direction.lower() not in self.direction_rules:
            raise ValueError(f"Unknown direction: {direction}")
            
        if direction.lower() == 'north':
            return lat + distance_m * lat_deg_per_m, lon
        elif direction.lower() == 'south':
            return lat - distance_m * lat_deg_per_m, lon
        elif direction.lower() == 'east':
            return lat, lon + distance_m * lon_deg_per_m
        elif direction.lower() == 'west':
            return lat, lon - distance_m * lon_deg_per_m
        else:
            lat_factor, lon_factor = self.direction_rules[direction.lower()]
            lat_offset = lat_factor * distance_m * lat_deg_per_m
            lon_offset = lon_factor * distance_m * lon_deg_per_m
            return lat + lat_offset, lon + lon_offset
    
    def create_straight_line_waypoints(self, start_lat: float, start_lon: float,
                                     direction: str, distance: float, 
                                     num_waypoints: int = 3) -> List[Dict]:
        """Create straight line waypoints."""
        waypoints = []
        segment_distance = distance / num_waypoints
        
        for i in range(1, num_waypoints + 1):
            current_distance = i * segment_distance
            wp_lat, wp_lon = self.calculate_coordinate_offset(
                start_lat, start_lon, current_distance, direction
            )
            waypoints.append({
                'lat': wp_lat, 'lon': wp_lon, 'altitude': 70,
                'hold_time': 0.0, 'accept_radius': 2.0
            })
        return waypoints
    
    def create_survey_waypoints(self, center_lat: float, center_lon: float,
                               altitude: int, grid_size: int = 4) -> List[Dict]:
        """Create survey grid."""
        waypoints = []
        spacing = 200
        
        for i in range(grid_size):
            for j in range(grid_size):
                north_offset = (i - grid_size/2) * spacing
                east_offset = (j - grid_size/2) * spacing
                
                wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_offset, 'north')
                wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_offset, 'east')
                
                waypoints.append({
                    'lat': wp_lat, 'lon': wp_lon, 'altitude': altitude,
                    'hold_time': 1.0, 'accept_radius': 3.0
                })
        return waypoints
    
    def create_patrol_waypoints(self, center_lat: float, center_lon: float,
                               altitude: int, radius: float = 300) -> List[Dict]:
        """Create patrol perimeter with optimized coverage."""
        waypoints = []
        num_points = 8
        
        for i in range(num_points):
            angle = (2 * math.pi * i) / num_points
            north_distance = radius * math.cos(angle)
            east_distance = radius * math.sin(angle)
            
            wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_distance, 'north')
            wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_distance, 'east')
            
            waypoints.append({
                'lat': wp_lat, 'lon': wp_lon, 'altitude': altitude,
                'hold_time': 2.0, 'accept_radius': 5.0
            })
        return waypoints
    
    def create_photography_waypoints(self, center_lat: float, center_lon: float,
                                   altitude: int) -> List[Dict]:
        """Create strategic photography positions with optimal angles."""
        waypoints = []
        
        # Enhanced photo positions with altitude variations
        photo_offsets = [
            (150, 150, 'northeast', 0),
            (-150, 150, 'northwest', 10),  
            (-150, -150, 'southwest', 0),
            (150, -150, 'southeast', 10),
            (0, 0, 'center_overhead', 20)
        ]
        
        for north_offset, east_offset, position, alt_adjustment in photo_offsets:
            wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_offset, 'north')
            wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_offset, 'east')
            
            waypoints.append({
                'lat': wp_lat, 'lon': wp_lon, 
                'altitude': altitude + alt_adjustment,
                'hold_time': 3.0, 'accept_radius': 2.0
            })
        return waypoints
    
    def create_inspection_waypoints(self, start_lat: float, start_lon: float,
                                  direction: str, distance: float, altitude: int) -> List[Dict]:
        """Create detailed inspection waypoints with varying altitudes."""
        waypoints = []
        num_points = 6
        
        for i in range(num_points):
            segment_distance = (distance * (i + 1)) / num_points
            alt_variation = (i - num_points//2) * 5  # Small altitude changes
            
            wp_lat, wp_lon = self.calculate_coordinate_offset(
                start_lat, start_lon, segment_distance, direction
            )
            
            waypoints.append({
                'lat': wp_lat, 'lon': wp_lon, 
                'altitude': altitude + alt_variation,
                'hold_time': 2.0, 'accept_radius': 1.5
            })
        return waypoints