/*
   Copyright (c) 2019 by The ThreadDB Project
   All Rights Reserved.

   ThreadDB undergoes the BSD License 2.0. You should have received a copy along with this program; if not, write to the ThreadDB Project.
   To obtain a full unlimited version contact thethreaddbproject(at)gmail.com.

   threaddbRGrid.h - Data structure for efficient geometrical range queries.
*/

#pragma once

#include <threaddbCPP.h>

/**
   \file
   \brief         Range grid implementation for performant spacial searches.
   \details       The underlying template class provides a data structure for performant range queries.
                  Basically the rgrid can be seen as a static rgrid with a fixed tiling and depth.
                  This gives the advantage, that the elements can be inserted directly without the additional
                  effort of re-balancing the tree.
                  A neccessary prerequisite is that the amount and range of elements to be stored are available
                  already during construction of the data structure.
*/

#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <cstddef>
#include <vector>
#include <istream>
#include <ostream>

namespace tdb
{
    template< typename T >
    struct array_deleter
    {
        void operator ()(T const* p)
        {
            delete[] p;
        }
    };

    /**
    *  @brief Functor class receiving the queried elements.
    */

    template< class T >
    struct Functor
    {
        virtual void operator() (const T&) = 0;
    };

    /**
    *  @brief Point basic class definition.
    */

    template<typename T, size_t D>
    struct Point
    {
        Point() {}
        Point(T x_p)
        {
            x() = x_p;
        }

        Point(T x_p, T y_p)
        {
            x() = x_p;
            y() = y_p;
        }

        Point(T x_p, T y_p, T z_p)
        {
            x() = x_p;
            y() = y_p;
            z() = z_p;
        }

        const T x()const { return _c[0]; }
        T& x() { return _c[0]; }

        const T y()const { return _c[1]; }
        T& y() { return _c[1]; }

        const T z()const { return _c[2]; }
        T& z() { return _c[2]; }

        Point& operator+=(const Point& rhs_p)
        { 
            for(uint8_t i=0;i<D;++i)
                _c[i] += rhs_p._c[i];
            return *this;
        }
        
        Point& operator-=(const Point& rhs_p)
        {
            for (uint8_t i=0; i < D; ++i)
                _c[i] -= rhs_p._c[i];
            return *this;
        }

        size_t dimension() { return D; }

        friend std::ostream& operator<<(std::ostream& os, const Point& P)
        {
            for (uint8_t i=0; i < D; ++i)
                os.write((const char*)&P._c[i], sizeof(P._c[i]));
            return os;
        }

        friend std::istream& operator>>(std::istream& is, Point& P)
        {
            for (uint8_t i=0; i < D; ++i)
                is.read((char*)&P._c[i], sizeof(P._c[i]));
            return is;
        }

        T _c[D];
    };

    /**
    *  @brief Box basic class definition.
    */

    template<class T, size_t D>
    class Box {
    public:
        typedef Point<T, D> Point_;

        Box() {}

        Box(const Point_& rMin_p, const Point_& rMax_p) :
            _min_corner(rMin_p),
            _max_corner(rMax_p)
        {
        }

        const Point_& min_corner()const { return _min_corner; }
        Point_& min_corner() { return _min_corner; }

        const Point_& max_corner()const { return _max_corner; }
        Point_& max_corner() { return _max_corner; }

        friend std::ostream& operator<<(std::ostream& os, const Box& L)
        {
            os << L._min_corner;
            os << L._max_corner;
            return os;
        }

        friend std::istream& operator>>(std::istream& is, Box& L)
        {
            is >> L._min_corner;
            is >> L._max_corner;
            return is;
        }

    private:
        Point_ _min_corner;
        Point_ _max_corner;
    };

    /**
    *  @brief Check intersection of two Boxes.
    */

    template<typename C, size_t D>
    bool intersects(const Box<C, D>& rLhs_p, const Box<C, D>& rRhs_p)
    {
        for (uint8_t i=0; i < D; ++i)
        {
            if (rLhs_p.min_corner()._c[i] >= rRhs_p.max_corner()._c[i])
                return false;
            if (rLhs_p.max_corner()._c[i] <= rRhs_p.min_corner()._c[i])
                return false;
        }

        return true;
    }
    

    template<class T, typename C, size_t D, class L>
    class rgrid;

    template<class T, typename C, size_t D, class L>
    class rgrid_;

    template<class L>
    struct TaskInfo;

    /**
    *  @brief Link helper class to the next node level.
    */

    template<class T, typename C, size_t D>
    struct Link
    {
        typedef Box<C, D> Box_;
        typedef TaskInfo<Link> TaskInfo_;

        Link() :
            m_InsertMutex(),
            m_pRgrid(new(m_Membuf) rgrid_<T, C, D, Link<T, C, D>>())
        {
        }

        Link(const Box_& rTile_p, uint32_t Depth_p, tdb::database& rDb_p) :
            m_InsertMutex(),
            m_pRgrid(new(m_Membuf) rgrid_<T, C, D, Link<T, C, D>>(rTile_p, Depth_p - 1, rDb_p))
        {
        }

        ~Link()
        {
            m_pRgrid->~rgrid_();
        }

        void add(const Box_& rBox_p, const T& rData_p, tdb::database& rDb_p);

        bool intersects(const Box_& rBox_p) const;

        void recurse(const Box_& rBox_p, Functor<T>& rCallback_p, tdb::database& rDb_p,
            std::atomic<uint16_t>* pTaskCount_p,
            std::mutex* pThreadMutex_p,
            std::vector<TaskInfo_*>* pTaskHeap_p) const;

        friend std::ostream& operator<<(std::ostream& os, const Link& L)
        {
            os << (*L.m_pRgrid);
            return os;
        }

        friend std::istream& operator>>(std::istream& is, Link& L)
        {
            is >> (*L.m_pRgrid);
            return is;
        }

        std::mutex m_InsertMutex;
        char m_Membuf[sizeof(rgrid_<T, C, D, Link>)];
        rgrid_<T, C, D, Link>* m_pRgrid;
    };

    /**
    *  @brief Query task information.
    */

    template<class L>
    struct TaskInfo
    {
        TaskInfo() : m_pNode(0) {}

        std::shared_ptr<const L> m_pNode;
        std::mutex m_Mutex;
        std::condition_variable m_Signal;
    };

    /**
    *  @brief Range index calculation specialization for n dimensional scenarios.
    */

    template<typename C, size_t D>
    std::pair<Box<C, D>, int32_t> index_(const Box<C, D>& lhs_p, const Box<C, D>& rhs_p)
    {
         int32_t idx(0);
         uint8_t inc(1);
         Box<C, D> box;

         for (uint8_t i = 0; idx !=-1 && i < D; ++i)
         {
             C c = (lhs_p.max_corner()._c[i] - lhs_p.min_corner()._c[i]) / 2 + lhs_p.min_corner()._c[i];
             if (rhs_p.min_corner()._c[i] >= c)
             {
                 idx += inc;
                 box.min_corner()._c[i] = c;
                 box.max_corner()._c[i] = lhs_p.max_corner()._c[i];
             }
             else if (rhs_p.max_corner()._c[i] <= c)
             {
                 box.min_corner()._c[i] = lhs_p.min_corner()._c[i];
                 box.max_corner()._c[i] = c;
             }
             else
             {
                 idx = -1;
                 break;
             }

             inc <<= 1;
         }

         return std::make_pair(box, idx);
     }

    /**
    *  @brief Range index calculation specialization for 1 dimensional scenarios.
    */

    template<typename C>
    std::pair<tdb::Box<C, 1>, int32_t> index_(const tdb::Box<C, 1>& lhs_p, const tdb::Box<C, 1>& rhs_p)
    {
        std::pair<tdb::Box<C, 1>, int32_t> index;

        C x = (lhs_p.max_corner().x() - lhs_p.min_corner().x()) / 2 + lhs_p.min_corner().x();
        if (rhs_p.min_corner().x() >= x)
        {
            index.second = 1;
            index.first.min_corner().x() = x;
            index.first.max_corner().x() = lhs_p.max_corner().x();
        }
        else if (rhs_p.max_corner().x() <= x)
        {
            index.second = 0;
            index.first.min_corner().x() = lhs_p.min_corner().x();
            index.first.max_corner().x() = x;
        }
        else
        {
            index.second = -1;
            return index;
        }

        return index;
    }

    /**
    *  @brief Range index calculation specialization for 2 dimensional scenarios.
    */

    template<typename C>
    std::pair<tdb::Box<C, 2>, int32_t> index_(const tdb::Box<C, 2>& lhs_p, const tdb::Box<C, 2>& rhs_p)
    {
        std::pair<tdb::Box<C, 2>, int32_t> index;

        C x = (lhs_p.max_corner().x() - lhs_p.min_corner().x()) / 2 + lhs_p.min_corner().x();
        if (rhs_p.min_corner().x() >= x)
        {
            index.second = 1;
            index.first.min_corner().x() = x;
            index.first.max_corner().x() = lhs_p.max_corner().x();
        }
        else if (rhs_p.max_corner().x() <= x)
        {
            index.second = 0;
            index.first.min_corner().x() = lhs_p.min_corner().x();
            index.first.max_corner().x() = x;
        }
        else
        {
            index.second = -1;
            return index;
        }

        C y = (lhs_p.max_corner().y() - lhs_p.min_corner().y()) / 2 + lhs_p.min_corner().y();
        if (rhs_p.min_corner().y() >= y)
        {
            index.second += 2;
            index.first.min_corner().y() = y;
            index.first.max_corner().y() = lhs_p.max_corner().y();
        }
        else if (rhs_p.max_corner().y() <= y)
        {
            index.first.min_corner().y() = lhs_p.min_corner().y();
            index.first.max_corner().y() = y;
        }
        else
        {
            index.second = -1;
            return index;
        }

        return index;
    }

    /**
    *  @brief Range index calculation specialization for 3 dimensional scenarios.
    */

    template<typename C>
    std::pair<tdb::Box<C, 3>, int32_t> index_(const tdb::Box<C, 3>& lhs_p, const tdb::Box<C, 3>& rhs_p)
    {
        std::pair<tdb::Box<C, 3>, int32_t> index;

        C x = (lhs_p.max_corner().x() - lhs_p.min_corner().x()) / 2 + lhs_p.min_corner().x();
        if (rhs_p.min_corner().x() >= x)
        {
            index.second = 1;
            index.first.min_corner().x() = x;
            index.first.max_corner().x() = lhs_p.max_corner().x();
        }
        else if (rhs_p.max_corner().x() <= x)
        {
            index.second = 0;
            index.first.min_corner().x() = lhs_p.min_corner().x();
            index.first.max_corner().x() = x;
        }
        else
        {
            index.second = -1;
            return index;
        }

        C y = (lhs_p.max_corner().y() - lhs_p.min_corner().y()) / 2 + lhs_p.min_corner().y();
        if (rhs_p.min_corner().y() >= y)
        {
            index.second += 2;
            index.first.min_corner().y() = y;
            index.first.max_corner().y() = lhs_p.max_corner().y();
        }
        else if (rhs_p.max_corner().y() <= y)
        {
            index.first.min_corner().y() = lhs_p.min_corner().y();
            index.first.max_corner().y() = y;
        }
        else
        {
            index.second = -1;
            return index;
        }

        C z = (lhs_p.max_corner().z() - lhs_p.min_corner().z()) / 2 + lhs_p.min_corner().z();
        if (rhs_p.min_corner().z() >= z)
        {
            index.second += 4;
            index.first.min_corner().z() = z;
            index.first.max_corner().z() = lhs_p.max_corner().z();
        }
        else if (rhs_p.max_corner().z() <= z)
        {
            index.first.min_corner().z() = lhs_p.min_corner().z();
            index.first.max_corner().z() = z;
        }
        else
        {
            index.second = -1;
            return index;
        }

        return index;
    }

    /**
    *  @brief Definition of a ode of the rgrid.
    */

    template<class T, typename C, size_t D, class L>
    struct Cells
    {
        typedef Box<C, D> Box_;

        Cells(const Box_& rBox_p) :
            m_Box(rBox_p),
            m_InsertMutex(),
            m_Subcells(),
            m_Subcount(0)
        {
        }

        ~Cells()
        {
        }

        size_t size() const { return 1 << D; }

        std::pair<Box_, int32_t> index(const Box_& rBox_p) const
        {
            return index_<C,D>(m_Box, rBox_p);
        }

        const std::shared_ptr<L> operator[](size_t idx_p) const { return m_Subcells[idx_p]; }
        std::shared_ptr<L> operator[](size_t idx_p) { return m_Subcells[idx_p]; }

        void add(size_t Idx_p, const Box_& rTile_p, const Box_& rBox_p, uint32_t Depth_p, const T& rData_p, tdb::database& rDb_p)
        {
            if (m_Subcells[Idx_p] == 0)
            {
                std::lock_guard<std::mutex> lock(m_InsertMutex);
                if (m_Subcells[Idx_p] == 0)
                    m_Subcells[Idx_p] = std::shared_ptr<L>(new L(rTile_p, Depth_p, rDb_p));
                ++m_Subcount;
            }
            m_Subcells[Idx_p]->add(rBox_p, rData_p, rDb_p);
        }

        bool intersects(size_t Idx_p, const Box_& rBox_p) const
        {
            return (m_Subcells[Idx_p] != 0) && m_Subcells[Idx_p]->intersects(rBox_p);
        }

        friend std::ostream& operator<<(std::ostream& os, const Cells& C_)
        {
            os << C_.m_Box;
            os.write((const char*)&C_.m_Subcount, sizeof(C_.m_Subcount));

            for (size_t idx=0; idx <= D; ++idx)
            {
                if (C_.m_Subcells[idx])
                {
                    os.write((const char*)&idx, sizeof(idx));
                    os << (*C_.m_Subcells[idx]);
                }
            }

            return os;
        }

        friend std::istream& operator>>(std::istream& is, Cells& C_)
        {
            is >> C_.m_Box;
            is.read((char*)&C_.m_Subcount, sizeof(C_.m_Subcount));

            for (size_t idx(0); idx < C_.m_Subcount; ++idx)
            {
                size_t cidx(0);
                is.read((char*)&cidx, sizeof(cidx));
                C_.m_Subcells[cidx] = std::shared_ptr<L>(new L());
                is >> (*C_.m_Subcells[cidx]);
            }
            return is;
        }

        Box_ m_Box;
        std::mutex m_InsertMutex;

        std::shared_ptr<L> m_Subcells[1 << D];
        uint8_t m_Subcount;
    };

    /**
    *  @brief Internal representation of the rgrid structure.
    */

    template<class T, typename C, size_t D, class L>
    class rgrid_
    {
    public:
        typedef Box<C, D> Box_;
        typedef TaskInfo<L> TaskInfo_;

        rgrid_(const Box_& rBox_p, uint32_t Depth_p, tdb::database& rDb_p) :
            m_PackageID(rDb_p.NewPackage()),
            m_Depth(Depth_p),
            m_Size(0),
            m_MaxShapeSize(0),
            m_Subcells(rBox_p)
        {
        }
        ~rgrid_() {}

        const Box_& bounding_box() const { return m_Subcells.m_Box; }

        friend std::ostream& operator<<(std::ostream& os, const rgrid_& R)
        {
            os.write((const char*)&R.m_PackageID, sizeof(R.m_PackageID));
            os.write((const char*)&R.m_Depth, sizeof(R.m_Depth));
            os.write((const char*)&R.m_Size, sizeof(R.m_Size));
            os.write((const char*)&R.m_MaxShapeSize, sizeof(R.m_MaxShapeSize));

            os << R.m_Subcells;

            return os;
        }

        friend std::istream& operator>>(std::istream& is, rgrid_& R)
        {
            is.read((char*)&R.m_PackageID, sizeof(R.m_PackageID));
            is.read((char*)&R.m_Depth, sizeof(R.m_Depth));
            is.read((char*)&R.m_Size, sizeof(R.m_Size));
            is.read((char*)&R.m_MaxShapeSize, sizeof(R.m_MaxShapeSize));

            is >> R.m_Subcells;

            return is;
        }

    protected:
        friend L;
        friend class rgrid<T, C, D, L>;

        rgrid_() :
            m_PackageID(0),
            m_Depth(0),
            m_Size(0),
            m_MaxShapeSize(0),
            m_Subcells(Box_())
        {
        }

        void add(const Box_& rBox_p, const T& rData_p, tdb::database& rDb_p);

        void query(const Box_& rBox_p, Functor<T>& rCallback_p, tdb::database& rDb_p, uint8_t Threads_p);

        static void loop(const Box_& rBox_p,
            Functor<T>* pCallback_p,
            tdb::database* pDb_p,
            bool Continue_p,

            TaskInfo_* pTaskInfo_p,
            std::atomic<uint16_t>* pTaskCount_p,
            std::mutex* pThreadMutex_p,
            std::vector<TaskInfo_*>* pTaskHeap_p,
            bool* pThreadExit_p);

        void recurse(const Box_& rBox_p, Functor<T>& rCallback_p, tdb::database& rDb_p,
            std::atomic<uint16_t>* pTaskCount_p,
            std::mutex* pThreadMutex_p,
            std::vector<TaskInfo_*>* pTaskHeap_p) const;

        uint64_t m_PackageID;
        uint32_t m_Depth;

        std::atomic<size_t> m_Size;
        std::atomic<size_t> m_MaxShapeSize;

        Cells<T, C, D, L> m_Subcells;
    };

    /**
    *  @brief External representation of the rgrid structure.
    */

    template<class T, typename C, size_t D, class L = Link<T, C, D> >
    class rgrid
    {
    public:
        typedef Box<C, D> Box_;
        typedef TaskInfo<L> TaskInfo_;

        rgrid(const Box_& rBox_p, uint32_t Depth_p, tdb::database& rDb_p) :
            m_rDatabase(rDb_p),
            m_pRoot(new L(rBox_p, Depth_p, rDb_p)),
            m_Depth(Depth_p)
        {
        }

        rgrid(std::istream& is, tdb::database& rDb_p) :
            m_rDatabase(rDb_p),
            m_pRoot(new L()),
            m_Depth(0)
        {
            is.read((char*)&m_Depth, sizeof(m_Depth));
            is >> (*m_pRoot);
        }

        void add(const Box_& rBox_p, const T& rData_p)
        {
            m_pRoot->add(rBox_p, rData_p, m_rDatabase);
        }

        /**
        *  @brief Main entry for query operation over multiple threads.
        */

        void query(const Box_& rBox_p, Functor<T>& rCallback_p, uint8_t Threads_p = 0)
        {
            std::mutex threadLock;
            std::atomic<uint16_t> taskCount(0);
            bool threadExit(false);

            std::vector<std::pair<TaskInfo_, std::shared_ptr<std::thread>>> threadHeap(++Threads_p);
            std::vector<TaskInfo_*> taskHeap;
            for (size_t iter(1); iter < Threads_p; ++iter)
            {
                TaskInfo_& rTaskInfo(threadHeap[iter].first);

                threadHeap[iter].second =
                    std::shared_ptr<std::thread>(new std::thread(&rgrid_<T, C, D, L>::loop, rBox_p, &rCallback_p, &m_rDatabase,
                        true, &rTaskInfo, &taskCount, &threadLock, &taskHeap, &threadExit));
                taskHeap.push_back(&rTaskInfo);
            }

            TaskInfo_* pTaskInfo = &threadHeap.front().first;
            pTaskInfo->m_pNode = m_pRoot;
            ++taskCount;

            while (taskCount != 0)
            {
                rgrid_<T, C, D, L>::loop(rBox_p, &rCallback_p, &m_rDatabase, false, pTaskInfo, &taskCount, &threadLock, &taskHeap, &threadExit);
            }

            threadExit = true;
            for (size_t iter(1); iter < Threads_p; ++iter)
            {
                TaskInfo_& rTaskInfo(threadHeap[iter].first);
                {
                    std::lock_guard<std::mutex> lock(rTaskInfo.m_Mutex);
                    rTaskInfo.m_Signal.notify_one();
                }
                threadHeap[iter].second->join();
            }
        }

        friend std::ostream& operator<<(std::ostream& os, const rgrid& R)
        {
            os.write((const char*)&R.m_Depth, sizeof(R.m_Depth));
            os << (*R.m_pRoot);

            return os;
        }

    private:
        tdb::database& m_rDatabase;
        std::shared_ptr<L> m_pRoot;
        uint32_t m_Depth;
    };
};

/**
*  @brief Routine to add a new element to the rgrid.
*/

template<class T, typename C, size_t D, class L>
void tdb::rgrid_<T, C, D, L>::add(const tdb::Box<C, D>& rBox_p, const T& rData_p, tdb::database& rDb_p)
{
    ++m_Size;
    const std::pair<Box<C, D>, int32_t> idx(m_Depth > 0 ? m_Subcells.index(rBox_p) : std::make_pair(m_Subcells.m_Box, int32_t(-1)));

    if (idx.second < 0)
    {
        size_t dataSize = rData_p.size();
        do
        {
            dataSize = m_MaxShapeSize.exchange(std::max(m_MaxShapeSize.load(), dataSize));
        } while (dataSize > m_MaxShapeSize);

        rDb_p.Store(m_PackageID, rData_p.size(), (const char*)& rData_p);
    }
    else
    {
        m_Subcells.add(idx.second, idx.first, rBox_p, m_Depth, rData_p, rDb_p);
    }
}

/**
*  @brief Main query loop for parallel thread execution.
*/

template<class T, typename C, size_t D, class L>
void tdb::rgrid_<T, C, D, L>::loop(const Box<C, D>& rBox_p,
    tdb::Functor<T>* pCallback_p,
    tdb::database* pDb_p,
    bool Continue_p,

    TaskInfo<L>* pTaskInfo_p,
    std::atomic<uint16_t>* pTaskCount_p,
    std::mutex* pThreadMutex_p,
    std::vector<TaskInfo<L>*>* pTaskHeap_p,
    bool* pThreadExit_p)
{
    bool continueLoop(Continue_p);
    do
    {
        std::unique_lock<std::mutex> lock(pTaskInfo_p->m_Mutex);

        if (pTaskInfo_p->m_pNode)
        {
            try
            {
                pTaskInfo_p->m_pNode->recurse(rBox_p, *pCallback_p, (*pDb_p), pTaskCount_p, pThreadMutex_p, pTaskHeap_p);
            }
            catch (...)
            {
                continueLoop = false;
            }

            pTaskInfo_p->m_pNode.reset();
            --(*pTaskCount_p);

            if (continueLoop)
            {
                std::lock_guard<std::mutex> lock_(*pThreadMutex_p);
                (*pTaskHeap_p).push_back(pTaskInfo_p);
            }
        }
        else if (continueLoop)
        {
            pTaskInfo_p->m_Signal.wait_for(lock, std::chrono::milliseconds(10));
        }

        continueLoop &= (*pThreadExit_p == false);
    } while (continueLoop);

    {
        std::unique_lock<std::mutex> lock(pTaskInfo_p->m_Mutex);
        pTaskInfo_p->m_pNode.reset();
    }
}

/**
*  @brief Recursion into next level of the rgrid cells.
*/

template<class T, typename C, size_t D, class L>
void tdb::rgrid_<T, C, D, L>::recurse(const Box<C, D>& rBox_p, tdb::Functor<T>& rCallback_p, tdb::database& rDb_p,

    std::atomic<uint16_t>* pTaskCount_p,
    std::mutex* pThreadMutex_p,
    std::vector<TaskInfo<L>*>* pTaskHeap_p) const
{
    tdb::ReadInfo readHandle = rDb_p.Open(m_PackageID);

    uint8_t subCells = m_Subcells.m_Subcount;
    for (uint8_t idx(0); idx < m_Subcells.size(); ++idx)
    {
        if (m_Subcells.intersects(idx, rBox_p))
        {
            std::unique_lock<std::mutex> lock(*pThreadMutex_p);

            if (((*pTaskHeap_p).empty() == false) && (subCells > 1))
            {
                TaskInfo<L>* pTaskInfo((*pTaskHeap_p).back());
                (*pTaskHeap_p).pop_back();
                ++(*pTaskCount_p);
                lock.unlock();

                std::lock_guard<std::mutex>(pTaskInfo->m_Mutex);
                pTaskInfo->m_pNode = m_Subcells[idx];
                pTaskInfo->m_Signal.notify_one();
            }
            else
            {
                lock.unlock();
                m_Subcells[idx]->recurse(rBox_p, rCallback_p, rDb_p, pTaskCount_p, pThreadMutex_p, pTaskHeap_p);
            }
        }
        --subCells;
    }

    if (m_MaxShapeSize > 0)
    {
        std::shared_ptr<char> pBuffer(new char[m_MaxShapeSize], tdb::array_deleter<char>());
        while (rDb_p.End(readHandle) == false)
        {
            T* pShape = new(pBuffer.get())T(rDb_p, readHandle);

            if (intersects<C, D>((*pShape).bounding_box(), rBox_p))
                rCallback_p(*pShape);
        }
    }
}

/**
*  @brief Routine to add a data element to the rgrid.
*/

template<class T, typename C, size_t D>
void tdb::Link<T, C, D>::add(const Box<C, D>& rBox_p, const T& rData_p, tdb::database& rDb_p)
{
    m_pRgrid->add(rBox_p, rData_p, rDb_p);
}

/**
*  @brief Box intersection of two elements.
*/

template<class T, typename C, size_t D>
bool tdb::Link<T, C, D>::intersects(const Box<C, D>& rBox_p) const
{
    return tdb::intersects<C, D>(rBox_p, m_pRgrid->bounding_box());
}

/**
*  @brief Recursion into the next cell level.
*/

template<class T, typename C, size_t D>
void tdb::Link<T, C, D>::recurse(const Box<C, D>& rBox_p, tdb::Functor<T>& rCallback_p, tdb::database& rDb_p,
    std::atomic<uint16_t>* pTaskCount_p,
    std::mutex* pThreadMutex_p,
    std::vector<TaskInfo<Link<T, C, D>>*>* pTaskHeap_p) const
{
    m_pRgrid->recurse(rBox_p, rCallback_p, rDb_p, pTaskCount_p, pThreadMutex_p, pTaskHeap_p);
}

/**
*  @brief Point increment operator.
*/

template<typename C, size_t D>
tdb::Point<C, D> operator+(const tdb::Point<C, D>& a, const tdb::Point<C, D>& b)
{
    return tdb::Point<C, D>(a) += b;
}

/**
*  @brief Point subtract operator.
*/

template<typename C, size_t D>
tdb::Point<C, D> operator-(const tdb::Point<C, D>& a, const tdb::Point<C, D>& b)
{
    return tdb::Point<C, D>(a) -= b;
}

/**
*  @brief Box increment operator.
*/

template<typename C, size_t D>
tdb::Box<C, D>& operator+=(tdb::Box<C, D>& a, const tdb::Point<C, D>& b)
{
    a.min_corner() += b;
    a.max_corner() += b;
    return a;
}
