/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
*  All Rights Reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Rutgers University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Andrew Dobson */

#include "ompl/geometric/planners/prm/SPARStwo.h"
#include "ompl/geometric/planners/prm/ConnectionStrategy.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
#include "ompl/tools/config/SelfConfig.h"
#include <boost/graph/astar_search.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/foreach.hpp>
#include <thread>

#include "GoalVisitor.hpp"

#define foreach BOOST_FOREACH
#define foreach_reverse BOOST_REVERSE_FOREACH

ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si)
  : base::Planner(si, "SPARStwo")
  , nearSamplePoints_((2 * si_->getStateDimension()))
  , stateProperty_(boost::get(vertex_state_t(), g_))
  , weightProperty_(boost::get(boost::edge_weight, g_))
  , colorProperty_(boost::get(vertex_color_t(), g_))
  , interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_))
  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
{
    specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
    specs_.approximateSolutions = false;
    specs_.optimizingPaths = true;
    specs_.multithreaded = true;

    psimp_ = std::make_shared<PathSimplifier>(si_);

    Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor,
                                  "1.1:0.1:3.0");
    Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction,
                                  &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
    Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction,
                                  &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
    Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures,
                                        "100:10:3000");

    addPlannerProgressProperty("iterations INTEGER", [this]
                               {
                                   return getIterationCount();
                               });
    addPlannerProgressProperty("best cost REAL", [this]
                               {
                                   return getBestCost();
                               });
}

ompl::geometric::SPARStwo::~SPARStwo()
{
    freeMemory();
}

void ompl::geometric::SPARStwo::setup()
{
    Planner::setup();
    if (!nn_)
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
    nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
                             {
                                 return distanceFunction(a, b);
                             });
    double maxExt = si_->getMaximumExtent();
    sparseDelta_ = sparseDeltaFraction_ * maxExt;
    denseDelta_ = denseDeltaFraction_ * maxExt;

    // Setup optimization objective
    //
    // If no optimization objective was specified, then default to
    // optimizing path length as computed by the distance() function
    // in the state space.
    if (pdef_)
    {
        if (pdef_->hasOptimizationObjective())
        {
            opt_ = pdef_->getOptimizationObjective();
            if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
                OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
                          "for other optimizaton objectives is not guaranteed.",
                          getName().c_str());
        }
        else
            opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
    }
    else
    {
        OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
        setup_ = false;
    }
}

void ompl::geometric::SPARStwo::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
{
    Planner::setProblemDefinition(pdef);
    clearQuery();
}

void ompl::geometric::SPARStwo::clearQuery()
{
    startM_.clear();
    goalM_.clear();
    pis_.restart();
}

void ompl::geometric::SPARStwo::clear()
{
    Planner::clear();
    clearQuery();
    resetFailures();
    iterations_ = 0;
    bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
    freeMemory();
    if (nn_)
        nn_->clear();
}

void ompl::geometric::SPARStwo::freeMemory()
{
    Planner::clear();
    sampler_.reset();

    foreach (Vertex v, boost::vertices(g_))
    {
        foreach (InterfaceData &d, interfaceDataProperty_[v] | boost::adaptors::map_values)
            d.clear(si_);
        if (stateProperty_[v] != nullptr)
            si_->freeState(stateProperty_[v]);
        stateProperty_[v] = nullptr;
    }
    g_.clear();

    if (nn_)
        nn_->clear();
}

bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
                                             base::PathPtr &solution)
{
    base::Goal *g = pdef_->getGoal().get();
    base::Cost sol_cost(opt_->infiniteCost());
    foreach (Vertex start, starts)
        foreach (Vertex goal, goals)
        {
            // we lock because the connected components algorithm is incremental and may change disjointSets_
            graphMutex_.lock();
            bool same_component = sameComponent(start, goal);
            graphMutex_.unlock();

            if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
            {
                base::PathPtr p = constructSolution(start, goal);
                if (p)
                {
                    base::Cost pathCost = p->cost(opt_);
                    if (opt_->isCostBetterThan(pathCost, bestCost_))
                        bestCost_ = pathCost;
                    // Check if optimization objective is satisfied
                    if (opt_->isSatisfied(pathCost))
                    {
                        solution = p;
                        return true;
                    }
                    if (opt_->isCostBetterThan(pathCost, sol_cost))
                    {
                        solution = p;
                        sol_cost = pathCost;
                    }
                }
            }
        }
    return false;
}

bool ompl::geometric::SPARStwo::sameComponent(Vertex m1, Vertex m2)
{
    return boost::same_component(m1, m2, disjointSets_);
}

bool ompl::geometric::SPARStwo::reachedFailureLimit() const
{
    return consecutiveFailures_ >= maxFailures_;
}

bool ompl::geometric::SPARStwo::reachedTerminationCriterion() const
{
    return consecutiveFailures_ >= maxFailures_ || addedSolution_;
}

void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail)
{
    if (stopOnMaxFail)
    {
        resetFailures();
        base::PlannerTerminationCondition ptcOrFail([this, &ptc]
                                                    {
                                                        return ptc || reachedFailureLimit();
                                                    });
        constructRoadmap(ptcOrFail);
    }
    else
        constructRoadmap(ptc);
}

void ompl::geometric::SPARStwo::constructRoadmap(const base::PlannerTerminationCondition &ptc)
{
    checkQueryStateInitialization();

    if (!isSetup())
        setup();
    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();

    base::State *qNew = si_->allocState();
    base::State *workState = si_->allocState();

    /* The whole neighborhood set which has been most recently computed */
    std::vector<Vertex> graphNeighborhood;
    /* The visible neighborhood set which has been most recently computed */
    std::vector<Vertex> visibleNeighborhood;

    bestCost_ = opt_->infiniteCost();
    while (!ptc)
    {
        ++iterations_;
        ++consecutiveFailures_;

        // Generate a single sample, and attempt to connect it to nearest neighbors.
        if (!sampler_->sample(qNew))
            continue;

        findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);

        if (!checkAddCoverage(qNew, visibleNeighborhood))
            if (!checkAddConnectivity(qNew, visibleNeighborhood))
                if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
                {
                    if (!visibleNeighborhood.empty())
                    {
                        std::map<Vertex, base::State *> closeRepresentatives;
                        findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
                        for (auto &closeRepresentative : closeRepresentatives)
                        {
                            updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
                                             closeRepresentative.second);
                            updatePairPoints(closeRepresentative.first, closeRepresentative.second,
                                             visibleNeighborhood[0], qNew);
                        }
                        checkAddPath(visibleNeighborhood[0]);
                        for (auto &closeRepresentative : closeRepresentatives)
                        {
                            checkAddPath(closeRepresentative.first);
                            si_->freeState(closeRepresentative.second);
                        }
                    }
                }
    }
    si_->freeState(workState);
    si_->freeState(qNew);
}

void ompl::geometric::SPARStwo::checkQueryStateInitialization()
{
    std::lock_guard<std::mutex> _(graphMutex_);
    if (boost::num_vertices(g_) < 1)
    {
        queryVertex_ = boost::add_vertex(g_);
        stateProperty_[queryVertex_] = nullptr;
    }
}

ompl::base::PlannerStatus ompl::geometric::SPARStwo::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    checkQueryStateInitialization();

    auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());

    if (goal == nullptr)
    {
        OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
        return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
    }

    // Add the valid start states as milestones
    while (const base::State *st = pis_.nextStart())
        startM_.push_back(addGuard(si_->cloneState(st), START));
    if (startM_.empty())
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!goal->couldSample())
    {
        OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    // Add the valid goal states as milestones
    while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
        goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
    if (goalM_.empty())
    {
        OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
        return base::PlannerStatus::INVALID_GOAL;
    }

    unsigned int nrStartStates = boost::num_vertices(g_) - 1;  // don't count query vertex
    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates);

    // Reset addedSolution_ member
    addedSolution_ = false;
    resetFailures();
    base::PathPtr sol;
    base::PlannerTerminationCondition ptcOrFail([this, &ptc]
                                                {
                                                    return ptc || reachedFailureLimit();
                                                });
    std::thread slnThread([this, &ptcOrFail, &sol]
                          {
                              checkForSolution(ptcOrFail, sol);
                          });

    // Construct planner termination condition which also takes M into account
    base::PlannerTerminationCondition ptcOrStop([this, &ptc]
                                                {
                                                    return ptc || reachedTerminationCriterion();
                                                });
    constructRoadmap(ptcOrStop);

    // Ensure slnThread is ceased before exiting solve
    slnThread.join();

    OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);

    if (sol)
        pdef_->addSolutionPath(sol, false, -1.0, getName());

    // Return true if any solution was found.
    return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}

void ompl::geometric::SPARStwo::checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
{
    auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
    while (!ptc && !addedSolution_)
    {
        // Check for any new goal states
        if (goal->maxSampleCount() > goalM_.size())
        {
            const base::State *st = pis_.nextGoal();
            if (st != nullptr)
                goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
        }

        // Check for a solution
        addedSolution_ = haveSolution(startM_, goalM_, solution);
        // Sleep for 1ms
        if (!addedSolution_)
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
{
    if (!visibleNeighborhood.empty())
        return false;
    // No free paths means we add for coverage
    addGuard(si_->cloneState(qNew), COVERAGE);
    return true;
}

bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
{
    std::vector<Vertex> links;
    if (visibleNeighborhood.size() > 1)
    {
        // For each neighbor
        for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
            // For each other neighbor
            for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
                // If they are in different components
                if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
                {
                    links.push_back(visibleNeighborhood[i]);
                    links.push_back(visibleNeighborhood[j]);
                }

        if (!links.empty())
        {
            // Add the node
            Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);

            for (unsigned long link : links)
                // If there's no edge
                if (!boost::edge(g, link, g_).second)
                    // And the components haven't been united by previous links
                    if (!sameComponent(link, g))
                        connectGuards(g, link);
            return true;
        }
    }
    return false;
}

bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
                                                  std::vector<Vertex> &visibleNeighborhood)
{
    // If we have more than 1 or 0 neighbors
    if (visibleNeighborhood.size() > 1)
        if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
            // If our two closest neighbors don't share an edge
            if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
            {
                // If they can be directly connected
                if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
                {
                    // Connect them
                    connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
                    // And report that we added to the roadmap
                    resetFailures();
                    // Report success
                    return true;
                }

                // Add the new node to the graph, to bridge the interface
                Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
                connectGuards(v, visibleNeighborhood[0]);
                connectGuards(v, visibleNeighborhood[1]);
                // Report success
                return true;
            }
    return false;
}

bool ompl::geometric::SPARStwo::checkAddPath(Vertex v)
{
    bool ret = false;

    std::vector<Vertex> rs;
    foreach (Vertex r, boost::adjacent_vertices(v, g_))
        rs.push_back(r);

    /* Candidate x vertices as described in the method, filled by function computeX(). */
    std::vector<Vertex> Xs;

    /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
    std::vector<Vertex> VPPs;

    for (std::size_t i = 0; i < rs.size() && !ret; ++i)
    {
        Vertex r = rs[i];
        computeVPP(v, r, VPPs);
        foreach (Vertex rp, VPPs)
        {
            // First, compute the longest path through the graph
            computeX(v, r, rp, Xs);
            double rm_dist = 0.0;
            foreach (Vertex rpp, Xs)
            {
                double tmp_dist = (si_->distance(stateProperty_[r], stateProperty_[v]) +
                                   si_->distance(stateProperty_[v], stateProperty_[rpp])) /
                                  2.0;
                if (tmp_dist > rm_dist)
                    rm_dist = tmp_dist;
            }

            InterfaceData &d = getData(v, r, rp);

            // Then, if the spanner property is violated
            if (rm_dist > stretchFactor_ * d.d_)
            {
                ret = true;  // Report that we added for the path
                if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
                    connectGuards(r, rp);
                else
                {
                    auto p(std::make_shared<PathGeometric>(si_));
                    if (r < rp)
                    {
                        p->append(d.sigmaA_);
                        p->append(d.pointA_);
                        p->append(stateProperty_[v]);
                        p->append(d.pointB_);
                        p->append(d.sigmaB_);
                    }
                    else
                    {
                        p->append(d.sigmaB_);
                        p->append(d.pointB_);
                        p->append(stateProperty_[v]);
                        p->append(d.pointA_);
                        p->append(d.sigmaA_);
                    }

                    psimp_->reduceVertices(*p, 10);
                    psimp_->shortcutPath(*p, 50);

                    if (p->checkAndRepair(100).second)
                    {
                        Vertex prior = r;
                        Vertex vnew;
                        std::vector<base::State *> &states = p->getStates();

                        foreach (base::State *st, states)
                        {
                            // no need to clone st, since we will destroy p; we just copy the pointer
                            vnew = addGuard(st, QUALITY);

                            connectGuards(prior, vnew);
                            prior = vnew;
                        }
                        // clear the states, so memory is not freed twice
                        states.clear();
                        connectGuards(prior, rp);
                    }
                }
            }
        }
    }

    return ret;
}

void ompl::geometric::SPARStwo::resetFailures()
{
    consecutiveFailures_ = 0;
}

void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
                                                   std::vector<Vertex> &visibleNeighborhood)
{
    visibleNeighborhood.clear();
    stateProperty_[queryVertex_] = st;
    nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
    stateProperty_[queryVertex_] = nullptr;

    // Now that we got the neighbors from the NN, we must remove any we can't see
    for (unsigned long i : graphNeighborhood)
        if (si_->checkMotion(st, stateProperty_[i]))
            visibleNeighborhood.push_back(i);
}

void ompl::geometric::SPARStwo::approachGraph(Vertex v)
{
    std::vector<Vertex> hold;
    nn_->nearestR(v, sparseDelta_, hold);

    std::vector<Vertex> neigh;
    for (unsigned long i : hold)
        if (si_->checkMotion(stateProperty_[v], stateProperty_[i]))
            neigh.push_back(i);

    foreach (Vertex vp, neigh)
        connectGuards(v, vp);
}

ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::findGraphRepresentative(base::State *st)
{
    std::vector<Vertex> nbh;
    stateProperty_[queryVertex_] = st;
    nn_->nearestR(queryVertex_, sparseDelta_, nbh);
    stateProperty_[queryVertex_] = nullptr;

    Vertex result = boost::graph_traits<Graph>::null_vertex();

    for (unsigned long i : nbh)
        if (si_->checkMotion(st, stateProperty_[i]))
        {
            result = i;
            break;
        }
    return result;
}

void ompl::geometric::SPARStwo::findCloseRepresentatives(base::State *workArea, const base::State *qNew,
                                                         const Vertex qRep,
                                                         std::map<Vertex, base::State *> &closeRepresentatives,
                                                         const base::PlannerTerminationCondition &ptc)
{
    for (auto &closeRepresentative : closeRepresentatives)
        si_->freeState(closeRepresentative.second);
    closeRepresentatives.clear();

    // Then, begin searching the space around him
    for (unsigned int i = 0; i < nearSamplePoints_; ++i)
    {
        do
        {
            sampler_->sampleNear(workArea, qNew, denseDelta_);
        } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ ||
                  !si_->checkMotion(qNew, workArea)) &&
                 !ptc);

        // if we were not successful at sampling a desirable state, we are out of time
        if (ptc)
            break;

        // Compute who his graph neighbors are
        Vertex representative = findGraphRepresentative(workArea);

        // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
        if (representative != boost::graph_traits<Graph>::null_vertex())
        {
            // If his representative is different than qNew
            if (qRep != representative)
                // And we haven't already tracked this representative
                if (closeRepresentatives.find(representative) == closeRepresentatives.end())
                    // Track the representative
                    closeRepresentatives[representative] = si_->cloneState(workArea);
        }
        else
        {
            // This guy can't be seen by anybody, so we should take this opportunity to add him
            addGuard(si_->cloneState(workArea), COVERAGE);

            // We should also stop our efforts to add a dense path
            for (auto &closeRepresentative : closeRepresentatives)
                si_->freeState(closeRepresentative.second);
            closeRepresentatives.clear();
            break;
        }
    }
}

void ompl::geometric::SPARStwo::updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
{
    // First of all, we need to compute all candidate r'
    std::vector<Vertex> VPPs;
    computeVPP(rep, r, VPPs);

    // Then, for each pair Pv(r,r')
    foreach (Vertex rp, VPPs)
        // Try updating the pair info
        distanceCheck(rep, q, r, s, rp);
}

void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
{
    VPPs.clear();
    foreach (Vertex cvpp, boost::adjacent_vertices(v, g_))
        if (cvpp != vp)
            if (!boost::edge(cvpp, vp, g_).second)
                VPPs.push_back(cvpp);
}

void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
{
    Xs.clear();

    foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
        if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
        {
            InterfaceData &d = getData(v, vpp, cx);
            if ((vpp < cx && (d.pointA_ != nullptr)) || (cx < vpp && (d.pointB_ != nullptr)))
                Xs.push_back(cx);
        }
    Xs.push_back(vpp);
}

ompl::geometric::SPARStwo::VertexPair ompl::geometric::SPARStwo::index(Vertex vp, Vertex vpp)
{
    if (vp < vpp)
        return VertexPair(vp, vpp);
    if (vpp < vp)
        return VertexPair(vpp, vp);
    else
        throw Exception(name_, "Trying to get an index where the pairs are the same point!");
}

ompl::geometric::SPARStwo::InterfaceData &ompl::geometric::SPARStwo::getData(Vertex v, Vertex vp, Vertex vpp)
{
    return interfaceDataProperty_[v][index(vp, vpp)];
}

void ompl::geometric::SPARStwo::distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s,
                                              Vertex rp)
{
    // Get the info for the current representative-neighbors pair
    InterfaceData &d = getData(rep, r, rp);

    if (r < rp)  // FIRST points represent r (the guy discovered through sampling)
    {
        if (d.pointA_ == nullptr)  // If the point we're considering replacing (P_v(r,.)) isn't there
            // Then we know we're doing better, so add it
            d.setFirst(q, s, si_);
        else  // Otherwise, he is there,
        {
            if (d.pointB_ == nullptr)  // But if the other guy doesn't exist, we can't compare.
            {
                // Should probably keep the one that is further away from rep?  Not known what to do in this case.
                // \todo: is this not part of the algorithm?
            }
            else  // We know both of these points exist, so we can check some distances
                if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
                // Distance with the new point is good, so set it.
                d.setFirst(q, s, si_);
        }
    }
    else  // SECOND points represent r (the guy discovered through sampling)
    {
        if (d.pointB_ == nullptr)  // If the point we're considering replacing (P_V(.,r)) isn't there...
            // Then we must be doing better, so add it
            d.setSecond(q, s, si_);
        else  // Otherwise, he is there
        {
            if (d.pointA_ == nullptr)  // But if the other guy doesn't exist, we can't compare.
            {
                // Should we be doing something cool here?
            }
            else if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
                // Distance with the new point is good, so set it
                d.setSecond(q, s, si_);
        }
    }

    // Lastly, save what we have discovered
    interfaceDataProperty_[rep][index(r, rp)] = d;
}

void ompl::geometric::SPARStwo::abandonLists(base::State *st)
{
    stateProperty_[queryVertex_] = st;

    std::vector<Vertex> hold;
    nn_->nearestR(queryVertex_, sparseDelta_, hold);

    stateProperty_[queryVertex_] = nullptr;

    // For each of the vertices
    foreach (Vertex v, hold)
    {
        foreach (VertexPair r, interfaceDataProperty_[v] | boost::adaptors::map_keys)
            interfaceDataProperty_[v][r].clear(si_);
    }
}

ompl::geometric::SPARStwo::Vertex ompl::geometric::SPARStwo::addGuard(base::State *state, GuardType type)
{
    std::lock_guard<std::mutex> _(graphMutex_);

    Vertex m = boost::add_vertex(g_);
    stateProperty_[m] = state;
    colorProperty_[m] = type;

    assert(si_->isValid(state));
    abandonLists(state);

    disjointSets_.make_set(m);
    nn_->add(m);
    resetFailures();

    return m;
}

void ompl::geometric::SPARStwo::connectGuards(Vertex v, Vertex vp)
{
    assert(v <= milestoneCount());
    assert(vp <= milestoneCount());

    const base::Cost weight(costHeuristic(v, vp));
    const Graph::edge_property_type properties(weight);
    std::lock_guard<std::mutex> _(graphMutex_);
    boost::add_edge(v, vp, properties, g_);
    disjointSets_.union_set(v, vp);
}

ompl::base::PathPtr ompl::geometric::SPARStwo::constructSolution(const Vertex start, const Vertex goal) const
{
    std::lock_guard<std::mutex> _(graphMutex_);

    boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));

    try
    {
        boost::astar_search(g_, start,
                            [this, goal](Vertex v)
                            {
                                return costHeuristic(v, goal);
                            },
                            boost::predecessor_map(prev)
                                .distance_compare([this](base::Cost c1, base::Cost c2)
                                                  {
                                                      return opt_->isCostBetterThan(c1, c2);
                                                  })
                                .distance_combine([this](base::Cost c1, base::Cost c2)
                                                  {
                                                      return opt_->combineCosts(c1, c2);
                                                  })
                                .distance_inf(opt_->infiniteCost())
                                .distance_zero(opt_->identityCost())
                                .visitor(AStarGoalVisitor<Vertex>(goal)));
    }
    catch (AStarFoundGoal &)
    {
    }

    if (prev[goal] == goal)
        throw Exception(name_, "Could not find solution path");
    else
    {
        auto p(std::make_shared<PathGeometric>(si_));
        for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
            p->append(stateProperty_[pos]);
        p->append(stateProperty_[start]);
        p->reverse();

        return p;
    }
}

void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const
{
    out << "SPARStwo Debug Output: " << std::endl;
    out << "  Settings: " << std::endl;
    out << "    Max Failures: " << getMaxFailures() << std::endl;
    out << "    Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
    out << "    Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
    out << "    Stretch Factor: " << getStretchFactor() << std::endl;
    out << "  Status: " << std::endl;
    out << "    Milestone Count: " << milestoneCount() << std::endl;
    out << "    Iterations: " << getIterationCount() << std::endl;
    out << "    Consecutive Failures: " << consecutiveFailures_ << std::endl;
}

void ompl::geometric::SPARStwo::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    // Explicitly add start and goal states:
    for (unsigned long i : startM_)
        data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], (int)START));

    for (unsigned long i : goalM_)
        data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], (int)GOAL));

    // If there are even edges here
    if (boost::num_edges(g_) > 0)
    {
        // Adding edges and all other vertices simultaneously
        foreach (const Edge e, boost::edges(g_))
        {
            const Vertex v1 = boost::source(e, g_);
            const Vertex v2 = boost::target(e, g_);
            data.addEdge(base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]),
                         base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]));

            // Add the reverse edge, since we're constructing an undirected roadmap
            data.addEdge(base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]),
                         base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]));
        }
    }
    else
        OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());

    // Make sure to add edge-less nodes as well
    foreach (const Vertex n, boost::vertices(g_))
        if (boost::out_degree(n, g_) == 0)
            data.addVertex(base::PlannerDataVertex(stateProperty_[n], (int)colorProperty_[n]));
}

ompl::base::Cost ompl::geometric::SPARStwo::costHeuristic(Vertex u, Vertex v) const
{
    return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
}


/*
    Labeling tool with Slic calculation
    <maintainer email="prodromou@acin.tuwien.ac.at">prodromou</maintainer>
    <maintainer email="faeulhammer@acin.tuwien.ac.at">Faeulhammer</maintainer>

    USAGE:
    key 0-9 > choose Label (0... background, 1-9 object labels)
    left click... label superpixel
    SPACE > start new Slic calculation when moving the Trackbar
    's'-key > save labeled image
 */

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <float.h>
#include <string>
#include <sstream>

#include <boost/filesystem.hpp>

#include <pcl/console/parse.h>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <v4r/common/pcl_opencv.h>
#include <v4r/common/io/filesystem_utils.h>
#include <v4r/segmentation/Slic.h>

using namespace std;
using namespace cv;

// ---- Define various colors for easier access -----------
Vec3b label_color[] = {
    Vec3b(255, 0, 0),
    Vec3b(0, 255, 0),
    Vec3b(0, 0, 255),
    Vec3b(255, 0, 255),
    Vec3b(0, 255, 255),
    Vec3b(255, 255, 0),
    Vec3b(64, 255, 128),
    Vec3b(128, 128, 128),
    Vec3b(255, 255, 255),
    Vec3b(0, 0, 0)
};

class Slic_Labelling
{
protected:
    int
    current_label,
    nSclicKMax,
    nSclicMMax,
    nSclicK,  // superpixels number
    nSclicM,    // compactness factor
    nSclicNumOfLabels;

    bool
    bLeftMousePressed,
    is_pcd;

    Mat_<cv::Vec3b>
    slicSrcMat,
    slicImgDRAWMat;

    Mat_<int>
    pxLabel,
    slicLabelsMat;

    std::vector< std::vector< std::pair<int, int> > > superpx_pxs;

    v4r::Slic slic;

    char cKeyPressed;

    std::string in_path;

    pcl::visualization::PCLVisualizer::Ptr vis;

public:
    Slic_Labelling()
    {

    }

    // ---- Function to calculate Slic on Image -----------
    void CalculateSLIC()
    {
        if (nSclicK > 0 && nSclicM > 0)
        {
            slicSrcMat.copyTo(slicImgDRAWMat);
            slic.segmentSuperpixelNumber(slicSrcMat, slicLabelsMat, nSclicNumOfLabels, nSclicK, nSclicM);
            superpx_pxs.resize(nSclicNumOfLabels);
            for(int superpx_id = 0; superpx_id < nSclicNumOfLabels; superpx_id++)
            {
                superpx_pxs[ superpx_id ].clear();
            }

            for (int row_id = 0; row_id < slicLabelsMat.rows; row_id++)
            {
                for (int col_id = 0; col_id < slicLabelsMat.cols; col_id++)
                {
                    int px_label = slicLabelsMat.at<int> ( row_id, col_id);
                    superpx_pxs[px_label].push_back ( std::pair<int,int> (row_id, col_id));

                    if ( pxLabel( row_id , col_id) ) //pixel already labelled by previous labelling steps
                    {
                        slicImgDRAWMat(row_id , col_id) = label_color[ pxLabel( row_id , col_id) - 1 ];
                    }
                }
            }

            slic.drawContours(slicImgDRAWMat, slicLabelsMat, 255,0,0);
            cout << "number of superpixel patches: "<< nSclicNumOfLabels <<endl;
            cv::imshow("Slic Results",slicImgDRAWMat);
        }
    }

    // ---- Function for the trackbar -----------
    static void Ontrackbar(int, void* = NULL)
    {
    }

    void onMouse_st(int nOnMouseEvent, int nOnMouseX, int nOnMouseY)
    {
        // ---- Left mousebutton to draw the selected label -----------
        if (nOnMouseEvent == EVENT_LBUTTONDOWN || (bLeftMousePressed && nOnMouseEvent == EVENT_MOUSEMOVE))
        {
            bLeftMousePressed = true;
            int clkdLabel = slicLabelsMat(nOnMouseY,nOnMouseX);

    #pragma omp parallel for
            for(size_t px_id = 0; px_id < superpx_pxs[ clkdLabel ].size(); px_id++)
            {
                int row_id = superpx_pxs[ slicLabelsMat(nOnMouseY,nOnMouseX) ][ px_id ].first;
                int col_id = superpx_pxs[ slicLabelsMat(nOnMouseY,nOnMouseX) ][ px_id ].second;

                pxLabel( row_id , col_id) = current_label;
                slicImgDRAWMat(row_id , col_id) = label_color[ current_label - 1];

                if (current_label == 0 )
                    slicImgDRAWMat(row_id , col_id) = slicSrcMat(row_id, col_id);
            }

            // ---- Update the image -----------
            slic.drawContours(slicImgDRAWMat, slicLabelsMat, 255,0,0);
            imshow("Slic Results", slicImgDRAWMat);
            //imshow("Labeled Image", labeledImgMat);
        }
        else if (nOnMouseEvent == EVENT_LBUTTONUP)
        {
            bLeftMousePressed = false;
        }

    }

    static void onMouse_st(int nOnMouseEvent, int nOnMouseX, int nOnMouseY, int nOnMouseFlags, void* this_)
    {
        static_cast<Slic_Labelling*>(this_)->onMouse_st(nOnMouseEvent, nOnMouseX, nOnMouseY);
    }

    static void CheckBox(int state, void* val){
         // do nothing for now
         return;
    }

    int init(const std::string &in_path)// const cv::Mat_< cv::Vec3b> &src_img)
    {
        nSclicNumOfLabels = 0;
        nSclicK = 5000;
        nSclicM = 10;
        current_label = 1;
        nSclicKMax=  10000;
        nSclicMMax = 60;
        cKeyPressed = -1;

        bLeftMousePressed = false;
        is_pcd = false;
        this->in_path = in_path;

        if(in_path.substr(in_path.find_last_of(".") + 1) == "pcd")    //check if given img is a point cloud
        {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pPCl(new pcl::PointCloud<pcl::PointXYZRGB>);
            pcl::io::loadPCDFile(in_path, *pPCl);
            if (pPCl->isOrganized())
            {
                PCLOpenCV::ConvertPCLCloud2Image<pcl::PointXYZRGB> (pPCl, slicSrcMat);
                is_pcd = true;
            }
            else
            {
                std::cerr << "Point cloud is not organized! Aborting..." << std::endl;
                return -1;
            }
        }
        else
        {
            slicSrcMat = imread(in_path);
        }

        // ---- Create Windows -----------
        namedWindow("Slic Results", cv::WINDOW_NORMAL);
        cv::namedWindow("Labelled Image", 1);
        pxLabel = cv::Mat::zeros(slicSrcMat.rows, slicSrcMat.cols, CV_64F);

        // ---- Create Trackbars -----------
        char cTrackbarName[50];
        sprintf(cTrackbarName, "superpixel max %d", nSclicKMax);
        createTrackbar(cTrackbarName, "Slic Results", &nSclicK, nSclicKMax, this->Ontrackbar);
        sprintf(cTrackbarName, "compactness max %d", nSclicMMax);
        createTrackbar(cTrackbarName, "Slic Results", &nSclicM, nSclicMMax, this->Ontrackbar);

        // ---- Load, Show and Copy src image to global Mat -----------
        imshow("Slic Results", slicSrcMat);

        CalculateSLIC();
        setMouseCallback("Slic Results", onMouse_st, this);

        return 0;
    }

    void run()
    {
        while(1)
        {
            if (isdigit(cKeyPressed))
            {
                current_label = cKeyPressed - '0';
                cout << "Label " << cKeyPressed << endl;
            }

            switch (cKeyPressed)
            {
            // ---- Save labeled img to disk -----------
            case 's':
            {
                cout << "labelling done..." << endl;
                return;
            }

                // ---- Reload slic in main window with SPACE -----------
            case 32:
            {
                CalculateSLIC();
            }
            }
            cKeyPressed = cvWaitKey(1);
        }
    }

    Mat_<int> getPxLabel() const
    {
        return pxLabel;
    }

    void show_and_save_file()
    {
        boost::filesystem::path p(in_path);
        std::string filename_only, filename_with_ext, dir, ext;

        filename_with_ext = p.filename().string(); // file.ext
        filename_only = p.stem().string();
#ifdef WIN32
        dir = in_path.substr(0, in_path.find_last_of('\\'));
#else
        dir = in_path.substr(0, in_path.find_last_of('/'));
#endif
        ext = in_path.substr(in_path.find_last_of('.'));

        for(int label_id = 1; label_id < static_cast<int> (sizeof(label_color)/sizeof(label_color[0]) ); label_id++)
        {
            size_t num_points = 0;

            std::stringstream label_path;
            label_path << dir << "/" << filename_only << "_object_" << label_id << ext;

            if(is_pcd)    //check if given img is a point cloud
            {
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr pLabelledPCl(new pcl::PointCloud<pcl::PointXYZRGB>);
                pcl::io::loadPCDFile(in_path, *pLabelledPCl);

                assert(pxLabel.rows == pLabelledPCl->height && pxLabel.cols == pLabelledPCl->width);

                for (int row_id = 0; row_id < pxLabel.rows; row_id++)
                {
                    for (int col_id = 0; col_id < pxLabel.cols; col_id++)
                    {
                        if ( pxLabel.at<int>(row_id, col_id) != label_id)
                        {
                            pLabelledPCl->at(col_id, row_id).x = std::numeric_limits<float>::quiet_NaN();
                            pLabelledPCl->at(col_id, row_id).y = std::numeric_limits<float>::quiet_NaN();
                            pLabelledPCl->at(col_id, row_id).z = std::numeric_limits<float>::quiet_NaN();
                        }
                        else
                            num_points++;
                    }
                }

                if ( num_points )
                {
                    if(!vis)
                    {
                        vis.reset(new pcl::visualization::PCLVisualizer("segmented object"));
                    }
                    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_handler (pLabelledPCl);
                    vis->addPointCloud<pcl::PointXYZRGB> (pLabelledPCl, rgb_handler, label_path.str());
                    vis->spin();
                    vis->close();

                    std::cout << "Saving segmented point cloud to " << label_path.str() << "." << std::endl;
                    pcl::io::savePCDFileBinary<pcl::PointXYZRGB>(label_path.str(), *pLabelledPCl);
                }
            }
            else
            {
                cv::Mat_ <cv::Vec3b> slicLabelledMat = cv::Mat_<cv::Vec3b>::zeros(slicSrcMat.rows, slicSrcMat.cols);
                for (int row_id = 0; row_id < pxLabel.rows; row_id++)
                {
                    for (int col_id = 0; col_id < pxLabel.cols; col_id++)
                    {
                        if ( pxLabel.at<int>(row_id, col_id) == label_id)
                        {
                            num_points++;
                            slicLabelledMat.at<Vec3b>(row_id, col_id) = slicSrcMat.at<Vec3b>(row_id, col_id);
                        }
                    }
                }
                if(num_points)
                {
                    cv::imshow("Labelled Image", slicLabelledMat );
                    cv::waitKey(0);
                    imwrite(label_path.str(), slicSrcMat);
                }
            }
        }
    }

};

// ---- Main function -----------
int main(int argc, char *argv[])
{
    Slic_Labelling slic_lbl;

    std::string img_path;
    pcl::console::parse_argument (argc, argv, "-img_path", img_path);

    bf::path img_path_bf = img_path;
    std::vector < std::string > files_intern;
    if (v4r::io::getFilesInDirectory(img_path, files_intern, "", ".*\\.(jpg|JPG|png|PNG|jpeg|JPEG|bmp|BMP|pcd|PCD)", true) != -1)
    {
        for(size_t file_id=0; file_id < files_intern.size(); file_id++)
        {
            std::stringstream image_path_ss;
            image_path_ss << img_path << "/" << files_intern[file_id];

            slic_lbl.init( image_path_ss.str() );
            slic_lbl.run();
            slic_lbl.show_and_save_file();
        }
    }
    else if (bf::is_regular_file(img_path_bf))
    {
        std::cout << "Loading file " << img_path << std::endl;

        slic_lbl.init( img_path);
        slic_lbl.run();
        slic_lbl.show_and_save_file();
    }
    else
        std::cerr << "Image path is not valid." << std::endl;

    return 0;
}


#include "subsystems/TurretFeederSubsystem.h"

#include <Eigen/Core>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc2/command/button/JoystickButton.h>

#include "ctre/Phoenix.h"

TurretFeederSubsystem::TurretFeederSubsystem()
{
    // Disable Talon FX velocity filtering so that our Kalman filter can do the
    // work

    m_feeder.SetNeutralMode(Brake);
    m_feeder.ConfigFactoryDefault();
    // m_feeder.ConfigVoltageMeasurementFilter(true);
    // m_feeder.ConfigVoltageCompSaturation(9);
    //  Set Feeder to coast and config Default
    //  frc::SmartDashboard::PutNumber("Flywheel.Setpoint", 0);
}

void TurretFeederSubsystem::StartFeeder(double FeedRpm)
{
    m_feeder.Set(ControlMode::PercentOutput, FeedRpm);
}

/*
frc2::SequentialCommandGroup FlywheelSubsystem::HoodShot(rad_per_s_t HoodRPM,  double FeedRPM) {
        [this](rad_per_s_t HoodRPM, double FeedRPM) {
            FlywheelSubsystem::StartFeeder(FeedRPM);
            FlywheelSubsystem::SetSetpoint(HoodRPM);
        }

        m_feeder.Set(ControlMode::PercentOutput, FeedRPm),
        FlywheelSubsystem::SetSetpoint(HoodRPM)
}
*/


#ifndef SPARSETENSOR_HYPERTRIE_POSCALC_HPP
#define SPARSETENSOR_HYPERTRIE_POSCALC_HPP

#include <cstdint>
#include <boost/container/flat_map.hpp>
#include <vector>
#include <memory>
#include <itertools.hpp>

namespace hypertrie::internal::util {
	/**
	 * Provides translation between position in superkeys with subkeys. (1,2,4) for example is a subkey of (1,2,3,4).
	 * And the other way around.
	 */
	// TODO: make threadsafe
	class PosCalc {
		template<typename key, typename value>
		using flat_map = ::boost::container::flat_map<key, value>;
	public:
		using key_pos_t = uint8_t;
		using subkey_mask_t = ::std::vector<bool>;
	private:
		/**
		 * Stores for superkey positions to which subkey positions they map.
		 */
		const std::vector<key_pos_t> key_to_subkey;

		/**
		 * Stores for subkey positions to which superkey positions they map. So this is also a list of all currently relevant superkey pos.
		 */
		const std::vector<key_pos_t> subkey_to_key;

		/**
		 * removed_positions bit vector of positions removed from superkey to subkey.
		 */
		const subkey_mask_t removed_positions;

		/**
		 * Cache for PosCalcs of subkeys that are 1 shorter.
		 */
		mutable std::vector<PosCalc const *> next_pos_calcs;

		PosCalc(std::vector<key_pos_t> &&keyToSubkey,
		        std::vector<key_pos_t> &&subkeyToKey,
		        subkey_mask_t removedPositions) :
				key_to_subkey{keyToSubkey},
				subkey_to_key{subkeyToKey},
				removed_positions{std::move(removedPositions)},
				next_pos_calcs(this->keyLength()) {}

	public:

		key_pos_t keyLength() const {
			return key_to_subkey.size();
		}

		key_pos_t subkeyLength() const {
			return subkey_to_key.size();
		}

		/**
		 * Convert a superkey position to a subkey key position.
		 * @param key_pos superkey position
		 * @return subkey position
		 */
		key_pos_t key_to_subkey_pos(key_pos_t key_pos) const {
			return key_to_subkey[key_pos];
		}

		/**
		 * Convert a subkey position to a superkey position.
		 * @param subkey_pos subkey position
		 * @return superkey position
		 */
		key_pos_t subkey_to_key_pos(key_pos_t subkey_pos) const {
			return subkey_to_key[subkey_pos];
		}

		/**
		 * Get all keypositions that this subkey contains.
		 * @return vector of key positions.
		 */
		const std::vector<key_pos_t> &getKeyPoss() const {
			return subkey_to_key;
		}

		/**
		 * bit vector of positions removed from superkey to subkey.
		 * @return bit vector
		 */
		const subkey_mask_t &getSubKeyMask() const {
			return removed_positions;
		}

		/**
		 * PosCalcs of subkeys that has one position less.
		 * @param key_pos position of superkey to be removed.
		 * @return PosCalc like this but without position key_pos.
		 */
		PosCalc const *use(const key_pos_t &key_pos) const {
			PosCalc const *child = next_pos_calcs.at(key_pos);
			if (child == nullptr) {
				std::vector<bool> used_pos_mask(this->keyLength(), true);
				for (auto subkey_pos : this->subkey_to_key)
					used_pos_mask[subkey_pos] = false;
				used_pos_mask[key_pos] = true;

				child = getInstance(used_pos_mask);
				next_pos_calcs[key_pos] = child;
			}
			return child;
		}

		/**
		 * Holds all instances.
		 */
		inline static flat_map<std::vector<bool>, std::unique_ptr<PosCalc const>> instances{};

		/**
		 * Get an instance for a vector of used positions.
		 * @param removed_positions bit vector of positions removed from superkey to subkey.
		 * @return an instance
		 */
		static PosCalc const *getInstance(const std::vector<bool> &removed_positions) {
			auto instance_ = instances.find(removed_positions);
			if (instance_ != instances.end()) {
				// if an instance already exists return it
				return instance_->second.get();
			} else {
				// else create it and store it for reuse
				auto key_length = key_pos_t(removed_positions.size());

				key_pos_t subkey_length = std::count(removed_positions.begin(), removed_positions.end(), false);

				subkey_mask_t next_removed_positions(key_length);
				std::vector<key_pos_t> next_key_to_subkey(key_length);
				std::vector<key_pos_t> next_subkey_to_key(subkey_length);

				key_pos_t offset = 0;

				for (auto key_pos : iter::range(key_length)) {

					if (removed_positions[key_pos]) {
						offset++;
					} else {
						const key_pos_t subkey_pos = key_pos - offset;

						next_key_to_subkey[key_pos] = subkey_pos;
						next_subkey_to_key[subkey_pos] = key_pos;
					}
				}

				instances[removed_positions] = std::unique_ptr<PosCalc const>{new PosCalc{
						std::move(next_key_to_subkey), std::move(next_subkey_to_key), removed_positions}};
				return instances[removed_positions].get();
			}
		}

		/**
		 * Get an instance for a empty vector of used positions of given length.
		 * @param removed_positions length of the empty vector.
		 * @return an instance
		 */
		static PosCalc const *getInstance(const size_t &length) {
			subkey_mask_t key_mask(length);
			return getInstance(key_mask);
		}

	};

}

#endif //SPARSETENSOR_HYPERTRIE_POSCALC_HPP


//==============================================================================
//          Copyright 2015 J.T.  Lapreste
//
//          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_BITWISE_FUNCTIONS_GENERIC_BITSET_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_GENERIC_BITSET_HPP_INCLUDED

#include <boost/simd/bitwise/functions/bitset.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/dec.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF( bitset_, tag::cpu_
                             , (A0)(A1)
                             , (boost::mpl::equal_to
                                < boost::simd::meta::cardinal_of<A0>
                               , boost::simd::meta::cardinal_of<A1>
                                >
                               )
                             , (generic_< arithmetic_<A0> >)
                               (generic_< integer_<A1> >)
                             )
  {
    typedef typename boost::dispatch::meta::as_integer<A0, unsigned>::type i_type;
    typedef typename boost::dispatch::meta::scalar_of<A0>::type               sA0;
    typedef A0                                                        result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(dec(a1)), "bitset :index is out of range");
      return bitwise_or(a0, shl(One<i_type>(), dec(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT( bitset_, tag::cpu_
                          , (A0)(A1)
                          , (generic_< arithmetic_<A0> >)
                            (scalar_< integer_<A1> >)
                          )
  {
    typedef typename boost::dispatch::meta::as_integer<A0, unsigned>::type i_type;
    typedef typename boost::dispatch::meta::scalar_of<A0>::type               sA0;
    typedef A0                                                        result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(dec(a1)), "bitset :index is out of range");
      return bitwise_or(a0, shl(One<i_type>(), dec(a1)));
    }

  };
} } }

#endif