// flight_model.rs
use crate::aircraft::AircraftConfig;
use crate::consts::PHYSICS_DT;

#[derive(Clone)]
pub struct FlightModel {
    // Airplane position in flight model coordinate system
    // Note: airplane_x = North direction, airplane_y = East direction
    // This differs from standard ENU where X=East, Y=North
    pub airplane_x: f64,  // North position (feet)
    pub airplane_y: f64,  // East position (feet)
    pub airplane_z: f64,  // Altitude (feet)

    // Angles
    pub side_tilt: f64,      // sideTiltRadians in banks.c
    pub forward_tilt: f64,   // forwardTiltRadians in banks.c
    pub compass: f64,        // compassRadians in banks.c

    // Flight parameters
    pub speed: f64,         // target speed (throttle setting)
    pub speed_feet: f64,    // current speed in feet/sec    // User inputs
    pub left_right: f64,    // for turning
    pub up_down: f64,       // for pitch

    // Autopilot system
    pub autopilot_enabled: bool,
    pub target_altitude: f64,
    pub target_heading: f64,
    // new state for the PD controller
    pub autopilot_prev_hdg_error: f64,
    pub autopilot_prev_alt_error: f64,
    pub autopilot_filtered_hdg_deriv: f64,  // Filtered derivative term to reduce noise

    // Waypoint navigation
    pub waypoint_enabled: bool,
    pub waypoints: Vec<crate::config::Waypoint>,
    pub current_waypoint_index: usize,
    pub waypoint_target_heading: f64,  // The actual bearing to waypoint (for HUD display)    // HUD display options
    pub show_coordinates: bool,  // Toggle coordinate display with X key
    pub show_scene_labels: bool, // Toggle scene origin labels with M key

    // Global coordinate system (set by event loop during recentering)
    pub current_lat: f64,
    pub current_lon: f64,

    // Reference coordinates for ENU conversion (set by event loop)
    pub ref_lat: f64,
    pub ref_lon: f64,

    engine_count: u32,
    power_hp: f64,
    mass_slugs: f64,
    cd0: f64,
    cl_max: f64,
    aspect_ratio: f64,
    critical_aoa_rad: f64,
    stall_pitch_down_rad: f64,
    stall_speed_fps: f64,
    thrust_lbf: f64,

    // Rotation matrix (camera transform)
    pub r11: f64, pub r12: f64, pub r13: f64,
    pub r21: f64, pub r22: f64, pub r23: f64,
    pub r31: f64, pub r32: f64, pub r33: f64,

    // Additional physics variables from banks.c
    d: f64,
    f: f64,
    p: f64,
    t: f64,
    v: f64,
    w: f64,
    m: f64,
    e: f64,
    i: f64,
    h: f64,
    x: f64,

    gravity_accel: f64,
    s: f64,
}

impl FlightModel {
    pub fn new(config: AircraftConfig) -> Self {
        let mut model = Self {
            airplane_x: 0.0,
            airplane_y: 0.0,
            airplane_z: config.initial_altitude_ft,

            side_tilt: 0.0,
            forward_tilt: config.initial_forward_tilt_rad,
            compass: 0.0,

            speed: 8.0,  // Still fixed throttle steps for now
            speed_feet: config.initial_speed_fps,

            left_right: 0.0,
            up_down: 0.0,            // Initialize autopilot system
            autopilot_enabled: false,
            target_altitude: 0.0,
            target_heading: 0.0,
            autopilot_prev_hdg_error: 0.0,
            autopilot_prev_alt_error: 0.0,
            autopilot_filtered_hdg_deriv: 0.0,
            // Initialize waypoint navigation
            waypoint_enabled: false,
            waypoints: Vec::new(),
            current_waypoint_index: 0,
            waypoint_target_heading: 0.0,

            // HUD display options
            show_coordinates: true,  // Enabled by default, toggle with X key
            show_scene_labels: true, // Enabled by default, toggle with M key

            // Global coordinate system (set by event loop during recentering)
            current_lat: 37.6213,  // Default SFO coordinates
            current_lon: -122.3790,

            // Reference coordinates for ENU conversion (set by event loop)
            ref_lat: 37.6213,  // Default SFO coordinates
            ref_lon: -122.3790,

            r11: 1.0, r12: 0.0, r13: 0.0,
            r21: 0.0, r22: 1.0, r23: 0.0,
            r31: 0.0, r32: 0.0, r33: 1.0,

            d: 0.0,
            f: 0.0,
            p: 0.0,
            t: 0.0,
            v: 0.0,
            w: 0.0,
            m: 0.0,
            e: 0.0,
            i: 0.0,
            h: 0.0,
            x: config.initial_lift_param,

            gravity_accel: config.gravity_accel,
            s: config.wing_area_ft2,

            mass_slugs: config.mass_lb / 32.174,
            engine_count: config.engine_count,

             // Aircraft config
            power_hp: config.power_hp,
            cd0: config.cd0,
            cl_max: config.cl_max,
            aspect_ratio: config.aspect_ratio,
            critical_aoa_rad: config.critical_aoa_rad,
            stall_pitch_down_rad: config.stall_pitch_down_rad,
            stall_speed_fps: config.stall_speed_kts * 1.68781,
            thrust_lbf: config.thrust_lbf,
        };

        model.calculate_angles();
        model
    }

    // Throttle control - increases target speed (directly impacts acceleration)
    pub fn increase_throttle(&mut self) {
        self.speed += 1.0;
    }

    // Throttle control - decreases target speed
    pub fn decrease_throttle(&mut self) {
        self.speed -= 1.0;
        if self.speed < 0.0 {
            self.speed = 0.0; // Prevent negative speed
        }
    }

    /// Calculate the new orientation angles and rotation matrix
    pub fn calculate_angles(&mut self) {
        // Trigonometric precomputations
        let cos_fwd = self.forward_tilt.cos();
        let sin_fwd = self.forward_tilt.sin();
        let cos_cmp = self.compass.cos();
        let cos_side = self.side_tilt.cos();
        let sin_side = self.side_tilt.sin();
        let sin_cmp = self.compass.sin();

        // Update F (moment arm)
        self.f += PHYSICS_DT * self.p;

        // Yaw (compass heading)
        self.compass += cos_side * PHYSICS_DT * self.f / cos_fwd
                      + self.d / cos_fwd * sin_side * PHYSICS_DT;

        // Pitch (forward/back tilt)
        self.forward_tilt += self.d * PHYSICS_DT * cos_side
                           - PHYSICS_DT * self.f * sin_side;
        // Optional: crude stall behavior (nose drop)
        if self.speed_feet < self.stall_speed_fps && self.forward_tilt > self.critical_aoa_rad {
            self.forward_tilt = self.stall_pitch_down_rad;
        }

        // Roll (sideways tilt)
        self.side_tilt += (sin_side * self.d / cos_fwd * sin_fwd
                         + self.v
                         + sin_fwd / cos_fwd * self.f * cos_side) * PHYSICS_DT;

        // Build rotation matrix
        self.r11 = cos_fwd * cos_cmp;
        self.r12 = cos_fwd * sin_cmp;
        self.r13 = -sin_fwd;

        self.r21 = cos_cmp * sin_side * sin_fwd - sin_cmp * cos_side;
        self.r22 = cos_side * cos_cmp + sin_side * sin_cmp * sin_fwd;
        self.r23 = sin_side * cos_fwd;

        self.r31 = sin_cmp * sin_side + cos_side * sin_fwd * cos_cmp;
        self.r32 = sin_fwd * sin_cmp * cos_side - sin_side * cos_cmp;
        self.r33 = cos_side * cos_fwd;
    }

    /// Update physics: momentum, position, forces, and rates
    pub fn update_physics(&mut self) {
        // 1) Momentum
        let local_momentum = self.h * PHYSICS_DT;
        self.m += local_momentum;

        // 2) Inertia
        self.i = self.m / self.speed_feet;

        // 3) Position update (world → airplane reference)
        self.airplane_x += (self.r11 * self.speed_feet + self.r21 * self.m + self.r31 * self.x) * PHYSICS_DT;
        self.airplane_y += (self.r12 * self.speed_feet + self.i    * self.m + self.r32 * self.x) * PHYSICS_DT;
        self.airplane_z += (-self.r13 * self.speed_feet - self.r23 * self.m - self.r33 * self.x) * PHYSICS_DT;

        // 4) Intermediate values
        let local_intermediate = 15.0 * self.f / self.speed_feet;
        self.e = 0.1 + self.x * 4.9 / self.speed_feet;
        self.t = self.x * self.x
               + self.speed_feet * self.speed_feet
               + self.m * self.m;           // now using momentum squared
        let t_temp = self.t * local_intermediate / 32.0
                   - self.i * self.t / 24.0;

        // 5) H (for next momentum)
        self.h = self.gravity_accel * self.r23
               + self.v * self.x
               - self.f * self.speed_feet
               + t_temp / self.s;

        // 6) Acceleration along fuselage
        // Atmospheric constants
        let rho = 0.0023769; // slugs/ft³ at sea level
        let q = 0.5 * rho * self.speed_feet.powi(2); // dynamic pressure

        // Approximate angle of attack from pitch angle (forward_tilt)
        let aoa = self.forward_tilt;

        // Clamp CL to critical AoA
        let cl = if aoa.abs() > self.critical_aoa_rad {
            self.cl_max * (1.0 - ((aoa.abs() - self.critical_aoa_rad) / self.critical_aoa_rad).min(1.0))
        } else {
            self.cl_max * aoa / self.critical_aoa_rad
        };

        // Drag = q * S * (cd0 + cl² / (π * AR))
        let induced_drag = cl.powi(2) / (std::f64::consts::PI * self.aspect_ratio);
        // Reduce drag coefficient to make it more realistic - small planes have less drag
        let drag_coefficient = self.cd0 + induced_drag * 0.5; // Apply 50% factor to induced drag
        let drag_force = q * self.s * drag_coefficient;

        // Use the configured thrust directly instead of calculating from power
        // Scale with throttle (range 0-10)
        let throttle_ratio = (self.speed / 10.0).min(1.0).max(0.0);

        // Use direct thrust with throttle control
        let thrust_force = self.thrust_lbf * throttle_ratio * self.engine_count as f64;

        // We can also calculate additional thrust from power for more realistic engine modeling
        // Power = Force * Velocity, so Force = Power / Velocity
        let power_thrust = if self.speed_feet > 20.0 {  // Only apply at reasonable speeds
            self.power_hp * 550.0 * throttle_ratio / self.speed_feet
        } else {
            0.0 // Avoid division by small numbers
        };

        // Use the greater of the two thrust values - at low speed use configured thrust,
        // at higher speeds might transition to power-based thrust
        let thrust_force = thrust_force.max(power_thrust);

        // Gravity force in body X (forward) direction
        let gravity_force = -self.mass_slugs * self.gravity_accel * self.forward_tilt.sin();

        // Net force = thrust - drag + gravity component
        let net_force = thrust_force - drag_force + gravity_force;

        // F = ma ⇒ a = F / m
        let accel = net_force / self.mass_slugs;

        // 7) Speed update
        self.speed_feet += accel * PHYSICS_DT;
        if self.speed_feet < 0.1 {
            self.speed_feet = 0.1;
        }

        // 8) Next‐step states
        let a = 2.63 / self.speed_feet * self.d;
        let m_momentum = self.m;  // ← accumulate momentum from step 1
        self.x += (self.d * self.speed_feet
                   - self.t / self.s * (0.19 * self.e + a * 0.64 + self.up_down / 1e3)
                   - m_momentum * self.v          // now matches banks.c’s M * v
                   + self.gravity_accel * self.r33) * PHYSICS_DT;

        self.w = self.d;
        self.d += self.t * (0.45 - 14.0 / self.speed_feet * self.x - a * 130.0 - self.up_down * 0.14)
                   * PHYSICS_DT / 125e2
               + self.f * PHYSICS_DT * self.v;

        let d_param = self.v / self.speed_feet * 15.0;

        // 9) Turn rate P
        self.p = (self.t * (47.0 * self.i
                    - local_intermediate * 52.0
                    + self.e * 94.0 * d_param
                    - t_temp * 0.38
                    + self.left_right * 0.21 * self.e) / 1e2
               + self.w * 179.0 * self.v) / 2312.0;

        // 10) Sideslip v
        self.v -= (self.w * self.f
                   - self.t * (0.63 * local_intermediate
                              - self.i * 0.086
                              + local_intermediate * self.e * 19.0
                              - d_param * 25.0
                              - 0.11 * self.left_right) / 107e2) * PHYSICS_DT;
    }

}