text: /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2020,
   *  Max Planck Institute for Intelligent Systems (MPI-IS).
   *  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 the MPI-IS 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: Andreas Orthey */
  
  #include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
  
  ompl::multilevel::BundleSpaceGraphSampler::BundleSpaceGraphSampler(BundleSpaceGraph *bundleSpaceGraph)
    : bundleSpaceGraph_(bundleSpaceGraph)
  {
      double mu = bundleSpaceGraph_->getBundle()->getMaximumExtent();
      epsilonGraphThickening_ = mu * epsilonGraphThickeningFraction_;
      OMPL_DEBUG("Epsilon Graph Thickening constant set to %f", epsilonGraphThickening_);
  
      pathBiasDecay_.setLambda(exponentialDecayLambda_);
      pathBiasDecay_.setValueInit(pathBiasFixed_);
  
      pathThickeningGrowth_.setLambda(exponentialDecayLambda_);
      pathThickeningGrowth_.setValueInit(epsilonGraphThickening_);
      pathThickeningGrowth_.setValueTarget(0.0);
  
      graphThickeningGrowth_.setLambda(exponentialDecayLambda_);
      graphThickeningGrowth_.setValueInit(epsilonGraphThickening_);
      graphThickeningGrowth_.setValueTarget(0.0);
  }
  
  void ompl::multilevel::BundleSpaceGraphSampler::clear()
  {
      double mu = bundleSpaceGraph_->getBundle()->getMaximumExtent();
      epsilonGraphThickening_ = mu * epsilonGraphThickeningFraction_;
  
      pathBiasDecay_.reset();
      pathThickeningGrowth_.reset();
      graphThickeningGrowth_.reset();
  }
  
  void ompl::multilevel::BundleSpaceGraphSampler::disableSegmentBias()
  {
      this->segmentBias_ = false;
  }
  
  void ompl::multilevel::BundleSpaceGraphSampler::disablePathBias()
  {
      pathBiasDecay_.setLambda(0);
      pathBiasDecay_.setValueTarget(0);
      pathBiasDecay_.setValueInit(0);
  }
  
  void ompl::multilevel::BundleSpaceGraphSampler::setPathBiasStartSegment(double s)
  {
      if (!segmentBias_)
      {
          this->pathBiasStartSegment_ = 0;
      }
      else
      {
          if (s > this->pathBiasStartSegment_)
          {
              geometric::PathGeometric &spath =
                  static_cast<geometric::PathGeometric &>(*bundleSpaceGraph_->getSolutionPathByReference());
              OMPL_DEBUG("Set path bias: %f/%f", s, spath.length());
              if (s > spath.length())
              {
                  s = spath.length();
              }
              this->pathBiasStartSegment_ = s;
          }
      }
  }
  
  double ompl::multilevel::BundleSpaceGraphSampler::getPathBiasStartSegment()
  {
      return this->pathBiasStartSegment_;
  }
  
  void ompl::multilevel::BundleSpaceGraphSampler::sample(base::State *xRandom)
  {
      base::SpaceInformationPtr bundle = bundleSpaceGraph_->getBundle();
  
      double p = rng_.uniform01();
      double pd = pathBiasDecay_();
  
      if (p < pd && !bundleSpaceGraph_->isDynamic())
      {
          geometric::PathGeometric &spath =
              static_cast<geometric::PathGeometric &>(*bundleSpaceGraph_->getSolutionPathByReference());
  
          std::vector<base::State *> states = spath.getStates();
  
          if (states.size() < 2)
          {
              // empty solution returned, cannot use path bias sampling
              sampleImplementation(xRandom);
          }
          else
          {
              // First one works well for SE3->R3, second one works best for
              // SE3^k->SE3^{k-1}
  
              double endLength = spath.length();
              double distStopping = pathBiasStartSegment_ + rng_.uniform01() * (endLength - pathBiasStartSegment_);
  
              base::State *s1 = nullptr;
              base::State *s2 = nullptr;
  
              int ctr = 0;
              double distLastSegment = 0;
              double distCountedSegments = 0;
              while (distCountedSegments < distStopping && (ctr < (int)states.size() - 1))
              {
                  s1 = states.at(ctr);
                  s2 = states.at(ctr + 1);
                  distLastSegment = bundle->distance(s1, s2);
                  distCountedSegments += distLastSegment;
                  ctr++;
              }
  
              //          |---- d -----|
              //---O------O------------O
              //|--------- t ----------|
              //|--------- s ------|
              //          |d-(t-s) |
              double step = (distLastSegment - (distCountedSegments - distStopping)) / (distLastSegment);
              bundle->getStateSpace()->interpolate(s1, s2, step, xRandom);
  
              if (epsilonGraphThickening_ > 0)
              {
                  double eps = pathThickeningGrowth_();
                  bundleSpaceGraph_->getBundleSamplerPtr()->sampleUniformNear(xRandom, xRandom, eps);
              }
          }
      }
      else
      {
          sampleImplementation(xRandom);
      }
  
      // Perturbate sample in epsilon neighborhood
      //  Note: Alternatively, we can use sampleGaussian (but seems to give similar
      //  results)
      if (epsilonGraphThickening_ > 0)
      {
          // Decay on graph thickening (reflects or believe in the usefullness of
          // the graph for biasing our sampling)
          double eps = graphThickeningGrowth_();
          bundleSpaceGraph_->getBundleSamplerPtr()->sampleUniformNear(xRandom, xRandom, eps);
      }
  }
  