use macroquad::prelude as mq;
use rapier3d::prelude::*;

mod engine;
use engine::*;

mod camera;
mod drone;
mod rendering;

mod graphics_util;

use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};

fn window_conf() -> mq::Conf {
    mq::Conf {
        window_title: "RustDroneSim".to_owned(),
        window_resizable: true,
        // fullscreen: true,
        platform: mq::miniquad::conf::Platform {
            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
            ..Default::default()
        },
        ..Default::default()
    }
}

#[macroquad::main(window_conf)]
async fn main() {
    let mut world = World::default();
    let mut renderer = Renderer::new();

    renderer.light.set_location(
        vector![70.0, 150.0, -90.0].into(),
        vector![-0.4, -0.7, 0.6].into(),
    );
    renderer.update_light(&world);

    // Ground collider
    world.register_free_collider(
        ColliderBuilder::cuboid(250.0, 5.0, 250.0)
            .restitution(0.5)
            .build(),
        None,
    );

    let mut controller = drone::pidcontroller::PIDController::default();
    controller.set_input(JoystickInput {
        throttle_input: 1.0,
        yaw_input: 0.05,
        roll_input: 0.0,
        pitch_input: 0.0,
    });
    let mut drone = drone::Drone::new(
        &mut world,
        Box::new(controller),
        drone::MotorCharacteristics {
            max_thrust: 3.0,
            max_torque: 2.0,
            ..Default::default()
        },
        0.350,
    );

    loop {
        renderer.update_camera();
        if mq::is_key_pressed(mq::KeyCode::L) {
            renderer
                .light
                .set_location(renderer.camera.position, renderer.camera.front);
            renderer.update_light(&world);
            println!(
                "Light Pos: {}, Light Front Vec{}",
                renderer.light.position, renderer.light.front
            );
        }

        if mq::is_key_pressed(mq::KeyCode::C) {
            add_objects(&mut world);
        }
        if mq::is_key_pressed(mq::KeyCode::M) {
            renderer.apply_config();
        }

        // Physics
        world.step();
        let _ = clearscreen::clear();
        drone.process_tick(&mut world);
        println!(
            "Translation: {:}",
            world.bodies.get(drone.rb_handle).unwrap().translation()
        );
        println!(
            "Angular Velocity: {:}",
            world.bodies.get(drone.rb_handle).unwrap().angvel()
        );
        println!(
            "Velocity: {:}",
            world.bodies.get(drone.rb_handle).unwrap().linvel()
        );

        println!(
            "Motor Throttles: {:?}",
            drone.controller.get_motor_throttles()
        );

        // Rendering
        renderer.draw(&mut world);

        if world.tick % 60 == 0 {
            world.clear_ofb();
            renderer.update_light(&world);
            mq::next_frame().await;
        }
    }
}

fn add_objects(world: &mut World) {
    for i in 0..1 {
        let body = world.register_body(
            RigidBodyBuilder::dynamic()
                .translation(vector![0.0, 50.0 + i as f32, 0.0])
                .rotation(vector![std::f32::consts::PI / 4.2, i as f32, i as f32])
                .build(),
        );
        world.register_collider(
            ColliderBuilder::cuboid(5.0, 5.0, 5.0)
                .restitution(0.3)
                .build(),
            body,
            None,
        );
    }
}