use super::SpatialTree;
use crate::NodeID;
use glam::{DVec3, dvec3};
use std::mem::swap;

pub struct Octtree {
    pub center: DVec3,
    pub size: DVec3,
    pub elems: Vec<(DVec3, NodeID)>,
    pub children: Option<Box<[[[Octtree; 2]; 2]; 2]>>,
}
impl Octtree {
    pub fn new(size: DVec3) -> Self {
        Self {
            center: DVec3::ZERO,
            size,
            children: None,
            elems: Vec::new(),
        }
    }
}
const MAX_CHILDREN: usize = 1024;
impl SpatialTree for Octtree {
    fn insert(&mut self, pos: DVec3, id: NodeID) {
        if let Some(c) = &mut self.children {
            let [x, y, z] = (pos - self.center)
                .to_array()
                .map(|e| if e > 0. { 1 } else { 0 });
            c[x][y][z].insert(pos, id);
        } else {
            self.elems.push((pos, id));
            if self.elems.len() > MAX_CHILDREN {
                self.children = Some(Box::new([-1., 1.].map(|x| {
                    [-1., 1.].map(|y| {
                        [-1., 1.].map(|z| Octtree {
                            center: self.center + (self.size / 4.) * dvec3(x, y, z),
                            size: self.size / 2.,
                            elems: Vec::new(),
                            children: None,
                        })
                    })
                })));
                let mut elems = Vec::new();
                swap(&mut elems, &mut self.elems);
                for (pos, id) in elems {
                    self.insert(pos, id);
                }
            }
        }
    }

    fn depth(&self) -> usize {
        if let Some(c) = &self.children {
            1 + c[0][0][0]
                .depth()
                .max(c[0][0][1].depth())
                .max(c[0][1][0].depth())
                .max(c[0][1][1].depth())
                .max(c[1][0][0].depth())
                .max(c[1][0][1].depth())
                .max(c[1][1][0].depth())
                .max(c[1][1][1].depth())
        } else {
            0
        }
    }
}