use nalgebra::{self as na, Vector3};
use rapier3d::prelude as rp;

use crate::engine::{ColliderExtraData, World};

pub mod controller;
pub mod fpvcontroller;
pub mod pidcontroller;
use controller::*;

const AREA_DENSITY: f32 = 1.23;
const DRAG_CONSTANT: f32 = 1.3;
const DRAG_MAGIC_NUM: f32 = 0.5;

pub struct MotorCharacteristics {
    pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
    pub max_thrust: f32,
    pub max_torque: f32,
}

impl Default for MotorCharacteristics {
    fn default() -> Self {
        Self {
            /*
             * Motor position indices
             *    ^ - Front
             *    |
             *    |
             * 1 --- 0
             * |     |
             * |     |
             * 2 --- 3
             */
            relative_motor_positions: [
                rp::point![5.0, 3.0, 5.0],
                rp::point![-5.0, 3.0, 5.0],
                rp::point![-5.0, 3.0, -5.0],
                rp::point![5.0, 3.0, -5.0],
            ],
            max_thrust: 10.0,
            max_torque: 1.0,
        }
    }
}

pub struct Drone {
    pub rb_handle: rp::RigidBodyHandle,
    motor_characteristics: MotorCharacteristics,
    pub controller: Box<dyn DroneController>,
    width: f32,
    height: f32,
}

fn calculate_drag(velocity: f32, area: f32, constant: f32) -> f32 {
    return 0.5 * AREA_DENSITY * (velocity * velocity) * area * constant * DRAG_MAGIC_NUM;
}

impl Drone {
    pub fn new(
        world: &mut World,
        controller: Box<dyn DroneController>,
        motor_characteristics: MotorCharacteristics,
        mass: f32,
    ) -> Drone {
        let drone_rb_handle = world.register_body(
            rp::RigidBodyBuilder::dynamic()
                .translation(rp::vector![0.0, 10.0, 0.0])
                .rotation(rp::vector![0.0, 0.0, 0.0])
                /*
                 * These damping values keep the simulation more realistic,
                 * They act as air resistance
                 *
                 * Values are kind of random for now. Calculating them requires the final model
                 * A Poor Man's fluid simulation :D
                 */
                // .linear_damping(0.2) // Damps velocity slowly
                // .angular_damping(0.2) // Damps angular velocity slowly
                .build(),
        );
        let width = 0.40;
        let height = 0.25;
        world.register_collider(
            rp::ColliderBuilder::cuboid(width / 2.0, height / 2.0, width / 2.0)
                .mass(mass)
                .restitution(0.3)
                .build(),
            drone_rb_handle,
            Some(ColliderExtraData {
                color: macroquad::color::RED,
            }),
        );

        // Should only need to be called once?
        // No one should be changing motor characteristics during flight time.
        controller.set_motor_characteristics(&motor_characteristics);

        return Drone {
            rb_handle: drone_rb_handle,
            controller: controller,
            motor_characteristics: motor_characteristics,
            width,
            height,
        };
    }

    fn apply_drag(&mut self, world: &mut World) {
        let body = world.bodies.get_mut(self.rb_handle).unwrap();
        let velocity = body.linvel();
        let mut drag = Vector3::<f32>::zeros();

        let side_area = self.width * self.height;
        let up_area = self.width * self.width;

        drag.x = calculate_drag(velocity.x, side_area, DRAG_CONSTANT);
        drag.y = calculate_drag(velocity.y, up_area, DRAG_CONSTANT);
        drag.z = calculate_drag(velocity.z, side_area, DRAG_CONSTANT);

        body.add_force(-drag, true);
    }

    fn apply_throttles(&mut self, world: &mut World) {
        let throttles = self.controller.get_motor_throttles();

        let drone_rb = world.bodies.get_mut(self.rb_handle).unwrap();

        /*
         * Clear all the previously applied forces and torques, or theyll add-up every tick
         */
        drone_rb.reset_forces(true);
        drone_rb.reset_torques(true);

        for (i, motor_position) in self
            .motor_characteristics
            .relative_motor_positions
            .iter()
            .enumerate()
        {
            let throttle = throttles[i].clamp(0.0, 1.0);
            let thrust = self.motor_characteristics.max_thrust * throttle;
            let torque = self.motor_characteristics.max_torque * throttle;

            // Thrust is applied upward at motor position.
            //
            // Force calculated with (0.0, thrust, 0.0) points to World Up,
            // Apply RB's rotation to point at RB's Up
            //
            // motor_position is relative, transform it by the RB's position first
            let mut thrust_force = nalgebra::Vector3::new(0.0, thrust, 0.0);
            thrust_force = drone_rb.rotation().transform_vector(&thrust_force);
            drone_rb.add_force_at_point(
                thrust_force,
                drone_rb.position().transform_point(motor_position),
                true,
            );

            let torque = if i % 2 == 0 {
                rp::vector![0.0, torque, 0.0]
            } else {
                rp::vector![0.0, -torque, 0.0]
            };
            drone_rb.add_torque(torque, true);
            println!("Torque: \n{:?}", torque);
            println!("Thrust: \n{:?}", thrust);
        }
    }

    fn update_controller(&mut self, world: &World) {
        let rb = world.bodies.get(self.rb_handle).unwrap();
        self.controller.set_time(world.get_time());
        self.controller.set_position(*rb.position());
    }

    pub fn process_tick(&mut self, world: &mut World) {
        self.update_controller(world);
        self.apply_throttles(world);
        // self.apply_drag(world);
    }
}