autopilot PD controller for generic flight Sim

use crate::flight_model::FlightModel;
use crate::geodetic::{bearing_measured_via_great_circle, distance_measured_via_great_circle};
use tracing::{debug, info, error};

pub fn update_autopilot(fm: &mut FlightModel, dt: f64) {
    // Handle waypoint navigation first - it may override autopilot targets
    if fm.waypoint_enabled {
        update_waypoint_navigation(fm, dt);
    }

    // Only run autopilot if it's enabled
    if !fm.autopilot_enabled {
        return;
    }

    // ────────────────────────────────
    // ALTITUDE CONTROL (PD controller)
    // ────────────────────────────────
    const KP_ALT: f64        = 0.08;  // proportional gain (ft → control units)
    const KD_ALT: f64        = 0.04;  // derivative gain (ft/s → control units)
    const ALT_DEADBAND: f64  = 15.0;  // feet
    const MAX_VERT_RATE: f64 = 20.0;  // max climb/descent rate (control units)

    let alt_err = fm.target_altitude - fm.airplane_z;
    let climb_cmd = if alt_err.abs() > ALT_DEADBAND {
        let derr = (alt_err - fm.autopilot_prev_alt_error) / dt;
        let u = KP_ALT * alt_err + KD_ALT * derr;
        fm.autopilot_prev_alt_error = alt_err;
        u.clamp(-MAX_VERT_RATE, MAX_VERT_RATE)
    } else {
        0.0    };
    fm.up_down = -climb_cmd;

    // ────────────────────────────────
    // HEADING CONTROL ( PD with deadband and gain scheduling)
    // ────────────────────────────────
    const KP_HDG: f64        = 0.6;   // proportional gain (degrees → control units)
    const KD_HDG: f64        = 0.3;   // derivative gain (deg/s → control units)
    const HDG_DEADBAND: f64  = 0.7;   // degrees
    const MAX_ROLL_RATE: f64 = 3.0;   // max roll rate in deg/sec

    // Reduce gains when close to target to prevent oscillation
    const FINE_TUNE_THRESHOLD: f64 = 3.0; // degrees - when to start reducing gains
    const DERIVATIVE_FILTER_ALPHA: f64 = 0.8; // low-pass filter for derivative term

    let hdg = fm.compass.to_degrees().rem_euclid(360.0);
      // Calculate heading error - use waypoint bearing if in waypoint mode, otherwise use autopilot target
    let hdg_err = if fm.waypoint_enabled {
        // In waypoint mode, use the full error to the waypoint bearing
        (fm.waypoint_target_heading - hdg + 540.0) % 360.0 - 180.0
    } else {
        // Normal autopilot mode - use target heading
        (fm.target_heading - hdg + 540.0) % 360.0 - 180.0
    };    // Always update previous error for derivative term, but filter it to reduce noise
    let raw_derr = (hdg_err - fm.autopilot_prev_hdg_error) / dt;
    let filtered_derr = DERIVATIVE_FILTER_ALPHA * fm.autopilot_filtered_hdg_deriv +
                       (1.0 - DERIVATIVE_FILTER_ALPHA) * raw_derr;
    fm.autopilot_filtered_hdg_deriv = filtered_derr;
    fm.autopilot_prev_hdg_error = hdg_err;      let roll_cmd = if fm.waypoint_enabled {
        // Two-stage control
        // 1. Heading controller determines desired bank angle
        // 2. Bank angle controller executes the bank

        let current_bank_deg = fm.side_tilt.to_degrees();

        // Stage 1: Heading controller - determine desired bank angle
        let desired_bank_deg = if hdg_err.abs() < HDG_DEADBAND {
            // Close to target heading - level wings
            0.0
        } else {
            // Calculate bank angle for turn
            // Typically 1:1 ratio up to max bank (15-25°)
            let max_bank = 20.0;
            let bank_cmd = hdg_err.clamp(-max_bank, max_bank);

            // Reduce bank angle as we approach target to prevent overshoot
            if hdg_err.abs() < 10.0 {
                bank_cmd * 0.5 // Gentle approach
            } else {
                bank_cmd
            }
        };

        // Stage 2: Bank angle controller - execute the bank
        let bank_error = desired_bank_deg - current_bank_deg;

        // Simple proportional controller for bank angle
        // Uses high gain for bank control (typically 2-5)
        let bank_gain = 3.0;
        let roll_cmd = bank_error * bank_gain;

        // Limit roll rate
        roll_cmd.clamp(-MAX_ROLL_RATE, MAX_ROLL_RATE)
    } else {
        // Normal autopilot mode with improved anti-oscillation
        if hdg_err.abs() > HDG_DEADBAND {
            // Apply gain scheduling: reduce gains when close to target
            let gain_factor = if hdg_err.abs() < FINE_TUNE_THRESHOLD {
                // Reduce gains when within fine-tune threshold to prevent oscillation
                0.3 + 0.7 * (hdg_err.abs() / FINE_TUNE_THRESHOLD)
            } else {
                1.0
            };

            let kp_adjusted = KP_HDG * gain_factor;
            let kd_adjusted = KD_HDG * gain_factor;

            let u = kp_adjusted * hdg_err + kd_adjusted * filtered_derr;
            u.clamp(-MAX_ROLL_RATE, MAX_ROLL_RATE)
        } else {
            0.0
        }
    };// Debug output for waypoint mode
    if fm.waypoint_enabled {
        let current_bank_deg = fm.side_tilt.to_degrees();

        // Calculate what the desired bank should be for debug
        let desired_bank_deg = if hdg_err.abs() < HDG_DEADBAND {
            0.0
        } else {
            let max_bank = 20.0;
            let bank_cmd = hdg_err.clamp(-max_bank, max_bank);

            if hdg_err.abs() < 10.0 {
                bank_cmd * 0.5
            } else {
                bank_cmd
            }
        };

        let bank_error = desired_bank_deg - current_bank_deg;

        let status = if hdg_err.abs() < HDG_DEADBAND {
            "ON_HEADING"
        } else if bank_error.abs() > 5.0 {
            "BANKING"
        } else {
            "FINE_TUNING"
        };
          debug!("Autopilot: hdg_err={:.1}°, roll_cmd={:.2}, current_bank={:.1}°, desired_bank={:.1}°, bank_err={:.1}°, status={}",
                 hdg_err, roll_cmd, current_bank_deg, desired_bank_deg, bank_error, status);
    }

    fm.left_right = -roll_cmd;
}

fn update_waypoint_navigation(fm: &mut FlightModel, _dt: f64) {
    // Update waypoint logic
    update_waypoint_navigation_internal(fm);

    if !fm.waypoint_enabled || fm.waypoints.is_empty() {
        return;
    }

    // Ensure autopilot is enabled for waypoint navigation
    if !fm.autopilot_enabled {
        fm.autopilot_enabled = true;
    }    // Update targets for current waypoint
    if let Some(waypoint) = fm.waypoints.get(fm.current_waypoint_index) {
        // Set altitude target
        fm.target_altitude = waypoint.alt;

        // Calculate desired bearing to waypoint using great circle
        let desired_bearing_deg = bearing_measured_via_great_circle(
            fm.current_lat, fm.current_lon,
            waypoint.lat, waypoint.lon
        );

        // Store the actual waypoint bearing for HUD display and autopilot control
        fm.waypoint_target_heading = desired_bearing_deg;

        debug!("Waypoint bearing: {:.0}° (from {:.6},{:.6} to {:.6},{:.6})",
                 desired_bearing_deg, fm.current_lat, fm.current_lon, waypoint.lat, waypoint.lon);
    }
}

// Autopilot control
pub fn toggle_autopilot(fm: &mut FlightModel) {
    fm.autopilot_enabled = !fm.autopilot_enabled;
    if fm.autopilot_enabled {
        // Capture current altitude and heading when enabling autopilot
        fm.target_altitude = fm.airplane_z;
        // Convert compass radians to degrees for easier handling
        fm.target_heading = (fm.compass * 57.2957795131).rem_euclid(360.0);
        info!("Autopilot ENABLED - Target altitude: {:.0} ft, Target heading: {:.0}°",
                    fm.target_altitude, fm.target_heading);
    } else {
        info!("Autopilot DISABLED");
    }
}

// Waypoint navigation control
pub fn toggle_waypoint(fm: &mut FlightModel) {
    fm.waypoint_enabled = !fm.waypoint_enabled;
    if fm.waypoint_enabled {
        if !fm.waypoints.is_empty() {
            // Reset to first waypoint when enabling
            fm.current_waypoint_index = 0;
            navigate_to_current_waypoint(fm);
            info!("Waypoint navigation ENABLED - Going to waypoint 1 of {}", fm.waypoints.len());
        } else {
            fm.waypoint_enabled = false;
            info!("No waypoints loaded - waypoint navigation disabled");
        }
    } else {
        // When disabling waypoint navigation, reset autopilot to current heading/altitude
        if fm.autopilot_enabled {
            fm.target_altitude = fm.airplane_z;
            fm.target_heading = (fm.compass * 57.2957795131).rem_euclid(360.0);
            info!("Waypoint navigation DISABLED - Autopilot set to hold current heading {:.0}° and altitude {:.0}ft",
                        fm.target_heading, fm.target_altitude);
        } else {
            info!("Waypoint navigation DISABLED");
        }
    }
}
pub fn set_waypoints(fm: &mut FlightModel, waypoints: Vec<crate::config::Waypoint>) {
    fm.waypoints = waypoints;
    fm.current_waypoint_index = 0;
    info!("Loaded {} waypoints", fm.waypoints.len());
}

fn navigate_to_current_waypoint(fm: &mut FlightModel) {
    if let Some(waypoint) = fm.waypoints.get(fm.current_waypoint_index) {
        debug!("Navigating to waypoint {} at ({:.6}°, {:.6}°, {:.0} ft)",
                    fm.current_waypoint_index + 1, waypoint.lat, waypoint.lon, waypoint.alt);
    }
}

pub fn update_waypoint_navigation_internal(fm: &mut FlightModel) {
    if !fm.waypoint_enabled || fm.waypoints.is_empty() {
        return;
    }

    if let Some(current_waypoint) = fm.waypoints.get(fm.current_waypoint_index) {
        // Check if we're within waypoint threshold using great circle distance
        let distance_nm = distance_measured_via_great_circle(
            fm.current_lat, fm.current_lon,
            current_waypoint.lat, current_waypoint.lon
        );
        let altitude_diff = (current_waypoint.alt - fm.airplane_z).abs();
            // Waypoint reached if within 0.2 nm horizontally and 300 ft vertically
        if distance_nm <= 0.2 && altitude_diff <= 300.0 {
            error!("Reached waypoint {} at ({:.6}°, {:.6}°, {:.0} ft)",
                        fm.current_waypoint_index + 1, current_waypoint.lat, current_waypoint.lon, current_waypoint.alt);

            // Move to next waypoint
            fm.current_waypoint_index += 1;

            if fm.current_waypoint_index >= fm.waypoints.len() {
                // All waypoints completed
                error!("All waypoints completed! Disabling waypoint navigation.");
                fm.waypoint_enabled = false;
            } else {
                // Navigate to next waypoint
                navigate_to_current_waypoint(fm);
            }
        }
    }
}

/// Calculate distance to current waypoint in nautical miles
pub fn distance_to_current_waypoint(fm: &FlightModel) -> Option<f64> {
    if !fm.waypoint_enabled || fm.current_waypoint_index >= fm.waypoints.len() {
        return None;
    }

    let waypoint = &fm.waypoints[fm.current_waypoint_index];

    // Use great circle distance since waypoints are now in lat/lon
    Some(distance_measured_via_great_circle(
        fm.current_lat, fm.current_lon,
        waypoint.lat, waypoint.lon
    ))
}