use std::collections::HashMap;

use galactica_util::{clockwise_angle, to_radians};
use nalgebra::Vector2;
use rapier2d::{dynamics::RigidBodySet, geometry::ColliderHandle};

use crate::phys::{
	objects::{PhysSimShip, ShipControls},
	PhysStepResources,
};

// TODO: no wobble
// TODO: slow down when near planet
// TODO: avoid obstacles

/// Land this ship on the given object
pub fn auto_landing(
	_res: &PhysStepResources,
	rigid_bodies: &RigidBodySet,
	ships: &HashMap<ColliderHandle, PhysSimShip>,
	this_ship: ColliderHandle,
	target_pos: Vector2<f32>,
) -> Option<ShipControls> {
	let rigid_body_handle = ships.get(&this_ship).unwrap().rigid_body;
	let rigid_body = rigid_bodies.get(rigid_body_handle).unwrap();
	let my_pos = *rigid_body.translation();
	let my_rot = rigid_body.rotation() * Vector2::new(1.0, 0.0);
	let my_vel = rigid_body.linvel();
	let my_angvel = rigid_body.angvel();
	let v_t = target_pos - my_pos; // Vector to target
	let v_d = v_t - my_vel; // Desired thrust vector
	let angle_delta = clockwise_angle(&my_rot, &v_d);
	let mut controls = ShipControls::new();

	if angle_delta < 0.0 && my_angvel > -0.3 {
		controls.right = true;
	} else if angle_delta > 0.0 && my_angvel < 0.3 {
		controls.left = true;
	}

	if my_rot.angle(&v_d) <= to_radians(15.0) {
		controls.thrust = true;
	}

	return Some(controls);
}