text: /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
   *  Copyright (c) 2014, University of Colorado, Boulder
   *  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, Dave Coleman */
  
  #include <ompl/base/goals/GoalSampleableRegion.h>
  #include <ompl/geometric/planners/prm/ConnectionStrategy.h>
  #include <ompl/tools/config/SelfConfig.h>
  #include <ompl/tools/thunder/SPARSdb.h>
  #include <ompl/util/Console.h>
  #include <boost/foreach.hpp>
  #include <boost/graph/astar_search.hpp>
  #include <boost/graph/incremental_components.hpp>
  #include <boost/property_map/vector_property_map.hpp>
  #include <random>
  
  // Allow hooks for visualizing planner
  //#define OMPL_THUNDER_DEBUG
  
  #define foreach BOOST_FOREACH
  #define foreach_reverse BOOST_REVERSE_FOREACH
  
  // edgeWeightMap methods ////////////////////////////////////////////////////////////////////////////
  
  BOOST_CONCEPT_ASSERT(
      (boost::ReadablePropertyMapConcept<ompl::geometric::SPARSdb::edgeWeightMap, ompl::geometric::SPARSdb::Edge>));
  
  ompl::geometric::SPARSdb::edgeWeightMap::edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
    : g_(graph), collisionStates_(collisionStates)
  {
  }
  
  double ompl::geometric::SPARSdb::edgeWeightMap::get(Edge e) const
  {
      // Get the status of collision checking for this edge
      if (collisionStates_[e] == IN_COLLISION)
          return std::numeric_limits<double>::infinity();
  
      return boost::get(boost::edge_weight, g_, e);
  }
  
  namespace boost
  {
      double get(const ompl::geometric::SPARSdb::edgeWeightMap &m, const ompl::geometric::SPARSdb::Edge &e)
      {
          return m.get(e);
      }
  }
  
  // CustomVisitor methods ////////////////////////////////////////////////////////////////////////////
  
  BOOST_CONCEPT_ASSERT(
      (boost::AStarVisitorConcept<ompl::geometric::SPARSdb::CustomVisitor, ompl::geometric::SPARSdb::Graph>));
  
  ompl::geometric::SPARSdb::CustomVisitor::CustomVisitor(Vertex goal) : goal(goal)
  {
  }
  
  void ompl::geometric::SPARSdb::CustomVisitor::examine_vertex(Vertex u, const Graph &) const
  {
      if (u == goal)
          throw foundGoalException();
  }
  
  // SPARSdb methods ////////////////////////////////////////////////////////////////////////////////////////
  
  ompl::geometric::SPARSdb::SPARSdb(const base::SpaceInformationPtr &si)
    : base::Planner(si, "SPARSdb")
    // Numeric variables
    , nearSamplePoints_((2 * si_->getStateDimension()))
    // Property accessors of edges
    , edgeWeightProperty_(boost::get(boost::edge_weight, g_))
    , edgeCollisionStateProperty_(boost::get(edge_collision_state_t(), g_))
    // Property accessors of vertices
    , stateProperty_(boost::get(vertex_state_t(), g_))
    , colorProperty_(boost::get(vertex_color_t(), g_))
    , interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_))
    // Disjoint set accessors
    , 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;
  
      psimp_ = std::make_shared<PathSimplifier>(si_);
  
      Planner::declareParam<double>("stretch_factor", this, &SPARSdb::setStretchFactor, &SPARSdb::getStretchFactor,
                                    "1.1:0.1:3.0");
      Planner::declareParam<double>("sparse_delta_fraction", this, &SPARSdb::setSparseDeltaFraction,
                                    &SPARSdb::getSparseDeltaFraction, "0.0:0.01:1.0");
      Planner::declareParam<double>("dense_delta_fraction", this, &SPARSdb::setDenseDeltaFraction,
                                    &SPARSdb::getDenseDeltaFraction, "0.0:0.0001:0.1");
      Planner::declareParam<unsigned int>("max_failures", this, &SPARSdb::setMaxFailures, &SPARSdb::getMaxFailures,
                                          "100:10:3000");
  }
  
  ompl::geometric::SPARSdb::~SPARSdb()
  {
      freeMemory();
  }
  
  void ompl::geometric::SPARSdb::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;
  
      if (!sampler_)
          sampler_ = si_->allocValidStateSampler();
  }
  
  void ompl::geometric::SPARSdb::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
  {
      Planner::setProblemDefinition(pdef);
      clearQuery();
  }
  
  void ompl::geometric::SPARSdb::clearQuery()
  {
      startM_.clear();
      goalM_.clear();
      pis_.restart();
  }
  
  void ompl::geometric::SPARSdb::clear()
  {
      Planner::clear();
      clearQuery();
      resetFailures();
      iterations_ = 0;
      freeMemory();
      if (nn_)
          nn_->clear();
  }
  
  void ompl::geometric::SPARSdb::freeMemory()
  {
      Planner::clear();
      sampler_.reset();
  
      foreach (Vertex v, boost::vertices(g_))
      {
          foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | 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::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::State *start, const base::State *goal,
                                                 CandidateSolution &candidateSolution,
                                                 const base::PlannerTerminationCondition &ptc)
  {
      // TODO: nearestK unused
  
      // Get neighbors near start and goal. Note: potentially they are not *visible* - will test for this later
  
      // Start
      OMPL_INFORM("Looking for a node near the problem start");
      if (!findGraphNeighbors(start, startVertexCandidateNeighbors_))
      {
          OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_);
          return false;
      }
      if (verbose_)
          OMPL_INFORM("Found %d nodes near start", startVertexCandidateNeighbors_.size());
  
      // Goal
      OMPL_INFORM("Looking for a node near the problem goal");
      if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_))
      {
          OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_);
          return false;
      }
      if (verbose_)
          OMPL_INFORM("Found %d nodes near goal", goalVertexCandidateNeighbors_.size());
  
      // Get paths between start and goal
      bool result =
          getPaths(startVertexCandidateNeighbors_, goalVertexCandidateNeighbors_, start, goal, candidateSolution, ptc);
  
      // Error check
      if (!result)
      {
          OMPL_INFORM("getSimilarPaths(): SPARSdb returned FALSE for getPaths");
          return false;
      }
      if (!candidateSolution.path_)
      {
          OMPL_ERROR("getSimilarPaths(): SPARSdb returned solution is nullptr");
          return false;
      }
  
      // Debug output
      if (false)
      {
          ompl::geometric::PathGeometric geometricSolution =
              static_cast<ompl::geometric::PathGeometric &>(*candidateSolution.path_);
  
          for (std::size_t i = 0; i < geometricSolution.getStateCount(); ++i)
          {
              OMPL_INFORM("  getSimilarPaths(): Adding state %f to plannerData", i);
              si_->printState(geometricSolution.getState(i), std::cout);
          }
      }
  
      return result;
  }
  
  bool ompl::geometric::SPARSdb::getPaths(const std::vector<Vertex> &candidateStarts,
                                          const std::vector<Vertex> &candidateGoals, const base::State *actualStart,
                                          const base::State *actualGoal, CandidateSolution &candidateSolution,
                                          const base::PlannerTerminationCondition &ptc)
  {
      // Try every combination of nearby start and goal pairs
      foreach (Vertex start, candidateStarts)
      {
          // Check if this start is visible from the actual start
          if (!si_->checkMotion(actualStart, stateProperty_[start]))
          {
              if (verbose_)
                  OMPL_WARN("FOUND CANDIDATE START THAT IS NOT VISIBLE ");
              continue;  // this is actually not visible
          }
  
          foreach (Vertex goal, candidateGoals)
          {
              if (verbose_)
                  OMPL_INFORM("  foreach_goal: Checking motion from  %d to %d", actualGoal, stateProperty_[goal]);
  
              // Check if our planner is out of time
              if (ptc == true)
              {
                  OMPL_DEBUG("getPaths function interrupted because termination condition is true.");
                  return false;
              }
  
              // Check if this goal is visible from the actual goal
              if (!si_->checkMotion(actualGoal, stateProperty_[goal]))
              {
                  if (verbose_)
                      OMPL_INFORM("FOUND CANDIDATE GOAL THAT IS NOT VISIBLE! ");
                  continue;  // this is actually not visible
              }
  
              // Repeatidly search through graph for connection then check for collisions then repeat
              if (lazyCollisionSearch(start, goal, actualStart, actualGoal, candidateSolution, ptc))
              {
                  // Found a path
                  return true;
              }
              else
              {
                  // Did not find a path
                  OMPL_INFORM("Did not find a path, looking for other start/goal combinations ");
              }
  
          }  // foreach
      }      // foreach
  
      return false;
  }
  
  bool ompl::geometric::SPARSdb::lazyCollisionSearch(const Vertex &start, const Vertex &goal,
                                                     const base::State *actualStart, const base::State *actualGoal,
                                                     CandidateSolution &candidateSolution,
                                                     const base::PlannerTerminationCondition &ptc)
  {
      base::Goal *g = pdef_->getGoal().get();  // for checking isStartGoalPairValid
  
      // Vector to store candidate paths in before they are converted to PathPtrs
      std::vector<Vertex> vertexPath;
  
      // decide if start and goal are connected
      // TODO this does not compute dynamic graphcs
      // i.e. it will say its the same components even when an edge has been disabled
      bool same_component =
          true;  // sameComponent(start, goal); // TODO is this important? I disabled it during dev and never used it
  
      // Check if the chosen start and goal can be used together to satisfy problem
      if (!same_component)
      {
          if (verbose_)
              OMPL_INFORM("    Goal and start are not part of same component, skipping ");
          return false;
      }
  
      // TODO: remove this because start and goal are not either start nor goals
      if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
      {
          if (verbose_)
              OMPL_INFORM("    Start and goal pair are not valid combinations, skipping ");
          return false;
      }
  
      // Make sure that the start and goal aren't so close together that they find the same vertex
      if (start == goal)
      {
          if (verbose_)
              OMPL_INFORM("    Start equals goal, skipping ");
          return false;
      }
  
      // Keep looking for paths between chosen start and goal until one is found that is valid,
      // or no further paths can be found between them because of disabled edges
      // this is necessary for lazy collision checking i.e. rerun after marking invalid edges we found
      bool havePartialSolution = false;
      while (true)
      {
          if (verbose_)
              OMPL_INFORM("      while true: look for valid paths between start and goal");
  
          // Check if our planner is out of time
          if (ptc == true)
          {
              OMPL_DEBUG("lazyCollisionSearch: function interrupted because termination condition is true.");
              return false;
          }
  
          // Attempt to find a solution from start to goal
          if (!constructSolution(start, goal, vertexPath))
          {
              // We will stop looking through this start-goal combination, but perhaps this partial solution is good
              if (verbose_)
                  OMPL_INFORM("        unable to construct solution between start and goal using astar");
  
              // no solution path found. check if a previous partially correct solution was found
              if (havePartialSolution && false)  // TODO: re-implement partial solution logic
              {
                  if (verbose_)
                      OMPL_INFORM("has partial solution ");
                  // Save this candidateSolution for later
                  convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
                  return false;
              }
  
              if (verbose_)
                  OMPL_INFORM("        no partial solution found on this astar search, keep looking through start-goal "
                              "combos");
  
              // no path found what so ever
              // return false;
              return false;
          }
          havePartialSolution = true;  // we have found at least one path at this point. may be invalid
  
          if (verbose_)
          {
              OMPL_INFORM("        has at least a partial solution, maybe exact solution");
              OMPL_INFORM("        Solution has %d vertices", vertexPath.size());
          }
  
          // Check if all the points in the potential solution are valid
          if (lazyCollisionCheck(vertexPath, ptc))
          {
              if (verbose_)
              {
                  OMPL_INFORM("---------- lazy collision check returned valid ");
              }
  
              // the path is valid, we are done!
              convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
              return true;
          }
          // else, loop with updated graph that has the invalid edges/states disabled
      }  // while
  
      // we never found a valid path
      return false;
  }
  
  bool ompl::geometric::SPARSdb::constructSolution(const Vertex start, const Vertex goal,
                                                   std::vector<Vertex> &vertexPath) const
  {
      auto *vertexPredecessors = new Vertex[boost::num_vertices(g_)];
      bool foundGoal = false;
  
      auto *vertexDistances = new double[boost::num_vertices(g_)];
  
      try
      {
          boost::astar_search(g_,     // graph
                              start,  // start state
                              [this, goal](const Vertex v)
                              {
                                  return distanceFunction(v, goal);
                              },  // the heuristic
                              // ability to disable edges (set cost to inifinity):
                              boost::weight_map(edgeWeightMap(g_, edgeCollisionStateProperty_))
                                  .predecessor_map(vertexPredecessors)
                                  .distance_map(&vertexDistances[0])
                                  .visitor(CustomVisitor(goal)));
      }
      catch (ompl::geometric::SPARSdb::foundGoalException &)
      {
          // the custom exception from CustomVisitor
          if (verbose_ && false)
          {
              OMPL_INFORM("constructSolution: Astar found goal vertex ------------------------");
              OMPL_INFORM("distance to goal: %f", vertexDistances[goal]);
          }
  
          if (vertexDistances[goal] > 1.7e+308)  // terrible hack for detecting infinity
          // double diff = d[goal] - std::numeric_limits<double>::infinity();
          // if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
          // check if the distance to goal is inifinity. if so, it is unreachable
          // if (d[goal] >= std::numeric_limits<double>::infinity())
          {
              if (verbose_)
                  OMPL_INFORM("Distance to goal is infinity");
              foundGoal = false;
          }
          else
          {
              // Only clear the vertexPath after we know we have a new solution, otherwise it might have a good
              // previous one
              vertexPath.clear();  // remove any old solutions
  
              // Trace back the shortest path in reverse and only save the states
              Vertex v;
              for (v = goal; v != vertexPredecessors[v]; v = vertexPredecessors[v])
              {
                  vertexPath.push_back(v);
                  std::cout <<"Solution Vertex: " <<  v << std::endl;
  
  
              }
  
              if (v != goal)  // TODO explain this because i don't understand
              {
                  vertexPath.push_back(v);
              }
  
              foundGoal = true;
          }
      }
  
      delete[] vertexPredecessors;
      delete[] vertexDistances;
  
      // No solution found from start to goal
      return foundGoal;
  }
  
  bool ompl::geometric::SPARSdb::lazyCollisionCheck(std::vector<Vertex> &vertexPath,
                                                    const base::PlannerTerminationCondition &ptc)
  {
      OMPL_DEBUG("Starting lazy collision checking");
  
      bool hasInvalidEdges = false;
  
      // Initialize
      Vertex fromVertex = vertexPath[0];
      Vertex toVertex;
  
      // Loop through every pair of states and make sure path is valid.
      for (std::size_t toID = 1; toID < vertexPath.size(); ++toID)
      {
          // Increment location on path
          toVertex = vertexPath[toID];
  
          // Check if our planner is out of time
          if (ptc == true)
          {
              OMPL_DEBUG("Lazy collision check function interrupted because termination condition is true.");
              return false;
          }
  
          Edge thisEdge = boost::edge(fromVertex, toVertex, g_).first;
  
          // Has this edge already been checked before?
          if (edgeCollisionStateProperty_[thisEdge] == NOT_CHECKED)
          {
              // Check path between states
              if (!si_->checkMotion(stateProperty_[fromVertex], stateProperty_[toVertex]))
              {
                  // Path between (from, to) states not valid, disable the edge
                  OMPL_INFORM("  DISABLING EDGE from vertex %f to vertex %f", fromVertex, toVertex);
  
                  // Disable edge
                  edgeCollisionStateProperty_[thisEdge] = IN_COLLISION;
              }
              else
              {
                  // Mark edge as free so we no longer need to check for collision
                  edgeCollisionStateProperty_[thisEdge] = FREE;
              }
          }
  
          // Check final result
          if (edgeCollisionStateProperty_[thisEdge] == IN_COLLISION)
          {
              // Remember that this path is no longer valid, but keep checking remainder of path edges
              hasInvalidEdges = true;
          }
  
          // switch vertex focus
          fromVertex = toVertex;
      }
  
      OMPL_INFORM("Done lazy collision checking");
  
      // TODO: somewhere in the code we need to reset all edges collision status back to NOT_CHECKED for future queries
  
      // Only return true if nothing was found invalid
      return !hasInvalidEdges;
  }
  
  bool ompl::geometric::SPARSdb::sameComponent(Vertex m1, Vertex m2)
  {
      return boost::same_component(m1, m2, disjointSets_);
  }
  
  bool ompl::geometric::SPARSdb::reachedFailureLimit() const
  {
      return consecutiveFailures_ >= maxFailures_;
  }
  
  void ompl::geometric::SPARSdb::printDebug(std::ostream &out) const
  {
      out << "SPARSdb 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 << "    Sparse Delta: " << sparseDelta_ << std::endl;
      out << "    Stretch Factor: " << getStretchFactor() << std::endl;
      out << "    Maximum Extent: " << si_->getMaximumExtent() << std::endl;
      out << "  Status: " << std::endl;
      out << "    Vertices Count: " << getNumVertices() << std::endl;
      out << "    Edges Count:    " << getNumEdges() << std::endl;
      out << "    Iterations: " << getIterations() << std::endl;
      out << "    Consecutive Failures: " << consecutiveFailures_ << std::endl;
      out << "    Number of guards: " << nn_->size() << std::endl << std::endl;
  }
  
  bool ompl::geometric::SPARSdb::getGuardSpacingFactor(const double pathLength, int &numGuards, double &spacingFactor)
  {
      static const double factorHigh = 1.9;
      static const double factorLow = 1.1;
      double minPathLength = sparseDelta_ * factorLow;
  
      // Check if the path length is too short
      if (pathLength < minPathLength)
      {
          OMPL_INFORM("Path length is too short to get a correct sparcing factor: length: %f, min: %f ", pathLength,
                      minPathLength);
          spacingFactor = factorLow;
          return true;  // still attempt
      }
  
      // Get initial guess using med value
      double numGuardsFraction = pathLength / (sparseDelta_ * factorLow);
      if (verbose_)
      {
          OMPL_INFORM("getGuardSpacingFactor: ");
          OMPL_INFORM("  pathLength: %f", pathLength);
          OMPL_INFORM("  sparseDelta: %f", sparseDelta_);
          OMPL_INFORM("  min pathLength: %f", minPathLength);
          OMPL_INFORM("  numGuardsFraction: %f", numGuardsFraction);
      }
  
      // Round down to nearest integer
      numGuards = numGuardsFraction;
  
      static std::size_t MAX_ATTEMPTS = 4;
      for (std::size_t i = 0; i < MAX_ATTEMPTS; ++i)
      {
          if (verbose_)
              OMPL_INFORM("  numGuards: %d", numGuards);
  
          // Find the factor to achieve this number of guards
          spacingFactor = pathLength / (sparseDelta_ * numGuards);
          if (verbose_)
              OMPL_INFORM("  new spacingFactor: %f", spacingFactor);
  
          // Check if this factor is too low
          if (spacingFactor < factorLow)
          {
              if (verbose_)
                  OMPL_INFORM("  spacing factor is too low ");
              numGuards++;
              continue;
          }
          else if (spacingFactor > factorHigh)
          {
              if (verbose_)
                  OMPL_INFORM("  spacing factor is too high ");
              numGuards--;
              continue;
          }
          else
              return true;  // a good value
      }
  
      OMPL_ERROR("Unable to find correct spacing factor - perhaps this is a bug");
      spacingFactor = factorLow;
      return true;  // still attempt
  }
  
  bool ompl::geometric::SPARSdb::addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
                                                  ompl::geometric::PathGeometric &solutionPath)
  {
      // Check that the query vertex is initialized (used for internal nearest neighbor searches)
      checkQueryStateInitialization();
  
      // Error check
      if (solutionPath.getStateCount() < 2)
      {
          OMPL_ERROR("Less than 2 states were passed to addPathToRoadmap in the solution path");
          return false;
      }
  
      // Find spacing factor - 2.0 would be a perfect amount, but we leave room for rounding/interpolation errors and
      // curves in path
      int numGuards;  // unused variable that indicates how many guards we will add
      double spacingFactor;
      if (!getGuardSpacingFactor(solutionPath.length(), numGuards, spacingFactor))
          return false;
  
      OMPL_DEBUG("Expected number of necessary coverage guards is calculated to be %i from the original path state count "
                 "%i",
                 numGuards, solutionPath.getStateCount());
  
      unsigned int n = 0;
      const int n1 = solutionPath.getStateCount() - 1;
      for (int i = 0; i < n1; ++i)
          n += si_->getStateSpace()->validSegmentCount(solutionPath.getState(i), solutionPath.getState(i + 1));
  
      solutionPath.interpolate(n);
  
      // Debug
      if (verbose_)
      {
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("Attempting to add %d states to roadmap", solutionPath.getStateCount());
          OMPL_INFORM("-------------------------------------------------------");
      }
  
      // Try to add the start first, but don't force it
      addStateToRoadmap(ptc, solutionPath.getState(0));
  
  #ifdef OMPL_THUNDER_DEBUG
      visualizeStateCallback(solutionPath.getState(solutionPath.getStateCount() - 1), 3, sparseDelta_);
  #endif
  
      // Add solution states to SPARSdb one by one ---------------------------
  
      // Track which nodes we've already tried to add
      std::vector<std::size_t> addedStateIDs;
      // Track which nodes we will attempt to use as connectivity states
      std::vector<std::size_t> connectivityStateIDs;
      // std::vector<base::State*> connectivityStates;
  
      double distanceFromLastState = 0;
  
      std::size_t lastStateID = 0;  // track the id in the solutionPath of the last state
  
      for (std::size_t i = 1; i < solutionPath.getStateCount();
           ++i)  // skip 0 and last because those are start/goal and are already added
      {
          distanceFromLastState = si_->distance(solutionPath.getState(i), solutionPath.getState(lastStateID));
  
          if (verbose_ && false)
          {
              OMPL_INFORM("Index %d at distance %f from last state ", i, distanceFromLastState);
          }
  
          if (distanceFromLastState >= sparseDelta_ * spacingFactor)
          {
              if (verbose_)
              {
                  OMPL_INFORM("Adding state %d of %d", i, solutionPath.getStateCount());
              }
  
  // Show the candidate state in Rviz for path insertion of GUARDS
  #ifdef OMPL_THUNDER_DEBUG
              visualizeStateCallback(solutionPath.getState(i), 1, sparseDelta_);
  #endif
  
              // Add a single state to the roadmap
              if (!addStateToRoadmap(ptc, solutionPath.getState(i)))
              {
                  if (verbose_)
                  {
                      OMPL_INFORM("Last state added to roadmap failed ");
                  }
              }
  
              // Now figure out midpoint state between lastState and i
              std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
              connectivityStateIDs.push_back(midStateID);
  
              double distA = si_->distance(solutionPath.getState(lastStateID), solutionPath.getState(midStateID));
              double distB = si_->distance(solutionPath.getState(i), solutionPath.getState(midStateID));
              double diff = distA - distB;
              if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
                  if (verbose_)
                      OMPL_WARN("DISTANCES ARE DIFFERENT ");
  
              // Save this state as the new last state
              lastStateID = i;
              // Remember which nodes we've already added / attempted to add
              addedStateIDs.push_back(midStateID);
              addedStateIDs.push_back(i);
          }
          // Close up if it doesn't do it automatically
          else if (i == solutionPath.getStateCount() - 1)
          {
              if (verbose_)
                  OMPL_INFORM("Last state - do special midpoint");
  
              // Now figure out midpoint state between lastState and i
              std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
              connectivityStateIDs.push_back(midStateID);
              addedStateIDs.push_back(midStateID);
              if (verbose_)
                  OMPL_INFORM("Mid state is %d", midStateID);
          }
      }
  
      // Attempt to add the goal directly
      addStateToRoadmap(ptc, solutionPath.getState(solutionPath.getStateCount() - 1));
  
      if (verbose_)
      {
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("Adding connectivity states ----------------------------");
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("-------------------------------------------------------");
      }
  
      for (std::size_t i = 0; i < connectivityStateIDs.size(); ++i)
      {
          base::State *connectivityState = solutionPath.getState(connectivityStateIDs[i]);
  
          if (verbose_)
          {
              OMPL_INFORM("Adding connectvity state ", i);
          }
  
  #ifdef OMPL_THUNDER_DEBUG
          // Show the candidate state in Rviz for path insertion of BRIDGES (CONNECTIVITY)
          visualizeStateCallback(connectivityState, 2, sparseDelta_);
          sleep(0.5);
  #endif
  
          // Add a single state to the roadmap
          addStateToRoadmap(ptc, connectivityState);
      }
  
      // Add remaining states at random
      if (verbose_)
      {
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("Adding remaining states randomly ----------------------");
          OMPL_INFORM("-------------------------------------------------------");
          OMPL_INFORM("-------------------------------------------------------");
      }
  
      // Create a vector of shuffled indexes
      std::vector<std::size_t> shuffledIDs;
      std::size_t usedIDTracker = 0;
      for (std::size_t i = 1; i < solutionPath.getStateCount(); ++i)  // skip 0 because start already added
      {
          // Check if we've already used this id
          if (usedIDTracker < addedStateIDs.size() && i == addedStateIDs[usedIDTracker])
          {
              // skip this id
              usedIDTracker++;
              continue;
          }
  
          shuffledIDs.push_back(i);  // 1 2 3...
      }
  
      std::shuffle(shuffledIDs.begin(), shuffledIDs.end(), std::mt19937(std::random_device()()));
  
      // Add each state randomly
      for (unsigned long shuffledID : shuffledIDs)
      {
  #ifdef OMPL_THUNDER_DEBUG
          visualizeStateCallback(solutionPath.getState(shuffledIDs[i]), 1, sparseDelta_);
  #endif
  
          // Add a single state to the roadmap
          addStateToRoadmap(ptc, solutionPath.getState(shuffledID));
      }
  
      bool benchmarkLogging = true;
      if (benchmarkLogging)
      {
          OMPL_DEBUG("ompl::geometric::SPARSdb: Benchmark logging enabled (slower)");
  
          // Return the result of inserting into database, if applicable
          return checkStartGoalConnection(solutionPath);
      }
  
      return true;
  }
  
  bool ompl::geometric::SPARSdb::checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath)
  {
      // Make sure path has states
      if (solutionPath.getStateCount() < 2)
      {
          OMPL_ERROR("Not enought states (< 2) in the solutionPath");
          return false;
      }
  
      bool error = false;
      CandidateSolution candidateSolution;
      do
      {
          base::State *actualStart = solutionPath.getState(0);
          base::State *actualGoal = solutionPath.getState(solutionPath.getStateCount() - 1);
  
          /* 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;
  
          // Get start vertex
          findGraphNeighbors(actualStart, graphNeighborhood, visibleNeighborhood);
          if (!visibleNeighborhood.size())
          {
              OMPL_ERROR("No vertexes found near start");
              error = true;
              break;
          }
          Vertex closeStart = visibleNeighborhood[0];
  
          // Get goal vertex
          findGraphNeighbors(actualGoal, graphNeighborhood, visibleNeighborhood);
          if (!visibleNeighborhood.size())
          {
              OMPL_ERROR("No vertexes found near goal");
              error = true;
              break;
          }
          Vertex closeGoal = visibleNeighborhood[0];
  
          // Check if connected
          if (false)
              if (!sameComponent(closeStart, closeGoal))
              {
                  OMPL_ERROR("Start and goal are not connected!");
                  error = true;
                  break;
              }
  
          // Get new path from start to goal
          std::vector<Vertex> vertexPath;
          if (!constructSolution(closeStart, closeGoal, vertexPath))
          {
              OMPL_ERROR("Unable to find path from start to goal - perhaps because of new obstacles");
              error = true;
              break;
          }
  
          // Convert to PathGeometric
          bool disableCollisionWarning = true;  // this is just for benchmarking purposes
          if (!convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution,
                                            disableCollisionWarning))
          {
              OMPL_ERROR("Unable to convert to state path");
              error = true;
              break;
          }
      } while (false);
  
      // Check distance of new path from old path
      double originalLength = solutionPath.length();
  
      OMPL_DEBUG("Results of attempting to make insertion in SPARSdb ");
      OMPL_DEBUG("-------------------------------------------------------");
      OMPL_DEBUG("Original length:    %f", originalLength);
  
      if (error)
      {
          OMPL_ERROR("UNABLE TO GET PATH");
  
          // Record this for plotting
          numPathInsertionFailures_++;
      }
      else
      {
          double newLength = candidateSolution.getGeometricPath().length();
          double percentIncrease = 100 - originalLength / newLength * 100;
          OMPL_DEBUG("New length:        %f", newLength);
          OMPL_DEBUG("Percent increase:  %f %%", percentIncrease);
      }
  
      return !error;  // return true if it inserted correctly
  }
  
  bool ompl::geometric::SPARSdb::addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState)
  {
      bool stateAdded = false;
      // Check that the query vertex is initialized (used for internal nearest neighbor searches)
      checkQueryStateInitialization();
  
      // Deep copy
      base::State *qNew = si_->cloneState(newState);
      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;
  
      ++iterations_;
  
      findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
  
      if (verbose_)
      {
          OMPL_INFORM(" graph neighborhood: %d | visible neighborhood: %d", graphNeighborhood.size(),
                      visibleNeighborhood.size());
  
          foreach (Vertex v, visibleNeighborhood)
          {
              OMPL_INFORM("Visible neighbor is vertex %f with distance %f ", v, si_->distance(qNew, stateProperty_[v]));
          }
      }
  
      if (verbose_)
          OMPL_INFORM(" - checkAddCoverage() Are other nodes around it visible?");
      // Coverage criterion
      if (!checkAddCoverage(qNew,
                            visibleNeighborhood))  // Always add a node if no other nodes around it are visible (GUARD)
      {
          if (verbose_)
              OMPL_INFORM(" -- checkAddConnectivity() Does this node connect neighboring nodes that are not connected? ");
          // Connectivity criterion
          if (!checkAddConnectivity(qNew, visibleNeighborhood))
          {
              if (verbose_)
                  OMPL_INFORM(" --- checkAddInterface() Does this node's neighbor's need it to better connect them? ");
              if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
              {
                  if (verbose_)
                      OMPL_INFORM(" ---- Ensure SPARS asymptotic optimality");
                  if (visibleNeighborhood.size() > 0)
                  {
                      std::map<Vertex, base::State *> closeRepresentatives;
                      if (verbose_)
                          OMPL_INFORM(" ----- findCloseRepresentatives()");
  
                      findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
                      if (verbose_)
                          OMPL_INFORM("------ Found %d close representatives", closeRepresentatives.size());
  
                      for (auto &closeRepresentative : closeRepresentatives)
                      {
                          if (verbose_)
                              OMPL_INFORM(" ------ Looping through close representatives");
                          updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
                                           closeRepresentative.second);
                          updatePairPoints(closeRepresentative.first, closeRepresentative.second, visibleNeighborhood[0],
                                           qNew);
                      }
                      if (verbose_)
                          OMPL_INFORM(" ------ checkAddPath()");
                      if (checkAddPath(visibleNeighborhood[0]))
                      {
                          if (verbose_)
                          {
                              OMPL_INFORM("nearest visible neighbor added ");
                          }
                      }
  
                      for (auto &closeRepresentative : closeRepresentatives)
                      {
                          if (verbose_)
                              OMPL_INFORM(" ------- Looping through close representatives to add path");
                          checkAddPath(closeRepresentative.first);
                          si_->freeState(closeRepresentative.second);
                      }
                      if (verbose_)
                          OMPL_INFORM("------ Done with inner most loop ");
                  }
              }
              else  //  added for interface
              {
                  stateAdded = true;
              }
          }
          else  // added for connectivity
          {
              stateAdded = true;
          }
      }
      else  // added for coverage
      {
          stateAdded = true;
      }
  
      if (!stateAdded)
          ++consecutiveFailures_;
  
      si_->freeState(workState);
      si_->freeState(qNew);
  
      return stateAdded;
  }
  
  void ompl::geometric::SPARSdb::checkQueryStateInitialization()
  {
      if (boost::num_vertices(g_) < 1)
      {
          queryVertex_ = boost::add_vertex(g_);
          stateProperty_[queryVertex_] = nullptr;
      }
  }
  
  ompl::base::PlannerStatus ompl::geometric::SPARSdb::solve(const base::PlannerTerminationCondition &)
  {
      // Disabled
      return base::PlannerStatus::TIMEOUT;
  }
  
  bool ompl::geometric::SPARSdb::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
  {
      if (visibleNeighborhood.size() > 0)
          return false;
      // No free paths means we add for coverage
      if (verbose_)
          OMPL_INFORM(" --- Adding node for COVERAGE ");
      Vertex v = addGuard(si_->cloneState(qNew), COVERAGE);
      if (verbose_)
          OMPL_INFORM("       Added vertex %f", v);
  
      return true;
  }
  
  bool ompl::geometric::SPARSdb::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
  {
      // Identify visibile nodes around our new state that are unconnected (in different connected components)
      // and connect them
  
      std::vector<Vertex> statesInDiffConnectedComponents;  // links
      if (visibleNeighborhood.size() >
          1)  // if less than 2 there is no way to find a pair of nodes in different connected components
      {
          // 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]))
                  {
                      statesInDiffConnectedComponents.push_back(visibleNeighborhood[i]);
                      statesInDiffConnectedComponents.push_back(visibleNeighborhood[j]);
                  }
              }
          }
  
          // Were any diconnected states found?
          if (statesInDiffConnectedComponents.size() > 0)
          {
              if (verbose_)
                  OMPL_INFORM(" --- Adding node for CONNECTIVITY ");
              // Add the node
              Vertex newVertex = addGuard(si_->cloneState(qNew), CONNECTIVITY);
  
              for (unsigned long statesInDiffConnectedComponent : statesInDiffConnectedComponents)
              {
                  // If there's no edge between the two new states
                  // DTC: this should actually never happen - we just created the new vertex so
                  // why would it be connected to anything?
                  if (!boost::edge(newVertex, statesInDiffConnectedComponent, g_).second)
                  {
                      // The components haven't been united by previous links
                      if (!sameComponent(statesInDiffConnectedComponent, newVertex))
                          connectGuards(newVertex, statesInDiffConnectedComponent);
                  }
              }
  
              return true;
          }
      }
      return false;
  }
  
  bool ompl::geometric::SPARSdb::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
                                                   std::vector<Vertex> &visibleNeighborhood)
  {
      // If we have at least 2 neighbors
      if (visibleNeighborhood.size() > 1)
      {
          // If the two closest nodes are also visible
          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
                      if (verbose_)
                          OMPL_INFORM(" ---   INTERFACE: directly connected nodes ");
                      connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
                      // And report that we added to the roadmap
                      resetFailures();
                      // Report success
                      return true;
                  }
                  else
                  {
                      // Add the new node to the graph, to bridge the interface
                      if (verbose_)
                          OMPL_INFORM(" --- Adding node for INTERFACE  ");
                      Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
                      connectGuards(v, visibleNeighborhood[0]);
                      connectGuards(v, visibleNeighborhood[1]);
                      if (verbose_)
                          OMPL_INFORM(" ---   INTERFACE: connected two neighbors through new interface node ");
                      // Report success
                      return true;
                  }
              }
          }
      }
      return false;
  }
  
  bool ompl::geometric::SPARSdb::checkAddPath(Vertex v)
  {
      bool spannerPropertyWasViolated = 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() && !spannerPropertyWasViolated; ++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_)
              {
                  spannerPropertyWasViolated = 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
                              if (verbose_)
                                  OMPL_INFORM(" --- Adding node for QUALITY");
                              vnew = addGuard(st, QUALITY);
  
                              connectGuards(prior, vnew);
                              prior = vnew;
                          }
                          // clear the states, so memory is not freed twice
                          states.clear();
                          connectGuards(prior, rp);
                      }
                  }
              }
          }
      }
  
      if (!spannerPropertyWasViolated)
      {
          if (verbose_)
          {
              OMPL_INFORM(" ------- Spanner property was NOT violated, SKIPPING");
          }
      }
  
      return spannerPropertyWasViolated;
  }
  
  void ompl::geometric::SPARSdb::resetFailures()
  {
      consecutiveFailures_ = 0;
  }
  
  void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
                                                    std::vector<Vertex> &visibleNeighborhood)
  {
      visibleNeighborhood.clear();
      stateProperty_[queryVertex_] = st;
      nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
      if (verbose_ && false)
          OMPL_INFORM("Finding nearest nodes in NN tree within radius %f", sparseDelta_);
      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);
  }
  
  bool ompl::geometric::SPARSdb::findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood)
  {
      base::State *stateCopy = si_->cloneState(state);
  
      // Don't check for visibility
      graphNeighborhood.clear();
      stateProperty_[queryVertex_] = stateCopy;
  
      // Double the range of sparseDelta_ up to 3 times until at least 1 neighbor is found
      std::size_t expandNeighborhoodSearchAttempts = 3;
      double neighborSearchRadius;
      static const double EXPAND_NEIGHBORHOOD_RATE =
          0.25;  // speed to which we look outside the original sparse delta neighborhood
      for (std::size_t i = 0; i < expandNeighborhoodSearchAttempts; ++i)
      {
          neighborSearchRadius = sparseDelta_ + i * EXPAND_NEIGHBORHOOD_RATE * sparseDelta_;
          if (verbose_)
          {
              OMPL_INFORM("-------------------------------------------------------");
              OMPL_INFORM("Attempt %d to find neighborhood at radius %f", i + 1, neighborSearchRadius);
              OMPL_INFORM("-------------------------------------------------------");
          }
  
          nn_->nearestR(queryVertex_, neighborSearchRadius, graphNeighborhood);
  
          // Check if at least one neighbor found
          if (graphNeighborhood.size() > 0)
              break;
      }
      stateProperty_[queryVertex_] = nullptr;
  
      // Check if no neighbors found
      if (!graphNeighborhood.size())
      {
          return false;
      }
      return true;
  }
  
  void ompl::geometric::SPARSdb::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::SPARSdb::Vertex ompl::geometric::SPARSdb::findGraphRepresentative(base::State *st)
  {
      std::vector<Vertex> nbh;
      stateProperty_[queryVertex_] = st;
      nn_->nearestR(queryVertex_, sparseDelta_, nbh);
      stateProperty_[queryVertex_] = nullptr;
  
      if (verbose_)
          OMPL_INFORM(" ------- findGraphRepresentative found %d nearest neighbors of distance %f", nbh.size(),
                      sparseDelta_);
  
      Vertex result = boost::graph_traits<Graph>::null_vertex();
  
      for (std::size_t i = 0; i < nbh.size(); ++i)
      {
          if (verbose_)
              OMPL_INFORM(" -------- Checking motion of graph rep candidate %d", i);
          if (si_->checkMotion(st, stateProperty_[nbh[i]]))
          {
              if (verbose_)
                  OMPL_INFORM(" --------- VALID ");
              result = nbh[i];
              break;
          }
      }
      return result;
  }
  
  void ompl::geometric::SPARSdb::findCloseRepresentatives(base::State *workState, const base::State *qNew,
                                                          const Vertex qRep,
                                                          std::map<Vertex, base::State *> &closeRepresentatives,
                                                          const base::PlannerTerminationCondition &ptc)
  {
      // Properly clear the vector by also deleting previously sampled unused states
      for (auto &closeRepresentative : closeRepresentatives)
          si_->freeState(closeRepresentative.second);
      closeRepresentatives.clear();
  
      // denseDelta_ = 0.25 * sparseDelta_;
      nearSamplePoints_ /= 10;  // HACK - this makes it look for the same number of samples as dimensions
  
      if (verbose_)
          OMPL_INFORM(" ----- nearSamplePoints: %f, denseDelta: %f", nearSamplePoints_, denseDelta_);
  
      // Then, begin searching the space around new potential state qNew
      for (unsigned int i = 0; i < nearSamplePoints_ && ptc == false; ++i)
      {
          do
          {
              sampler_->sampleNear(workState, qNew, denseDelta_);
  
  #ifdef OMPL_THUNDER_DEBUG
              visualizeStateCallback(workState, 3, sparseDelta_);
              sleep(0.1);
  #endif
  
              if (verbose_)
              {
                  OMPL_INFORM(" ------ findCloseRepresentatives sampled state ");
  
                  if (!si_->isValid(workState))
                  {
                      OMPL_INFORM(" ------ isValid ");
                  }
                  if (si_->distance(qNew, workState) > denseDelta_)
                  {
                      OMPL_INFORM(" ------ Distance too far ");
                  }
                  if (!si_->checkMotion(qNew, workState))
                  {
                      OMPL_INFORM(" ------ Motion invalid ");
                  }
              }
  
          } while ((!si_->isValid(workState) || si_->distance(qNew, workState) > denseDelta_ ||
                    !si_->checkMotion(qNew, workState)) &&
                   ptc == false);
  
          // if we were not successful at sampling a desirable state, we are out of time
          if (ptc == true)
          {
              if (verbose_)
                  OMPL_INFORM(" ------ We are out of time ");
              break;
          }
  
          if (verbose_)
              OMPL_INFORM(" ------ Find graph representative ");
  
          // Compute who his graph neighbors are
          Vertex representative = findGraphRepresentative(workState);
  
          // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
          if (representative != boost::graph_traits<Graph>::null_vertex())
          {
              if (verbose_)
                  OMPL_INFORM(" ------ Representative is not null ");
  
              // If his representative is different than qNew
              if (qRep != representative)
              {
                  if (verbose_)
                      OMPL_INFORM(" ------ qRep != representative ");
  
                  // And we haven't already tracked this representative
                  if (closeRepresentatives.find(representative) == closeRepresentatives.end())
                  {
                      if (verbose_)
                          OMPL_INFORM(" ------ Track the representative");
                      // Track the representativen
                      closeRepresentatives[representative] = si_->cloneState(workState);
                  }
              }
              else
              {
                  if (verbose_)
                      OMPL_INFORM(" ------ qRep == representative, no good ");
              }
          }
          else
          {
              if (verbose_)
                  OMPL_INFORM(" ------ Rep is null ");
  
              // This guy can't be seen by anybody, so we should take this opportunity to add him
              if (verbose_)
                  OMPL_INFORM(" --- Adding node for COVERAGE");
              addGuard(si_->cloneState(workState), COVERAGE);
  
              if (verbose_)
              {
                  OMPL_INFORM(" ------ STOP EFFORS TO ADD A DENSE PATH");
              }
  
              // We should also stop our efforts to add a dense path
              for (auto &closeRepresentative : closeRepresentatives)
                  si_->freeState(closeRepresentative.second);
              closeRepresentatives.clear();
              break;
          }
      }  // for loop
  }
  
  void ompl::geometric::SPARSdb::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::SPARSdb::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::SPARSdb::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_) || (cx < vpp && d.pointB_))
                  Xs.push_back(cx);
          }
      Xs.push_back(vpp);
  }
  
  ompl::geometric::SPARSdb::VertexPair ompl::geometric::SPARSdb::index(Vertex vp, Vertex vpp)
  {
      if (vp < vpp)
          return VertexPair(vp, vpp);
      else 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::SPARSdb::InterfaceData &ompl::geometric::SPARSdb::getData(Vertex v, Vertex vp, Vertex vpp)
  {
      return interfaceDataProperty_[v].interfaceHash[index(vp, vpp)];
  }
  
  void ompl::geometric::SPARSdb::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].interfaceHash[index(r, rp)] = d;
  }
  
  void ompl::geometric::SPARSdb::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].interfaceHash | boost::adaptors::map_keys)
              interfaceDataProperty_[v].interfaceHash[r].clear(si_);
      }
  }
  
  ompl::geometric::SPARSdb::Vertex ompl::geometric::SPARSdb::addGuard(base::State *state, GuardType type)
  {
      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();
  
      if (verbose_)
      {
          OMPL_INFORM(" ---- addGuard() of type %f", type);
      }
  #ifdef OMPL_THUNDER_DEBUG
      visualizeStateCallback(state, 4, sparseDelta_);  // Candidate node has already (just) been added
      sleep(0.1);
  #endif
  
      return m;
  }
  
  void ompl::geometric::SPARSdb::connectGuards(Vertex v, Vertex vp)
  {
      // OMPL_INFORM("connectGuards called ---------------------------------------------------------------- ");
      assert(v <= getNumVertices());
      assert(vp <= getNumVertices());
  
      if (verbose_)
      {
          OMPL_INFORM(" ------- connectGuards/addEdge: Connecting vertex %f to vertex %f", v, vp);
      }
  
      // Create the new edge
      Edge e = (boost::add_edge(v, vp, g_)).first;
      std::cout <<"edge: " <<  e << std::endl;
  
      // Add associated properties to the edge
      edgeWeightProperty_[e] = distanceFunction(v, vp);  // TODO: use this value with astar
      edgeCollisionStateProperty_[e] = NOT_CHECKED;
  
      // Add the edge to the incrementeal connected components datastructure
      disjointSets_.union_set(v, vp);
  
  // Debug in Rviz
  #ifdef OMPL_THUNDER_DEBUG
      visualizeEdgeCallback(stateProperty_[v], stateProperty_[vp]);
      sleep(0.8);
  #endif
  }
  
  bool ompl::geometric::SPARSdb::convertVertexPathToStatePath(std::vector<Vertex> &vertexPath,
                                                              const base::State *actualStart,
                                                              const base::State *actualGoal,
                                                              CandidateSolution &candidateSolution,
                                                              bool disableCollisionWarning)
  {
      if (!vertexPath.size())
          return false;
  
      auto pathGeometric(std::make_shared<ompl::geometric::PathGeometric>(si_));
      candidateSolution.isApproximate_ = false;  // assume path is valid
  
      // Add original start if it is different than the first state
      if (actualStart != stateProperty_[vertexPath.back()])
      {
          pathGeometric->append(actualStart);
  
          // Add the edge status
          // the edge from actualStart to start is always valid otherwise we would not have used that start
          candidateSolution.edgeCollisionStatus_.push_back(FREE);
      }
  
      // Reverse the vertexPath and convert to state path
      for (std::size_t i = vertexPath.size(); i > 0; --i)
      {
          pathGeometric->append(stateProperty_[vertexPath[i - 1]]);
  
          // Add the edge status
          if (i > 1)  // skip the last vertex (its reversed)
          {
              Edge thisEdge = boost::edge(vertexPath[i - 1], vertexPath[i - 2], g_).first;
  
              // Check if any edges in path are not free (then it an approximate path)
              if (edgeCollisionStateProperty_[thisEdge] == IN_COLLISION)
              {
                  candidateSolution.isApproximate_ = true;
                  candidateSolution.edgeCollisionStatus_.push_back(IN_COLLISION);
              }
              else if (edgeCollisionStateProperty_[thisEdge] == NOT_CHECKED)
              {
                  if (!disableCollisionWarning)
                      OMPL_ERROR("A chosen path has an edge that has not been checked for collision. This should not "
                                 "happen");
                  candidateSolution.edgeCollisionStatus_.push_back(NOT_CHECKED);
              }
              else
              {
                  candidateSolution.edgeCollisionStatus_.push_back(FREE);
              }
          }
      }
  
      // Add original goal if it is different than the last state
      if (actualGoal != stateProperty_[vertexPath.front()])
      {
          pathGeometric->append(actualGoal);
  
          // Add the edge status
          // the edge from actualGoal to goal is always valid otherwise we would not have used that goal
          candidateSolution.edgeCollisionStatus_.push_back(FREE);
      }
  
      candidateSolution.path_ = pathGeometric;
  
      return true;
  }
  
  void ompl::geometric::SPARSdb::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));
  
      // I'm curious:
      if (goalM_.size() > 0)
      {
          throw Exception(name_, "SPARS2 has goal states?");
      }
      if (startM_.size() > 0)
      {
          throw Exception(name_, "SPARS2 has start states?");
      }
  
      // 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_);
  
              // TODO save weights!
              data.addEdge(base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]),
                           base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]));
  
              // OMPL_INFORM("Adding edge from vertex of type %d to vertex of type %d", colorProperty_[v1],
              // colorProperty_[v2]);
          }
      }
      // 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]));
  
      data.properties["iterations INTEGER"] = std::to_string(iterations_);
  }
  
  void ompl::geometric::SPARSdb::setPlannerData(const base::PlannerData &data)
  {
      // Check that the query vertex is initialized (used for internal nearest neighbor searches)
      checkQueryStateInitialization();
  
      // Add all vertices
      if (verbose_)
      {
          OMPL_INFORM("SPARS::setPlannerData: numVertices=%d", data.numVertices());
      }
      OMPL_INFORM("Loading PlannerData into SPARSdb");
  
      std::vector<Vertex> idToVertex;
  
      // Temp disable verbose mode for loading database
      bool wasVerbose = verbose_;
      verbose_ = false;
  
      OMPL_INFORM("Loading vertices:");
      // Add the nodes to the graph
      for (std::size_t vertexID = 0; vertexID < data.numVertices(); ++vertexID)
      {
          // Get the state from loaded planner data
          const base::State *oldState = data.getVertex(vertexID).getState();
          base::State *state = si_->cloneState(oldState);
  
          // Get the tag, which in this application represents the vertex type
          auto type = static_cast<GuardType>(data.getVertex(vertexID).getTag());
  
          // ADD GUARD
          idToVertex.push_back(addGuard(state, type));
      }
  
      OMPL_INFORM("Loading edges:");
      // Add the corresponding edges to the graph
      std::vector<unsigned int> edgeList;
      for (std::size_t fromVertex = 0; fromVertex < data.numVertices(); ++fromVertex)
      {
          edgeList.clear();
  
          // Get the edges
          data.getEdges(fromVertex, edgeList);  // returns num of edges
  
          Vertex m = idToVertex[fromVertex];
  
          // Process edges
          for (unsigned int toVertex : edgeList)
          {
              Vertex n = idToVertex[toVertex];
  
              // Add the edge to the graph
              const base::Cost weight(0);
              if (verbose_ && false)
              {
                  OMPL_INFORM("    Adding edge from vertex id %d to id %d into edgeList", fromVertex, toVertex);
                  OMPL_INFORM("      Vertex %d to %d", m, n);
              }
              connectGuards(m, n);
          }
      }  // for
  
      // Re-enable verbose mode, if necessary
      verbose_ = wasVerbose;
  }
  
  void ompl::geometric::SPARSdb::clearEdgeCollisionStates()
  {
      foreach (const Edge e, boost::edges(g_))
          edgeCollisionStateProperty_[e] = NOT_CHECKED;  // each edge has an unknown state
  }
  