"""
AUV Controller Module - Autonomous underwater vehicle control
"""

import numpy as np
from typing import List, Tuple, Dict, Optional
from dataclasses import dataclass
from enum import Enum


class VehicleState(Enum):
    """Vehicle operational states"""
    INITIALIZING = "initializing"
    DESCENDING = "descending"
    SURVEYING = "surveying"
    ASCENDING = "ascending"
    EMERGENCY = "emergency"
    DOCKED = "docked"


class MissionPhase(Enum):
    """Mission phases"""
    DESCENT = "descent"
    SURVEY = "survey"
    STANDBY = "standby"
    ASCENT = "ascent"
    RECOVERY = "recovery"


@dataclass
class Waypoint:
    """Mission waypoint"""
    position: np.ndarray  # [x, y, z] in meters
    speed: float  # m/s
    heading: float  # degrees
    actions: List[str]  # Actions to perform
    tolerance: float  # meters


@dataclass
class VehicleStatus:
    """Current vehicle status"""
    position: np.ndarray
    velocity: np.ndarray
    heading: float
    depth: float
    battery: float
    state: VehicleState
    active_sensors: List[str]
    last_communication: float


class AUVController:
    """
    Autonomous Underwater Vehicle controller
    
    Manages vehicle navigation, waypoint following, and mission execution
    Supports swarm operations with 12 vehicles at 0.78m accuracy
    """
    
    def __init__(self,
                 vehicle_id: int,
                 swarm_size: int = 12,
                 max_depth: float = 11000,
                 cruise_speed: float = 0.25):
        """
        Initialize AUV controller
        
        Args:
            vehicle_id: Unique vehicle identifier
            swarm_size: Total swarm size
            max_depth: Maximum operating depth (m)
            cruise_speed: Normal cruising speed (m/s)
        """
        self.vehicle_id = vehicle_id
        self.swarm_size = swarm_size
        self.max_depth = max_depth
        self.cruise_speed = cruise_speed
        
        # Vehicle state
        self.position = np.zeros(3)
        self.velocity = np.zeros(3)
        self.heading = 0.0
        self.depth = 0.0
        self.state = VehicleState.INITIALIZING
        self.mission_phase = MissionPhase.DESCENT
        
        # Waypoint following
        self.waypoints = []
        self.current_waypoint_idx = 0
        self.waypoint_tolerance = 5.0  # meters
        
        # Mission parameters
        self.mission_start_time = 0.0
        self.mission_duration = 72.0  # hours
        self.battery_capacity = 180.0  # Wh
        self.battery_level = 100.0  # percent
        
        # Sensor suite
        self.active_sensors = ['pressure', 'temperature']
        self.sensor_data = {}
        
        # Swarm communication
        self.neighbor_positions = {}
        self.last_communication = 0.0
        
        # Control gains
        self.kp_position = 0.1
        self.kd_velocity = 0.05
        self.kp_heading = 0.01
        
        # History
        self.trajectory = []
        self.status_history = []
    
    def set_waypoints(self, waypoints: List[np.ndarray]):
        """Set mission waypoints"""
        self.waypoints = [Waypoint(
            position=wp,
            speed=self.cruise_speed,
            heading=0.0,
            actions=[],
            tolerance=self.waypoint_tolerance
        ) for wp in waypoints]
        self.current_waypoint_idx = 0
    
    def set_survey_pattern(self,
                          area_size: float = 500,
                          transect_spacing: float = 50,
                          altitude: float = 5):
        """
        Generate lawnmower survey pattern
        
        Args:
            area_size: Survey area size (meters)
            transect_spacing: Spacing between transects
            altitude: Altitude above bottom
        """
        waypoints = []
        
        # Start at corner
        start_x = -area_size / 2
        start_y = -area_size / 2
        
        num_transects = int(area_size / transect_spacing)
        
        for i in range(num_transects + 1):
            y = start_y + i * transect_spacing
            
            if i % 2 == 0:
                # Go east
                waypoints.append(np.array([start_x + area_size, y, -altitude]))
            else:
                # Go west
                waypoints.append(np.array([start_x, y, -altitude]))
        
        self.set_waypoints(waypoints)
    
    def update(self, dt: float):
        """
        Update vehicle state
        
        Args:
            dt: Time step in seconds
        """
        # Update based on current state
        if self.state == VehicleState.DESCENDING:
            self._update_descending(dt)
        elif self.state == VehicleState.SURVEYING:
            self._update_surveying(dt)
        elif self.state == VehicleState.ASCENDING:
            self._update_ascending(dt)
        
        # Update battery
        self._update_battery(dt)
        
        # Record trajectory
        self.trajectory.append(self.position.copy())
        
        # Check for waypoint completion
        self._check_waypoint()
    
    def _update_descending(self, dt: float):
        """Update during descent phase"""
        target_depth = self.max_depth
        
        # Simple depth control
        depth_error = target_depth - self.depth
        vertical_velocity = np.clip(depth_error * 0.1, -0.5, 0.5)
        
        self.velocity = np.array([0, 0, vertical_velocity])
        self.position += self.velocity * dt
        self.depth = -self.position[2]
        
        # Check if at depth
        if abs(self.depth - target_depth) < 10:
            self.state = VehicleState.SURVEYING
            self.mission_phase = MissionPhase.SURVEY
            self.active_sensors = ['oxygen', 'pressure', 'temperature', 
                                  'acoustic', 'bioluminescence', 'altimeter']
    
    def _update_surveying(self, dt: float):
        """Update during survey phase"""
        if not self.waypoints or self.current_waypoint_idx >= len(self.waypoints):
            return
        
        target = self.waypoints[self.current_waypoint_idx].position
        
        # Position error
        error = target - self.position
        distance = np.linalg.norm(error)
        
        if distance > self.waypoint_tolerance:
            # Move toward waypoint
            direction = error / distance
            self.velocity = direction * self.cruise_speed
            
            # Add random currents (simulation)
            current = np.random.randn(3) * 0.05
            self.velocity += current
        else:
            # At waypoint - hold position
            self.velocity = np.zeros(3)
        
        self.position += self.velocity * dt
        self.depth = -self.position[2]
    
    def _update_ascending(self, dt: float):
        """Update during ascent phase"""
        target_depth = 0
        
        # Simple depth control
        depth_error = self.depth - target_depth
        vertical_velocity = -np.clip(depth_error * 0.1, -0.3, 0.3)
        
        self.velocity = np.array([0, 0, vertical_velocity])
        self.position += self.velocity * dt
        self.depth = -self.position[2]
        
        # Check if at surface
        if self.depth < 1:
            self.state = VehicleState.DOCKED
    
    def _update_battery(self, dt: float):
        """Update battery level based on power consumption"""
        # Power consumption depends on state and active sensors
        if self.state == VehicleState.SURVEYING:
            power_rate = 2.5  # W
        elif self.state in [VehicleState.DESCENDING, VehicleState.ASCENDING]:
            power_rate = 1.5  # W
        else:
            power_rate = 0.5  # W
        
        # Add sensor power
        sensor_power = len(self.active_sensors) * 0.2  # W per sensor
        
        total_power = power_rate + sensor_power
        energy_used = total_power * dt / 3600  # Wh
        
        self.battery_level -= (energy_used / self.battery_capacity) * 100
        self.battery_level = max(0, self.battery_level)
    
    def _check_waypoint(self):
        """Check if current waypoint is reached"""
        if not self.waypoints or self.current_waypoint_idx >= len(self.waypoints):
            return
        
        target = self.waypoints[self.current_waypoint_idx]
        distance = np.linalg.norm(self.position - target.position)
        
        if distance < target.tolerance:
            self.current_waypoint_idx += 1
            
            if self.current_waypoint_idx >= len(self.waypoints):
                # Survey complete - ascend
                self.state = VehicleState.ASCENDING
                self.mission_phase = MissionPhase.ASCENT
    
    def emergency_ascent(self):
        """Execute emergency ascent procedure"""
        self.state = VehicleState.EMERGENCY
        self.mission_phase = MissionPhase.ASCENT
        
        # Drop weights, increase buoyancy
        self.velocity = np.array([0, 0, 0.5])  # Fast ascent
        
        # Log emergency
        print(f"Vehicle {self.vehicle_id}: EMERGENCY ASCENT initiated")
    
    def communicate(self, neighbor_id: int, position: np.ndarray):
        """Share position with neighbor"""
        self.neighbor_positions[neighbor_id] = position.copy()
        self.last_communication = self.mission_start_time
    
    def get_status(self) -> VehicleStatus:
        """Get current vehicle status"""
        return VehicleStatus(
            position=self.position.copy(),
            velocity=self.velocity.copy(),
            heading=self.heading,
            depth=self.depth,
            battery=self.battery_level,
            state=self.state,
            active_sensors=self.active_sensors.copy(),
            last_communication=self.last_communication
        )
    
    def lawnmower_survey(self,
                        area_size: float = 500,
                        transect_spacing: float = 50,
                        altitude: float = 5):
        """Execute lawnmower survey pattern"""
        self.set_survey_pattern(area_size, transect_spacing, altitude)
        self.state = VehicleState.SURVEYING
        self.mission_phase = MissionPhase.SURVEY
    
    def get_trajectory(self) -> np.ndarray:
        """Get recorded trajectory"""
        return np.array(self.trajectory)
    
    def __repr__(self) -> str:
        return (f"AUVController(id={self.vehicle_id}, "
                f"state={self.state.value}, "
                f"depth={self.depth:.0f}m, "
                f"battery={self.battery_level:.1f}%)")
