zwo_mount_control 0.2.0

//! Satellite tracking module using space-dust for TLE propagation.
//!
//! This module provides satellite tracking capabilities for ZWO mounts,
//! allowing you to track satellites, ISS, and other space objects using
//! Two-Line Element (TLE) data.
//!
//! # Example
//!
//! ```rust,ignore
//! use zwo_mount_control::{SatelliteTracker, Mount, MockMount};
//!
//! // ISS TLE data
//! let tle_line1 = "1 25544U 98067A   24001.50000000  .00016717  00000-0  10270-3 0  9025";
//! let tle_line2 = "2 25544  51.6400 208.9163 0006703  35.6028  75.3281 15.49560066429339";
//!
//! let mut tracker = SatelliteTracker::from_tle(tle_line1, tle_line2)?;
//!
//! // Set observer location (Los Angeles)
//! tracker.set_observer_location(34.0522, -118.2437, 71.0);
//!
//! // Get current satellite position
//! let position = tracker.get_current_position()?;
//! println!("Satellite position: {}", position);
//!
//! // Start tracking with a mount
//! let mut mount = MockMount::new();
//! mount.connect()?;
//! tracker.start_tracking(&mut mount)?;
//! ```

use std::time::{Duration, Instant};

use chrono::{DateTime, Utc};
use space_dust::observations::Observations;
use space_dust::state::GeodeticState;
use space_dust::tle::Tle;

use crate::coordinates::{EquatorialPosition, HorizontalPosition, TrackingRates};
use crate::error::{MountError, MountResult};
use crate::mount::{Mount, SiteLocation};

/// Minimum elevation angle (degrees) for satellite visibility.
pub const DEFAULT_MIN_ELEVATION: f64 = 10.0;

/// Default tracking update interval in milliseconds.
pub const DEFAULT_UPDATE_INTERVAL_MS: u64 = 100;

/// Default pre-slew time in seconds (how early to slew to AOS position).
pub const DEFAULT_PRE_SLEW_SEC: f64 = 30.0;

/// Default minimum azimuth limit for cable wrap prevention (degrees).
pub const DEFAULT_AZ_MIN_LIMIT: f64 = -180.0;

/// Default maximum azimuth limit for cable wrap prevention (degrees).
pub const DEFAULT_AZ_MAX_LIMIT: f64 = 540.0;

/// Default altitude lower limit (degrees).
pub const DEFAULT_ALT_MIN_LIMIT: f64 = 0.0;

/// Default altitude upper limit (degrees).
pub const DEFAULT_ALT_MAX_LIMIT: f64 = 90.0;

/// PID controller for smooth tracking.
#[derive(Debug, Clone)]
pub struct PidController {
    /// Proportional gain
    pub kp: f64,
    /// Integral gain
    pub ki: f64,
    /// Derivative gain
    pub kd: f64,
    /// Accumulated integral error
    integral: f64,
    /// Previous error for derivative calculation
    prev_error: f64,
    /// Maximum integral windup limit
    pub integral_limit: f64,
    /// Output limit (max correction in deg/s)
    pub output_limit: f64,
}

impl PidController {
    /// Create a new PID controller with the given gains.
    pub fn new(kp: f64, ki: f64, kd: f64) -> Self {
        Self {
            kp,
            ki,
            kd,
            integral: 0.0,
            prev_error: 0.0,
            integral_limit: 10.0,
            output_limit: 5.0,
        }
    }

    /// Reset the controller state.
    pub fn reset(&mut self) {
        self.integral = 0.0;
        self.prev_error = 0.0;
    }

    /// Update the controller with a new error value.
    ///
    /// # Arguments
    /// * `error` - Current error (target - actual) in degrees
    /// * `dt` - Time step in seconds
    ///
    /// # Returns
    /// Correction rate in degrees per second
    pub fn update(&mut self, error: f64, dt: f64) -> f64 {
        if dt <= 0.0 {
            return 0.0;
        }

        // Proportional term
        let p_term = self.kp * error;

        // Integral term with anti-windup
        self.integral += error * dt;
        self.integral = self
            .integral
            .clamp(-self.integral_limit, self.integral_limit);
        let i_term = self.ki * self.integral;

        // Derivative term
        let derivative = (error - self.prev_error) / dt;
        let d_term = self.kd * derivative;
        self.prev_error = error;

        // Combined output with limiting
        let output = p_term + i_term + d_term;
        output.clamp(-self.output_limit, self.output_limit)
    }
}

impl Default for PidController {
    fn default() -> Self {
        // Default gains tuned for satellite tracking
        Self::new(2.0, 0.1, 0.5)
    }
}

/// Satellite pass information.
#[derive(Debug, Clone)]
pub struct SatellitePass {
    /// Name of the satellite
    pub name: String,
    /// NORAD catalog ID
    pub norad_id: i32,
    /// Start time of the pass (when satellite rises above minimum elevation)
    pub aos_time: DateTime<Utc>,
    /// Time of closest approach / maximum elevation
    pub tca_time: DateTime<Utc>,
    /// End time of the pass (when satellite sets below minimum elevation)
    pub los_time: DateTime<Utc>,
    /// Maximum elevation during the pass (degrees)
    pub max_elevation: f64,
    /// Azimuth at AOS (degrees)
    pub aos_azimuth: f64,
    /// Azimuth at LOS (degrees)
    pub los_azimuth: f64,
}

impl SatellitePass {
    /// Get the duration of the pass.
    pub fn duration(&self) -> Duration {
        let duration_secs = (self.los_time - self.aos_time).num_seconds();
        Duration::from_secs(duration_secs.max(0) as u64)
    }

    /// Check if the pass is currently active.
    pub fn is_active(&self, now: DateTime<Utc>) -> bool {
        now >= self.aos_time && now <= self.los_time
    }

    /// Check if this is a good pass (high elevation).
    pub fn is_good_pass(&self, min_max_elevation: f64) -> bool {
        self.max_elevation >= min_max_elevation
    }
}

impl std::fmt::Display for SatellitePass {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "{} ({}): AOS {} @ {:.1}° Az, Max El {:.1}°, LOS {} @ {:.1}° Az, Duration {:?}",
            self.name,
            self.norad_id,
            self.aos_time.format("%H:%M:%S"),
            self.aos_azimuth,
            self.max_elevation,
            self.los_time.format("%H:%M:%S"),
            self.los_azimuth,
            self.duration()
        )
    }
}

/// Tracking state information.
#[derive(Debug, Clone)]
pub struct TrackingState {
    /// Current RA/Dec position
    pub equatorial: EquatorialPosition,
    /// Current Az/Alt position
    pub horizontal: HorizontalPosition,
    /// Current RA tracking rate (arcsec/sec)
    pub ra_rate: f64,
    /// Current Dec tracking rate (arcsec/sec)
    pub dec_rate: f64,
    /// Range to satellite (km)
    pub range_km: f64,
    /// Range rate (km/s, positive = moving away)
    pub range_rate_km_s: f64,
    /// Current time
    pub timestamp: DateTime<Utc>,
    /// Is satellite above horizon?
    pub is_visible: bool,
}

impl std::fmt::Display for TrackingState {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "{} | {} | Range: {:.1} km | Rates: RA {:.2}\"/s, Dec {:.2}\"/s | {}",
            self.equatorial,
            self.horizontal,
            self.range_km,
            self.ra_rate,
            self.dec_rate,
            if self.is_visible {
                "VISIBLE"
            } else {
                "BELOW HORIZON"
            }
        )
    }
}

/// Configuration for satellite tracking.
#[derive(Debug, Clone)]
pub struct TrackingConfig {
    /// Minimum elevation for tracking (degrees)
    pub min_elevation: f64,
    /// Update interval for tracking loop (milliseconds)
    pub update_interval_ms: u64,
    /// Lead time for position calculation (seconds)
    /// Accounts for mount slew delay
    pub lead_time_sec: f64,
    /// Whether to use rate tracking (vs. continuous goto)
    pub use_rate_tracking: bool,
    /// Maximum slew rate (deg/sec) - limits goto speed
    pub max_slew_rate: f64,
    /// Pre-slew time in seconds (how early to slew to AOS position)
    pub pre_slew_sec: f64,
    /// PID proportional gain for azimuth tracking
    pub pid_kp_az: f64,
    /// PID integral gain for azimuth tracking
    pub pid_ki_az: f64,
    /// PID derivative gain for azimuth tracking
    pub pid_kd_az: f64,
    /// PID proportional gain for altitude tracking
    pub pid_kp_alt: f64,
    /// PID integral gain for altitude tracking
    pub pid_ki_alt: f64,
    /// PID derivative gain for altitude tracking
    pub pid_kd_alt: f64,
    /// Minimum azimuth limit for cable wrap prevention (degrees)
    /// Mount will not track below this azimuth
    pub az_min_limit: f64,
    /// Maximum azimuth limit for cable wrap prevention (degrees)
    /// Mount will not track above this azimuth
    pub az_max_limit: f64,
    /// Minimum altitude limit (degrees)
    pub alt_min_limit: f64,
    /// Maximum altitude limit (degrees)
    pub alt_max_limit: f64,
    /// Enable cable wrap prevention
    pub enable_cable_wrap_prevention: bool,
}

impl Default for TrackingConfig {
    fn default() -> Self {
        Self {
            min_elevation: DEFAULT_MIN_ELEVATION,
            update_interval_ms: DEFAULT_UPDATE_INTERVAL_MS,
            lead_time_sec: 0.5,
            use_rate_tracking: true,
            max_slew_rate: 5.0,
            pre_slew_sec: DEFAULT_PRE_SLEW_SEC,
            // Default PID gains for Az/Alt tracking
            pid_kp_az: 2.0,
            pid_ki_az: 0.1,
            pid_kd_az: 0.5,
            pid_kp_alt: 2.0,
            pid_ki_alt: 0.1,
            pid_kd_alt: 0.5,
            // Cable wrap prevention limits
            az_min_limit: DEFAULT_AZ_MIN_LIMIT,
            az_max_limit: DEFAULT_AZ_MAX_LIMIT,
            alt_min_limit: DEFAULT_ALT_MIN_LIMIT,
            alt_max_limit: DEFAULT_ALT_MAX_LIMIT,
            enable_cable_wrap_prevention: true,
        }
    }
}

/// Cable wrap state tracking
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum CableWrapState {
    /// Mount is within safe limits
    Safe,
    /// Mount is approaching a limit
    Warning,
    /// Mount has reached a limit and cannot continue in this direction
    AtLimit,
}

/// Result of checking cable wrap limits
#[derive(Debug, Clone)]
pub struct CableWrapCheck {
    /// Current cable wrap state
    pub state: CableWrapState,
    /// Current unwrapped azimuth (can be outside 0-360 range)
    pub unwrapped_az: f64,
    /// Distance to minimum limit (negative if past limit)
    pub distance_to_min: f64,
    /// Distance to maximum limit (negative if past limit)
    pub distance_to_max: f64,
    /// Suggested safe azimuth if at limit
    pub safe_azimuth: Option<f64>,
}

/// Satellite tracker using space-dust for orbit propagation.
pub struct SatelliteTracker {
    /// TLE data
    tle: Tle,
    /// Satellite name (optional, from 3-line TLE)
    name: Option<String>,
    /// Observer location as space-dust GeodeticState
    observer: GeodeticState,
    /// Observer site location
    site_location: SiteLocation,
    /// Tracking configuration
    config: TrackingConfig,
    /// Last calculated state
    last_state: Option<TrackingState>,
    /// Current unwrapped azimuth for cable wrap tracking
    /// This tracks cumulative rotation, not just 0-360 position
    unwrapped_azimuth: f64,
    /// Whether the unwrapped azimuth has been initialized
    azimuth_initialized: bool,
    /// Time of last update
    last_update: Option<Instant>,
}

impl SatelliteTracker {
    /// Create a new satellite tracker from TLE lines.
    ///
    /// # Arguments
    /// * `line1` - First line of TLE
    /// * `line2` - Second line of TLE
    ///
    /// # Example
    /// ```rust,ignore
    /// let tracker = SatelliteTracker::from_tle(
    ///     "1 25544U 98067A   24001.50000000  .00016717  00000-0  10270-3 0  9025",
    ///     "2 25544  51.6400 208.9163 0006703  35.6028  75.3281 15.49560066429339"
    /// )?;
    /// ```
    pub fn from_tle(line1: &str, line2: &str) -> MountResult<Self> {
        let tle =
            Tle::parse(line1, line2).map_err(|e| MountError::satellite_error(e.to_string()))?;

        Ok(Self {
            tle,
            name: None,
            observer: GeodeticState::new(0.0, 0.0, 0.0),
            site_location: SiteLocation::default(),
            config: TrackingConfig::default(),
            last_state: None,
            unwrapped_azimuth: 0.0,
            azimuth_initialized: false,
            last_update: None,
        })
    }

    /// Create a new satellite tracker from three-line TLE (with name).
    ///
    /// # Arguments
    /// * `name` - Satellite name
    /// * `line1` - First line of TLE
    /// * `line2` - Second line of TLE
    pub fn from_tle_with_name(name: &str, line1: &str, line2: &str) -> MountResult<Self> {
        let tle = Tle::parse_3le(name, line1, line2)
            .map_err(|e| MountError::satellite_error(e.to_string()))?;

        Ok(Self {
            tle,
            name: Some(name.trim().to_string()),
            observer: GeodeticState::new(0.0, 0.0, 0.0),
            site_location: SiteLocation::default(),
            config: TrackingConfig::default(),
            last_state: None,
            unwrapped_azimuth: 0.0,
            azimuth_initialized: false,
            last_update: None,
        })
    }

    /// Initialize the unwrapped azimuth from the current mount position.
    /// Call this before starting tracking to establish the reference point.
    pub fn initialize_cable_wrap(&mut self, current_azimuth: f64) {
        self.unwrapped_azimuth = current_azimuth;
        self.azimuth_initialized = true;
        log::info!(
            "Cable wrap tracking initialized at Az {:.1}°",
            current_azimuth
        );
    }

    /// Update the unwrapped azimuth based on a new position.
    /// This tracks cumulative rotation to detect cable wrap.
    fn update_unwrapped_azimuth(&mut self, new_azimuth: f64) {
        if !self.azimuth_initialized {
            self.initialize_cable_wrap(new_azimuth);
            return;
        }

        // Calculate the shortest angular difference
        let mut delta = new_azimuth - (self.unwrapped_azimuth.rem_euclid(360.0));

        // Normalize to [-180, 180] to handle wrap-around
        if delta > 180.0 {
            delta -= 360.0;
        } else if delta < -180.0 {
            delta += 360.0;
        }

        self.unwrapped_azimuth += delta;
    }

    /// Check cable wrap limits and return the current state.
    pub fn check_cable_wrap(&self, target_azimuth: f64) -> CableWrapCheck {
        if !self.config.enable_cable_wrap_prevention {
            return CableWrapCheck {
                state: CableWrapState::Safe,
                unwrapped_az: self.unwrapped_azimuth,
                distance_to_min: f64::MAX,
                distance_to_max: f64::MAX,
                safe_azimuth: None,
            };
        }

        // Calculate what the unwrapped azimuth would be after moving to target
        let mut delta = target_azimuth - (self.unwrapped_azimuth.rem_euclid(360.0));
        if delta > 180.0 {
            delta -= 360.0;
        } else if delta < -180.0 {
            delta += 360.0;
        }
        let projected_unwrapped = self.unwrapped_azimuth + delta;

        let distance_to_min = projected_unwrapped - self.config.az_min_limit;
        let distance_to_max = self.config.az_max_limit - projected_unwrapped;

        // Warning threshold: 30 degrees from limit
        const WARNING_THRESHOLD: f64 = 30.0;

        let state = if distance_to_min < 0.0 || distance_to_max < 0.0 {
            CableWrapState::AtLimit
        } else if distance_to_min < WARNING_THRESHOLD || distance_to_max < WARNING_THRESHOLD {
            CableWrapState::Warning
        } else {
            CableWrapState::Safe
        };

        // Calculate safe azimuth if at limit
        let safe_azimuth = if state == CableWrapState::AtLimit {
            if distance_to_min < 0.0 {
                // Past minimum, suggest moving to minimum + margin
                Some((self.config.az_min_limit + 10.0).rem_euclid(360.0))
            } else {
                // Past maximum, suggest moving to maximum - margin
                Some((self.config.az_max_limit - 10.0).rem_euclid(360.0))
            }
        } else {
            None
        };

        CableWrapCheck {
            state,
            unwrapped_az: projected_unwrapped,
            distance_to_min,
            distance_to_max,
            safe_azimuth,
        }
    }

    /// Get the current unwrapped azimuth.
    pub fn get_unwrapped_azimuth(&self) -> f64 {
        self.unwrapped_azimuth
    }

    /// Check if a target position is safe considering cable wrap limits.
    /// Returns true if the target is within limits, false otherwise.
    pub fn is_position_safe(&self, target_azimuth: f64) -> bool {
        let check = self.check_cable_wrap(target_azimuth);
        check.state != CableWrapState::AtLimit
    }

    /// Create a safe horizontal position clamped to configured limits.
    /// This ensures the mount never receives commands that could cause:
    /// - Negative elevation (which can flip the mount)
    /// - Elevation above maximum limit
    ///
    /// # Arguments
    /// * `azimuth` - Desired azimuth in degrees
    /// * `altitude` - Desired altitude in degrees
    ///
    /// # Returns
    /// A HorizontalPosition with altitude clamped to safe limits
    pub fn safe_horizontal_position(&self, azimuth: f64, altitude: f64) -> HorizontalPosition {
        let safe_alt = altitude.clamp(self.config.alt_min_limit, self.config.alt_max_limit);
        let safe_az = azimuth.rem_euclid(360.0);

        if (safe_alt - altitude).abs() > 0.01 {
            log::debug!(
                "Altitude clamped from {:.2}° to {:.2}° (limits: {:.1}° to {:.1}°)",
                altitude,
                safe_alt,
                self.config.alt_min_limit,
                self.config.alt_max_limit
            );
        }

        HorizontalPosition::new(safe_az, safe_alt)
    }

    /// Check if an altitude value is within safe limits.
    /// Returns true if altitude is safe, false if it would be clamped.
    pub fn is_altitude_safe(&self, altitude: f64) -> bool {
        altitude >= self.config.alt_min_limit && altitude <= self.config.alt_max_limit
    }

    /// Set the observer location.
    ///
    /// # Arguments
    /// * `latitude` - Observer latitude in degrees (positive = North)
    /// * `longitude` - Observer longitude in degrees (positive = East)
    /// * `altitude` - Observer altitude in meters
    pub fn set_observer_location(&mut self, latitude: f64, longitude: f64, altitude: f64) {
        // space-dust uses km for altitude
        self.observer = GeodeticState::new(latitude, longitude, altitude / 1000.0);
        self.site_location = SiteLocation::new(latitude, longitude, altitude);
        log::info!(
            "Observer location set to lat={:.4}°, lon={:.4}°, alt={:.1}m",
            latitude,
            longitude,
            altitude
        );
    }

    /// Set the observer location from a SiteLocation.
    pub fn set_site_location(&mut self, location: SiteLocation) {
        self.set_observer_location(location.latitude, location.longitude, location.altitude);
    }

    /// Set tracking configuration.
    pub fn set_config(&mut self, config: TrackingConfig) {
        self.config = config;
    }

    /// Get the satellite name.
    pub fn get_name(&self) -> String {
        self.name
            .clone()
            .unwrap_or_else(|| format!("NORAD {}", self.tle.catalog_number()))
    }

    /// Get the NORAD catalog ID.
    pub fn get_norad_id(&self) -> i32 {
        self.tle.catalog_number().parse().unwrap_or(0)
    }

    /// Get the TLE epoch.
    pub fn get_tle_epoch(&self) -> DateTime<Utc> {
        self.tle.epoch()
    }

    /// Get the current tracking state.
    pub fn get_current_state(&mut self) -> MountResult<TrackingState> {
        let now = Utc::now();
        self.get_state_at_time(now)
    }

    /// Get the tracking state at a specific time.
    pub fn get_state_at_time(&mut self, time: DateTime<Utc>) -> MountResult<TrackingState> {
        // Propagate satellite to the given time
        let teme_state = self
            .tle
            .propagate(&time)
            .map_err(|e| MountError::satellite_error(e.to_string()))?;

        // Convert to ECI for observation calculations
        let eci_state = teme_state.to_eci();

        // Compute RA/Dec with rates from observer to satellite
        let ra_dec = Observations::compute_ra_dec_with_rates(&self.observer, &eci_state);

        // Extract values from RA/Dec observation
        let ra_deg = ra_dec.ra_deg();
        let dec_deg = ra_dec.dec_deg();
        let range = ra_dec.range.unwrap_or(0.0);
        let range_rate = ra_dec.range_rate.unwrap_or(0.0);

        // Convert RA rate and Dec rate from rad/s to arcsec/s
        // rad/s * (180/pi) * 3600 = arcsec/s
        let ra_rate_arcsec = ra_dec.right_ascension_rate.unwrap_or(0.0) * 206264.806;
        let dec_rate_arcsec = ra_dec.declination_rate.unwrap_or(0.0) * 206264.806;

        // Convert RA from degrees to hours
        let ra_hours = ra_deg / 15.0;

        let equatorial = EquatorialPosition::new(ra_hours, dec_deg);

        // Convert RA/Dec to Az/El using space-dust's conversion function
        // This properly handles the local sidereal time calculation
        let az_el = Observations::ra_dec_to_az_el(&ra_dec, &self.observer);
        let horizontal = HorizontalPosition::new(az_el.azimuth_deg(), az_el.elevation_deg());

        let state = TrackingState {
            equatorial,
            horizontal,
            ra_rate: ra_rate_arcsec,
            dec_rate: dec_rate_arcsec,
            range_km: range,
            range_rate_km_s: range_rate,
            timestamp: time,
            is_visible: horizontal.altitude > self.config.min_elevation,
        };

        self.last_state = Some(state.clone());
        self.last_update = Some(Instant::now());

        Ok(state)
    }

    /// Get the current position as EquatorialPosition.
    pub fn get_current_position(&mut self) -> MountResult<EquatorialPosition> {
        let state = self.get_current_state()?;
        Ok(state.equatorial)
    }

    /// Get the current horizontal position (Az/Alt).
    pub fn get_current_altaz(&mut self) -> MountResult<HorizontalPosition> {
        let state = self.get_current_state()?;
        Ok(state.horizontal)
    }

    /// Get current tracking rates.
    pub fn get_current_rates(&mut self) -> MountResult<TrackingRates> {
        let state = self.get_current_state()?;
        Ok(TrackingRates::new(state.ra_rate, state.dec_rate))
    }

    /// Check if the satellite is currently visible.
    pub fn is_visible(&mut self) -> MountResult<bool> {
        let state = self.get_current_state()?;
        Ok(state.is_visible)
    }

    /// Calculate the position with lead time compensation.
    ///
    /// This accounts for mount slew delay by calculating where the satellite
    /// will be slightly in the future.
    pub fn get_lead_position(&mut self) -> MountResult<(EquatorialPosition, TrackingRates)> {
        let future_time = Utc::now()
            + chrono::Duration::milliseconds((self.config.lead_time_sec * 1000.0) as i64);

        let state = self.get_state_at_time(future_time)?;
        Ok((
            state.equatorial,
            TrackingRates::new(state.ra_rate, state.dec_rate),
        ))
    }

    /// Find the next satellite pass.
    ///
    /// # Arguments
    /// * `start_time` - Start searching from this time
    /// * `search_hours` - How many hours to search ahead
    pub fn find_next_pass(
        &mut self,
        start_time: DateTime<Utc>,
        search_hours: f64,
    ) -> MountResult<Option<SatellitePass>> {
        let step_seconds = 60.0; // 1-minute steps for initial search
        let _fine_step_seconds = 1.0; // 1-second steps for refinement

        let mut current_time = start_time;
        let end_time = start_time + chrono::Duration::seconds((search_hours * 3600.0) as i64);

        let mut pass_start: Option<DateTime<Utc>> = None;
        let mut max_elevation = 0.0f64;
        let mut tca_time = start_time;
        let mut aos_azimuth = 0.0;

        while current_time < end_time {
            let state = self.get_state_at_time(current_time)?;

            if state.is_visible {
                if pass_start.is_none() {
                    // Refine AOS time
                    let refined_aos = self.refine_crossing_time(
                        current_time - chrono::Duration::seconds(step_seconds as i64),
                        current_time,
                        true,
                    )?;
                    pass_start = Some(refined_aos);

                    let aos_state = self.get_state_at_time(refined_aos)?;
                    aos_azimuth = aos_state.horizontal.azimuth;
                }

                if state.horizontal.altitude > max_elevation {
                    max_elevation = state.horizontal.altitude;
                    tca_time = current_time;
                }
            } else if pass_start.is_some() {
                // End of pass - refine LOS time
                let refined_los = self.refine_crossing_time(
                    current_time - chrono::Duration::seconds(step_seconds as i64),
                    current_time,
                    false,
                )?;

                let los_state = self.get_state_at_time(refined_los)?;

                return Ok(Some(SatellitePass {
                    name: self.get_name(),
                    norad_id: self.get_norad_id(),
                    aos_time: pass_start.unwrap(),
                    tca_time,
                    los_time: refined_los,
                    max_elevation,
                    aos_azimuth,
                    los_azimuth: los_state.horizontal.azimuth,
                }));
            }

            current_time = current_time + chrono::Duration::seconds(step_seconds as i64);
        }

        // Check if we're still in a pass at the end of search
        if let Some(aos) = pass_start {
            let final_state = self.get_state_at_time(end_time)?;
            return Ok(Some(SatellitePass {
                name: self.get_name(),
                norad_id: self.get_norad_id(),
                aos_time: aos,
                tca_time,
                los_time: end_time,
                max_elevation,
                aos_azimuth,
                los_azimuth: final_state.horizontal.azimuth,
            }));
        }

        Ok(None)
    }

    /// Find all passes within a time window.
    pub fn find_passes(
        &mut self,
        start_time: DateTime<Utc>,
        search_hours: f64,
        min_max_elevation: f64,
    ) -> MountResult<Vec<SatellitePass>> {
        let mut passes = Vec::new();
        let mut current_search_time = start_time;
        let end_time = start_time + chrono::Duration::seconds((search_hours * 3600.0) as i64);

        while current_search_time < end_time {
            let remaining_hours = (end_time - current_search_time).num_seconds() as f64 / 3600.0;

            if let Some(pass) = self.find_next_pass(current_search_time, remaining_hours)? {
                current_search_time = pass.los_time + chrono::Duration::seconds(60);

                if pass.max_elevation >= min_max_elevation {
                    passes.push(pass);
                }
            } else {
                break;
            }
        }

        Ok(passes)
    }

    /// Refine the horizon crossing time using binary search.
    fn refine_crossing_time(
        &mut self,
        before: DateTime<Utc>,
        after: DateTime<Utc>,
        rising: bool,
    ) -> MountResult<DateTime<Utc>> {
        let mut low = before;
        let mut high = after;

        for _ in 0..20 {
            // ~1ms precision
            let mid =
                low + chrono::Duration::milliseconds(((high - low).num_milliseconds() / 2) as i64);

            let state = self.get_state_at_time(mid)?;
            let is_visible = state.horizontal.altitude > self.config.min_elevation;

            if rising {
                if is_visible {
                    high = mid;
                } else {
                    low = mid;
                }
            } else {
                if is_visible {
                    low = mid;
                } else {
                    high = mid;
                }
            }

            if (high - low).num_milliseconds() < 100 {
                break;
            }
        }

        Ok(if rising { high } else { low })
    }

    /// Start continuous tracking on a mount.
    ///
    /// This method runs a tracking loop that continuously updates the mount
    /// position to follow the satellite using PID control in Az/Alt coordinates.
    ///
    /// If the satellite is below the horizon, it will wait for it to rise.
    /// The mount will pre-slew to the AOS position before the satellite becomes visible.
    ///
    /// # Arguments
    /// * `mount` - The mount to control
    ///
    /// # Returns
    /// The total duration of tracking
    pub fn track_pass(&mut self, mount: &mut dyn Mount) -> MountResult<Duration> {
        let start_time = Instant::now();

        // Ensure mount is ready
        if mount.is_parked()? {
            mount.unpark()?;
        }

        // Ensure mount is in Alt-Az mode for satellite tracking
        mount.set_altaz_mode()?;

        // Check current state
        let mut state = self.get_current_state()?;

        // If satellite is below horizon, wait for it to rise
        if !state.is_visible {
            log::info!(
                "Satellite {} is below horizon (El: {:.1}°), searching for next pass...",
                self.get_name(),
                state.horizontal.altitude
            );

            // Find the next pass
            let next_pass = self.find_next_pass(Utc::now(), 24.0)?;

            match next_pass {
                Some(pass) => {
                    log::info!(
                        "Next pass: AOS at {} UTC, Max El: {:.1}°",
                        pass.aos_time.format("%H:%M:%S"),
                        pass.max_elevation
                    );

                    // Calculate when to start slewing (pre_slew_sec before AOS)
                    let pre_slew_time = pass.aos_time
                        - chrono::Duration::milliseconds(
                            (self.config.pre_slew_sec * 1000.0) as i64,
                        );

                    // Wait until pre-slew time
                    loop {
                        let now = Utc::now();
                        if now >= pre_slew_time {
                            break;
                        }

                        let wait_secs = (pre_slew_time - now).num_seconds();
                        log::info!(
                            "Waiting for pre-slew time... {} seconds remaining",
                            wait_secs
                        );

                        // Sleep in chunks so we can be responsive
                        let sleep_ms = std::cmp::min(wait_secs as u64 * 1000, 5000);
                        std::thread::sleep(Duration::from_millis(sleep_ms));
                    }

                    // Get the predicted AOS position and slew there
                    let aos_state = self.get_state_at_time(pass.aos_time)?;

                    // Clamp altitude to safe limits (never go negative)
                    let safe_alt = aos_state
                        .horizontal
                        .altitude
                        .clamp(self.config.alt_min_limit, self.config.alt_max_limit);
                    let safe_pos = HorizontalPosition::new(aos_state.horizontal.azimuth, safe_alt);

                    log::info!(
                        "Pre-slewing to AOS position: Az {:.1}°, Alt {:.1}°",
                        safe_pos.azimuth,
                        safe_pos.altitude
                    );

                    mount.goto_altaz(safe_pos)?;

                    // Wait for slew to complete
                    while mount.is_slewing()? {
                        std::thread::sleep(Duration::from_millis(100));
                    }
                    log::info!("Pre-slew complete, waiting for AOS...");

                    // Wait for AOS
                    loop {
                        let now = Utc::now();
                        if now >= pass.aos_time {
                            break;
                        }
                        std::thread::sleep(Duration::from_millis(100));
                    }

                    // Update state after waiting
                    state = self.get_current_state()?;
                }
                None => {
                    log::warn!("No passes found in the next 24 hours");
                    return Err(MountError::BelowHorizon(state.horizontal.altitude));
                }
            }
        } else {
            // Satellite is already visible, slew to current position
            // Clamp altitude to safe limits (never go negative)
            let safe_alt = state
                .horizontal
                .altitude
                .clamp(self.config.alt_min_limit, self.config.alt_max_limit);
            let safe_pos = HorizontalPosition::new(state.horizontal.azimuth, safe_alt);

            log::info!(
                "Satellite {} is visible, slewing to current position: Az {:.1}°, Alt {:.1}°",
                self.get_name(),
                safe_pos.azimuth,
                safe_pos.altitude
            );

            mount.goto_altaz(safe_pos)?;

            // Wait for slew to complete
            while mount.is_slewing()? {
                std::thread::sleep(Duration::from_millis(100));
            }
        }

        // Initialize cable wrap tracking from current mount position
        let initial_mount_pos = mount.get_altaz()?;
        self.initialize_cable_wrap(initial_mount_pos.azimuth);

        log::info!(
            "Starting PID tracking of {} at Az {:.2}°, Alt {:.2}°",
            self.get_name(),
            state.horizontal.azimuth,
            state.horizontal.altitude
        );

        // Initialize PID controllers
        let mut pid_az = PidController::new(
            self.config.pid_kp_az,
            self.config.pid_ki_az,
            self.config.pid_kd_az,
        );
        let mut pid_alt = PidController::new(
            self.config.pid_kp_alt,
            self.config.pid_ki_alt,
            self.config.pid_kd_alt,
        );

        // Main tracking loop
        let update_interval = Duration::from_millis(self.config.update_interval_ms);
        let mut last_update = Instant::now();

        loop {
            let loop_start = Instant::now();
            let dt = last_update.elapsed().as_secs_f64();
            last_update = loop_start;

            // Get target satellite position (with lead time compensation)
            let target_state = self.get_state_at_time(
                Utc::now()
                    + chrono::Duration::milliseconds((self.config.lead_time_sec * 1000.0) as i64),
            )?;

            // Check if still visible (use configured minimum, not just 0)
            if target_state.horizontal.altitude < self.config.alt_min_limit {
                log::info!(
                    "Satellite {} below minimum elevation (El: {:.1}° < {:.1}°), stopping tracking",
                    self.get_name(),
                    target_state.horizontal.altitude,
                    self.config.alt_min_limit
                );
                break;
            }

            // Get current mount position
            let current_pos = mount.get_altaz()?;

            // Calculate errors (handle azimuth wrap-around)
            let mut az_error = target_state.horizontal.azimuth - current_pos.azimuth;
            // Normalize azimuth error to [-180, 180]
            if az_error > 180.0 {
                az_error -= 360.0;
            } else if az_error < -180.0 {
                az_error += 360.0;
            }
            let alt_error = target_state.horizontal.altitude - current_pos.altitude;

            // Calculate PID corrections (in deg/s)
            let az_correction = pid_az.update(az_error, dt);
            let alt_correction = pid_alt.update(alt_error, dt);

            // Apply corrections by slewing to corrected position
            // We compute target position for next update cycle
            let corrected_az = current_pos.azimuth
                + az_correction * (self.config.update_interval_ms as f64 / 1000.0);
            let corrected_alt = current_pos.altitude
                + alt_correction * (self.config.update_interval_ms as f64 / 1000.0);

            // Update cable wrap tracking
            self.update_unwrapped_azimuth(current_pos.azimuth);

            // Check cable wrap limits before moving
            let cable_check = self.check_cable_wrap(corrected_az);

            let final_az = match cable_check.state {
                CableWrapState::AtLimit => {
                    log::warn!(
                        "Cable wrap limit reached! Unwrapped Az: {:.1}°, Limits: [{:.1}°, {:.1}°]",
                        cable_check.unwrapped_az,
                        self.config.az_min_limit,
                        self.config.az_max_limit
                    );
                    log::warn!("Stopping tracking to prevent cable damage.");
                    // Stop tracking when at limit
                    break;
                }
                CableWrapState::Warning => {
                    log::warn!(
                        "Cable wrap WARNING: Approaching limit. Unwrapped Az: {:.1}°, Distance to limits: min={:.1}°, max={:.1}°",
                        cable_check.unwrapped_az,
                        cable_check.distance_to_min,
                        cable_check.distance_to_max
                    );
                    corrected_az.rem_euclid(360.0)
                }
                CableWrapState::Safe => corrected_az.rem_euclid(360.0),
            };

            // Clamp altitude to safe limits (NEVER go negative to prevent mount flip)
            let final_alt =
                corrected_alt.clamp(self.config.alt_min_limit, self.config.alt_max_limit);

            // Extra safety check - if we're trying to go to very low altitude, warn and stop
            if final_alt <= 0.0 {
                log::warn!(
                    "Altitude would go to {:.1}° (clamped from {:.1}°) - stopping to prevent mount flip",
                    final_alt,
                    corrected_alt
                );
                break;
            }

            // Log tracking status
            log::debug!(
                "Track: Target Az={:.2}° Alt={:.2}° | Current Az={:.2}° Alt={:.2}° | Error Az={:.3}° Alt={:.3}° | Unwrapped Az={:.1}°",
                target_state.horizontal.azimuth,
                target_state.horizontal.altitude,
                current_pos.azimuth,
                current_pos.altitude,
                az_error,
                alt_error,
                self.unwrapped_azimuth
            );

            let corrected_pos = HorizontalPosition::new(final_az, final_alt);

            // Send goto command (non-blocking micro-slews)
            mount.goto_altaz(corrected_pos)?;

            // Wait for next update
            let elapsed = loop_start.elapsed();
            if elapsed < update_interval {
                std::thread::sleep(update_interval - elapsed);
            }
        }

        // Stop tracking
        mount.tracking_off()?;

        let tracking_duration = start_time.elapsed();
        log::info!(
            "Tracking complete. Duration: {:.1}s, Final unwrapped Az: {:.1}°",
            tracking_duration.as_secs_f64(),
            self.unwrapped_azimuth
        );

        Ok(tracking_duration)
    }

    /// Perform a single tracking update (for custom tracking loops).
    ///
    /// Returns the updated tracking state.
    pub fn update_tracking(&mut self, mount: &mut dyn Mount) -> MountResult<TrackingState> {
        let (position, rates) = self.get_lead_position()?;
        let state = self.get_current_state()?;

        if !state.is_visible {
            return Err(MountError::BelowHorizon(state.horizontal.altitude));
        }

        if self.config.use_rate_tracking && rates.is_valid_for_satellite() {
            mount.set_custom_tracking_rates(rates)?;
        } else {
            mount.goto_equatorial(position)?;
        }

        Ok(state)
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    // ISS TLE for testing (valid format with correct checksums)
    const ISS_LINE1: &str = "1 25544U 98067A   26014.62805721  .00006818  00000+0  13044-3 0  9991";
    const ISS_LINE2: &str = "2 25544  51.6333 339.6562 0007763  17.9854 342.1408 15.49289811547943";

    #[test]
    fn test_create_tracker() {
        let result = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2);
        assert!(result.is_ok());

        let tracker = result.unwrap();
        assert_eq!(tracker.get_norad_id(), 25544);
    }

    #[test]
    fn test_create_tracker_with_name() {
        let result = SatelliteTracker::from_tle_with_name("ISS (ZARYA)", ISS_LINE1, ISS_LINE2);
        assert!(result.is_ok());

        let tracker = result.unwrap();
        assert_eq!(tracker.get_name(), "ISS (ZARYA)");
    }

    #[test]
    fn test_set_observer_location() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();
        tracker.set_observer_location(34.0522, -118.2437, 71.0);

        assert!((tracker.site_location.latitude - 34.0522).abs() < 0.0001);
        assert!((tracker.site_location.longitude - (-118.2437)).abs() < 0.0001);
    }

    #[test]
    fn test_tracking_config_default() {
        let config = TrackingConfig::default();
        assert!((config.min_elevation - DEFAULT_MIN_ELEVATION).abs() < 0.01);
        assert_eq!(config.update_interval_ms, DEFAULT_UPDATE_INTERVAL_MS);
    }

    #[test]
    fn test_satellite_pass_duration() {
        let pass = SatellitePass {
            name: "ISS".to_string(),
            norad_id: 25544,
            aos_time: Utc::now(),
            tca_time: Utc::now() + chrono::Duration::seconds(300),
            los_time: Utc::now() + chrono::Duration::seconds(600),
            max_elevation: 75.0,
            aos_azimuth: 180.0,
            los_azimuth: 45.0,
        };

        assert_eq!(pass.duration(), Duration::from_secs(600));
        assert!(pass.is_good_pass(45.0));
        assert!(!pass.is_good_pass(80.0));
    }

    #[test]
    fn test_get_state() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();
        tracker.set_observer_location(34.0522, -118.2437, 71.0);

        // Just verify we can get a state without errors
        let result = tracker.get_current_state();
        assert!(result.is_ok());

        let state = result.unwrap();
        // Range should be positive and reasonable for LEO satellite
        assert!(state.range_km > 0.0);
        assert!(state.range_km < 50000.0); // LEO satellite shouldn't be more than ~50000 km away
    }

    #[test]
    fn test_pid_controller() {
        let mut pid = PidController::new(1.0, 0.1, 0.5);

        // First update with error of 10 degrees
        let correction = pid.update(10.0, 0.1);
        // Should have a positive correction
        assert!(correction > 0.0);

        // Reset and verify it clears state
        pid.reset();
        assert_eq!(pid.integral, 0.0);
        assert_eq!(pid.prev_error, 0.0);
    }

    #[test]
    fn test_cable_wrap_initialization() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        // Initially not initialized
        assert!(!tracker.azimuth_initialized);

        // Initialize at 180 degrees
        tracker.initialize_cable_wrap(180.0);
        assert!(tracker.azimuth_initialized);
        assert!((tracker.unwrapped_azimuth - 180.0).abs() < 0.001);
    }

    #[test]
    fn test_cable_wrap_tracking() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();
        tracker.initialize_cable_wrap(0.0);

        // Move to 90 degrees (no wrap)
        tracker.update_unwrapped_azimuth(90.0);
        assert!((tracker.unwrapped_azimuth - 90.0).abs() < 0.001);

        // Move to 180 degrees
        tracker.update_unwrapped_azimuth(180.0);
        assert!((tracker.unwrapped_azimuth - 180.0).abs() < 0.001);

        // Move to 270 degrees
        tracker.update_unwrapped_azimuth(270.0);
        assert!((tracker.unwrapped_azimuth - 270.0).abs() < 0.001);

        // Move across 360->0 boundary to 10 degrees (should continue to ~370)
        tracker.update_unwrapped_azimuth(10.0);
        assert!((tracker.unwrapped_azimuth - 370.0).abs() < 0.001);
    }

    #[test]
    fn test_cable_wrap_reverse_tracking() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();
        tracker.initialize_cable_wrap(180.0);

        // Move backwards to 90 degrees
        tracker.update_unwrapped_azimuth(90.0);
        assert!((tracker.unwrapped_azimuth - 90.0).abs() < 0.001);

        // Move backwards across 0/360 boundary to 350 degrees (should be -10 from start)
        tracker.update_unwrapped_azimuth(350.0);
        assert!((tracker.unwrapped_azimuth - (-10.0)).abs() < 0.001);
    }

    #[test]
    fn test_cable_wrap_limit_check() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        // Set conservative limits
        let mut config = TrackingConfig::default();
        config.az_min_limit = -90.0;
        config.az_max_limit = 450.0;
        config.enable_cable_wrap_prevention = true;
        tracker.set_config(config);

        // Initialize at home (0 degrees)
        tracker.initialize_cable_wrap(0.0);

        // Check a safe position
        let check = tracker.check_cable_wrap(180.0);
        assert_eq!(check.state, CableWrapState::Safe);

        // Move close to the max limit by simulating multiple moves around the circle
        // Start at 0, move through 90, 180, 270, 360(0), 90, 180, 270, 360(0), 90 = 450
        tracker.update_unwrapped_azimuth(90.0);
        tracker.update_unwrapped_azimuth(180.0);
        tracker.update_unwrapped_azimuth(270.0);
        tracker.update_unwrapped_azimuth(0.0); // First wrap: 360
        tracker.update_unwrapped_azimuth(90.0); // 450

        // Now we're at unwrapped 450 (the limit), checking going further should be at limit
        let check = tracker.check_cable_wrap(100.0); // Would try to go to ~460
        assert_eq!(check.state, CableWrapState::AtLimit);
    }

    #[test]
    fn test_cable_wrap_warning() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        let mut config = TrackingConfig::default();
        config.az_min_limit = -90.0;
        config.az_max_limit = 450.0;
        config.enable_cable_wrap_prevention = true;
        tracker.set_config(config);

        // Initialize and move to near max limit by going around
        tracker.initialize_cable_wrap(0.0);
        tracker.update_unwrapped_azimuth(90.0);
        tracker.update_unwrapped_azimuth(180.0);
        tracker.update_unwrapped_azimuth(270.0);
        tracker.update_unwrapped_azimuth(0.0); // 360
        tracker.update_unwrapped_azimuth(60.0); // 420, within 30 of 450

        // Check current position - should be in warning zone
        let check = tracker.check_cable_wrap(70.0); // Would be ~430, still in warning
        assert_eq!(check.state, CableWrapState::Warning);
    }

    #[test]
    fn test_cable_wrap_disabled() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        let mut config = TrackingConfig::default();
        config.enable_cable_wrap_prevention = false;
        tracker.set_config(config);

        tracker.initialize_cable_wrap(0.0);

        // Even extreme positions should be "safe" when disabled
        let check = tracker.check_cable_wrap(1000.0);
        assert_eq!(check.state, CableWrapState::Safe);
    }

    #[test]
    fn test_is_position_safe() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        let mut config = TrackingConfig::default();
        config.az_min_limit = -90.0;
        config.az_max_limit = 450.0;
        config.enable_cable_wrap_prevention = true;
        tracker.set_config(config);

        tracker.initialize_cable_wrap(0.0);

        // Safe position
        assert!(tracker.is_position_safe(180.0));

        // Move to near limit by going around
        tracker.update_unwrapped_azimuth(90.0);
        tracker.update_unwrapped_azimuth(180.0);
        tracker.update_unwrapped_azimuth(270.0);
        tracker.update_unwrapped_azimuth(0.0); // 360
        tracker.update_unwrapped_azimuth(80.0); // 440

        // Position that would exceed limit (440 + ~20 to get to 100 = 460 > 450)
        assert!(!tracker.is_position_safe(100.0)); // Would be ~460 unwrapped
    }

    #[test]
    fn test_altitude_safety() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        // Default config has alt_min_limit = 0.0, alt_max_limit = 90.0
        assert!(tracker.is_altitude_safe(45.0));
        assert!(tracker.is_altitude_safe(0.0));
        assert!(tracker.is_altitude_safe(90.0));
        assert!(!tracker.is_altitude_safe(-5.0));
        assert!(!tracker.is_altitude_safe(95.0));

        // Set custom limits
        let mut config = TrackingConfig::default();
        config.alt_min_limit = 5.0;
        config.alt_max_limit = 85.0;
        tracker.set_config(config);

        assert!(tracker.is_altitude_safe(45.0));
        assert!(!tracker.is_altitude_safe(0.0)); // Below new minimum
        assert!(!tracker.is_altitude_safe(90.0)); // Above new maximum
        assert!(tracker.is_altitude_safe(5.0)); // At minimum
        assert!(tracker.is_altitude_safe(85.0)); // At maximum
    }

    #[test]
    fn test_safe_horizontal_position() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        let mut config = TrackingConfig::default();
        config.alt_min_limit = 5.0;
        config.alt_max_limit = 85.0;
        tracker.set_config(config);

        // Normal position - no clamping needed
        let pos = tracker.safe_horizontal_position(180.0, 45.0);
        assert!((pos.azimuth - 180.0).abs() < 0.001);
        assert!((pos.altitude - 45.0).abs() < 0.001);

        // Negative altitude - should be clamped to minimum
        let pos = tracker.safe_horizontal_position(90.0, -10.0);
        assert!((pos.azimuth - 90.0).abs() < 0.001);
        assert!((pos.altitude - 5.0).abs() < 0.001); // Clamped to min

        // Too high altitude - should be clamped to maximum
        let pos = tracker.safe_horizontal_position(270.0, 95.0);
        assert!((pos.azimuth - 270.0).abs() < 0.001);
        assert!((pos.altitude - 85.0).abs() < 0.001); // Clamped to max

        // Azimuth normalization
        let pos = tracker.safe_horizontal_position(450.0, 45.0);
        assert!((pos.azimuth - 90.0).abs() < 0.001); // 450 % 360 = 90

        let pos = tracker.safe_horizontal_position(-90.0, 45.0);
        assert!((pos.azimuth - 270.0).abs() < 0.001); // -90 + 360 = 270
    }

    #[test]
    fn test_safe_horizontal_position_prevents_flip() {
        let mut tracker = SatelliteTracker::from_tle(ISS_LINE1, ISS_LINE2).unwrap();

        // With default limits (0 to 90), negative altitude is clamped to 0
        let pos = tracker.safe_horizontal_position(180.0, -30.0);
        assert!(pos.altitude >= 0.0);

        // Set a safety margin minimum
        let mut config = TrackingConfig::default();
        config.alt_min_limit = 5.0;
        tracker.set_config(config);

        // Now even 0 would be clamped to 5
        let pos = tracker.safe_horizontal_position(180.0, 0.0);
        assert!((pos.altitude - 5.0).abs() < 0.001);

        // Negative altitude definitely clamped
        let pos = tracker.safe_horizontal_position(180.0, -45.0);
        assert!((pos.altitude - 5.0).abs() < 0.001);
    }
}