use std::{time::Duration, sync::Arc};
use log::{debug, error};
use nalgebra::{vector, point, Vector2};
use rapier2d_f64::prelude::{PhysicsPipeline};
use tokio::{time::sleep, sync::RwLock};
use starkingdoms_protocol::player::Player;
use crate::{manager::{ClientHandlerMessage, ClientManager, PhysicsData}, SCALE, planet::Planets};

pub const ROTATIONAL_FORCE: f64 = 200.0;
pub const LATERAL_FORCE: f64 = 80.0;

pub async fn timer_main(mgr: ClientManager, physics_data: Arc<RwLock<PhysicsData>>, world_data: Arc<RwLock<Planets>>) {
    let mut pipeline = PhysicsPipeline::new();
    loop {
        sleep(Duration::from_millis(5)).await;

        physics_data.write().await.tick(&mut pipeline);

        let mut protocol_players = vec![];

        {
            for (player_id, player) in mgr.players.write().await.iter() {
                let mut physics_data = physics_data.write().await;
                let player_handle = player.handle;
                let player_body = physics_data.rigid_body_set.get_mut(player_handle).unwrap();
                player_body.reset_forces(true);
                player_body.reset_torques(true);
                let planets = world_data.read().await;
                let grav_force = planets.gravity((player_body.translation().x, player_body.translation().y), player_body.mass());
                player_body.apply_impulse(vector![grav_force.0, grav_force.1], true);

                let mut torque = 0.0;
                let mut left_top_thruster: f64 = 0.0;
                let mut right_top_thruster: f64 = 0.0;
                let mut left_bottom_thruster: f64 = 0.0;
                let mut right_bottom_thruster: f64 = 0.0;

                if player.input.right {
                    torque += ROTATIONAL_FORCE;
                    left_top_thruster -= 1.0;
                    right_bottom_thruster += 1.0;
                }
                if player.input.left {
                    torque -= ROTATIONAL_FORCE;
                    right_top_thruster -= 1.0;
                    left_bottom_thruster += 1.0;
                }

                //player_body.apply_torque_impulse(torque, true);

                let mut lateral = vector![0.0, 0.0];

                if player.input.up {
                    lateral -= vector![0.0, LATERAL_FORCE];
                    left_bottom_thruster -= 1.0;
                    right_bottom_thruster -= 1.0;
                }
                if player.input.down {
                    lateral += vector![0.0, LATERAL_FORCE];
                    left_top_thruster += 1.0;
                    right_top_thruster += 1.0;
                }
                left_top_thruster = LATERAL_FORCE * left_top_thruster.clamp(-1.0, 1.0);
                right_top_thruster = LATERAL_FORCE * right_top_thruster.clamp(-1.0, 1.0);
                left_bottom_thruster = LATERAL_FORCE * left_bottom_thruster.clamp(-1.0, 1.0);
                right_bottom_thruster = LATERAL_FORCE * right_bottom_thruster.clamp(-1.0, 1.0);

                let rotation = player_body.rotation().clone().angle();
                let left_top_thruster = vector![
                    -left_top_thruster * rotation.sin(),
                    left_top_thruster * rotation.cos()
                ];
                let right_top_thruster = vector![
                    -right_top_thruster * rotation.sin(),
                    right_top_thruster * rotation.cos()
                ];
                let left_bottom_thruster = vector![
                    -left_bottom_thruster * rotation.sin(),
                    left_bottom_thruster * rotation.cos()
                ];
                let right_bottom_thruster = vector![
                    -right_bottom_thruster * rotation.sin(),
                    right_bottom_thruster * rotation.cos()
                ];
                let scale = SCALE as f64;
                let top_left_point = point![
                    -25. / scale * rotation.cos() +25. / scale * rotation.sin(), 
                    -25. / scale * rotation.sin() -25. / scale * rotation.cos()
                ] + player_body.translation();
                let top_right_point = point![
                     25. / scale * rotation.cos() +25. / scale * rotation.sin(), 
                     25. / scale * rotation.sin() -25. / scale * rotation.cos()
                ] + player_body.translation();
                let bottom_left_point = point![
                    -25. / scale * rotation.cos() -25. / scale * rotation.sin(), 
                    -25. / scale * rotation.sin() +25. / scale * rotation.cos()
                ] + player_body.translation();
                let bottom_right_point = point![
                     25. / scale * rotation.cos() -25. / scale * rotation.sin(), 
                     25. / scale * rotation.sin() +25. / scale * rotation.cos()
                ] + player_body.translation();

                player_body.add_force_at_point(
                    left_top_thruster,
                    top_left_point, true);
                player_body.add_force_at_point(
                    right_top_thruster,
                    top_right_point, true);
                player_body.add_force_at_point(
                    left_bottom_thruster,
                    bottom_left_point, true);
                player_body.add_force_at_point(
                    right_bottom_thruster,
                    bottom_right_point, true);

                let translation = player_body.translation();

                let username;
                {
                    let usernames = mgr.usernames.read().await;
                    username = usernames.get(player_id).unwrap().clone();
                }

                // TODO: Figure out how to adjust codegen to use f64
                protocol_players.push(Player {
                    rotation: rotation as f32,
                    x: (translation.x * SCALE) as f32,
                    y: (translation.y * SCALE) as f32,
                    username,
                    special_fields: Default::default(),
                });
            }
        }

        for (_addr, client_thread) in mgr.handlers.read().await.iter() {
            match client_thread.tx.send(ClientHandlerMessage::Tick).await {
                Ok(_) => {
                    match client_thread.tx.send(ClientHandlerMessage::PlayersUpdate {players: protocol_players.clone()}).await {
                        Ok(_) => (),
                        Err(e) => {
                            error!("unable to send position packet: {}", e);
                        }
                    };

                    let world = world_data.read().await;
                    let planet_data = world.to_protocol();
                    match client_thread.tx.send(planet_data).await {
                        Ok(_) => (),
                        Err(e) => {
                            error!("unable to send earth packet: {}", e);
                        }
                    };
                }
                Err(e) => {
                    error!("unable to update a client thread: {}", e);
                }
            }
        }
    }
}