// MIT License
//
// Copyright (c) 2021 Oliver Schick
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <mpo19/channel.hpp>
#include <mpo19/paillier_wrapper.hpp>
#include <mpo19/point.hpp>
#include <chrono>
#include <thread>
#include <exception>

#define BOOST_TEST_MODULE channel_test
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

using namespace mpo19;
namespace bdata = boost::unit_test::data;

constexpr int m = 1000;
constexpr key_id client_key = "CLIENT";
constexpr key_id server_peer_key = "SERVER";

bool equal(plaintext const& lhs, plaintext const& rhs)
{
    std::vector<char> dec_lhs = lhs;
    std::vector<char>dec_rhs = rhs;
    assert(!dec_lhs.empty() && !dec_rhs.empty());
    return strcmp(dec_lhs.data(), dec_rhs.data()) == 0;
}

template<key_id ID>
bool equal(paillier<ID> const& lhs, paillier<ID> const& rhs)
{
    return equal(decrypt(lhs), decrypt(rhs));
}

BOOST_DATA_TEST_CASE(channel_paillier_test
                     , bdata::random(0, m) ^ bdata::random(0, m) ^ bdata::xrange(10)
                     , s1, s2, idx)
{
    using namespace std::chrono_literals;
    (void) idx;

    boost::asio::io_context io_context;
    channel chan{io_context};

    std::thread server{[]()
    {
        boost::asio::io_context io_context;
        channel chan{io_context};
        auto peer = chan.open(7766);
        if(peer.address().to_string() != "127.0.0.1")
            throw std::runtime_error("Not connected to localhost as expected!");
        paillier_keys::public_key pub;
        chan >> pub >> channel::seperator;
        paillier<server_peer_key>::init_keys(pub);

        paillier<server_peer_key> p1, p2;
        chan >> p1 >> p2;
        chan << p1 + p2 << channel::send;

        int s1;
        chan.get(s1);
        int s2 = chan.get<int>();
        chan.put(s1 + s2);
        chan.transmit();
        chan.put(chan.get<paillier<server_peer_key>>()
                 + chan.get<paillier<server_peer_key>>())
        .transmit();
        std::vector<char> hello = chan.get(strlen("Hello")+1);
        hello.resize(hello.size() + strlen(" World!") + 1);
        chan.put(strcat(hello.data(), " World!")).transmit();

        paillier<server_peer_key>::deinit_keys();
    }};

    //Try to connect to server
    for(int i = 0; i != 1000; ++i) {
        boost::system::error_code ec;
        auto peer = chan.open("localhost", "7766", ec);
        if(ec) {
            std::this_thread::sleep_for(10ms);
        }
        //break and skip else statement (python-like for-else)
        else {
            BOOST_TEST(peer.address().to_string() == "127.0.0.1");
            goto CONNECTED;
        }
    }
    /*else*/{
        throw std::runtime_error("Could not connect to server");
    }
CONNECTED:

    paillier<client_key> p1 = paillier<client_key>::gen_random(40);
    paillier<client_key> p2 = paillier<client_key>::gen_random(40);
    paillier<client_key> result;

    chan << paillier<client_key>::get_keys().get_public()<< channel::seperator
         << p1 << p2 << channel::send;
    chan >> result;

    BOOST_TEST(equal(p1 + p2, result));

    chan.put(s1);
    chan.put(s2);
    chan.transmit();
    BOOST_TEST(s1 + s2 == chan.get<int>());

    paillier<client_key> p_s1, p_s2;
    p_s1 = p_s1.encrypt(s1);
    p_s2 = p_s2.encrypt(s2);
    chan.put(p_s1).put(p_s2).transmit();
    paillier<client_key> p_res;
    chan.get(p_res);
    BOOST_TEST(equal(p_res, p_s1 + p_s2));

    chan.put("Hello").transmit();
    std::vector<char> hello_world(strlen("Hello World!") + 1);
    chan.get(hello_world.data(), hello_world.size());

    BOOST_TEST(strcmp(hello_world.data(), "Hello World!") == 0);

    server.join();
    paillier<client_key>::deinit_keys();
}

BOOST_DATA_TEST_CASE(channel_point_test
                     , bdata::xrange(10)
                     , idx)
{
    using namespace std::chrono_literals;
    (void) idx;

    boost::asio::io_context io_context;
    channel chan{io_context};

    std::thread server{[]()
    {
        boost::asio::io_context io_context;
        channel chan{io_context};
        auto peer = chan.open(7766);
        if(peer.address().to_string() != "127.0.0.1")
            throw std::runtime_error("Not connected to localhost as expected!");
        paillier_keys::public_key pub;
        chan >> pub >> channel::seperator;
        paillier<server_peer_key>::init_keys(pub);
        point<paillier<server_peer_key>> p, q;
        chan >> p >> q;
        chan << p + q << channel::send;
        paillier<server_peer_key>::deinit_keys();
    }};

    //Try to connect to server
    for(int i = 0; i != 1000; ++i) {
        boost::system::error_code ec;
        auto peer = chan.open("localhost", "7766", ec);
        if(ec) {
            std::this_thread::sleep_for(10ms);
        }
        //break and skip else statement (python-like for-else)
        else {
            BOOST_TEST(peer.address().to_string() == "127.0.0.1");
            goto CONNECTED;
        }
    }
    /*else*/ {
        throw std::runtime_error("Could not connect to server");
    }
CONNECTED:

    size_t dim = 3 + idx;
    point<paillier<client_key>> p(dim);
    point<paillier<client_key>> q(dim);

    for(size_t i = 0; i != dim; ++i) {
        p[i] = paillier<client_key>::gen_random(40);
        q[i] = paillier<client_key>::gen_random(40);
    }
    point<paillier<client_key>> result, sum = p + q;
    chan << paillier<client_key>::get_keys().get_public()
         << channel::seperator << p << q << channel::send;
    chan >> result;
    server.join();

    for(size_t i = 0; i != dim; ++i) {
        equal(sum[i], result[i]);
    }

}


#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

#include <iostream>
#include "species.h"

using namespace std;
namespace p = boost::python;
namespace np = boost::python::numpy;
namespace bm = boost::math;

template<typename T>
static T det(const array<array<T, 3>, 3> &p) {
    T sum{};
    for (size_t i = 0; i < 3; i++) {
        sum += p[1][(i + 1) % 3] * p[2][(i + 2) % 3] * p[0][i];
        sum -= p[1][(i + 2) % 3] * p[2][(i + 1) % 3] * p[0][i];
    }
    return sum;
};

class Solver {

    double kperp, kpara;

    vector<Species> species;

public:

    Solver(const p::list &list, const double B) {
        Py_Initialize();
        np::initialize();
        for (int i = 0; i < len(list); i++) {
            species.push_back(Species(list[i], B));
        }
    }

    void push_kperp(const double kperp) {
        this->kperp = kperp;
        for (Species &spec: species) {
            spec.push_kperp(kperp);
        }
    }

    void push_kpara(const double kpara) {
        this->kpara = kpara;
    }

    array<array<cdouble, 3>, 3> evaluateM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M{};
        for (Species &spec: species) {
            M += spec.push_omega(kpara, kperp, wr, wi);
        }
        double kperp2 = kperp*kperp;
        double kperp3 = kperp2*kperp;
        double kperp4 = kperp2*kperp2;
        double kpara2 = kpara*kpara;
        double kpara4 = kpara2*kpara2;
        double kpara5 = kpara4*kpara;
        double kpara6 = kpara4*kpara2;

        M[0][0] += kperp2*kpara4;
        M[1][1] += kperp2*kpara4;
        M[2][2] += kperp2*kpara4;

        cdouble c2_w2 = pow(cl / cdouble{wr, wi}, 2);

        M[0][0] -= c2_w2*kperp2*kpara6;
        M[0][2] += c2_w2*kperp3*kpara5;
        M[1][1] -= c2_w2*kperp2*kpara6;
        M[1][1] -= c2_w2*kperp4*kpara4;
        M[2][0] += c2_w2*kperp3*kpara5;
        M[2][2] -= c2_w2*kperp4*kpara4;

        M *= (1.0/(kpara4*kperp2));

        return M;
    };

    array<array<cdouble, 3>, 3> evaluateEps(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> Eps{};
        for (Species &spec: species) {
            Eps += spec.push_omega(kpara, kperp, wr, wi);
        }

        double kperp2 = kperp*kperp;
        double kperp3 = kperp2*kperp;
        double kperp4 = kperp2*kperp2;
        double kpara2 = kpara*kpara;
        double kpara4 = kpara2*kpara2;

        Eps[0][0] += kperp2*kpara4;
        Eps[1][1] += kperp2*kpara4;
        Eps[2][2] += kperp2*kpara4;

        Eps *= (1.0/(kpara4*kperp2));

        return Eps;
    };

    np::ndarray convertM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);

        p::tuple shape = p::make_tuple(3, 3);
        np::ndarray npM = np::zeros(shape, np::dtype::get_builtin<cdouble>());

        for (size_t i=0;i<3;i++){
            for (size_t j=0;j<3;j++){
                npM[i][j] = M[i][j];
            }
        }

        return npM;
    }

    np::ndarray convertEps(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateEps(wr, wi);

        p::tuple shape = p::make_tuple(3, 3);
        np::ndarray npM = np::zeros(shape, np::dtype::get_builtin<cdouble>());

        for (size_t i=0;i<3;i++){
            for (size_t j=0;j<3;j++){
                npM[i][j] = M[i][j];
            }
        }

        return npM;
    }

    cdouble evaluateDetM(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);
        cdouble detM = det(M);
        double wa = sqrt(wr*wr + wi*wi);
        return detM * pow(wa, 4) / pow((kpara * kpara + kperp * kperp) * cl * cl, 2);
    }

    cdouble evaluateDetMLongitudinal(const double wr, const double wi) {
        array<array<cdouble, 3>, 3> M = evaluateM(wr, wi);
        array<cdouble, 3> ML{};
        ML[0] = kperp*M[0][0] + kpara*M[0][2];
        ML[2] = kperp*M[2][0] + kpara*M[2][2];

        cdouble detML = (ML[0]*kperp + ML[2]*kpara)/(kperp*kperp + kpara*kpara);
        return detML;
    }

    np::ndarray marginalize(const np::ndarray &arr) {
        int nd = arr.get_nd();
        Py_intptr_t const *shape = arr.get_shape();
        Py_intptr_t const *stride = arr.get_strides();
        np::ndarray result = np::zeros(nd, shape, arr.get_dtype());
        if (nd == 1) {
            for (size_t i = 0; i < (size_t) shape[0]; i++) {
                cdouble w = *reinterpret_cast<cdouble const *>(arr.get_data() + i * stride[0]);
                cdouble res = evaluateDetM(w.real(), w.imag());
                *reinterpret_cast<cdouble *>(result.get_data() + i * stride[0]) = res;
            }
        } else if (nd == 2) {
            for (size_t i = 0; i < (size_t) shape[0]; i++) {
                for (size_t j = 0; j < (size_t) shape[1]; j++) {
                    cdouble w = *reinterpret_cast<cdouble const *>(arr.get_data() + i * stride[0] + j * stride[1]);
                    cdouble res = evaluateDetM(w.real(), w.imag());
                    *reinterpret_cast<cdouble *>(result.get_data() + i * stride[0] + j * stride[1]) = res;
                }
            }
        } else
            throw std::runtime_error("Unsupported marginalization dimensionality. ");
        return result;
    }

};

BOOST_PYTHON_MODULE (libSolver) {
    p::class_<Solver>("Solver", p::init<const boost::python::list, double>())
            .def("push_kperp", &Solver::push_kperp)
            .def("push_kpara", &Solver::push_kpara)
            .def("evaluateDetM", &Solver::evaluateDetM)
            .def("evaluateDetMLongitudinal", &Solver::evaluateDetMLongitudinal)
            .def("evaluateM", &Solver::convertM)
            .def("evaluateEps", &Solver::convertEps)
            .def("marginalize", &Solver::marginalize);
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "KrylovDiagonalizer.h"
#include "../MathUtils.h"
#include "DiagonalizerSettings.h"
#include "PreconditionerEvaluator.h"
#include "SigmaVectorEvaluator.h"
#include "SubspaceCollapser.h"
#include "SubspaceOrthogonalizer.h"
#include <Core/Log.h>
#include <Eigen/Eigenvalues>
#include <numeric>

namespace Scine {
namespace Utils {

KrylovDiagonalizer::KrylovDiagonalizer(int eigenvaluesToCompute, int totalDimension)
  : IterativeDiagonalizer(eigenvaluesToCompute, totalDimension) {
  settings_ = std::make_unique<KrylovSettings>(eigenvaluesToCompute, totalDimension);
  initialize(totalDimension);
  notConvergedRoots_.resize(eigenvaluesToCompute_);
  std::iota(notConvergedRoots_.begin(), notConvergedRoots_.end(), 0);
}

void KrylovDiagonalizer::applySettings() {
  IterativeDiagonalizer::applySettings();
  subspaceCollapser_ = std::make_unique<SubspaceCollapser>();
  subspaceCollapser_->setMaxSubspaceDimension(settings_->getInt(subspaceCollapseDimensionOption));
  subspaceCollapser_->setEigenvaluesToCompute(eigenvaluesToCompute_);
}

void KrylovDiagonalizer::performIteration(Core::Log& log) {
  // Handle for, for instance, orthogonalizing
  onIterationStart();

  const Eigen::MatrixXd& projector = guessVectors_.leftCols(subspaceDimension_);
  // Handle for, for instance, calculating basis overlap
  onSigmaMatrixEvaluation(projector);
  // Sigma vectors evaluation + Hamiltonian projection
  const Eigen::MatrixXd& sigmaMatrix = sigmaVectorEvaluator_->evaluate(projector);
  Eigen::MatrixXd projectedMatrix = (projector.transpose() * sigmaMatrix).selfadjointView<Eigen::Lower>();

  // Solve eigenproblem in subspace
  subspaceEigenpairs_ = eigenDecomposition(projectedMatrix);

  calculateResiduals(sigmaMatrix, projector);

  checkConvergence();

  if (converged_) {
    ritzEstimate_ = {ritzEstimate_.eigenValues.head(eigenvaluesToCompute_),
                     ritzEstimate_.eigenVectors.leftCols(eigenvaluesToCompute_)};
    return;
  }
  // Collapse subspace
  collapse(projector, log);
}

inline void KrylovDiagonalizer::calculateResiduals(const Eigen::MatrixXd& sigmaMatrix, const Eigen::MatrixXd& projector) {
  ritzEstimate_.eigenVectors = projector * subspaceEigenpairs_.eigenVectors.leftCols(eigenvaluesToCompute_);
  ritzEstimate_.eigenValues = subspaceEigenpairs_.eigenValues.head(eigenvaluesToCompute_);
  // Calculate residuals
  // TODO: Allow for expansion of more than just not converged roots (f.i. 2 new vectors per root)
  residualVectors_.resize(sigmaMatrix.rows(), notConvergedRoots_.size());
  int index = 0;
  for (int notConvergedRoot : notConvergedRoots_) {
    residualVectors_.col(index) = sigmaMatrix * subspaceEigenpairs_.eigenVectors.col(notConvergedRoot);
    residualVectors_.col(index).noalias() -=
        ritzEstimate_.eigenVectors.col(notConvergedRoot) * ritzEstimate_.eigenValues(notConvergedRoot);
    ++index;
  }
}

inline void KrylovDiagonalizer::checkConvergence() {
  residualNorms_ = residualVectors_.colwise().norm();
  numberConvergedRoots_ = 0;
  // Maps the index in the residual norms to the actual root number
  std::vector<int> oldNotConvergedRoots = notConvergedRoots_;
  notConvergedRoots_.clear();

  // Keep track of converged roots
  for (int i = 0; i < residualVectors_.cols(); ++i) {
    if (residualNorms_(i) < settings_->getDouble(residualNormToleranceOption)) {
      rootConverged_[oldNotConvergedRoots[i]] = true;
    }
    else {
      rootConverged_[oldNotConvergedRoots[i]] = false;
      notConvergedRoots_.push_back(oldNotConvergedRoots[i]);
    }
  }
  numberConvergedRoots_ = std::count(rootConverged_.begin(), rootConverged_.end(), true);
  converged_ = std::count(rootConverged_.begin(), rootConverged_.end(), false) == 0;

  assert(numberConvergedRoots_ <= eigenvaluesToCompute_);
}

void KrylovDiagonalizer::collapse(const Eigen::MatrixXd& projector, Core::Log& log) {
  subspaceCollapser_->setMaxSubspaceDimension(
      SubspaceCollapser::calculateSubspaceCollapserIterations(eigenvaluesToCompute_, numberConvergedRoots_, maxDimension_));
  if (!subspaceCollapser_->collapseNeeded(ritzEstimate_, subspaceDimension_, notConvergedRoots_)) {
    expandSubspace(projector);
  }
  else {
    callCollapserImpl();
    // If total space is small, one-shot this
    if (guessVectors_.cols() >= maxDimension_) {
      guessVectors_ = Eigen::MatrixXd::Identity(maxDimension_, maxDimension_);
      subspaceDimension_ = maxDimension_;
    }
    sigmaVectorEvaluator_->collapsed(subspaceDimension_);
    std::fill(rootConverged_.begin(), rootConverged_.end(), false);
    numberConvergedRoots_ = 0;
    converged_ = false;
    notConvergedRoots_.resize(eigenvaluesToCompute_);
    std::iota(notConvergedRoots_.begin(), notConvergedRoots_.end(), 0);
    log.output << Core::Log::nl << "Subspace collapsed. New dimension is " << subspaceDimension_ << Core::Log::nl
               << Core::Log::endl;

    printHeader(log);
  }
}

inline void KrylovDiagonalizer::expandSubspace(const Eigen::MatrixXd& projector) {
  // look at each residual vector
  Eigen::MatrixXd newGuessVectors(guessVectors_.rows(), notConvergedRoots_.size());
  int index = 0;
  for (int notConvergedRoot : notConvergedRoots_) {
    newGuessVectors.col(index) =
        preconditionerEvaluator_->evaluate(residualVectors_.col(index), ritzEstimate_.eigenValues(notConvergedRoot));
    ++index;
  }
  filterCorrectionVectors(projector, newGuessVectors);

  addVectorsToGuessBasis(newGuessVectors);
}

void KrylovDiagonalizer::filterCorrectionVectors(const Eigen::MatrixXd& /*projector*/,
                                                 Eigen::MatrixXd& /*newGuessVectors*/) const {
}

void KrylovDiagonalizer::addVectorsToGuessBasis(const Eigen::MatrixXd& newVectors) {
  int vectorsAdded = newVectors.cols();
  if (subspaceDimension_ + vectorsAdded < maxDimension_) {
    subspaceDimension_ += vectorsAdded;
    guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
    guessVectors_.rightCols(vectorsAdded) = newVectors;
  }
  else {
    guessVectors_.conservativeResize(Eigen::NoChange, maxDimension_);
    guessVectors_.rightCols(maxDimension_ - subspaceDimension_) = newVectors.leftCols(maxDimension_ - subspaceDimension_);
    subspaceDimension_ = maxDimension_;
  }
}

void KrylovDiagonalizer::onIterationStart() {
}

void KrylovDiagonalizer::onSigmaMatrixEvaluation(const Eigen::MatrixXd& /*projector*/) {
}

KrylovDiagonalizer::~KrylovDiagonalizer() = default;

} // namespace Utils
} // namespace Scine


#ifndef STAN_MATH_PRIM_SCAL_FUN_LB_FREE_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LB_FREE_HPP

#include <stan/math/prim/scal/fun/identity_free.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <boost/math/tools/promotion.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the unconstrained value that produces the specified
 * lower-bound constrained value.
 *
 * If the lower bound is negative infinity, it is ignored and
 * the function reduces to identity_free(y).
 *
 * @tparam T type of scalar
 * @tparam L type of lower bound
 * @param[in] y input scalar
 * @param[in] lb lower bound
 * @return unconstrained value that produces the input when
 * constrained
 * @throw std::domain_error if y is lower than the lower bound
 */
template <typename T, typename L>
inline typename boost::math::tools::promote_args<T, L>::type lb_free(
    const T& y, const L& lb) {
  using std::log;
  if (lb == -std::numeric_limits<double>::infinity())
    return identity_free(y);
  check_greater_or_equal("lb_free", "Lower bounded variable", y, lb);
  return log(y - lb);
}

}  // namespace math
}  // namespace stan
#endif


#include "laser_line_extraction/line_extraction.h"
#include <algorithm>
#include <Eigen/Dense>
#include <iostream>

namespace line_extraction
{

///////////////////////////////////////////////////////////////////////////////
// Constructor / destructor
///////////////////////////////////////////////////////////////////////////////
LineExtraction::LineExtraction()
{
}

LineExtraction::~LineExtraction()
{
}

///////////////////////////////////////////////////////////////////////////////
// Main run function
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::extractLines(std::vector<Line>& lines)
{
  // Resets
  filtered_indices_ = c_data_.indices;
  lines_.clear();

  // Filter indices
  filterCloseAndFarPoints();
  filterOutlierPoints();

  // Return no lines if not enough points left
  if (filtered_indices_.size() <= std::max(params_.min_line_points, static_cast<unsigned int>(3)))
  {
    return;
  }

  // Split indices into lines and filter out short and sparse lines
  split(filtered_indices_);
  filterLines();

  // Fit each line using least squares and merge colinear lines
  for (std::vector<Line>::iterator it = lines_.begin(); it != lines_.end(); ++it)
  {
    it->leastSqFit();
  }

  // If there is more than one line, check if lines should be merged based on the merging criteria
  if (lines_.size() > 1)
  {
    mergeLines();
  }

  lines = lines_;
}

///////////////////////////////////////////////////////////////////////////////
// Data setting
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::setCachedData(const std::vector<double>& bearings,
                                   const std::vector<double>& cos_bearings,
                                   const std::vector<double>& sin_bearings,
                                   const std::vector<unsigned int>& indices)
{
  c_data_.bearings = bearings;
  c_data_.cos_bearings = cos_bearings;
  c_data_.sin_bearings = sin_bearings;
  c_data_.indices = indices;
}

void LineExtraction::setRangeData(const std::vector<double>& ranges)
{
  r_data_.ranges = ranges;
  r_data_.xs.clear();
  r_data_.ys.clear();
  for (std::vector<unsigned int>::const_iterator cit = c_data_.indices.begin();
       cit != c_data_.indices.end(); ++cit)
  {
    r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]);
    r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]);
  }
}

///////////////////////////////////////////////////////////////////////////////
// Parameter setting
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::setBearingVariance(double value)
{
  params_.bearing_var = value;
}

void LineExtraction::setRangeVariance(double value)
{
  params_.range_var = value;
}

void LineExtraction::setLeastSqAngleThresh(double value)
{
  params_.least_sq_angle_thresh = value;
}

void LineExtraction::setLeastSqRadiusThresh(double value)
{
  params_.least_sq_radius_thresh = value;
}

void LineExtraction::setMaxLineGap(double value)
{
  params_.max_line_gap = value;
}

void LineExtraction::setMinLineLength(double value)
{
  params_.min_line_length = value;
}

void LineExtraction::setMinLinePoints(unsigned int value)
{
  params_.min_line_points = value;
}

void LineExtraction::setMinRange(double value)
{
  params_.min_range = value;
}

void LineExtraction::setMaxRange(double value)
{
  params_.max_range = value;
}

void LineExtraction::setMinSplitDist(double value)
{
  params_.min_split_dist = value;
}

void LineExtraction::setOutlierDist(double value)
{
  params_.outlier_dist = value;
}

///////////////////////////////////////////////////////////////////////////////
// Utility methods
///////////////////////////////////////////////////////////////////////////////
double LineExtraction::chiSquared(const Eigen::Vector2d &dL, const Eigen::Matrix2d &P_1,
                                  const Eigen::Matrix2d &P_2)
{
  return dL.transpose() * (P_1 + P_2).inverse() * dL;
}

double LineExtraction::distBetweenPoints(unsigned int index_1, unsigned int index_2)
{
  return sqrt(pow(r_data_.xs[index_1] - r_data_.xs[index_2], 2) +
              pow(r_data_.ys[index_1] - r_data_.ys[index_2], 2));
}

///////////////////////////////////////////////////////////////////////////////
// Filtering points
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::filterCloseAndFarPoints()
{
  std::vector<unsigned int> output;
  for (std::vector<unsigned int>::const_iterator cit = filtered_indices_.begin();
       cit != filtered_indices_.end(); ++cit)
  {
    const double& range = r_data_.ranges[*cit];
    if (range >= params_.min_range && range <= params_.max_range)
    {
      output.push_back(*cit);
    }
  }
  filtered_indices_ = output;
}

void LineExtraction::filterOutlierPoints()
{
  if (filtered_indices_.size() < 3)
  {
    return;
  }

  std::vector<unsigned int> output;
  unsigned int p_i, p_j, p_k;
  for (std::size_t i = 0; i < filtered_indices_.size(); ++i)
  {

    // Get two closest neighbours

    p_i = filtered_indices_[i];
    if (i == 0) // first point
    {
      p_j = filtered_indices_[i + 1];
      p_k = filtered_indices_[i + 2];
    }
    else if (i == filtered_indices_.size() - 1) // last point
    {
      p_j = filtered_indices_[i - 1];
      p_k = filtered_indices_[i - 2];
    }
    else // middle points
    {
      p_j = filtered_indices_[i - 1];
      p_k = filtered_indices_[i + 1];
    }

    // Check if point is an outlier

    if (fabs(r_data_.ranges[p_i] - r_data_.ranges[p_j]) > params_.outlier_dist &&
        fabs(r_data_.ranges[p_i] - r_data_.ranges[p_k]) > params_.outlier_dist)
    {
      // Check if it is close to line connecting its neighbours
      std::vector<unsigned int> line_indices;
      line_indices.push_back(p_j);
      line_indices.push_back(p_k);
      Line line(c_data_, r_data_, params_, line_indices);
      line.endpointFit();
      if (line.distToPoint(p_i) > params_.min_split_dist)
      {
        continue; // point is an outlier
      }
    }

    output.push_back(p_i);
  }

  filtered_indices_ = output;
}

///////////////////////////////////////////////////////////////////////////////
// Filtering and merging lines
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::filterLines()
{
  std::vector<Line> output;
  for (std::vector<Line>::const_iterator cit = lines_.begin(); cit != lines_.end(); ++cit)
  {
    if (cit->length() >= params_.min_line_length && cit->numPoints() >= params_.min_line_points)
    {
      output.push_back(*cit);
    }
  }
  lines_ = output;
}

void LineExtraction::mergeLines()
{
  std::vector<Line> merged_lines;

  for (std::size_t i = 1; i < lines_.size(); ++i)
  {
    // Get L, P_1, P_2 of consecutive lines
    Eigen::Vector2d L_1(lines_[i-1].getRadius(), lines_[i-1].getAngle());
    Eigen::Vector2d L_2(lines_[i].getRadius(), lines_[i].getAngle());
    Eigen::Matrix2d P_1;
    P_1 << lines_[i-1].getCovariance()[0], lines_[i-1].getCovariance()[1],
           lines_[i-1].getCovariance()[2], lines_[i-1].getCovariance()[3];
    Eigen::Matrix2d P_2;
    P_2 << lines_[i].getCovariance()[0], lines_[i].getCovariance()[1],
           lines_[i].getCovariance()[2], lines_[i].getCovariance()[3];

    // Merge lines if chi-squared distance is less than 3
    if (chiSquared(L_1 - L_2, P_1, P_2) < 3)
    {
      // Get merged angle, radius, and covariance
      Eigen::Matrix2d P_m = (P_1.inverse() + P_2.inverse()).inverse();
      Eigen::Vector2d L_m = P_m * (P_1.inverse() * L_1 + P_2.inverse() * L_2);
      // Populate new line with these merged parameters
      boost::array<double, 4> cov;
      cov[0] = P_m(0,0);
      cov[1] = P_m(0,1);
      cov[2] = P_m(1,0);
      cov[3] = P_m(1,1);
      std::vector<unsigned int> indices;
      const std::vector<unsigned int> &ind_1 = lines_[i-1].getIndices();
      const std::vector<unsigned int> &ind_2 = lines_[i].getIndices();
      indices.resize(ind_1.size() + ind_2.size());
      indices.insert(indices.end(), ind_1.begin(), ind_1.end());
      indices.insert(indices.end(), ind_2.begin(), ind_2.end());
      Line merged_line(L_m[1], L_m[0], cov, lines_[i-1].getStart(), lines_[i].getEnd(), indices);
      // Project the new endpoints
      merged_line.projectEndpoints();
      lines_[i] = merged_line;
    }
    else
    {
      merged_lines.push_back(lines_[i-1]);
    }

    if (i == lines_.size() - 1)
    {
      merged_lines.push_back(lines_[i]);
    }
  }
  lines_ = merged_lines;
}

///////////////////////////////////////////////////////////////////////////////
// Splitting points into lines
///////////////////////////////////////////////////////////////////////////////
void LineExtraction::split(const std::vector<unsigned int>& indices)
{
  // Don't split if only a single point (only occurs when orphaned by gap)
  if (indices.size() <= 1)
  {
    return;
  }

  Line line(c_data_, r_data_, params_, indices);
  line.endpointFit();
  double dist_max = 0;
  double gap_max = 0;
  double dist, gap;
  int i_max, i_gap;

  // Find the farthest point and largest gap
  for (std::size_t i = 1; i < indices.size() - 1; ++i)
  {
    dist = line.distToPoint(indices[i]);
    if (dist > dist_max)
    {
      dist_max = dist;
      i_max = i;
    }
    gap = distBetweenPoints(indices[i], indices[i+1]);
    if (gap > gap_max)
    {
      gap_max = gap;
      i_gap = i;
    }
  }

  // Check for gaps at endpoints
  double gap_start = distBetweenPoints(indices[0], indices[1]);
  if (gap_start > gap_max)
  {
    gap_max = gap_start;
    i_gap = 1;
  }
  double gap_end = distBetweenPoints(indices.rbegin()[1], indices.rbegin()[0]);
  if (gap_end > gap_max)
  {
    gap_max = gap_end;
    i_gap = indices.size() - 1;
  }

  // Check if line meets requirements or should be split
  if (dist_max < params_.min_split_dist && gap_max < params_.max_line_gap)
  {
    lines_.push_back(line);
  }
  else
  {
    int i_split = dist_max >= params_.min_split_dist ? i_max : i_gap;
    std::vector<unsigned int> first_split(&indices[0], &indices[i_split - 1]);
    std::vector<unsigned int> second_split(&indices[i_split], &indices.back());
    split(first_split);
    split(second_split);
  }

}

} // namespace line_extraction


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIVFLOOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIVFLOOR_HPP_INCLUDED

#include <boost/simd/function/definition/floor.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::floor_
                          , bd::scalar_< bd::int64_<A0> >
                          , bd::scalar_< bd::int64_<A0> >
                                    )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::floor_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (!a0) return  Zero<A0>();
      if(a1)
      {
        A0 q = saturated_(divides)(a0, a1);
        A0 r = a0-q*a1;
        if ((r != Zero<A0>())&&((a0^a1) <= 0)) return saturated_(dec)(q);
        return q;
      }
      else
        return ((a0>0) ? Valmax<A0>() : Valmin<A0>());
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::floor_
                          , bd::scalar_< bd::signed_<A0> >
                          , bd::scalar_< bd::signed_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::floor_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
        return static_cast<A0 >(-iceil(-static_cast<double>(a0)/static_cast<double>(a1)));
      else
      {
        return (a0) ? ((a0>0) ? Valmax<A0>() : Valmin<A0>()) : Zero<A0>();
      }
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::floor_
                          , bd::scalar_< bd::unsigned_<A0> >
                          , bd::scalar_< bd::unsigned_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::floor_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if(a1)
        return saturated_(divides)(a0, a1);
      else
        return (a0) ? Valmax<A0>() : Zero<A0>();
    }
  };

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::floor_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::floor_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return boost::simd::floor(a0/a1);
    }
  };
} } }

#endif


/* Authors: Luis G. Torres */

#include "RRG.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/base/Goal.h"

#include <boost/utility.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <map>
#include <boost/timer/timer.hpp>

ompl::geometric::RRG::RRG(const base::SpaceInformationPtr &si) :
    base::Planner(si, "RRG")
{
    specs_.approximateSolutions = true;
    specs_.optimizingPaths = true;

    maxDistance_ = 0.0;
    ballRadiusMax_ = 0.0;
    ballRadiusConst_ = 0.0;
    numCollisionChecks_ = 0;

    Planner::declareParam<double>("range", this, &RRG::setRange, &RRG::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("ball_radius_constant", this, &RRG::setBallRadiusConstant, &RRG::getBallRadiusConstant);
    Planner::declareParam<double>("max_ball_radius", this, &RRG::setMaxBallRadius, &RRG::getMaxBallRadius);
}

ompl::geometric::RRG::~RRG(void)
{
    freeMemory();
}

void ompl::geometric::RRG::setup(void)
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (ballRadiusMax_ == 0.0)
        ballRadiusMax_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
    if (ballRadiusConst_ == 0.0)
        ballRadiusConst_ = si_->getMaximumExtent();

    if (!nn_)
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DistItem>(si_->getStateSpace()));
    nn_->setDistanceFunction(boost::bind(&RRG::distanceFunction, this, _1, _2));

    // 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_->hasOptimizationObjective())
        opt_ = pdef_->getOptimizationObjective();
    else
    {
        OMPL_INFORM("Defaulting to optimizing path length.");
        opt_.reset(new base::PathLengthOptimizationObjective(si_));
    }
}

void ompl::geometric::RRG::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (nn_)
        nn_->clear();
    numCollisionChecks_ = 0;
}

void ompl::geometric::RRG::preprocess(unsigned numIterations)
{
    this->initPreprocess();

    base::State *state = si_->allocState();
    boost::timer::cpu_timer timer;
    for (unsigned i = 0; i < numIterations; ++i)
    {
        sampler_->sampleUniform(state);

        Node newNode, nearNode;
        if (this->extend(state, &newNode, &nearNode))
        {
            this->connectNear(newNode, nearNode);

            DistItem newDistItem;
            newDistItem.state = graph_[newNode];
            newDistItem.node = newNode;
            nn_->add(newDistItem);
        }
    }
    OMPL_INFORM("RRG preprocess took %s seconds.", timer.format(6,"%w").c_str());
    si_->freeState(state);

    predMap_.resize(boost::num_vertices(graph_));
    vertexCostMap_.resize(boost::num_vertices(graph_));
}

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

    // Initialize start node
    Node startNode = boost::graph_traits<Graph>::null_vertex();
    while (const base::State *st = pis_.nextStart())
    {
        // given the start state, get the vertex in the roadmap
        // closest to the start state: this will be our start node
        //
        // TODO: make this less horrible
        DistItem queryItem;
        queryItem.state = st;
        queryItem.node = boost::graph_traits<Graph>::null_vertex();
        DistItem nearDistItem = nn_->nearest(queryItem);
        startNode = nearDistItem.node;
    }
    if (startNode == boost::graph_traits<Graph>::null_vertex())
    {
        OMPL_ERROR("There are no valid initial states!");
        return base::PlannerStatus::INVALID_START;
    }

    // Initialize goal
    base::GoalPtr goal = pdef_->getGoal();
    if (!goal)
    {
        OMPL_ERROR("Goal undefined");
        return base::PlannerStatus::INVALID_GOAL;
    }

    // Find path and add it to problem definition
    //
    // TODO: figure out how to deal with approximate solutions and
    // such
    boost::timer::cpu_timer timer;
    base::PathPtr path = getBestPath(startNode, goal);
    timer.stop();

    pdef_->addSolutionPath(path);
    return base::PlannerStatus(true, false);
}

void ompl::geometric::RRG::initPreprocess(void)
{
    while (const base::State *st = pis_.nextStart())
    {
        Node n = boost::add_vertex(graph_);
        graph_[n] = si_->allocState();
        si_->copyState(graph_[n], st);
        DistItem d;
        d.state = graph_[n];
        d.node = n;
        nn_->add(d);
    }

    // TODO: throw an exception which solve() catches so that solve()
    // can return the proper PlannerStatus
    if (nn_->size() == 0)
    {
        OMPL_ERROR("There are no valid initial states!");
        // return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    // OMPL_INFORM("Starting with %u states", boost::num_vertices(graph_));
}

bool ompl::geometric::RRG::extend(const base::State *s, Node* newNode, Node* nearNode)
{
    // Find closest state in graph
    DistItem queryItem;
    queryItem.state = s;

    DistItem nearDistItem = nn_->nearest(queryItem);
    *nearNode = nearDistItem.node;
    const base::State *nearState = nearDistItem.state;

    // If distance between nearest state and sampled state is greater
    // than maxDistance_, find state maxDistance_ away from near in
    // direction of sampled
    base::State* newState = 0;
    double dist = si_->distance(nearState, s);
    if (dist > maxDistance_)
    {
        newState = si_->allocState();
        si_->getStateSpace()->interpolate(nearState, s, maxDistance_ / dist, newState);
    }

    // If motion between near and new state is collision-free, add new
    // state and edge to graph. We don't add this new state to the
    // near-neighbor structure until after we call connectNear() in
    // preprocess().
    ++numCollisionChecks_;
    bool collisionFree = si_->checkMotion(nearState, s);
    if (collisionFree)
    {
        *newNode = boost::add_vertex(graph_);
        if (!newState)
        {
            newState = si_->allocState();
            si_->copyState(newState, s);
        }
        graph_[*newNode] = newState;

        Edge newEdge;
        bool success;
        boost::tie(newEdge, success) = boost::add_edge(*nearNode, *newNode, graph_);
        graph_[newEdge] = opt_->motionCost(nearState, newState);

        // If in a fully symmetric space, we go and add the reverse edge too
        if (si_->getStateSpace()->hasSymmetricDistance() &&
            si_->getStateSpace()->hasSymmetricInterpolate() &&
            opt_->isSymmetric())
        {
            Edge revEdge;
            boost::tie(revEdge, success) = boost::add_edge(*newNode, *nearNode, graph_);
            graph_[revEdge] = graph_[newEdge];
        }
    }
    else if (newState)
        si_->freeState(newState);

    return collisionFree;
}

// We make connections within a certain radius from other states to
// the new state, and then vice versa.
void ompl::geometric::RRG::connectNear(Node node, Node nearNode)
{
    bool symDist = si_->getStateSpace()->hasSymmetricDistance();
    bool symInterp = si_->getStateSpace()->hasSymmetricInterpolate();
    bool symCost = opt_->isSymmetric();

    // If this state space doesn't have a symmetric distance function,
    // we have to set the distance function to be FROM other states TO
    // the new state
    if (!symDist)
        nn_->setDistanceFunction(boost::bind(&RRG::distanceFunction, this, _1, _2));

    // Get nearest neighbors (FROM other states TO this state;
    // important distinction for asymmetric distance metric)
    //
    // TODO: make neighbors vector static so that we're not
    // constructing/destructing so much crap all the time
    std::vector<DistItem> neighbors;
    DistItem queryItem;
    queryItem.state = graph_[node];
    queryItem.node = node;
    double neighborRadius = this->getNeighborhoodRadius();
    nn_->nearestR(queryItem, neighborRadius, neighbors);

    // Try to connect to every node (except the one node this new node
    // is already connected to, from the extension step)
    for (std::vector<DistItem>::const_iterator nbhr = neighbors.begin();
         nbhr != neighbors.end();
         ++nbhr)
    {
        if (nbhr->node != nearNode)
        {
            ++numCollisionChecks_;
            if (si_->checkMotion(graph_[nbhr->node], graph_[node]))
            {
                Edge newEdge;
                bool success;
                boost::tie(newEdge, success) = boost::add_edge(nbhr->node, node, graph_);
                graph_[newEdge] = opt_->motionCost(graph_[nbhr->node], graph_[node]);

                // Add reverse edge if this space is completely
                // symmetric (effectively create an undirected graph)
                if (symDist && symInterp && symCost)
                {
                    base::Cost cost = graph_[newEdge];
                    boost::tie(newEdge, success) = boost::add_edge(node, nbhr->node, graph_);
                    graph_[newEdge] = cost;
                }
            }
        }
    }

    // If our distance function is asymmetric, we have to run another
    // near neighbors query going FROM our new node TO every other
    // node
    if (!symDist)
    {
        nn_->setDistanceFunction(boost::bind(&RRG::distanceFunction, this, _2, _1));
        nn_->nearestR(queryItem, neighborRadius, neighbors);
    }

    // We have to iterate back over our neighbors if:
    //
    // a) our distance metric is asymmetric (different set of
    // neighbors)
    //
    // b) interpolation between states is asymmetric [(s1,s2) might be
    // valid but (s2,s1) might not]
    //
    // c) cost function between states is asymmetric
    if (!symDist || !symInterp || !symCost)
    {
        for (std::vector<DistItem>::const_iterator nbhr = neighbors.begin();
             nbhr != neighbors.end();
             ++nbhr)
        {
            // If distance or interpolation are asymmetric, we need to
            // do another collision check from the new node to the
            // neighbor. If the above are symmetric and there's no
            // edge already between them, it means there's a collision
            // so this edge is invalid.
            //
            // TODO: see if it's more efficient to cache the collision
            // check queries, instead of checking for edge existence.
            bool validConnection = true;
            Edge reverseEdge;
            bool reverseEdgeExists = false;
            if (!symDist || !symInterp)
            {
                ++numCollisionChecks_;
                validConnection = si_->checkMotion(graph_[node], graph_[nbhr->node]);
            }
            else
            {
                boost::tie(reverseEdge, reverseEdgeExists) = boost::edge(nbhr->node,node,graph_);
                if (!reverseEdgeExists)
                    validConnection = false;
            }

            if (validConnection)
            {
                Edge newEdge;
                bool success;
                boost::tie(newEdge, success) = boost::add_edge(node, nbhr->node, graph_);

                // If our cost function is symmetric and we have an
                // edge in the reverse direction, we just copy over
                // that cost. If not, we directly compute the cost.
                if (symCost && reverseEdgeExists)
                    graph_[newEdge] = graph_[reverseEdge];
                else
                    graph_[newEdge] = opt_->motionCost(graph_[node], graph_[nbhr->node]);
            }
        } // end for each neighbor
    } // end if asymmetric
}

// We iterate through each vertex and free their states, and then
// through each edge and free their costs, then clear the graph
// structure
void ompl::geometric::RRG::freeMemory(void)
{
    // Free state at each vertex
    boost::graph_traits<Graph>::vertex_iterator vi, vLast;
    for (boost::tie(vi,vLast) = boost::vertices(graph_);
         vi != vLast;
         ++vi)
    {
        si_->freeState(graph_[*vi]);
    }

    // clear graph
    graph_.clear();
}

ompl::base::PathPtr
ompl::geometric::RRG::getBestPath(Node startNode, const base::GoalPtr& goal)
{
    // Initialized structures used for Astar
    double nearestGoalDist = std::numeric_limits<double>::infinity();
    Node nearestToGoal = boost::graph_traits<Graph>::null_vertex();
    astar_goal_visitor astarVisitor(boost::dynamic_pointer_cast<base::GoalRegion>(goal).get(),
                                    &nearestGoalDist,
                                    &nearestToGoal);

    Node goalNode = boost::graph_traits<Graph>::null_vertex();
    try
    {
        boost::astar_search(graph_, startNode,
                            boost::bind(&geometric::RRG::costToGo, this, _1, goal.get()),
                            weight_map(boost::get(boost::edge_bundle, graph_)).
                            predecessor_map(&(predMap_[0])).
                            distance_map(&(vertexCostMap_[0])).
                            distance_compare(boost::bind(&base::OptimizationObjective::
                                                         isCostBetterThan, opt_.get(), _1, _2)).
                            distance_combine(boost::bind(&base::OptimizationObjective::
                                                         combineCosts, opt_.get(), _1, _2)).
                            distance_inf(opt_->infiniteCost()).
                            distance_zero(opt_->identityCost()).
                            visitor(astarVisitor));
    }
    catch (found_goal& e)
    {
        goalNode = e.goalNode;
    }

    // If no node was found that actually satisfied the goal, we'll
    // just use the node closest to the goal
    //
    // BTW this is gonna be super specific to heuristic = distanceGoal
    // so sorry mother
    if (goalNode == boost::graph_traits<Graph>::null_vertex())
        goalNode = *(astarVisitor.nearestToGoal);

    // Populate path to goal. Simply populate a vector backwards
    // using the predecessor map and the minimum-cost goal node,
    // then populate the actual path structure that we'll return.
    std::vector<base::State*> rPath;
    Node currentNode = boost::graph_traits<Graph>::null_vertex();
    Node nextNode = goalNode;
    do
    {
        currentNode = nextNode;
        rPath.push_back(graph_[currentNode]);
        nextNode = predMap_[currentNode];
    } while (currentNode != nextNode);
    PathGeometric *geomPath = new PathGeometric(si_);
    for (std::vector<base::State*>::const_reverse_iterator i = rPath.rbegin();
         i != rPath.rend();
         ++i)
    {
        geomPath->append(*i);
    }

    return base::PathPtr(geomPath);
}

ompl::base::Cost ompl::geometric::RRG::costToGo(Node v, const base::Goal* goal) const
{
    return opt_->costToGo(graph_[v], goal);
}

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

    // Add vertices and edges simultaneously
    boost::graph_traits<Graph>::edge_iterator ei, eLast;
    for (boost::tie(ei,eLast) = boost::edges(graph_);
         ei != eLast;
         ++ei)
    {
        Node src = boost::source(*ei, graph_);
        Node trg = boost::target(*ei, graph_);
        data.addEdge(base::PlannerDataVertex(graph_[src]),
                     base::PlannerDataVertex(graph_[trg]));
    }
}