zwo_mount_control 0.2.1

//! 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};
use crate::protocol::{Direction, SlewRate};

/// Tracking mode for satellite tracking
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub enum TrackingMode {
    /// Use goto_altaz commands for micro-slews (original behavior)
    #[default]
    Goto,
    /// Use guide pulses (too slow for LEO satellites)
    GuidePulse,
    /// Use continuous motion commands with slew rate control (best for LEO)
    Motion,
}

/// 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,
    /// Tracking mode selection
    pub tracking_mode: TrackingMode,
    /// Use guide pulses for PID tracking instead of goto commands.
    /// DEPRECATED: Use tracking_mode = TrackingMode::GuidePulse instead
    pub use_guide_pulse_tracking: bool,
    /// Guide rate to use for guide pulse tracking (fraction of sidereal, 0.1-0.9)
    /// Higher values allow faster corrections but may be less precise.
    pub guide_rate: f64,
    /// Maximum guide pulse duration in milliseconds (1-9999)
    /// Limits the maximum correction per update cycle.
    pub max_guide_pulse_ms: u32,
    /// Minimum error threshold (degrees) before applying corrections.
    /// Helps prevent oscillation from tiny corrections.
    pub guide_pulse_deadband: f64,
    /// Slew rate for motion tracking mode (0-9, where 9 is fastest)
    /// Rate 6 (FIND) is good for LEO, rate 9 (MAX) for very fast satellites
    pub motion_slew_rate: u8,
    /// Error threshold to start motion (degrees)
    /// Below this, no motion commands are sent
    pub motion_start_threshold: f64,
    /// Error threshold to stop motion (degrees)
    /// When error drops below this, motion stops (should be < start_threshold)
    pub motion_stop_threshold: f64,
}

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,
            // Tracking mode - Motion is best for LEO satellites
            tracking_mode: TrackingMode::default(),
            // Guide pulse tracking settings (legacy, use tracking_mode instead)
            use_guide_pulse_tracking: false,
            guide_rate: 0.9,
            max_guide_pulse_ms: 1000,
            guide_pulse_deadband: 0.01,
            // Motion tracking settings for LEO satellites
            motion_slew_rate: 6, // FIND rate - good balance of speed/precision
            motion_start_threshold: 0.1, // Start moving when error > 0.1°
            motion_stop_threshold: 0.05, // Stop moving when error < 0.05°
        }
    }
}

/// 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()?;

        // Set maximum slew rate for fast initial slew to satellite position
        log::info!("Setting maximum slew rate for initial slew");
        mount.set_slew_rate(SlewRate::MAX)?;

        // 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);

        // Determine effective tracking mode
        let tracking_mode = if self.config.use_guide_pulse_tracking {
            TrackingMode::GuidePulse // Legacy compatibility
        } else {
            self.config.tracking_mode
        };

        // Set up for the selected tracking mode
        match tracking_mode {
            TrackingMode::GuidePulse => {
                log::info!(
                    "Setting guide rate to {:.1}x sidereal for guide pulse tracking",
                    self.config.guide_rate
                );
                mount.set_guide_rate(self.config.guide_rate)?;
            }
            TrackingMode::Motion => {
                log::info!(
                    "Setting slew rate to {} for motion tracking",
                    self.config.motion_slew_rate
                );
                mount.set_slew_rate(SlewRate::new(self.config.motion_slew_rate))?;
            }
            TrackingMode::Goto => {
                // No special setup needed for goto mode
            }
        }

        log::info!(
            "Starting {:?} tracking of {} at Az {:.2}°, Alt {:.2}°",
            tracking_mode,
            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,
        );

        // For guide pulse tracking, adjust PID output limits based on guide rate
        // Guide pulses have different dynamics than goto commands
        if self.config.use_guide_pulse_tracking {
            // Sidereal rate is ~15 arcsec/sec = 0.00417 deg/sec
            // Guide rate is a multiplier of this
            // We want PID output in degrees, then convert to pulse duration
            pid_az.output_limit = 10.0; // Allow larger corrections for fast LEO
            pid_alt.output_limit = 10.0;
        }

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

        log::info!(
            "PID config: Kp_az={:.2}, Ki_az={:.3}, Kd_az={:.2}, Kp_alt={:.2}, Ki_alt={:.3}, Kd_alt={:.2}",
            self.config.pid_kp_az,
            self.config.pid_ki_az,
            self.config.pid_kd_az,
            self.config.pid_kp_alt,
            self.config.pid_ki_alt,
            self.config.pid_kd_alt
        );
        log::info!(
            "Tracking config: update_interval={}ms, lead_time={:.2}s, mode={:?}",
            self.config.update_interval_ms,
            self.config.lead_time_sec,
            tracking_mode
        );
        if tracking_mode == TrackingMode::Motion {
            log::info!(
                "Motion config: slew_rate={}, start_threshold={:.3}°, stop_threshold={:.3}°",
                self.config.motion_slew_rate,
                self.config.motion_start_threshold,
                self.config.motion_stop_threshold
            );
        }

        // Track current motion state for Motion mode
        let mut az_moving: Option<Direction> = None;
        let mut alt_moving: Option<Direction> = None;

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

            // 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;

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

            // Check cable wrap limits before moving
            let cable_check = self.check_cable_wrap(current_pos.azimuth + az_error);

            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.");
                    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
                    );
                }
                CableWrapState::Safe => {}
            }

            // Log tracking status - use info level every 20 iterations for visibility
            if loop_count % 20 == 1 {
                log::info!(
                    "[{}] Target Az={:.2}° Alt={:.2}° | Mount Az={:.2}° Alt={:.2}° | Error Az={:.3}° Alt={:.3}° | dt={:.3}s",
                    loop_count,
                    target_state.horizontal.azimuth,
                    target_state.horizontal.altitude,
                    current_pos.azimuth,
                    current_pos.altitude,
                    az_error,
                    alt_error,
                    dt
                );
            } else {
                log::debug!(
                    "[{}] Target Az={:.2}° Alt={:.2}° | Mount Az={:.2}° Alt={:.2}° | Error Az={:.3}° Alt={:.3}° | dt={:.3}s",
                    loop_count,
                    target_state.horizontal.azimuth,
                    target_state.horizontal.altitude,
                    current_pos.azimuth,
                    current_pos.altitude,
                    az_error,
                    alt_error,
                    dt
                );
            }

            if tracking_mode == TrackingMode::Motion {
                // Motion tracking mode - use continuous move commands
                // This is much faster than guide pulses for LEO satellites

                // Azimuth control
                let az_abs_error = az_error.abs();
                let desired_az_dir = if az_error > 0.0 {
                    Some(Direction::East)
                } else if az_error < 0.0 {
                    Some(Direction::West)
                } else {
                    None
                };

                // Determine if we should be moving in azimuth
                let should_move_az = az_abs_error > self.config.motion_start_threshold
                    || (az_moving.is_some() && az_abs_error > self.config.motion_stop_threshold);

                if should_move_az {
                    if let Some(dir) = desired_az_dir {
                        // Check if we need to change direction or start moving
                        if az_moving != Some(dir) {
                            // Stop current motion if moving opposite direction
                            if let Some(old_dir) = az_moving {
                                log::debug!(
                                    "[{}] Az STOP: {} (changing direction)",
                                    loop_count,
                                    old_dir
                                );
                                mount.stop_axis(old_dir)?;
                            }
                            // Start moving in new direction
                            log::info!(
                                "[{}] Az MOVE: {} (error={:.3}°, threshold={:.3}°)",
                                loop_count,
                                dir,
                                az_error,
                                self.config.motion_start_threshold
                            );
                            mount.move_axis(dir)?;
                            az_moving = Some(dir);
                        }
                    }
                } else if az_moving.is_some() {
                    // Error is small enough, stop azimuth motion
                    let dir = az_moving.unwrap();
                    log::info!(
                        "[{}] Az STOP: {} (error={:.3}° < threshold={:.3}°)",
                        loop_count,
                        dir,
                        az_abs_error,
                        self.config.motion_stop_threshold
                    );
                    mount.stop_axis(dir)?;
                    az_moving = None;
                }

                // Altitude control
                let alt_abs_error = alt_error.abs();
                let desired_alt_dir = if alt_error > 0.0 {
                    Some(Direction::North)
                } else if alt_error < 0.0 {
                    Some(Direction::South)
                } else {
                    None
                };

                // Determine if we should be moving in altitude
                let should_move_alt = alt_abs_error > self.config.motion_start_threshold
                    || (alt_moving.is_some() && alt_abs_error > self.config.motion_stop_threshold);

                // Safety check for altitude
                let alt_safe = !(desired_alt_dir == Some(Direction::South)
                    && current_pos.altitude <= self.config.alt_min_limit + 1.0);

                if should_move_alt && alt_safe {
                    if let Some(dir) = desired_alt_dir {
                        // Check if we need to change direction or start moving
                        if alt_moving != Some(dir) {
                            // Stop current motion if moving opposite direction
                            if let Some(old_dir) = alt_moving {
                                log::debug!(
                                    "[{}] Alt STOP: {} (changing direction)",
                                    loop_count,
                                    old_dir
                                );
                                mount.stop_axis(old_dir)?;
                            }
                            // Start moving in new direction
                            log::info!(
                                "[{}] Alt MOVE: {} (error={:.3}°, threshold={:.3}°)",
                                loop_count,
                                dir,
                                alt_error,
                                self.config.motion_start_threshold
                            );
                            mount.move_axis(dir)?;
                            alt_moving = Some(dir);
                        }
                    }
                } else if alt_moving.is_some() {
                    // Error is small enough or unsafe, stop altitude motion
                    let dir = alt_moving.unwrap();
                    log::info!(
                        "[{}] Alt STOP: {} (error={:.3}° < threshold={:.3}° or safety)",
                        loop_count,
                        dir,
                        alt_abs_error,
                        self.config.motion_stop_threshold
                    );
                    mount.stop_axis(dir)?;
                    alt_moving = None;
                }
            } else if tracking_mode == TrackingMode::GuidePulse {
                // Guide pulse tracking mode - faster response for LEO satellites
                // Calculate PID corrections (output is in degrees of error to correct)
                let az_correction = pid_az.update(az_error, dt);
                let alt_correction = pid_alt.update(alt_error, dt);

                log::debug!(
                    "[{}] PID output: az_correction={:.4}°, alt_correction={:.4}° (limits: ±{:.1}°)",
                    loop_count,
                    az_correction,
                    alt_correction,
                    pid_az.output_limit
                );

                // Convert corrections to guide pulse durations
                // Guide rate is fraction of sidereal (15 arcsec/sec = 0.00417 deg/sec)
                // So at guide_rate = 0.9, we move at 0.9 * 0.00417 = 0.00375 deg/sec
                // But in Alt-Az mode, guide pulses move the physical axes directly
                // Typical guide speed is around 0.5-1 deg/sec at max guide rate
                // We'll use an empirical conversion factor that can be tuned
                let guide_speed_deg_per_sec = self.config.guide_rate * 1.0; // ~1 deg/sec at max rate

                // Apply azimuth correction via guide pulses (East/West)
                if az_correction.abs() > self.config.guide_pulse_deadband {
                    // Calculate pulse duration: time = distance / speed
                    let az_pulse_ms_raw =
                        ((az_correction.abs() / guide_speed_deg_per_sec) * 1000.0) as u32;
                    let az_pulse_ms = az_pulse_ms_raw.min(self.config.max_guide_pulse_ms).max(1);

                    // Determine direction: positive error means target is East of current
                    // In Alt-Az mode: East pulse moves mount East (increases Az)
                    let az_direction = if az_correction > 0.0 {
                        Direction::East
                    } else {
                        Direction::West
                    };

                    log::info!(
                        "[{}] Az PULSE: {} {}ms (raw: {}ms) | error={:.3}° correction={:.3}° speed={:.3}°/s",
                        loop_count,
                        az_direction,
                        az_pulse_ms,
                        az_pulse_ms_raw,
                        az_error,
                        az_correction,
                        guide_speed_deg_per_sec
                    );
                    mount.guide_pulse(az_direction, az_pulse_ms)?;
                } else {
                    log::debug!(
                        "[{}] Az SKIP: correction {:.4}° below deadband {:.4}°",
                        loop_count,
                        az_correction.abs(),
                        self.config.guide_pulse_deadband
                    );
                }

                // Apply altitude correction via guide pulses (North/South)
                if alt_correction.abs() > self.config.guide_pulse_deadband {
                    // Calculate pulse duration
                    let alt_pulse_ms_raw =
                        ((alt_correction.abs() / guide_speed_deg_per_sec) * 1000.0) as u32;
                    let alt_pulse_ms = alt_pulse_ms_raw.min(self.config.max_guide_pulse_ms).max(1);

                    // Determine direction: positive error means target is higher
                    // In Alt-Az mode: North pulse increases altitude
                    let alt_direction = if alt_correction > 0.0 {
                        Direction::North
                    } else {
                        Direction::South
                    };

                    // Safety check - don't pulse if it would take us below minimum altitude
                    if alt_direction == Direction::South
                        && current_pos.altitude <= self.config.alt_min_limit + 1.0
                    {
                        log::warn!(
                            "[{}] Alt BLOCKED: South pulse blocked - altitude {:.1}° near minimum limit {:.1}°",
                            loop_count,
                            current_pos.altitude,
                            self.config.alt_min_limit
                        );
                    } else {
                        log::info!(
                            "[{}] Alt PULSE: {} {}ms (raw: {}ms) | error={:.3}° correction={:.3}° speed={:.3}°/s",
                            loop_count,
                            alt_direction,
                            alt_pulse_ms,
                            alt_pulse_ms_raw,
                            alt_error,
                            alt_correction,
                            guide_speed_deg_per_sec
                        );
                        mount.guide_pulse(alt_direction, alt_pulse_ms)?;
                    }
                } else {
                    log::debug!(
                        "[{}] Alt SKIP: correction {:.4}° below deadband {:.4}°",
                        loop_count,
                        alt_correction.abs(),
                        self.config.guide_pulse_deadband
                    );
                }
            } else {
                // Goto tracking mode - original behavior using micro-slews
                // Calculate PID corrections (in deg/s)
                let az_correction = pid_az.update(az_error, dt);
                let alt_correction = pid_alt.update(alt_error, dt);

                log::debug!(
                    "[{}] PID output: az_correction={:.4}°/s, alt_correction={:.4}°/s (limits: ±{:.1}°/s)",
                    loop_count,
                    az_correction,
                    alt_correction,
                    pid_az.output_limit
                );

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

                // Normalize azimuth to [0, 360)
                let final_az = 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;
                }

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

                // Log goto command every 20 iterations
                if loop_count % 20 == 1 {
                    log::info!(
                        "[{}] GOTO: Az {:.3}° -> {:.3}° (Δ{:.4}°), Alt {:.3}° -> {:.3}° (Δ{:.4}°)",
                        loop_count,
                        current_pos.azimuth,
                        final_az,
                        az_delta,
                        current_pos.altitude,
                        final_alt,
                        alt_delta
                    );
                }

                // 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 all motion and tracking
        if tracking_mode == TrackingMode::Motion {
            log::info!("Stopping all axis motion");
            mount.stop_all()?;
        }
        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);
    }

    #[test]
    fn test_guide_pulse_tracking_config() {
        // Test default configuration
        let config = TrackingConfig::default();
        assert!(!config.use_guide_pulse_tracking); // Disabled by default
        assert!((config.guide_rate - 0.9).abs() < 0.001);
        assert_eq!(config.max_guide_pulse_ms, 1000);
        assert!((config.guide_pulse_deadband - 0.01).abs() < 0.001);

        // Test LEO-optimized configuration
        let leo_config = TrackingConfig {
            use_guide_pulse_tracking: true,
            guide_rate: 0.9,
            max_guide_pulse_ms: 500,
            guide_pulse_deadband: 0.02,
            update_interval_ms: 50, // Faster updates for LEO
            lead_time_sec: 0.3,     // Shorter lead time
            pid_kp_az: 3.0,         // Higher P gain
            pid_ki_az: 0.05,        // Lower I gain
            pid_kd_az: 0.3,
            pid_kp_alt: 3.0,
            pid_ki_alt: 0.05,
            pid_kd_alt: 0.3,
            ..TrackingConfig::default()
        };

        assert!(leo_config.use_guide_pulse_tracking);
        assert_eq!(leo_config.max_guide_pulse_ms, 500);
        assert_eq!(leo_config.update_interval_ms, 50);
    }

    #[test]
    fn test_pid_controller_output_limits() {
        let mut pid = PidController::new(10.0, 0.0, 0.0); // High P gain

        // Default output limit is 5.0
        assert!((pid.output_limit - 5.0).abs() < 0.001);

        // Large error should be clamped to output limit
        let correction = pid.update(100.0, 0.1);
        assert!(correction <= 5.0);
        assert!(correction >= -5.0);

        // Test with custom output limit (as used in guide pulse mode)
        pid.output_limit = 10.0;
        pid.reset();
        let correction = pid.update(100.0, 0.1);
        assert!(correction <= 10.0);
    }
}