//#####################################################################
// Copyright 2004-2009, Zhaosheng Bao, Ron Fedkiw, Jon Gretarsson, Geoffrey Irving, Andrew Selle, Jonathan Su, Joseph Teran.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class BOX_HIERARCHY
//##################################################################### 
#ifndef __BOX_HIERARCHY__
#define __BOX_HIERARCHY__

#include <Core/Arrays/ARRAY.h>
#include <Core/Math_Tools/RANGE.h>
namespace PhysBAM{

template<class TV>
class BOX_HIERARCHY
{
    typedef typename TV::SCALAR T;

protected:
    int first_leaf=0;
    ARRAY<RANGE<TV> > box_hierarchy;
    ARRAY<int> leaf_to_index;
public:

    BOX_HIERARCHY()=default;
    BOX_HIERARCHY(const BOX_HIERARCHY&) = delete;
    void operator=(const BOX_HIERARCHY&) = delete;
    ~BOX_HIERARCHY()=default;

    void Clean_Memory();
    void Compute(const ARRAY<RANGE<TV> >& boxes,bool update_order);
    void Thicken_Boxes(T extra_thickness);
    
    // bool test(const RANGE& r);   false = discard, true = keep
    // bool add(int leaf_index);   record leaf index.
    // Return true to keep looking for more intersections.
    template<class F,class A>
    void Traverse(const F& test,const A& add) const
    {
        if(box_hierarchy.m) Traverse(0,test,add);
    }

    // bool test(const RANGE& r_this, const RANGE& r_h);   false = discard, true = keep
    // bool add(int leaf_index_this, int leaf_index_h);   record leaf pairs.
    // Return true to keep looking for more intersections.
    template<class F,class A>
    void Traverse(const BOX_HIERARCHY<TV>& h,const F& test,const A& add,
        T extra_thickness=0) const
    {
        if(box_hierarchy.m && h.box_hierarchy.m)
        {
            auto rev_add=[&add](int a, int b){return add(b,a);};
            if(&h==this) Traverse_Self(0,test,add,rev_add,extra_thickness);
            else Traverse(h,0,0,test,add,rev_add,extra_thickness);
        }
    }

    // bool add(int leaf_index);   record leaf index.
    // Return true to keep looking for more intersections.
    template<class A>
    void Traverse(const TV& X,const A& add,T extra_thickness=0) const
    {
        Traverse(
            [&X,extra_thickness](const RANGE<TV>& r)
            {return r.Inside(X,-extra_thickness);},add);
    }

    // bool add(int leaf_index);   record leaf index.
    // Return true to keep looking for more intersections.
    template<class A>
    void Traverse(const RANGE<TV>& range,const A& add,T extra_thickness=0) const
    {
        Traverse(
            [&range,extra_thickness](const RANGE<TV>& r)
            {return r.Intersection(range,extra_thickness);},add);
    }

    // Like Traverse, but stores indices in list rather than calling a function.
    template<class F,class... Args>
    void Intersection_List(const F& f,ARRAY<int>& list,Args&&... args) const
    {
        Traverse(f,[&list](int i){list.Append(i);return true;},std::forward<Args>(args)...);
    }

    // bool add(int leaf_index, T distance);   record leaf index.
    // Return true to keep looking for more intersections.
    template<class A>
    void Traverse_By_Distance(const TV& X,const A& add) const
    {
        auto cmp=[](auto& a,auto& b){return a.first<b.first;};
        ARRAY<std::pair<T,int> > queue;
        auto push=[&,this](int box)
        {
            T dist=box_hierarchy(box).Signed_Distance(X);
            queue.Append({dist,box});
            std::push_heap(queue.begin(),queue.end(),cmp);
        };
        auto pop=[&]()
        {
            std::pop_heap(queue.begin(),queue.end(),cmp);
            return queue.Pop_Value();
        };
        push(0);
        while(queue.m)
        {
            auto p=pop();
            if(p.second>=first_leaf)
            {
                if(!add(leaf_to_index(p.second-first_leaf),p.first))
                    return;
            }
            else
            {
                push(p.second*2+1);
                push(p.second*2+2);
            }
        }
    }
    
protected:
    template<class F,class A>
    bool Traverse(int box,const F& test,const A& add) const
    {
        if(!test(box_hierarchy(box))) return true;
        if(box>=first_leaf) return add(leaf_to_index(box-first_leaf));
        return Traverse(2*box+1,test,add) && Traverse(2*box+2,test,add);
    }

    template<class F,class A,class RA>
    bool Traverse(const BOX_HIERARCHY<TV>& h,int box_a,int box_b,const F& test,
        const A& add,const RA& rev_add,T extra_thickness) const
    {
        auto &C=box_hierarchy(box_a), &D=h.box_hierarchy(box_b);
        if(!C.Intersection(D,extra_thickness) || !test(C,D)) return true;
        if(box_a<first_leaf)
        {
            return h.Traverse(*this,box_b,2*box_a+1,test,rev_add,add,extra_thickness) &&
                h.Traverse(*this,box_b,2*box_a+2,test,rev_add,add,extra_thickness);
        }
        if(box_b<h.first_leaf)
        {
            return Traverse(h,box_a,2*box_b+1,test,add,rev_add,extra_thickness) &&
                Traverse(h,box_a,2*box_b+2,test,add,rev_add,extra_thickness);
        }
        return add(leaf_to_index(box_a-first_leaf),h.leaf_to_index(box_b-h.first_leaf));
    }

    template<class F,class A,class RA>
    bool Traverse_Self(int box,const F& test,
        const A& add,const RA& rev_add,T extra_thickness) const
    {
        if(box>=first_leaf) return true;
        auto &C=box_hierarchy(box);
        if(!test(C,C)) return true;
        return Traverse(*this,2*box+1,2*box+2,test,rev_add,add,extra_thickness) &&
            Traverse_Self(2*box+1,test,rev_add,add,extra_thickness) &&
            Traverse_Self(2*box+2,test,rev_add,add,extra_thickness);
    }
    
//#####################################################################
};
}
#endif

