thimni 0.3.2

use rstar::{PointDistance, RTree, RTreeObject};

use crate::vector::Vector;

#[derive(Copy, Clone)]
pub struct AABB<const N: usize, T: Vector<N>> {
    pub min: T,
    pub max: T,
}

impl<const N: usize, T: Vector<N>> AABB<N, T> {
    pub fn get_dim(&self) -> T {
        self.max.sub_self(&self.min)
    }

    pub fn overlap(&self, other: &Self) -> bool {
        self.min.less_than(&other.max) && self.max.greater_than(&other.min)
    }

    pub fn intersect(&self, other: &Self) -> Option<Self> {
        if self.overlap(other) {
            Some(Self {
                min: self.min.cmax(other.min),
                max: self.max.cmin(other.max),
            })
        } else {
            None
        }
    }

    pub fn get_center(&self) -> T {
        self.max.mid_point(self.min)
    }
}

#[derive(Copy, Clone)]
pub struct Circle<const N: usize, T: Vector<N>> {
    pub c: T,
    pub r: f32,
}

impl<const N: usize, T: Vector<N>> Circle<N, T> {
    pub fn new(c: T, r: f32) -> Self {
        Self { c, r }
    }
}

fn factorial(number: usize) -> usize {
    let mut factorial: usize = 1;

    for i in 1..(number + 1) {
        factorial *= i;
    }

    return factorial;
}

pub struct RPoint<const N: usize> {
    pub idx: usize,
    pub p: [f32; N],
}

impl<const N: usize> RTreeObject for RPoint<N> {
    type Envelope = rstar::AABB<[f32; N]>;

    fn envelope(&self) -> Self::Envelope {
        rstar::AABB::from_point(self.p)
    }
}

impl<const N: usize> PointDistance for RPoint<N> {
    fn distance_2(
        &self,
        point: &[f32; N],
    ) -> <<Self::Envelope as rstar::Envelope>::Point as rstar::Point>::Scalar {
        self.p
            .iter()
            .zip(point.iter())
            .map(|(a, b)| (*b - *a).powi(2))
            .sum()
    }
}

pub fn generate_circles<const N: usize, T: Vector<N>>(
    rect: &AABB<N, T>,
    k: f32,
    max_iter: usize,
    min_radius: f32,
    candidates_per_iter: usize,
) -> Vec<Circle<N, T>> {
    let dim = rect.get_dim();

    let target_area = k * dim.cprod();
    let mut covered_area = 0.0;
    let mut circles: Vec<Circle<N, T>> = vec![];

    let mut tree = RTree::new();

    let mut itera = 0;

    while itera < max_iter && covered_area < target_area {
        let mut best_candidate = None;
        let mut best_radius = 0.0;

        for _ in 0..candidates_per_iter {
            let np = dim.cmap_mut(|x: f32| quad_rand::gen_range(0.0, x));

            let dp = np.cmin(dim.sub_self(&np));

            let edge_dist = dp.min_elem();

            let max_radius = if circles.len() > 0 {
                let npa = np.to_arr();
                let nearest: &RPoint<N> = tree.nearest_neighbor(&npa).unwrap();
                let nearest_circle = circles[nearest.idx as usize];

                let d = nearest.distance_2(&npa).sqrt();

                let radius_from_circles = d - nearest_circle.r;
                edge_dist.min(radius_from_circles)
            } else {
                edge_dist
            };

            if max_radius >= min_radius && max_radius > best_radius {
                best_radius = max_radius;
                best_candidate = Some(np.to_arr());
            }
        }

        if let Some(best) = best_candidate {
            circles.push(Circle::new(T::from_arr(best), best_radius));

            if N == 2 {
                covered_area += std::f32::consts::PI * best_radius * best_radius;
            } else if N == 3 {
                covered_area +=
                    (4.0 / 3.0) * std::f32::consts::PI * best_radius * best_radius * best_radius;
            } else {
                covered_area += (std::f32::consts::PI.powf(N as f32 / 2.0)
                    * best_radius.powi(N as i32))
                    / factorial(N.div_ceil(2) + 1) as f32;
            }

            tree.insert(RPoint {
                idx: tree.size(),
                p: best.to_arr(),
            });
        }

        itera += 1;
    }

    circles
}

#[derive(Clone)]
pub struct CollisionParameters {
    /// step size used when approximating the gradient
    pub normal_epsilon: f32,
    /// minimum distance from surface that is considered a hit
    pub collision_epsilon: f32,
    /// learning rate for gradient descent
    pub learning_rate: f32,
    /// maximum number of iterations of gradeint descent
    pub max_gds_iter: usize,
    /// maximum number of iterations of sphere/circle packing
    pub max_packing_iter: usize,
    /// target area percentage to be covered by sphere/circle packing. range 0 to 1
    pub area_percentage: f32,
    /// minimum radius to be used during sphere/circle packing
    pub minimum_radius: f32,
}

impl Default for CollisionParameters {
    fn default() -> Self {
        Self {
            normal_epsilon: 2.0,
            collision_epsilon: 1.0,
            learning_rate: 0.2,
            max_gds_iter: 100,
            max_packing_iter: 50,
            area_percentage: 0.95,
            minimum_radius: 0.1,
        }
    }
}

pub(crate) fn random_biased(min: f32, max: f32, bias: f32, factor: f32) -> f32 {
    let rnd = quad_rand::gen_range(min, max);
    let mix = quad_rand::gen_range(0.0, 1.0) * factor;
    rnd * (1.0 - mix) + bias * mix
}