"""
Tests for Navigation modules
"""

import pytest
import numpy as np
from src.navigation.swarm_algorithm import SwarmNavigationEngine, SwarmState
from src.navigation.position_estimator import PositionEstimator
from src.navigation.adaptive_ranging import AdaptiveRanging
from src.navigation.bio_navigation import BioNavigation, BioEventType


class TestSwarmNavigation:
    """Test suite for SwarmNavigationEngine"""
    
    def test_initialization(self):
        """Test swarm initialization"""
        swarm = SwarmNavigationEngine(num_vehicles=12)
        assert swarm.num_vehicles == 12
        assert swarm.state == SwarmState.INITIALIZING
        assert len(swarm.vehicles) == 12
    
    def test_acoustic_ranging(self):
        """Test acoustic ranging"""
        swarm = SwarmNavigationEngine(num_vehicles=2)
        
        # Range between two vehicles
        r = swarm.acoustic_ranging(0, 1, sound_velocity=1500)
        
        assert r > 0
        assert r < swarm.max_range
    
    def test_multi_lateration(self):
        """Test position estimation"""
        swarm = SwarmNavigationEngine(num_vehicles=5)
        
        # Simulate ranges
        ranges = {1: 100.0, 2: 150.0, 3: 120.0, 4: 130.0}
        anchors = {
            1: np.array([0, 0, 0]),
            2: np.array([100, 0, 0]),
            3: np.array([0, 100, 0]),
            4: np.array([0, 0, 100])
        }
        
        pos = swarm.multi_lateration(0, ranges, anchors)
        assert len(pos) == 3
    
    def test_simulate_mission(self):
        """Test mission simulation"""
        swarm = SwarmNavigationEngine(num_vehicles=6)
        results = swarm.simulate_mission(duration_hours=1, depth=1000)
        
        assert 'rms_error' in results
        assert results['num_vehicles'] == 6
        assert results['rms_error'] > 0


class TestPositionEstimator:
    """Test suite for PositionEstimator"""
    
    def test_solve(self):
        """Test position solving"""
        estimator = PositionEstimator()
        
        ranges = [141.4, 141.4, 141.4, 141.4]
        anchors = [
            np.array([0, 0, 0]),
            np.array([100, 0, 0]),
            np.array([0, 100, 0]),
            np.array([0, 0, 100])
        ]
        
        pos = estimator.solve(ranges, anchors)
        assert len(pos) == 3
    
    def test_weighted_solve(self):
        """Test weighted least squares"""
        estimator = PositionEstimator()
        
        ranges = [141.4, 141.4, 141.4, 141.4]
        anchors = [
            np.array([0, 0, 0]),
            np.array([100, 0, 0]),
            np.array([0, 100, 0]),
            np.array([0, 0, 100])
        ]
        weights = [1.0, 0.8, 0.9, 1.0]
        
        pos = estimator.solve_weighted(ranges, anchors, weights)
        assert len(pos) == 3
    
    def test_kalman_update(self):
        """Test Kalman filter"""
        estimator = PositionEstimator()
        
        measurement = np.array([10, 10, 10])
        prior_pos = np.array([9, 9, 9])
        prior_cov = np.eye(3)
        
        pos, cov = estimator.kalman_update(
            measurement, 1.0, prior_pos, prior_cov, dt=1.0
        )
        
        assert len(pos) == 3
        assert cov.shape == (3, 3)
    
    def test_outlier_rejection(self):
        """Test outlier rejection"""
        estimator = PositionEstimator()
        
        measurements = [100, 101, 99, 102, 200]  # 200 is outlier
        filtered, indices = estimator.reject_outliers(measurements)
        
        assert len(filtered) == 4
        assert 200 not in filtered
    
    def test_dilution_of_precision(self):
        """Test DOP calculation"""
        estimator = PositionEstimator()
        
        anchors = [
            np.array([0, 0, 0]),
            np.array([100, 0, 0]),
            np.array([0, 100, 0]),
            np.array([0, 0, 100])
        ]
        position = np.array([50, 50, 50])
        
        dop = estimator.dilution_of_precision(anchors, position)
        
        assert 'GDOP' in dop
        assert 'PDOP' in dop
        assert 'HDOP' in dop
        assert 'VDOP' in dop


class TestAdaptiveRanging:
    """Test suite for AdaptiveRanging"""
    
    def test_initialization(self):
        """Test initialization"""
        ar = AdaptiveRanging(history_size=50)
        assert ar.history_size == 50
        assert len(ar.range_history) == 0
    
    def test_correct_sound_velocity(self):
        """Test sound velocity correction"""
        ar = AdaptiveRanging()
        
        corrected = ar.correct_sound_velocity(
            measured_c=1660,
            temperature=1.9,
            salinity=34.68,
            depth=10911
        )
        
        assert corrected > 0
    
    def test_anomaly_detection(self):
        """Test anomaly detection"""
        ar = AdaptiveRanging()
        
        measurements = [100, 101, 99, 102, 200]  # 200 is anomaly
        is_anomaly, score = ar.detect_anomaly(measurements)
        
        assert is_anomaly is True
        assert score > 2.0
    
    def test_thermocline_detection(self):
        """Test thermocline detection"""
        ar = AdaptiveRanging()
        
        temps = [2.0, 1.9, 1.8, 1.5, 1.0]  # Sharp gradient at end
        depths = [0, 100, 200, 300, 400]
        
        detected, gradient, depth = ar.detect_thermocline(temps, depths)
        
        assert detected is True
        assert gradient > 0
    
    def test_adaptive_filter(self):
        """Test adaptive filtering"""
        ar = AdaptiveRanging()
        
        measurements = [100, 101, 102, 200, 101, 102]
        filtered = ar.adaptive_filter(measurements)
        
        assert len(filtered) == len(measurements)
        assert filtered[3] != 200  # Should filter outlier


class TestBioNavigation:
    """Test suite for BioNavigation"""
    
    def test_initialization(self):
        """Test initialization"""
        bio = BioNavigation(min_intensity=1e4)
        assert bio.min_intensity == 1e4
        assert bio.wavelength_range == (400, 600)
    
    def test_detect_event(self):
        """Test event detection"""
        bio = BioNavigation()
        
        event = bio.detect_event(
            intensity=8.2e4,
            wavelength=472,
            duration=1.2
        )
        
        assert event is not None
        assert event.intensity == 8.2e4
        assert event.wavelength == 472
    
    def test_classify_event(self):
        """Test event classification"""
        bio = BioNavigation()
        
        # Amphipod
        event = bio.detect_event(8.2e4, 472, 1.2)
        assert event.event_type == BioEventType.AMPHIPOD
        
        # Bacteria
        event = bio.detect_event(1.5e4, 485, 10.0)
        assert event.event_type == BioEventType.BACTERIA
    
    def test_find_hotspots(self):
        """Test hotspot identification"""
        bio = BioNavigation()
        
        # Add some events
        for i in range(20):
            bio.detect_event(
                intensity=5e4,
                wavelength=470,
                duration=1.0,
                position=np.random.randn(3) * 50
            )
        
        hotspots = bio.find_hotspots(min_events=3)
        assert len(hotspots) > 0
    
    def test_correction_factor(self):
        """Test navigation correction"""
        bio = BioNavigation()
        
        # Add a hotspot
        for i in range(10):
            bio.detect_event(
                intensity=5e4,
                wavelength=470,
                duration=1.0,
                position=np.array([0, 0, 0])
            )
        bio.find_hotspots()
        
        # Near hotspot
        improved = bio.correction_factor(
            event_position=np.array([10, 10, 0]),
            current_uncertainty=1.0
        )
        assert improved < 1.0
    
    def test_predict_activity(self):
        """Test activity prediction"""
        bio = BioNavigation()
        
        # Add events
        for i in range(10):
            bio.detect_event(
                intensity=5e4,
                wavelength=470,
                duration=1.0,
                position=np.random.randn(3) * 50
            )
        
        prob = bio.predict_activity(np.array([0, 0, 0]))
        assert 0 <= prob <= 1
