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);

        for turn in 0..=TURNS_TO_TEST {
            {
                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() || hits[0].1.distance.squared() > dir.length_squared() {
                    return Some(from + dir);
                }
            }
            if turn != 0 && turn != TURNS_TO_TEST {
                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() || hits[0].1.distance.squared() > dir.length_squared() {
                    return Some(from + dir);
                }
            }
        }

        None
    }
}