//g++ -std=c++20 octree.cpp -o main && ./main
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cfloat>
#include <concepts>

#define real double

struct Item{
    real x;
    real y;
    real z;
};

template <typename T>
concept SpatialItem = requires(T t) {
    { t.x } -> std::convertible_to<real>;
    { t.y } -> std::convertible_to<real>;
    { t.z } -> std::convertible_to<real>;
};

struct BoundingBox{
    real x;
    real y;
    real z;
    real maxX;
    real maxY;
    real maxZ;
};

template <size_t Dimensions, SpatialItem T = Item>
struct NTreeNode {
    BoundingBox bounds;
	
	size_t maxDepth = 1;

    static constexpr size_t NumChildren = 1 << Dimensions;
    
    NTreeNode<Dimensions,T>* children[NumChildren];
    std::vector<T*> objects;
    bool boundsComputed = false;
	bool generated = false;
	
	NTreeNode<Dimensions,T>* parent = nullptr;
    size_t level = 0;

    NTreeNode(const BoundingBox& box) : bounds(box) {
        for (int i = 0; i < NumChildren; ++i) {
            children[i] = nullptr;
        }
        boundsComputed = true;
    }
    NTreeNode(){
        for (int i = 0; i < NumChildren; ++i) {
            children[i] = nullptr;
        }
        boundsComputed = false;
    }

    void addItem(T* i){
        objects.push_back(i);
        computeBounds();
    }

    void computeBounds() noexcept{
        bounds.x = FLT_MAX;
        bounds.y = FLT_MAX;
        bounds.z = FLT_MAX;
        bounds.maxX = 0.0;
        bounds.maxY = 0.0;
        bounds.maxZ = 0.0;

        for(size_t i=0;i<objects.size();i++){
            bounds.x = std::min(bounds.x,objects[i]->x);
            bounds.y = std::min(bounds.y,objects[i]->y);
            bounds.z = std::min(bounds.z,objects[i]->z);
            bounds.maxX = std::max(bounds.maxX,objects[i]->x);
            bounds.maxY = std::max(bounds.maxY,objects[i]->y);
            bounds.maxZ = std::max(bounds.maxZ,objects[i]->z);
        }

        boundsComputed = true;
    }

    void generateLowerLevel(){ //size_t until = 3
        for (int i = 0; i < NumChildren; ++i) {
            children[i] = new NTreeNode<Dimensions,T>();
            children[i]->level = level+1;
			children[i]->parent = this;
        }
		generated = true;
    }
    void generateLowerLevel(size_t until){ //size_t until = 3
        for (int i = 0; i < NumChildren; ++i) {
            children[i] = new NTreeNode<Dimensions,T>();
            children[i]->level = level+1;
			children[i]->parent = this;
			if(until > 0){
				children[i]->generateLowerLevel(until-1);
			}
        }
		generated = true;
    }

    void placeObjectsToLowerLevel(){
        real halfX = bounds.x + bounds.maxX/2.0;
        real halfY = bounds.y + bounds.maxY/2.0;
        real halfZ = bounds.z + bounds.maxZ/2.0;

        for (auto* item : objects) {
            int index = 0;
            if constexpr (Dimensions >= 3) {
                if (item->z >= halfZ){
					index += 4;
				}
            }
            if constexpr (Dimensions >= 2) {
                if (item->y >= halfY){
					index += 2;
				}
            }
            if constexpr (Dimensions >= 1) {
                if (item->x >= halfX){
					index += 1;
				}
            }

            children[index]->addItem(item);
        }
        objects.clear();
    }
    void placeObjectsToLowerLevel(size_t until){
        real halfX = bounds.x + bounds.maxX/2.0;
        real halfY = bounds.y + bounds.maxY/2.0;
        real halfZ = bounds.z + bounds.maxZ/2.0;

        for (auto* item : objects) {
            int index = 0;
            if constexpr (Dimensions >= 3) {
                if (item->z >= halfZ) {
					index += 4;
				}
            }
            if constexpr (Dimensions >= 2) {
                if (item->y >= halfY) {
					index += 2;
				}
            }
            if constexpr (Dimensions >= 1) {
                if (item->x >= halfX) {
					index += 1;
				}
            }

            children[index]->addItem(item);
        }
		
        objects.clear();
		
		if(until>0 && generated == true){
			for (int i = 0; i < NumChildren; ++i) {
				children[i]->placeObjectsToLowerLevel(until-1);
			}
		}
    }
	
	[[nodiscard]] bool collides(const BoundingBox& bound) noexcept{
		if constexpr (Dimensions >= 3) {
			if (bound.z >= bounds.z || bound.maxZ <= bounds.maxZ) {
				return true;
			}
		}
		if constexpr (Dimensions >= 2) {
			if (bound.y >= bounds.y || bound.maxY <= bounds.maxY) {
				return true;
			}
		}
		if constexpr (Dimensions >= 1) {
			if (bound.z >= bounds.z || bound.maxZ <= bounds.maxZ) {
				return true;
			}
		}
		return false;
	}
	
	[[nodiscard]] std::vector<NTreeNode<Dimensions,T>*> getOverlappingNodes(const BoundingBox& bound) noexcept{
		std::vector<NTreeNode<Dimensions,T>*> elems;
		if(collides(bound)){
			elems.push_back(this);
			if(generated == true){
				for (int i = 0; i < NumChildren; ++i) {
					if(children[i]->collides(bound) == true){
						std::vector<NTreeNode<Dimensions,T>*> gotElems = children[i]->getOverlappingNodes(bound);
						for(size_t j=0;j<gotElems.size();j++){
							elems.push_back(gotElems[j]);
						}
						gotElems.clear();
					}
				}
			}
			
		}
		return elems;
	}
	
	[[nodiscard]] bool deleteItem(T* item) noexcept{
		NTreeNode<Dimensions,T>* node = getNodeOfItem(item);
		if(node == nullptr){
			return false;
		}
		
		
		bool success = false;
		
		for(size_t i=0;i<node->objects.size();i++){
			if(item == node->objects[i]){
				success = true;
				node->objects.erase(node->objects.begin() + i);
				break;
			}
		}
		
		node->parent->tryMerge();
		
		return true;
	}
	
	[[nodiscard]] bool updateItem(T* item) noexcept{ //Update position in octree
		bool success = false;
		if(deleteItem(item)){
			success = true;
			
			addItem(item);
			computeBounds();
			if(generated){
				placeObjectsToLowerLevel(maxDepth);
			}
		}
		
		return success;
	}
	
	void tryMerge() noexcept{
		if(!generated){
			return;
		}
		
		generated = false;
		for(size_t i=0;i<NumChildren;i++){
            delete children[i];
			children[i] = nullptr;
        }
	}
	
	[[nodiscard]] size_t getLevelOfItem(T* item){
		for(size_t i=0;i<objects.size();i++){
			if(objects[i] == item){
				return level;
			}
		}
		if(!generated){
			return static_cast<size_t>(-1);
		}
		
		for (int i = 0; i < NumChildren; ++i) {
			if(children[i]->contains(item) == true){
				return children[i]->getLevelOfItem(item);
			}
		}
		
		return static_cast<size_t>(-1);
		
	}
	
	[[nodiscard]] NTreeNode<Dimensions,T>* getNodeOfItem(T* item){
		for(size_t i=0;i<objects.size();i++){
			if(objects[i] == item){
				return this;
			}
		}
		if(!generated){
			return nullptr;
		}
		
		for (int i = 0; i < NumChildren; ++i) {
			if(children[i]->contains(item) == true){
				return children[i]->getNodeOfItem(item);
			}
		}
		
		return nullptr;
		
	}
	
	[[nodiscard]] bool contains(T* item) noexcept{
		for(size_t i=0;i<objects.size();i++){
			if(objects[i] == item){
				return true;
			}
		}
		if(!generated){
			return false;
		}
		for (int i = 0; i < NumChildren; ++i) {
			if(children[i]->contains(item) == true){
				return true;
			}
		}
		return false;
	}

    ~NTreeNode(){
        for(size_t i=0;i<NumChildren;i++){
            delete children[i];
        }
    }
};

int main(){
    Item a{0.1,0.1,0.3};
    Item b{0.4,0.3,0.8};

    NTreeNode<3> n; //Octree
    n.addItem(&a);
    n.addItem(&b);

    n.generateLowerLevel();
    n.placeObjectsToLowerLevel();
	
	std::cout << n.getOverlappingNodes(n.bounds).size() << std::endl;

    auto q = *(n.children[0]);

    std::cout << q.bounds.x << " " << q.bounds.y << " " << q.bounds.maxX << " " << q.bounds.maxY << std::endl;
	std::cout << n.getLevelOfItem(&a) << std::endl;
	std::cout << n.deleteItem(&b) << std::endl;
    return 0;
}