#ifndef PERIOR_TREE_RTREE_HPP
#define PERIOR_TREE_RTREE_HPP
#include <periortree/point_traits.hpp>
#include <periortree/rectangle_traits.hpp>
#include <periortree/indexable.hpp>
#include <periortree/node.hpp>
#include <periortree/expand.hpp>
#include <periortree/within.hpp>
#include <periortree/area.hpp>
#include <periortree/to_svg.hpp>
#include <periortree/containers.hpp>

#include <boost/optional.hpp>
#include <limits>

namespace perior
{

template<std::size_t Max, std::size_t Min = Max / 3>
struct quadratic
{
    static constexpr std::size_t min_entry = Min;
    static constexpr std::size_t max_entry = Max;
};

template<typename T,
         typename Params,
         typename Boundary,
         typename IndexableGetter = indexable_getter<T>,
         typename EqualTo         = std::equal_to<T>,
         typename Allocator       = std::allocator<T> >
class rtree
{
  public:
    typedef T               value_type;
    typedef Params          parameter_type;
    typedef Boundary        boundary_type;
    typedef IndexableGetter indexable_getter_type;
    typedef EqualTo         equal_to_type;
    typedef Allocator       allocator_type;

    typedef typename indexable_getter_type::indexable_type        indexable_type;
    typedef typename traits::point_type_of<indexable_type>::type  point_type;
    typedef typename traits::scalar_type_of<indexable_type>::type scalar_type;

    BOOST_STATIC_CONSTEXPR std::size_t dimension = traits::dimension<point_type>::value;
    BOOST_STATIC_CONSTEXPR std::size_t min_entry = parameter_type::min_entry;
    BOOST_STATIC_CONSTEXPR std::size_t max_entry = parameter_type::max_entry;

    typedef boost::container::vector<value_type, allocator_type> container_type;
    typedef typename container_type::iterator       iterator;
    typedef typename container_type::const_iterator const_iterator;

    typedef detail::rtree_node<point_type, min_entry, max_entry> node_type;
    typedef typename node_type::aabb_type aabb_type;

    typedef typename allocator_type::template rebind<node_type>::other
            node_allocator_type;
    typedef typename allocator_type::template rebind<std::size_t>::other
            size_t_allocator_type;
    typedef typename gen_vector<node_type, node_allocator_type>::type tree_type;
    typedef typename gen_small_vector<std::size_t, 8, size_t_allocator_type
        >::type index_buffer_type;

    BOOST_STATIC_CONSTEXPR std::size_t nil = std::numeric_limits<std::size_t>::max();

  public:

    rtree(): root_(nil){}
    ~rtree(){}
    rtree(const rtree& rhs)
        : root_(rhs.root_), equal_to_(rhs.equal_to_), boundary_(rhs.boundary_),
          tree_(rhs.tree_), container_(rhs.container_),
          overwritable_values_(rhs.overwritable_values_),
          overwritable_nodes_(rhs.overwritable_nodes_)
    {}
    rtree& operator=(const rtree& rhs)
    {
        root_      = rhs.root_;
        equal_to_  = rhs.equal_to_;
        boundary_  = rhs.boundary_;
        tree_      = rhs.tree_;
        container_ = rhs.container_;
        overwritable_values_ = rhs.overwritable_values_;
        overwritable_nodes_  = rhs.overwritable_nodes_;
        return *this;
    }

    explicit rtree(const boundary_type& b): root_(nil), boundary_(b){}
    explicit rtree(const equal_to_type& e): root_(nil), equal_to_(e){}
    rtree(const boundary_type& b, const equal_to_type& e)
        : root_(nil), equal_to_(e), boundary_(b)
    {}

    std::size_t size() const BOOST_NOEXCEPT_OR_NOTHROW {return container_.size();}
    bool empty()       const BOOST_NOEXCEPT_OR_NOTHROW {return this->root_ == nil;}
    void clear()
    {
        this->root_ = nil;
        this->tree_.clear();
        this->container_.clear();
        this->overwritable_nodes_.clear();
        this->overwritable_values_.clear();
        return;
    }

    void insert(const value_type& v)
    {
        const std::size_t     idx   = this->add_value(v);
        const indexable_type& entry = indexable_getter_(v);
        const std::size_t     L     = this->choose_leaf(entry);

        if(tree_.at(L).has_enough_storage())
        {
            tree_.at(L).entry.push_back(idx);
            tree_.at(L).box = expand(tree_.at(L).box, entry, this->boundary_);
            this->adjust_tree(L);
        }
        else
        {
            const std::size_t LL = this->add_node(this->split_leaf(L, idx, entry));
            this->adjust_tree(L, LL);
        }
        return;
    }
    // if found, erase and return true. if not found, return false.
    bool remove(const value_type& v)
    {
        if(this->root_ == nil)
        {
            return false;
        }
        boost::optional<
            std::pair<std::size_t, typename node_type::const_iterator>
            > found = this->find_leaf(this->root_, v);
        if(found)
        {
            const std::size_t node_idx  =   found->first;
            const std::size_t value_idx = *(found->second);
            this->tree_.at(node_idx).entry.erase(found->second);
            this->erase_value(value_idx);
            this->condense_box(this->tree_.at(node_idx));
            this->condense_leaf(node_idx);
            return true;
        }
        return false;
    }

    template<typename Query, typename OutputIterator>
    void query(Query q, OutputIterator out) const
    {
        if(this->root_ == nil){return;}
        return query_impl(this->root_, q, out);
    }

    std::ostream& dump(std::ostream& os) const
    {
        if(this->empty()){return os;}

        std::vector<std::string> colors; colors.reserve(3);
        colors.push_back("red");
        colors.push_back("green");
        colors.push_back("blue");
        return dump_node(os, this->root_, 0, colors);
    }

  private:

    std::ostream& dump_node(std::ostream& os,
              const std::size_t N, const std::size_t depth,
              const std::vector<std::string>& clrs) const
    {
        const std::string& clr = clrs.at(depth % 3);
        const node_type& node = tree_.at(N);
        to_svg(os, node.box, this->boundary_, clr, 1, "none");
        os << '\n';
        if(node.is_leaf)
        {
            for(typename node_type::const_iterator
                    i(node.entry.cbegin()), e(node.entry.cend()); i!=e; ++i)
            {
                to_svg(os, indexable_getter_(this->container_.at(*i)),
                       this->boundary_, "black", 1, "black");
                os << '\n';
            }
            return os;
        }
        else // internal node
        {
            for(typename node_type::const_iterator
                    i(node.entry.cbegin()), e(node.entry.cend()); i!=e; ++i)
            {
                this->dump_node(os, *i, depth+1, clrs);
            }
            return os;
        }
    }

    std::size_t choose_leaf(const indexable_type& entry)
    {
        if(this->root_ == nil)
        {
            node_type n(true, nil);
            n.box = make_aabb(entry);
            this->root_ = this->add_node(n);

            return this->root_;
        }

        // choose a leaf to insert
        // so if root is a leaf, return it
        std::size_t node_idx = this->root_;
        while(!(this->tree_.at(node_idx).is_leaf))
        {
            // find minimum expansion
            scalar_type diff_area_min = std::numeric_limits<scalar_type>::max();
            scalar_type area_min      = std::numeric_limits<scalar_type>::max();

            const node_type& node = this->tree_.at(node_idx);
            for(typename node_type::const_iterator
                    i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                const scalar_type area_initial
                    = area(this->tree_.at(*i).box, this->boundary_);

                const scalar_type area_expanded
                    = area(expand(this->tree_.at(*i).box, entry, this->boundary_),
                           this->boundary_);

                const scalar_type diff_area = area_expanded - area_initial;
                if((diff_area <  diff_area_min) ||
                   (diff_area == diff_area_min  && area_expanded < area_min))
                {
                    node_idx = *i;
                    diff_area_min = diff_area;
                    area_min      = std::min(area_min, area_expanded);
                }
            }
        }
        return node_idx;
    }

    void adjust_tree(std::size_t node_idx)
    {
        while(tree_.at(node_idx).parent != nil)
        {
            const node_type& node = tree_.at(node_idx);
            node_type& parent_ = tree_.at(node.parent);
            parent_.box = expand(parent_.box, node.box, this->boundary_);
            node_idx = node.parent;
        }
        return;
    }
    void adjust_tree(const std::size_t N, const std::size_t NN)
    {
        if(tree_.at(N).parent == nil) // grow tree taller
        {
            node_type new_root(false, nil);
            new_root.entry.push_back(N);
            new_root.entry.push_back(NN);
            new_root.box = expand(tree_.at(N).box, tree_.at(NN).box, this->boundary_);
            this->root_ = this->add_node(new_root);

            this->tree_.at(N).parent = this->root_;
            this->tree_.at(NN).parent = this->root_;
            return;
        }
        else
        {
            const node_type& node    = tree_.at(N);
            const node_type& partner = tree_.at(NN);
            assert(node.parent == partner.parent);

            node_type& parent_ = tree_.at(node.parent);
            parent_.box = expand(parent_.box, node.box, this->boundary_); // for N

            if(parent_.has_enough_storage())
            {
                parent_.box = expand(parent_.box, partner.box, this->boundary_); // for NN
                parent_.entry.push_back(NN);
                return this->adjust_tree(node.parent);
            }
            else
            {
                const std::size_t PP = this->split_node(node.parent, NN);
                return this->adjust_tree(node.parent, PP);
            }
        }
    }

    boost::optional<std::pair<std::size_t, typename node_type::const_iterator> >
    find_leaf(std::size_t node_idx, const value_type& entry) const
    {
        const node_type& node = tree_.at(node_idx);
        if(within(indexable_getter_(entry), node.box, this->boundary_) == false)
        {
            return boost::none;
        }

        if(node.is_leaf)
        {
            for(typename node_type::const_iterator
                    i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                if(equal_to_(container_.at(*i), entry))
                {
                    return std::make_pair(node_idx, i);
                }
            }
            return boost::none;
        }
        else
        {
            for(typename node_type::const_iterator
                    i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                if(!within(indexable_getter_(entry), tree_.at(*i).box, this->boundary_))
                {
                    continue;
                }

                boost::optional<
                    std::pair<std::size_t, typename node_type::const_iterator>
                    > found = this->find_leaf(*i, entry);
                if(found)
                {
                    return found;
                }
            }
            return boost::none;
        }
    }

    void condense_leaf(const std::size_t N)
    {
        const node_type& node = this->tree_.at(N);
        assert(node.is_leaf);

        if(node.has_enough_entry() || node.parent == nil)
        {
            return;
        }

        // copy index of objects
        typedef typename gen_small_vector<std::size_t, min_entry>::type temporal_vec_type;
        temporal_vec_type eliminated_objs;
        std::copy(node.entry.begin(), node.entry.end(),
                  std::back_inserter(eliminated_objs));

        // erase the node N from its parent and condense aabb
        typename node_type::iterator found = std::find(
                this->tree_.at(node.parent).entry.begin(),
                this->tree_.at(node.parent).entry.end(), N);
        assert(found != this->tree_.at(node.parent).entry.end());
        this->tree_.at(node.parent).entry.erase(found);
        this->condense_box(this->tree_.at(node.parent));

        // re-insert entries eliminated from node N
        for(typename temporal_vec_type::const_iterator
                i(eliminated_objs.begin()), e(eliminated_objs.end()); i!=e; ++i)
        {
            this->insert(this->container_.at(*i));
        }

        // condense ancester nodes...
        condense_node(node.parent);
        return;
    }

    void condense_node(const std::size_t N)
    {
        const node_type& node = this->tree_.at(N);
        assert(node.is_leaf == false);

        if(node.has_enough_entry())
        {
            return;
        }
        if(node.parent == nil && node.entry.size() == 1)
        {
            this->root_ = node.entry.front();
            this->erase_node(N);
            return;
        }

        // collect index of nodes that are children of the node to be removed
        typedef typename gen_small_vector<std::size_t, min_entry>::type temporal_vec_type;
        temporal_vec_type eliminated_nodes;
        std::copy(node.entry.begin(), node.entry.end(),
                  std::back_inserter(eliminated_nodes));

        // erase the node N from its parent and condense its aabb
        typename node_type::iterator found = std::find(
                this->tree_.at(node.parent).entry.begin(),
                this->tree_.at(node.parent).entry.end(), N);
        assert(found != this->tree_.at(node.parent).entry.end());
        this->tree_.at(node.parent).entry.erase(found);
        this->condense_box(this->tree_.at(node.parent));

        // re-insert nodes eliminated from node N
        for(typename temporal_vec_type::const_iterator
                i(eliminated_nodes.begin()), e(eliminated_nodes.end()); i!=e; ++i)
        {
            this->re_insert(*i);
        }
        this->condense_node(node.parent);
        return;
    }

    // TODO move it to quadratic struct
    node_type split_leaf(const std::size_t N,
                         const std::size_t vidx, const indexable_type& entry)
    {
        node_type& node = tree_.at(N);
        node_type  partner(true, node.parent);

        typedef typename gen_static_vector<std::pair<std::size_t, indexable_type>,
                max_entry+1>::type temporal_entry_container;
        temporal_entry_container entries;
        entries.push_back(std::make_pair(vidx, entry));

        for(typename node_type::const_iterator
                i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
        {
            entries.push_back(std::make_pair(
                        *i, indexable_getter_(container_.at(*i))));
        }
        node.entry.clear();
        partner.entry.clear(); // for make it sure

        /* assign first 2 entries to node and partner */
        {
            const boost::array<std::size_t, 2> seeds =
                this->pick_seeds(entries.begin(), entries.end());
               node.entry.push_back(entries.at(seeds[0]).first);
            partner.entry.push_back(entries.at(seeds[1]).first);

               node.box = make_aabb(entries.at(seeds[0]).second);
            partner.box = make_aabb(entries.at(seeds[1]).second);

            // remove them from entries pool
            entries.erase(entries.begin() + std::max(seeds[0], seeds[1]));
            entries.erase(entries.begin() + std::min(seeds[0], seeds[1]));
        }

        while(!entries.empty())
        {
            if(min_entry > node.entry.size() &&
               min_entry - node.entry.size() >= entries.size())
            {
                for(typename temporal_entry_container::const_iterator
                        i(entries.begin()), e(entries.end()); i != e; ++i)
                {
                    node.entry.push_back(i->first);
                    node.box = expand(node.box, i->second, this->boundary_);
                }
                return partner;
            }
            if(min_entry > partner.entry.size() &&
               min_entry - partner.entry.size() >= entries.size())
            {
                for(typename temporal_entry_container::const_iterator
                        i(entries.begin()), e(entries.end()); i != e; ++i)
                {
                    partner.entry.push_back(i->first);
                    partner.box = expand(partner.box, i->second, this->boundary_);
                }
                return partner;
            }

            const std::pair<std::size_t, bool> next =
                this->pick_next(entries.begin(), entries.end(), node.box, partner.box);
            if(next.second) // next is for node
            {
                node.entry.push_back(entries.at(next.first).first);
                node.box =
                    expand(node.box, entries.at(next.first).second, this->boundary_);
            }
            else // next is for partner
            {
                partner.entry.push_back(entries.at(next.first).first);
                partner.box =
                    expand(partner.box, entries.at(next.first).second, this->boundary_);
            }
            entries.erase(entries.begin() + next.first);
        }
        return partner;
    }

    // ConstIterator::value_type should be
    // std::pair<std::size_t, {indexable_type or aabb_type}>
    template<typename ConstIterator>
    boost::array<std::size_t, 2>
    pick_seeds(const ConstIterator first, const ConstIterator last)
    {
        assert(std::distance(first, last) >= 2);

        boost::array<std::size_t, 2> retval;

        scalar_type max_d = 0;
        for(ConstIterator iter(first), iend(last - 1); iter != iend; ++iter)
        {
            for(ConstIterator jter(iter+1), jend(last); jter != jend; ++jter)
            {
                const aabb_type E1I = make_aabb(iter->second);
                const aabb_type E2I = make_aabb(jter->second);
                const aabb_type J = expand(E1I, E2I, this->boundary_);
                const scalar_type d =
                    area(J, boundary_) - area(E1I, boundary_) - area(E2I, boundary_);
//                 assert(d >= 0); // if two overlaps, ...
                if(max_d < d)
                {
                    max_d = d;
                    retval[0] = std::distance(first, iter);
                    retval[1] = std::distance(first, jter);
                }
            }
        }
        return retval;
    }

    // ConstIterator::value_type should be
    // std::pair<std::size_t, {indexable_type or aabb_type}>
    template<typename ConstIterator>
    std::pair<std::size_t, bool> pick_next(
            const ConstIterator first, const ConstIterator last,
            const aabb_type& node, const aabb_type& ptnr)
    {
        assert(first != last);
        bool is_node;
        std::size_t idx;
        scalar_type max_dd = -1;
        for(ConstIterator iter(first); iter != last; ++iter)
        {
            const aabb_type box1 =
                expand(node, make_aabb(iter->second), this->boundary_);
            const aabb_type box2 =
                expand(ptnr, make_aabb(iter->second), this->boundary_);

            const scalar_type d1 = area(box1, this->boundary_) - area(node, this->boundary_);
            const scalar_type d2 = area(box2, this->boundary_) - area(ptnr, this->boundary_);
            const scalar_type dd = d1 - d2;
            if(max_dd < std::abs(dd))
            {
                max_dd = std::abs(dd);
                idx = std::distance(first, iter);
                is_node = (dd < 0);
            }
        }
        return std::make_pair(idx, is_node);
    }

    // split nodes because of new node NN by quadratic algorithm
    std::size_t split_node(const std::size_t P, const std::size_t NN)
    {
        node_type& node = tree_.at(P);
        const std::size_t PP = this->add_node(node_type(false, node.parent));
        node_type& partner = tree_.at(PP);

        typedef typename gen_static_vector<std::pair<std::size_t, aabb_type>,
                max_entry+1>::type temporal_entry_container;
        temporal_entry_container entries;
        entries.push_back(std::make_pair(NN, tree_.at(NN).box));

        for(typename node_type::const_iterator
                i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
        {
            entries.push_back(std::make_pair(*i, tree_.at(*i).box));

        }
        node.entry.clear();
        partner.entry.clear(); // for make it sure

        /* assign first 2 entries to node and partner */{
            const boost::array<std::size_t, 2> seeds =
                this->pick_seeds(entries.begin(), entries.end());

               node.entry.push_back(entries.at(seeds[0]).first);
            partner.entry.push_back(entries.at(seeds[1]).first);

            tree_.at(entries.at(seeds[0]).first).parent = P;
            tree_.at(entries.at(seeds[1]).first).parent = PP;

               node.box = entries.at(seeds[0]).second;// these are AABB.
            partner.box = entries.at(seeds[1]).second;

            // remove them from entries pool
            entries.erase(entries.begin() + std::min(seeds[0], seeds[1]));
            entries.erase(entries.begin() + std::max(seeds[0], seeds[1]) - 1);
        }

        while(!entries.empty())
        {
            if(min_entry > node.entry.size() &&
               min_entry - node.entry.size() >= entries.size())
            {
                for(typename temporal_entry_container::const_iterator
                        i(entries.begin()), e(entries.end()); i != e; ++i)
                {
                    node.entry.push_back(i->first);
                    node.box = expand(node.box, i->second, this->boundary_);
                }
                return PP;
            }
            if(min_entry > partner.entry.size() &&
               min_entry - partner.entry.size() >= entries.size())
            {
                for(typename temporal_entry_container::const_iterator
                        i(entries.begin()), e(entries.end()); i != e; ++i)
                {
                    partner.entry.push_back(i->first);
                    partner.box = expand(partner.box, i->second, this->boundary_);
                }
                return PP;
            }

            const std::pair<std::size_t, bool> next =
                this->pick_next(entries.begin(), entries.end(), node.box, partner.box);
            if(next.second) // next is partner
            {
                node.entry.push_back(entries.at(next.first).first);
                tree_.at(entries.at(next.first).first).parent = P;
                node.box =
                    expand(node.box, entries.at(next.first).second, this->boundary_);
            }
            else // next is for partner
            {
                partner.entry.push_back(entries.at(next.first).first);
                tree_.at(entries.at(next.first).first).parent = PP;
                partner.box =
                    expand(partner.box, entries.at(next.first).second, this->boundary_);
            }
            entries.erase(entries.begin() + next.first);
        }
        tree_.at(P) = node;
        tree_.at(PP) = partner;
        return PP;
    }

    template<typename Query, typename OutputIterator>
    void query_impl(std::size_t node_idx, Query q, OutputIterator out) const
    {
        const node_type& node = tree_.at(node_idx);
        if(node.is_leaf)
        {
            for(typename node_type::const_iterator
                i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                value_type const& val = container_.at(*i);
                if(q.match(indexable_getter_(val), this->boundary_) && q.match(val))
                {
                    *out = val;
                    ++out;
                }
            }
        }
        else
        {
            for(typename node_type::const_iterator
                i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                const std::size_t next = *i;
                if(intersects(q.box(), tree_.at(next).box, this->boundary_))
                {
                    this->query_impl(next, q, out);
                }
            }
        }
        return;
    }

  private:

    std::size_t level_of(std::size_t node_idx) const
    {
        std::size_t level = 0;
        while(!(tree_.at(node_idx).is_leaf))
        {
            ++level;
            node_idx = tree_.at(node_idx).entry.front();
        }
        return level;
    }

    void re_insert(const std::size_t N)
    {
        // reset connection to the parent!

        // insert node to its proper parent. to find the parent of this node N,
        // add 1 to level. root node should NOT come here.
        const std::size_t lvl = level_of(N) + 1;
        const aabb_type&  entry = tree_.at(N).box;
        const std::size_t L = choose_node_with_level(entry, lvl);

        if(tree_.at(L).has_enough_storage())
        {
            tree_.at(L).entry.push_back(N);
            tree_.at(N).parent = L;
            expand(tree_.at(L).box, entry, this->boundary_);
            this->adjust_tree(L);
        }
        else
        {
            const std::size_t LL = this->split_node(L, N);
            this->adjust_tree(L, LL);
        }
        return;
    }

    std::size_t
    choose_node_with_level(const aabb_type& entry, const std::size_t lvl)
    {
        std::size_t node_idx = this->root_;
        if(level_of(this->root_) < lvl)
        {
            throw std::logic_error("root is under the node");
        }

        while(level_of(node_idx) != lvl)
        {
            scalar_type diff_area_min = std::numeric_limits<scalar_type>::max();
            scalar_type area_min      = std::numeric_limits<scalar_type>::max();

            const node_type& node = this->tree_.at(node_idx);
            for(typename node_type::const_iterator
                    i(node.entry.begin()), e(node.entry.end()); i != e; ++i)
            {
                const scalar_type area_initial = area(tree_.at(*i).box, this->boundary_);
                const aabb_type box = expand(tree_.at(*i).box, entry, this->boundary_);

                const scalar_type area_expanded = area(box, this->boundary_);
                const scalar_type diff_area     = area_expanded - area_initial;
                if(diff_area < diff_area_min ||
                   (diff_area == diff_area_min && area_expanded < area_min))
                {
                    node_idx = *i;
                    diff_area_min = diff_area;
                    area_min = std::min(area_expanded, area_min);
                }
            }
        }
        return node_idx;
    }

    void condense_box(node_type& node)
    {
        assert(!node.entry.empty());
        if(node.is_leaf)
        {
            typename node_type::const_iterator i(node.entry.begin());
            node.box = indexable_getter_(this->container_.at(*i));
            ++i;
            for(typename node_type::const_iterator e(node.entry.end()); i != e; ++i)
            {
                node.box = expand(node.box,
                        indexable_getter_(this->container_.at(*i)), this->boundary_);
            }
        }
        else
        {
            typename node_type::const_iterator i = node.entry.begin();
            node.box = this->tree_.at(*i).box;
            ++i;
            for(typename node_type::const_iterator e(node.entry.end()); i != e; ++i)
            {
                node.box = expand(node.box, this->tree_.at(*i).box, this->boundary_);
            }
        }
        return;
    }

  private:

    std::size_t add_value(const value_type& v)
    {
        if(overwritable_values_.empty())
        {
            const std::size_t idx = container_.size();
            container_.push_back(v);
            return idx;
        }
        else
        {
            const std::size_t idx = overwritable_values_.back();
            overwritable_values_.pop_back();
            container_.at(idx) = v;
            return idx;
        }
    }
    void erase_value(const std::size_t i)
    {
        overwritable_values_.push_back(i);
        return;
    }

    std::size_t add_node(const node_type& n)
    {
        if(overwritable_nodes_.empty())
        {
            const std::size_t idx = tree_.size();
            tree_.push_back(n);
            return idx;
        }
        else
        {
            const std::size_t idx = overwritable_nodes_.back();
            overwritable_nodes_.pop_back();
            tree_.at(idx) = n;
            return idx;
        }
    }
    void erase_node(const std::size_t i)
    {
        overwritable_nodes_.push_back(i);
        return;
    }

  private:

    std::size_t       root_;
    equal_to_type     equal_to_;
    boundary_type     boundary_;
    tree_type         tree_;
    container_type    container_;
    index_buffer_type overwritable_values_;
    index_buffer_type overwritable_nodes_;
    indexable_getter_type indexable_getter_;
};

} // perior
#endif//PERIOR_TREE_RTREE_HPP


#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <thread>
#include <geometry_msgs/PointStamped.h>
#include <std_srvs/Empty.h>
#include <Eigen/Core>

#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

ros::Publisher trajectory_pub;
geometry_msgs::PointStamped current_position;

float MAX_ERROR = 0.3; //与目标点的最大误差距离，因为数据有误差，所以需要根据运动尺度设置合适的值
float HEIGHT = 2; //飞机起飞高度
float linear_smoothing_navigation_step = 0.5; //数字越大导航速度越快
bool flag_gps_initialized_OK = false;
bool flag_take_off_OK = false;
int flag_tasks_OK = 0;
Eigen::Vector3d home;

/*
Description:
    updateUavPosition(const geometry_msgs::PointStamped& msg)
    gps数据更新的回调函数.

Parameters:
    msg 位置信息

Return:
    无
*/
void updateUavPosition(const geometry_msgs::PointStamped& msg)
{
    if (!flag_gps_initialized_OK)
    {
        flag_gps_initialized_OK= true;
        home[0] = msg.point.x;
        home[1] = msg.point.y;
        home[2] = msg.point.z;
    }
    current_position = msg;
    // std::cout<<"UAV current position is: "<<msg.point.x<< msg.point.y<< msg.point.z<<std::endl;
}

/*
Description:
    getDistanceToTarget(const Eigen::Vector3d& target)
    获取当前位置到指定位置位置的距离.

Parameters:
    target 需要飞达的位置点

Return:
    double 当前位置到达目标点的位置
*/
double getDistanceToTarget(const Eigen::Vector3d& target)
{
    // std::cout << std::to_string(current_position.point.x) << std::endl;
    // std::cout << std::to_string(current_position.point.y) << std::endl;
    // std::cout << std::to_string(current_position.point.z) << std::endl;
    double temp = 0;
    temp += pow((target[0] - current_position.point.x), 2);
    temp += pow((target[1] - current_position.point.y), 2);
    temp += pow((target[2] - current_position.point.z), 2);
    temp = sqrt(temp);
    return temp;
}

/*
Description:
    reachTargetPosition(const Eigen::Vector3d& target, float max_error)
    判定是否到达指定的目标点.

Parameters:
    target 需要飞达的位置点
    max_error 允许的位置误差阈值,当前位置和目标位置小于该阈值时,判定无人机到达目标点

Return:
    bool 到达目标点时返回 true
            未到达目标点时返回 false
*/
bool reachTargetPosition(const Eigen::Vector3d& target, float max_error)
{
    double temp = getDistanceToTarget(target);
    // std::cout << "Dsitance: " << std::to_string(temp) << std::endl;
    // std::cout << "Max Error: " << std::to_string(max_error) << std::endl;
    if (temp < max_error)
        return true;
    return false;
}

/*
Description:
    linearSmoothingNavigationTask(const Eigen::Vector3d& target)
    控制无人机从当前位置飞向指定位置.

Parameters:
    target 需要飞达的位置点

Return:
    bool 起飞结束后返回 true
            起飞过程中返回 false
*/
bool linearSmoothingNavigationTask(const Eigen::Vector3d& target)
{
    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();

    if (reachTargetPosition(target, MAX_ERROR))
        return true;

    double dist = getDistanceToTarget(target);
    Eigen::Vector3d next_step;

    if(dist<linear_smoothing_navigation_step)
    {
        next_step = target;
    }
    else
    {
        next_step[0] = current_position.point.x+(target[0]-current_position.point.x)/dist*linear_smoothing_navigation_step;
        next_step[1] = current_position.point.y+(target[1]-current_position.point.y)/dist*linear_smoothing_navigation_step;
        next_step[2] = current_position.point.z+(target[2]-current_position.point.z)/dist*linear_smoothing_navigation_step;
    }

    double desired_yaw = 0.0;

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(next_step, desired_yaw, &trajectory_msg);
    trajectory_pub.publish(trajectory_msg);
    return false;
}

/*
Description:
    takeOffTask(float height)
    起飞函数,调用后无人机从起始位置起飞指定高度.

Parameters:
    height 指定的起飞高度

Return:
    bool 起飞结束后返回 true
            起飞过程中返回 false
*/
bool takeOffTask(float height)
{
    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();

    static Eigen::Vector3d desired_position(current_position.point.x, current_position.point.y, height);
    double desired_yaw = 0.0;

    // std::cout << std::to_string(desired_position[0]) << std::endl;
    // std::cout << std::to_string(desired_position[1]) << std::endl;
    // std::cout << std::to_string(desired_position[2]) << std::endl;
    if (reachTargetPosition(desired_position, MAX_ERROR))
        return true;

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);
    trajectory_pub.publish(trajectory_msg);
    return false;
}

/*
Description:
    gohome()
    反航函数,调用后无人机先沿着当前高度飞到反航点正上方,然后降落.

Parameters:
    无

Return:
    无
*/
void gohome()
{
    static Eigen::Vector3d temp(home[0], home[1], current_position.point.z);
    static bool flag_temp = false;

    if (!flag_temp)
    {
        flag_temp = linearSmoothingNavigationTask(temp);
    }
    else
    {
        linearSmoothingNavigationTask(home);
    }
}

int main(int argc, char** argv) {
    // ROS_INFO("video record is running...");
    ros::init(argc, argv, "Video_Record");
    ros::NodeHandle nh;
    // Create a private node handle for accessing node parameters.
    ros::NodeHandle nh_private("~");

    std::string uav_name = "";
    ros::param::get("~mav_name",uav_name);

    // 订阅话题
    // /odometry_sensor1/position   无人机位置信息(包含噪声)
    ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUavPosition);

    trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

    // 等待5s,让Gazebo可以成功启动.
    ros::Duration(5.0).sleep();

    // 创建控制Gazebo自动运行的服务,这里自动运行是指让Gazebo自动Play
    std_srvs::Empty srv;
    bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);

    // 尝试让Gazebo自动运行
    int i=0;
    while (i <= 10 && !unpaused) {
        ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        unpaused = ros::service::call("/gazebo/unpause_physics", srv);
        ++i;
    }

    // 判断Gazebo有没有自动运行,没有成功Play则退出
    if (!unpaused) {
        ROS_FATAL("Could not wake up Gazebo.");
        return -1;
    } else {
        ROS_INFO("Unpaused the Gazebo simulation.");
    }

    std::vector<Eigen::Vector3d> path;
    path.push_back(Eigen::Vector3d(0.f,2.f,2.f));
    path.push_back(Eigen::Vector3d(0.f,-2.f,3.f));
    path.push_back(Eigen::Vector3d(2.f,-2.f,4.f));
    path.push_back(Eigen::Vector3d(2.f,2.f,4.f));
    path.push_back(Eigen::Vector3d(2.f,3.f,3.f));
    path.push_back(Eigen::Vector3d(1.f,2.f,2.f));
    path.push_back(Eigen::Vector3d(1.f,-2.f,2.f));
    std::cout << path.size() << std::endl;

    ros::Rate loop_rate(20);
    while (ros::ok())
    {
        if(flag_gps_initialized_OK && !flag_take_off_OK)
        {
            // ROS_INFO("UAV take off task is running...");
            flag_take_off_OK = takeOffTask(HEIGHT);
        }
        else if(flag_take_off_OK && flag_tasks_OK<path.size())
        {
            // ROS_INFO("UAV navigation task is running...");
            if(flag_tasks_OK<path.size())
            {
                bool temp = linearSmoothingNavigationTask(path[flag_tasks_OK]);
                if (temp)
                    flag_tasks_OK ++;
            }
        }
        else if(flag_tasks_OK >= path.size())
        {
            flag_tasks_OK = 0;
            // gohome();
        }

        ros::spinOnce();
        loop_rate.sleep();
    }
}


#include <state_estimation_ros/ros_conversions.h>
#include <Eigen/Geometry>

namespace state_estimation {

void toMsg(const Eigen::VectorXd& x, const std::array<int, 3>& map, geometry_msgs::Point* msg) {
    if (map[0] >= 0) {
        msg->x = x(map[0]);
    }
    if (map[1] >= 0) {
        msg->y = x(map[1]);
    }
    if (map[2] >= 0) {
        msg->z = x(map[2]);
    }
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 3>& map, geometry_msgs::Vector3* msg) {
    if (map[0] >= 0) {
        msg->x = x(map[0]);
    }
    if (map[1] >= 0) {
        msg->y = x(map[1]);
    }
    if (map[2] >= 0) {
        msg->z = x(map[2]);
    }
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 4>& map,
           geometry_msgs::Quaternion* msg) {
    if (map[0] >= 0) {
        msg->x = x(map[0]);
    }
    if (map[1] >= 0) {
        msg->y = x(map[1]);
    }
    if (map[2] >= 0) {
        msg->z = x(map[2]);
    }
    if (map[3] >= 0) {
        msg->w = x(map[3]);
    }
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 3>& map,
           geometry_msgs::Quaternion* msg) {
    const double phi = map[0] >= 0 ? x(map[0]) : 0.0;
    const double theta = map[1] >= 0 ? x(map[1]) : 0.0;
    const double psi = map[2] >= 0 ? x(map[2]) : 0.0;

    const Eigen::Quaterniond quat =
        (Eigen::Quaterniond)Eigen::AngleAxisd(psi, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(phi, Eigen::Vector3d::UnitX());

    msg->x = quat.x();
    msg->y = quat.y();
    msg->z = quat.z();
    msg->w = quat.w();
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 7>& map, geometry_msgs::Pose* msg) {
    toMsg(x, {map[0], map[1], map[2]}, &(msg->position));
    toMsg(x, {map[3], map[4], map[5], map[6]}, &(msg->orientation));
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 6>& map, geometry_msgs::Pose* msg) {
    toMsg(x, {map[0], map[1], map[2]}, &(msg->position));

    std::array<int, 3> rpy_map = {map[3], map[4], map[5]};
    toMsg(x, rpy_map, &(msg->orientation));
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 6>& map, geometry_msgs::Twist* msg) {
    toMsg(x, {map[0], map[1], map[2]}, &(msg->linear));
    toMsg(x, {map[3], map[4], map[5]}, &(msg->angular));
}

void toMsg(const Eigen::VectorXd& x, const std::array<int, 6>& map, sensor_msgs::Imu* msg) {
    toMsg(x, {map[0], map[1], map[2]}, &(msg->angular_velocity));
    toMsg(x, {map[3], map[4], map[5]}, &(msg->linear_acceleration));
}

void toMsg(double timestamp, const Eigen::VectorXd& x, const Eigen::MatrixXd& cov,
           std_msgs::Float64MultiArray* msg) {
    msg->layout.data_offset = 0;
    msg->layout.dim.resize(1);
    msg->layout.dim[0].label = "data";
    msg->layout.dim[0].size = 1 + x.size() + cov.rows() * cov.cols();
    msg->data.resize(msg->layout.dim[0].size);

    msg->data[0] = timestamp;

    for (int i = 0; i < x.size(); ++i) {
        msg->data[1 + i] = x(i);
    }

    for (int i = 0; i < cov.rows(); ++i) {
        for (int j = 0; j < cov.cols(); ++j) {
            uint32_t offset = i * cov.rows() + j;
            msg->data[1 + x.size() + offset] = cov(i, j);
        }
    }
}

void fromMsg(const geometry_msgs::Point& msg, const std::array<int, 3>& map, Eigen::VectorXd* x) {
    if (map[0] >= 0) {
        (*x)(map[0]) = msg.x;
    }
    if (map[1] >= 0) {
        (*x)(map[1]) = msg.y;
    }
    if (map[2] >= 0) {
        (*x)(map[2]) = msg.z;
    }
}

void fromMsg(const geometry_msgs::Vector3& msg, const std::array<int, 3>& map, Eigen::VectorXd* x) {
    if (map[0] >= 0) {
        (*x)(map[0]) = msg.x;
    }
    if (map[1] >= 0) {
        (*x)(map[1]) = msg.y;
    }
    if (map[2] >= 0) {
        (*x)(map[2]) = msg.z;
    }
}

void fromMsg(const geometry_msgs::Quaternion& msg, const std::array<int, 4>& map,
             Eigen::VectorXd* x) {
    if (map[0] >= 0) {
        (*x)(map[0]) = msg.x;
    }
    if (map[1] >= 0) {
        (*x)(map[1]) = msg.y;
    }
    if (map[2] >= 0) {
        (*x)(map[2]) = msg.z;
    }
    if (map[3] >= 0) {
        (*x)(map[3]) = msg.w;
    }
}

void fromMsg(const geometry_msgs::Quaternion& msg, const std::array<int, 3>& map,
             Eigen::VectorXd* x) {
    const Eigen::Quaterniond quat(msg.w, msg.x, msg.y, msg.z);
    Eigen::Vector3d rpy = quat.toRotationMatrix().eulerAngles(2, 1, 0);
    (*x)(map[0]) = rpy(2);
    (*x)(map[1]) = rpy(1);
    (*x)(map[2]) = rpy(0);
}

void fromMsg(const geometry_msgs::Pose& msg, const std::array<int, 7>& map, Eigen::VectorXd* x) {
    fromMsg(msg.position, {map[0], map[1], map[2]}, x);
    fromMsg(msg.orientation, {map[3], map[4], map[5], map[6]}, x);
}

void fromMsg(const geometry_msgs::Pose& msg, const std::array<int, 6>& map, Eigen::VectorXd* x) {
    fromMsg(msg.position, {map[0], map[1], map[2]}, x);

    std::array<int, 3> rpy_map = {map[3], map[4], map[5]};
    fromMsg(msg.orientation, rpy_map, x);
}

void fromMsg(const geometry_msgs::Twist& msg, const std::array<int, 6>& map, Eigen::VectorXd* x) {
    fromMsg(msg.linear, {map[0], map[1], map[2]}, x);
    fromMsg(msg.angular, {map[3], map[4], map[5]}, x);
}

void fromMsg(const sensor_msgs::Imu& msg, const std::array<int, 6>& map, Eigen::VectorXd* x) {
    fromMsg(msg.angular_velocity, {map[0], map[1], map[2]}, x);
    fromMsg(msg.linear_acceleration, {map[3], map[4], map[5]}, x);
}

}  // namespace state_estimation


#include <boost/math/constants/info.hpp>


#include "index_builder.h"
#include "definition.h"
#include "gdindex.h"
#include <boost/log/trivial.hpp>
#include <string>
#include <vector>

using namespace std;

namespace vrs
{
namespace components
{
bool index_builder::build_frame_based(const vector<string> &feature_file_paths,
                                      const char *out_file_path, uint num_gaussians, uint ld_mode, int verbose_level,
                                      const char *gdindex_parameters_path,
                                      bool gd_intra_normalization, bool gd_unbinarized)
{
	uint ld_length, ld_frame_length;
	string ld_extension, ld_name;
	switch (ld_mode) {
	case SIFT_LOCAL_DESCRIPTOR:
		//sift局部描述算子模式
		ld_length = SIFT_LENGTH;
		ld_frame_length = SIFT_FRAME_LENGTH;
		ld_extension = SIFT_EXTENSION;
		ld_name = SIFT_NAME;
		break;
	case SIFTGEO_LOCAL_DESCRIPTOR:
		//sift geo局部描述算子模式
		ld_length = SIFTGEO_LENGTH;
		ld_frame_length = SIFTGEO_FRAME_LENGTH;
		ld_extension = SIFTGEO_EXTENSION;
		ld_name = SIFTGEO_NAME;
		break;
	default:
		BOOST_LOG_TRIVIAL(error)<< "无法识别的局部描述算子:" << ld_mode;
		return false;
	}

	gdindex gi;
	gi.set_index_parameters(ld_length, ld_frame_length, ld_extension, ld_name,
	                        LD_PCA_DIM, LD_PRE_PCA_POWER_DEFAULT, num_gaussians,
	                        GD_POWER_DEFAULT, gd_intra_normalization, gd_unbinarized,
	                        gdindex_parameters_path, verbose_level);
	gi.set_query_parameters(0, ASYM_OFF, WORD_L1_NORM, 0, 0,
	                        gdindex_parameters_path, verbose_level);

	//生成数据库索引
	gi.generate_index(feature_file_paths,verbose_level);
	gi.write(out_file_path);
	return true;
}

}
} /* namespace vrs */


// Copyright (C) 2020 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_TEXT_ESTIMATED_WIDTH_HPP
#define BOOST_TEXT_ESTIMATED_WIDTH_HPP

#include <boost/text/concepts.hpp>
#include <boost/text/grapheme_view.hpp>

namespace boost { namespace text { namespace detail {
    inline int width_implied_by_cp(uint32_t cp)
    {
        using pair_type = std::pair<uint32_t, uint32_t>;
        constexpr pair_type double_wides[] = {
            {0x1100, 0x115f},
            {0x2329, 0x232a},
            {0x2e80, 0x303e},
            {0x3040, 0xa4cf},
            {0xac00, 0xd7a3},
            {0xf900, 0xfaff},
            {0xfe10, 0xfe19},
            {0xfe30, 0xfe6f},
            {0xff00, 0xff60},
            {0xffe0, 0xffe6},
            {0x1f300, 0x1f64f},
            {0x1f900, 0x1f9ff},
            {0x20000, 0x2fffd},
            {0x30000, 0x3fffd},
        };
        auto const last = std::end(double_wides);
        auto const it = std::lower_bound(
            std::begin(double_wides), last, cp, [](pair_type y, uint32_t x) {
                return y.second < x;
            });
        if (it != last && it->first <= cp && cp <= it->second)
            return 2;
        return 1;
    }

    template<typename I, typename S>
    std::size_t estimated_width_of_graphemes_impl(I first, S last)
    {
        std::size_t retval = 0;
        for (auto grapheme : boost::text::as_graphemes(first, last)) {
            retval += detail::width_implied_by_cp(grapheme.front());
        }
        return retval;
    }
}}}

namespace boost { namespace text { BOOST_TEXT_NAMESPACE_V1 {

    /** Returns the estimated width of the graphemes within `[first, last)`,
        using the same technique that used by `std::format()`.

        This function only participates in overload resolution if `CPIter`
        models the CPIter concept.

        \see [format.string.std] for a full description */
    template<typename CPIter, typename Sentinel>
    auto estimated_width_of_graphemes(CPIter first, Sentinel last)
        ->detail::cp_iter_ret_t<std::size_t, CPIter>
    {
        return detail::estimated_width_of_graphemes_impl(first, last);
    }

    /** Returns the estimated width of the graphemes within `r`, using the
        same technique that used by `std::format()`.

        This function only participates in overload resolution if `CPRange`
        models the CPRange concept.

        \see [format.string.std] for a full description */
    template<typename CPRange>
    auto estimated_width_of_graphemes(CPRange && r)
        ->decltype(detail::estimated_width_of_graphemes_impl(
            std::begin(r), std::end(r)))
    {
        return detail::estimated_width_of_graphemes_impl(
            std::begin(r), std::end(r));
    }

}}}

#if defined(BOOST_TEXT_DOXYGEN) || BOOST_TEXT_USE_CONCEPTS

namespace boost { namespace text { BOOST_TEXT_NAMESPACE_V2 {

    /** Returns the estimated width of the graphemes within `[first, last)`,
        using the same technique that used by `std::format()`.

        \see [format.string.std] for a full description */
    template<code_point_iter I, std::sentinel_for<I> S>
    std::size_t estimated_width_of_graphemes(I first, S last)
    {
        return detail::estimated_width_of_graphemes_impl(first, last);
    }

    /** Returns the estimated width of the graphemes within `r`, using the
        same technique that used by `std::format()`.

        \see [format.string.std] for a full description */
    template<code_point_range R>
    std::size_t estimated_width_of_graphemes(R && r)
    {
        return detail::estimated_width_of_graphemes_impl(
            std::ranges::begin(r), std::ranges::end(r));
    }

}}}

#endif

#endif


#include <boost/geometry/arithmetic/arithmetic.hpp>


#pragma once

#include <cstdint>
#include <map>
#include <memory>
#include <set>
#include <vector>

#ifdef BOOST_PYTHON_EXTENSIONS
#include <boost/python.hpp>
namespace p = boost::python;
#endif

#include "../Variable.hpp"
#include "it/EntropyCalculator.hpp"

namespace mist {

namespace algorithm {

/** Interface for processing tuples in the TupleSpace
 *
 * A class can specialize the TupleSpaceTraverser to gain access to the stream
 * of tuples generated by TupleSpace::traverse family of functions.
 */
class TupleSpaceTraverser {
public:
  using tuple_t = Variable::indexes;
  using count_t = std::uint64_t;
  virtual void process_tuple(count_t tuple_no, tuple_t const& tuple) = 0;
  virtual void process_tuple_entropy(
      count_t tuple_no, tuple_t const& tuple, it::Entropy const& e) = 0;
};

/** Tuple Space defines the set of tuples over which to run a computation
 * search.
 */
class TupleSpace
{
public:
  using tuple_t = Variable::indexes;
  using index_t = Variable::index_t;
  using count_t = std::uint64_t;
  TupleSpace();
  TupleSpace(int N, int d);
  ~TupleSpace();
  /** Define a named logical group of variables
   * @param name group name
   * @param vars set of variables in the group, duplicates will be ignored
   * @return index of created variable group
   * @throws TupleSpaceException variable already listed in existing variable
   * group
   */
  int addVariableGroup(std::string const& name, tuple_t const& vars);
  /** Add a variable group tuple
   *
   * The cross product of groups in the group tuple generates a set of
   * variable tuples that will be added to the TupleSpace by
   * TupleSpaceTupleProducer.
   *
   * @param groups Array of group names
   * @throws TupleSpaceException group does not exists
   */
  void addVariableGroupTuple(std::vector<std::string> const& groups);
  /** Add a variable group tuple
   *
   * The cross product of groups in the group tuple generates a set of
   * variable tuples that will be added to the TupleSpace by
   * TupleSpaceTupleProducer.
   *
   * @param groups Array of group indexed by order created
   * @throws TupleSpaceException group index out of range
   */
  void addVariableGroupTuple(tuple_t const& groups);
  /** Get variable names
   */
  std::vector<std::string> names() const;
  /** Set variable names
   */
  void set_names(std::vector<std::string> const& names);
  int tupleSize() const;

#if BOOST_PYTHON_EXTENSIONS
  int pyAddVariableGroup(std::string const& name, p::list const& list);
  void pyAddVariableGroupTuple(p::list const& list);
#endif

  /** Calculate the size of the tuple space, i.e. count the generated number of
   * tuples.
   */
  count_t count_tuples() const;
  count_t count_tuples_group_tuple(tuple_t const&) const;
  tuple_t find_tuple(count_t target) const;
  tuple_t const& getVariableGroup(int index) const;
  tuple_t const& getVariableGroup(std::string const& name) const;
  std::vector<std::size_t> const& getVariableGroupSizes() const;
  tuple_t const& getVariableGroupAppearances(int index) const;
  std::vector<tuple_t> const& getVariableGroups() const;
  std::vector<tuple_t> const& getVariableGroupTuples() const;

  /** Walk through all tuples in the tuple space
   * @param traverser Process each tuple with methods defined in specialization
   */
  void traverse(TupleSpaceTraverser& traverser) const;
  /** Walk through as subset of tuples in the tuple space
   *
   * TupleSpace generates an ordered list of tuples, that can begin at any
   * position in the list.
   *
   * @param start Begin the walk at tuple in position start
   * @param stop End the walk at tuple in position stop
   * @param traverser Process each tuple with methods defined in specialization
   */
  void traverse(count_t start, count_t stop, TupleSpaceTraverser& traverser) const;
  /** Walk through all tuples in the tuple space, computing entropy values as
   * you go.
   *
   * Some it::Measure classes compute the entropy values of sub-tuples. It is
   * most efficient to compute these as you walk through the tuple space so
   * intermediary values can be reused many times.
   *
   * @param ecalc it::EntropyCalculator object to perform entropy computations
   * @param traverser Process each tuple with methods defined in specialization
   */
  void traverse_entropy(it::EntropyCalculator &ecalc, TupleSpaceTraverser& traverser) const;
  /** Walk through a subset of tuples in the tuple space, computing entropy values as
   * you go.
   */
  void traverse_entropy(count_t start, count_t stop, it::EntropyCalculator &ecalc, TupleSpaceTraverser& traverser) const;

private:
  // variable names, e.g. from data header
  std::vector<std::string> variableNames;
  // groups of variables that construct a tuple
  std::vector<tuple_t> variableGroups;
  std::vector<std::size_t> variableGroupSizes;
  // names for the variable groups and their identifying index
  std::map<std::string, int> variableGroupNames;
  // list of groups that define variable tuples
  std::vector<tuple_t> variableGroupTuples;
  // keep track of seen variables to detect duplicates
  std::set<int> seen_vars;
  int tuple_size = 0;
};

class TupleSpaceException : public std::exception
{
private:
  std::string msg;

public:
  TupleSpaceException(std::string const& method, std::string const& msg)
    : msg("TupleSpace" + method + " : " + msg)
  {}
  virtual const char* what() const throw() { return msg.c_str(); };
};

} // algorithm
} // mist


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_OPTIMIZE_UNARY_MINUS_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_OPTIMIZE_UNARY_MINUS_HPP_INCLUDED

#include <boost/simd/operator/functions/unary_minus.hpp>
#include <boost/simd/arithmetic/functions/fma.hpp>
#include <boost/simd/arithmetic/functions/fms.hpp>
#include <boost/simd/include/functions/fnma.hpp>
#include <boost/simd/include/functions/fnms.hpp>
#include <boost/dispatch/dsl/category.hpp>
#include <boost/dispatch/functor/preprocessor/call.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( unary_minus_ , tag::formal_, (D)(A0)
                                   , ((node_<A0, boost::simd::tag::fma_, mpl::long_<3> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(1, (A0 const& a0),
      fnma(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), boost::proto::child_c<2>(a0))
    )
  };

  BOOST_DISPATCH_IMPLEMENT         ( unary_minus_ , tag::formal_, (D)(A0)
                                   , ((node_<A0, boost::simd::tag::fms_, mpl::long_<3> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(1, (A0 const& a0),
      fnms(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), boost::proto::child_c<2>(a0))
    )
  };
} } }

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the dlib C++
    Library.  In it, we will show how to do face recognition.  This example uses the
    pretrained dlib_face_recognition_resnet_model_v1 model which is freely available from
    the dlib web site.  This model has a 99.38% accuracy on the standard LFW face
    recognition benchmark, which is comparable to other state-of-the-art methods for face
    recognition as of February 2017.

    In this example, we will use dlib to do face clustering.  Included in the examples
    folder is an image, bald_guys.jpg, which contains a bunch of photos of action movie
    stars Vin Diesel, The Rock, Jason Statham, and Bruce Willis.   We will use dlib to
    automatically find their faces in the image and then to automatically determine how
    many people there are (4 in this case) as well as which faces belong to each person.

    Finally, this example uses a network with the loss_metric loss.  Therefore, if you want
    to learn how to train your own models, or to get a general introduction to this loss
    layer, you should read the dnn_metric_learning_ex.cpp and
    dnn_metric_learning_on_images_ex.cpp examples.
*/

#include <dlib/dnn.h>
#include <dlib/gui_widgets.h>
#include <dlib/clustering.h>
#include <dlib/string.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>

using namespace dlib;
using namespace std;

// ----------------------------------------------------------------------------------------

// The next bit of code defines a ResNet network.  It's basically copied
// and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
// layer with loss_metric and made the network somewhat smaller.  Go read the introductory
// dlib DNN examples to learn what all this stuff means.
//
// Also, the dnn_metric_learning_on_images_ex.cpp example shows how to train this network.
// The dlib_face_recognition_resnet_model_v1 model used by this example was trained using
// essentially the code shown in dnn_metric_learning_on_images_ex.cpp except the
// mini-batches were made larger (35x15 instead of 5x5), the iterations without progress
// was set to 10000, and the training dataset consisted of about 3 million images instead of
// 55.  Also, the input layer was locked to images of size 150.
template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

// ----------------------------------------------------------------------------------------

std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img
);

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_dnn_face_recognition_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
try
{
    if (argc != 2)
    {
        cout << "Run this example by invoking it like this: " << endl;
        cout << "   ./dnn_face_recognition_ex faces/bald_guys.jpg" << endl;
        cout << endl;
        cout << "You will also need to get the face landmarking model file as well as " << endl;
        cout << "the face recognition model file.  Download and then decompress these files from: " << endl;
        cout << "http://dlib.net/files/shape_predictor_5_face_landmarks.dat.bz2" << endl;
        cout << "http://dlib.net/files/dlib_face_recognition_resnet_model_v1.dat.bz2" << endl;
        cout << endl;
        return 1;
    }

    // The first thing we are going to do is load all our models.  First, since we need to
    // find faces in the image we will need a face detector:
    frontal_face_detector detector = get_frontal_face_detector();
    // We will also use a face landmarking model to align faces to a standard pose:  (see face_landmark_detection_ex.cpp for an introduction)
    shape_predictor sp;
    deserialize("shape_predictor_5_face_landmarks.dat") >> sp;
    // And finally we load the DNN responsible for face recognition.
    anet_type net;
    deserialize("dlib_face_recognition_resnet_model_v1.dat") >> net;

    matrix<rgb_pixel> img;
    load_image(img, argv[1]);
    // Display the raw image on the screen
    image_window win(img);

    // Run the face detector on the image of our action heroes, and for each face extract a
    // copy that has been normalized to 150x150 pixels in size and appropriately rotated
    // and centered.
    std::vector<matrix<rgb_pixel>> faces;
    for (auto face : detector(img))
    {
        auto shape = sp(img, face);
        matrix<rgb_pixel> face_chip;
        extract_image_chip(img, get_face_chip_details(shape,150,0.25), face_chip);
        faces.push_back(move(face_chip));
        // Also put some boxes on the faces so we can see that the detector is finding
        // them.
        win.add_overlay(face);
    }

    if (faces.size() == 0)
    {
        cout << "No faces found in image!" << endl;
        return 1;
    }

    // This call asks the DNN to convert each face image in faces into a 128D vector.
    // In this 128D vector space, images from the same person will be close to each other
    // but vectors from different people will be far apart.  So we can use these vectors to
    // identify if a pair of images are from the same person or from different people.
    std::vector<matrix<float,0,1>> face_descriptors = net(faces);

    // In particular, one simple thing we can do is face clustering.  This next bit of code
    // creates a graph of connected faces and then uses the Chinese whispers graph clustering
    // algorithm to identify how many people there are and which faces belong to whom.
    std::vector<sample_pair> edges;
    for (size_t i = 0; i < face_descriptors.size(); ++i)
    {
        for (size_t j = i; j < face_descriptors.size(); ++j)
        {
            // Faces are connected in the graph if they are close enough.  Here we check if
            // the distance between two face descriptors is less than 0.6, which is the
            // decision threshold the network was trained to use.  Although you can
            // certainly use any other threshold you find useful.
            if (length(face_descriptors[i]-face_descriptors[j]) < 0.6)
                edges.push_back(sample_pair(i,j));
        }
    }
    std::vector<unsigned long> labels;
    const auto num_clusters = chinese_whispers(edges, labels);
    // This will correctly indicate that there are 4 people in the image.
    cout << "number of people found in the image: "<< num_clusters << endl;

    // Now let's display the face clustering results on the screen.  You will see that it
    // correctly grouped all the faces.
    std::vector<image_window> win_clusters(num_clusters);
    for (size_t cluster_id = 0; cluster_id < num_clusters; ++cluster_id)
    {
        std::vector<matrix<rgb_pixel>> temp;
        for (size_t j = 0; j < labels.size(); ++j)
        {
            if (cluster_id == labels[j])
                temp.push_back(faces[j]);
        }
        win_clusters[cluster_id].set_title("face cluster " + cast_to_string(cluster_id));
        win_clusters[cluster_id].set_image(tile_images(temp));
    }

    // Finally, let's print one of the face descriptors to the screen.
    cout << "face descriptor for one face: " << trans(face_descriptors[0]) << endl;

    // It should also be noted that face recognition accuracy can be improved if jittering
    // is used when creating face descriptors.  In particular, to get 99.38% on the LFW
    // benchmark you need to use the jitter_image() routine to compute the descriptors,
    // like so:
    matrix<float,0,1> face_descriptor = mean(mat(net(jitter_image(faces[0]))));
    cout << "jittered face descriptor for one face: " << trans(face_descriptor) << endl;
    // If you use the model without jittering, as we did when clustering the bald guys, it
    // gets an accuracy of 99.13% on the LFW benchmark.  So jittering makes the whole
    // procedure a little more accurate but makes face descriptor calculation slower.

    cout << "hit enter to terminate" << endl;
    cin.get();
}
catch (std::exception& e)
{
    cout << e.what() << endl;
}

// ----------------------------------------------------------------------------------------

std::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img
)
{
    // All this function does is make 100 copies of img, all slightly jittered by being
    // zoomed, rotated, and translated a little bit differently. They are also randomly
    // mirrored left to right.
    thread_local dlib::rand rnd;

    std::vector<matrix<rgb_pixel>> crops;
    for (int i = 0; i < 100; ++i)
        crops.push_back(jitter_image(img,rnd));

    return crops;
}

// ----------------------------------------------------------------------------------------


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_LESS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_LESS_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/operator/functions/is_less.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/toolbox/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                              ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmplt_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                              ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmplt_ps(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<unsigned_<A0>,boost::simd::tag::sse_>))
                              ((simd_<unsigned_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type stype;
      return  bitwise_cast<result_type>
              (boost::simd::lt( bitwise_cast<stype>(a0) - Signmask<stype>()
                      , bitwise_cast<stype>(a1) - Signmask<stype>()
                      )
              );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<int8_<A0>,boost::simd::tag::sse_>))
                              ((simd_<int8_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmplt_epi8(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                              ((simd_<int16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmplt_epi16(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                              ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmplt_epi32(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_less_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                              ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::downgrade<A0, signed>::type type;
      typedef typename dispatch::meta::downgrade<A0, unsigned>::type utype;
      utype al = details::shuffle<0,0,2,2>(a0);
      utype bl = details::shuffle<0,0,2,2>(a1);
      type ah  = details::shuffle<1,1,3,3>(a0);
      type bh  = details::shuffle<1,1,3,3>(a1);

      return bitwise_cast<result_type>( l_or( boost::simd::lt(ah,bh)
                                            , l_and( boost::simd::eq(ah,bh)
                                                   , boost::simd::lt(al,bl)
                                                   )
                                            )
                                      );
    }
  };
} } }

#endif
#endif


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_INDEX_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_INDEX_GEOGRAPHIC_HPP

#include <boost/geometry/strategies/index/services.hpp>
#include <boost/geometry/strategies/relate/geographic.hpp>

// TEMP - move to strategy
#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/geographic/distance_segment_box.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>

namespace boost { namespace geometry { namespace strategies { namespace index
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    // TODO: Is SeriesOrder argument needed here?
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>
{
    typedef relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType> base_t;

public:
    geographic()
        : base_t()
    {}

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value
                                > * = nullptr) const
    {
        return geometry::strategy::distance::geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_pointlike<Geometry1>::value
                                    && util::is_segment<Geometry2>::value)
                                    || (util::is_segment<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value)
                                > * = nullptr) const
    {
        return geometry::strategy::distance::geographic_cross_track
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_pointlike<Geometry1>::value
                                    && util::is_box<Geometry2>::value)
                                    || (util::is_box<Geometry1>::value
                                    && util::is_pointlike<Geometry2>::value)
                                > * = nullptr) const
    {
        return geometry::strategy::distance::geographic_cross_track_point_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    (util::is_segment<Geometry1>::value
                                    && util::is_box<Geometry2>::value)
                                    || (util::is_box<Geometry1>::value
                                        && util::is_segment<Geometry2>::value)
                                > * = nullptr) const
    {
        return geometry::strategy::distance::geographic_segment_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto comparable_distance(Geometry1 const&, Geometry2 const&,
                             std::enable_if_t
                                <
                                    util::is_segment<Geometry1>::value
                                    && util::is_segment<Geometry2>::value
                                > * = nullptr) const
    {
        return geometry::strategy::distance::geographic_cross_track
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, geographic_tag>
{
    using type = strategies::index::geographic<>;
};

// TEMP - needed in distance until umbrella strategies are supported
template <typename FormulaPolicy, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::distance::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>>
{
    static auto get(strategy::distance::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
    {
        return strategies::index::geographic
            <
                FormulaPolicy,
                strategy::default_order<FormulaPolicy>::value,
                Spheroid,
                CalculationType
            >(strategy.model());
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename FormulaPolicy, typename Spheroid, typename CalculationType, bool Bisection, bool EnableClosestPoint>
struct strategy_converter<strategy::distance::detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint>>
{
    typedef strategies::index::geographic
        <
            FormulaPolicy,
            strategy::default_order<FormulaPolicy>::value,
            Spheroid,
            CalculationType
        > base_strategy;

    struct altered_strategy : base_strategy
    {
        explicit altered_strategy(Spheroid const& spheroid)
            : base_strategy(spheroid)
        {}

        // It seems that this declaration is needed because comparable_distance
        // is not static function.
        using base_strategy::comparable_distance;

        template <typename Geometry1, typename Geometry2>
        auto comparable_distance(Geometry1 const&, Geometry2 const&,
                                 std::enable_if_t
                                    <
                                        (util::is_pointlike<Geometry1>::value
                                        && util::is_segment<Geometry2>::value)
                                        || (util::is_segment<Geometry1>::value
                                        && util::is_pointlike<Geometry2>::value)
                                    > * = nullptr) const
        {
            return geometry::strategy::distance::detail::geographic_cross_track
                <
                    FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
                >(base_strategy::m_spheroid);
        }

        template <typename Geometry1, typename Geometry2>
        auto comparable_distance(Geometry1 const&, Geometry2 const&,
                                 std::enable_if_t
                                    <
                                        util::is_segment<Geometry1>::value
                                        && util::is_segment<Geometry2>::value
                                    > * = nullptr) const
        {
            return geometry::strategy::distance::detail::geographic_cross_track
                <
                    FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint
                >(base_strategy::m_spheroid);
        }
    };

    static auto get(strategy::distance::detail::geographic_cross_track<FormulaPolicy, Spheroid, CalculationType, Bisection, EnableClosestPoint> const& strategy)
    {
        return altered_strategy(strategy.model());
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename FormulaPolicy, typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::distance::geographic<FormulaPolicy, Spheroid, CalculationType>>
{
    static auto get(strategy::distance::geographic<FormulaPolicy, Spheroid, CalculationType> const& strategy)
    {
        return strategies::index::geographic
            <
                FormulaPolicy,
                strategy::default_order<FormulaPolicy>::value,
                Spheroid,
                CalculationType
            >(strategy.model());
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::distance::andoyer<Spheroid, CalculationType>>
{
    static auto get(strategy::distance::andoyer<Spheroid, CalculationType> const& strategy)
    {
        return strategies::index::geographic
            <
                strategy::andoyer,
                strategy::default_order<strategy::andoyer>::value,
                Spheroid,
                CalculationType
            >(strategy.model());
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::distance::thomas<Spheroid, CalculationType>>
{
    static auto get(strategy::distance::thomas<Spheroid, CalculationType> const& strategy)
    {
        return strategies::index::geographic
            <
                strategy::thomas,
                strategy::default_order<strategy::thomas>::value,
                Spheroid,
                CalculationType
            >(strategy.model());
    }
};
// TEMP - needed in distance until umbrella strategies are supported
template <typename Spheroid, typename CalculationType>
struct strategy_converter<strategy::distance::vincenty<Spheroid, CalculationType>>
{
    static auto get(strategy::distance::vincenty<Spheroid, CalculationType> const& strategy)
    {
        return strategies::index::geographic
            <
                strategy::vincenty,
                strategy::default_order<strategy::vincenty>::value,
                Spheroid,
                CalculationType
            >(strategy.model());
    }
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_INDEX_GEOGRAPHIC_HPP