//! Implements a vehicle model based on the bicycle model.

use godot::classes::IRigidBody2D;
use godot::classes::RigidBody2D;
use godot::global::deg_to_rad;
use godot::prelude::*;
use std::f32::consts::PI;

#[derive(GodotClass)]
#[class(base=RigidBody2D)]
struct Car {
    engine_force: f32,
    brake_force: f32,
    air_drag_coefficient: f32,
    mechanical_drag: f32,

    max_cornering_force: f32,
    front_wheel: Wheel,
    rear_wheel: Wheel,

    /// Unit: radians
    max_steering_angle: f32,

    base: Base<RigidBody2D>,
}

struct Wheel {
    /// Distance from center of mass.
    y_distance: f32,

    /// Whether the wheels have static (good) grip or are slipping (as in drifting)
    // TODO
    has_grip: bool,
}

impl Wheel {
    /// Returns position vector to wheel.
    fn world_offset(&self, rotation: f32) -> Vector2 {
        Vector2::new(0.0, self.y_distance).rotated(rotation)
    }
}

#[godot_api]
impl IRigidBody2D for Car {
    fn init(base: Base<RigidBody2D>) -> Self {
        let engine_force = 4000000.0;
        let brake_force = 2.5 * engine_force;
        let max_cornering_force = 7000000.0;

        Self {
            base,
            engine_force,
            brake_force,
            air_drag_coefficient: 0.05,
            mechanical_drag: 2000.0,
            max_steering_angle: deg_to_rad(32.0) as f32,
            max_cornering_force,

            front_wheel: Wheel {
                y_distance: 200.0,
                has_grip: true,
            },

            rear_wheel: Wheel {
                y_distance: -200.0,
                has_grip: true,
            },
        }
    }

    fn physics_process(&mut self, _delta: f64) {
        // self.base_mut().set_mass(1300.0);
        // self.base_mut().set_inertia(260000000000.0);

        let input = Input::singleton();
        let move_forward = input.is_action_pressed("move_forward");
        let move_backward = input.is_action_pressed("move_backward");
        let turn_left = input.is_action_pressed("turn_left");
        let turn_right = input.is_action_pressed("turn_right");

        // TODO: Handbrake
        let _handbrake = input.is_action_pressed("handbrake");

        let rotation = self.base().get_rotation();

        let forward = self.forward();

        let front_wheel_velocity = self.wheel_velocity(&self.front_wheel);
        let rear_wheel_velocity = self.wheel_velocity(&self.rear_wheel);

        let front_wheel_angle = match (turn_left, turn_right) {
            (true, false) => self.steering_angle(),
            (false, true) => -self.steering_angle(),
            (true, true) | (false, false) => 0.0,
        };

        let front_wheel_position = self.front_wheel.world_offset(rotation);
        let rear_wheel_position = self.rear_wheel.world_offset(rotation);

        let front_wheels_direction = forward.rotated(front_wheel_angle);
        let rear_wheels_direction = forward;

        // Engine/thrust force
        let thrust_force = match (move_forward, move_backward) {
            (true, false) => self.engine_force * front_wheels_direction,
            (false, true) => {
                if forward.dot(self.base().get_linear_velocity()) > 0.0 {
                    // Moving ahead, so use brakes
                    -self.brake_force * front_wheels_direction
                } else {
                    // Moving backwards, so reverse
                    -0.5 * self.engine_force * front_wheels_direction
                }
            }
            _ => Vector2::default(),
        };

        // front wheel drive
        self.base_mut().apply_force_ex(thrust_force).position(front_wheel_position).done();

        let front_slip_angle = front_wheels_direction.angle_to(front_wheel_velocity);
        let rear_slip_angle = rear_wheels_direction.angle_to(rear_wheel_velocity);

        let speed_is_negligible = self.speed_is_negligible();

        if speed_is_negligible {
            self.base_mut().set_linear_velocity(Vector2::default());
        }

        // Cornering forces
        if !speed_is_negligible {
            self.apply_cornering_force(front_wheel_position, front_slip_angle);
            self.apply_cornering_force(rear_wheel_position, rear_slip_angle);
        };

        // Air drag
        self.apply_air_drag();

        // Mechanical friction
        self.apply_mechanical_drag();
    }
}

impl Car {
    fn apply_cornering_force(&mut self, wheel_position: Vector2, slip_angle: f32) {
        let right = self.right();

        let a = slip_angle.abs();
        let a = if a > PI / 2.0 { PI - a } else { a };
        let f = cornering_force(self.max_cornering_force, a);

        let force_vector = -slip_angle.sign() * right * f;
        self.base_mut().apply_force_ex(force_vector).position(wheel_position).done();
    }

    /// Returns normal vector in the direction the car is facing.
    fn forward(&self) -> Vector2 {
        let rotation = self.base().get_rotation();
        Vector2::UP.rotated(rotation)
    }

    /// Returns normal vector to the right of where the car is facing.
    fn right(&self) -> Vector2 {
        let rotation = self.base().get_rotation();
        Vector2::RIGHT.rotated(rotation)
    }

    /// Returns velocity vector of a wheel.
    fn wheel_velocity(&self, wheel: &Wheel) -> Vector2 {
        let vel = self.base().get_linear_velocity();
        let ang_vel = self.base().get_angular_velocity();
        let rot = self.base().get_rotation();
        vel + ang_vel * wheel.world_offset(rot).rotated(PI / 2.0)
    }

    /// Returns current speed.
    // TODO: in what unit?
    fn speed(&self) -> f32 {
        self.base().get_linear_velocity().length()
    }

    /// Returns true if current speed is low enough that lateral friction on wheels etc should not be calculated.
    fn speed_is_negligible(&self) -> bool {
        // Touch and feel value
        const THRESHOLD: f32 = 10.0;

        self.speed() < THRESHOLD
    }

    /// Returns steering angle, which is reduced from the max steering angle, the faster the car goes.
    fn steering_angle(&self) -> f32 {
        // Steering angle as function of speed s.
        // f(0) = 1
        // f(s) -> 0 as s -> infinity
        // Model:
        // f(s) = 1 / (1 + s^q)
        // where q controls dropoff aggressiveness.
        let s = self.speed();
        let scale = 1800.0;
        let s = s / scale;

        let q = 1.5;

        let f = 1.0 / (1.0 + s.powf(q));

        self.max_steering_angle * f
    }

    fn apply_air_drag(&mut self) {
        let vel = self.base().get_linear_velocity();
        if let Some(direction) = vel.try_normalized() {
            let drag = self.air_drag_coefficient * vel.length_squared();
            let drag = -direction * drag;
            self.base_mut().apply_force(drag);
        }
    }

    /// Mechanical loss.
    ///
    /// Sublinear force. Intented to slow the car when it is already at low speed.
    fn apply_mechanical_drag(&mut self) {
        let vel = self.base().get_linear_velocity();
        if let Some(direction) = vel.try_normalized() {
            // TODO: Note in book that powf is non-deterministic.
            let f = self.mechanical_drag * vel.length().powf(0.8);
            let f_vec = -direction * f;
            self.base_mut().apply_force(f_vec);
        }
    }
}

/// Lateral wheel force as a function of slip angle in radians
///
/// Takes positive slip angle in [0; pi/2]
///
/// Returns a non-negative value.
fn cornering_force(peak: f32, slip_angle: f32) -> f32 {
    debug_assert!(slip_angle >= 0.0);
    debug_assert!(slip_angle <= PI / 2.0);

    // TODO: Where should the peak be?
    // Sources say
    // - 3 degrees
    // - 5-6 degrees for street tires and 8 degrees for racing ties.
    // - 11 degrees for road tires with smooth curve, 6 degrees for semi-slick/competition track tires with faster drop past peak. Slick tires: 3 degrees for competitive racing.
    //
    // TODO what about when the input angle is more than 90 degrees? Still return peak then?

    let peak_angle = deg_to_rad(6.0) as f32;

    let magnitude = if slip_angle < peak_angle {
        slip_angle * peak
    } else {
        // TODO: falloff after peak
        // Past the peak street tires fall off slower and racing tires fall off faster.

        let c = 0.5;
        peak - c * peak * (slip_angle - peak_angle) / deg_to_rad(90.0) as f32
    };

    // dbg!(rad_to_deg(slip_angle as f64), peak, magnitude);
    debug_assert!(magnitude >= 0.0);
    debug_assert!(peak >= magnitude);

    magnitude
}
