use bevy::{
    ecs::system::SystemParam,
    math::{FloatPow, Ray3d, Vec3},
    picking::mesh_picking::ray_cast::{MeshRayCast, MeshRayCastSettings},
};

#[derive(SystemParam)]
pub struct Obstacles<'w, 's> {
    ray_caster: MeshRayCast<'w, 's>,
}

const TURNS_TO_TEST: usize = 4;
const ROTATION: f32 = std::f32::consts::PI / TURNS_TO_TEST as f32;
impl<'w, 's> Obstacles<'w, 's> {
    pub fn avoid(&mut self, from: Vec3, to: Vec3) -> Option<Vec3> {
        let dir = to - from;
        let ray_settings = MeshRayCastSettings::default().with_early_exit_test(&|_| true);

        let mut best_effort = None;
        for turn in 0..=TURNS_TO_TEST {
            macro_rules! try_turn {
                ($rotation:expr) => {
                    let dir = dir.rotate_y($rotation * turn as f32);

                    let hits = self
                        .ray_caster
                        .cast_ray(Ray3d::new(from, dir.try_into().unwrap()), &ray_settings);
                    if hits.is_empty() {
                        return Some(from + dir);
                    }
                    let distance_squared = hits[0].1.distance.squared();
                    if distance_squared > dir.length_squared() {
                        return Some(from + dir);
                    }
                    if if let Some((best_distance_squared, _)) = best_effort {
                        best_distance_squared > distance_squared
                    } else {
                        true
                    } {
                        best_effort = Some((
                            distance_squared,
                            dir.clamp_length_max(distance_squared.sqrt()),
                        ));
                    }
                };
            }

            try_turn!(ROTATION);
            try_turn!(-ROTATION);
        }

        best_effort.map(|(_, dir)| from + dir)
    }
}