// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
// Copyright (c) 2016, Luc Oth
// 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.
//
// * All advertising materials mentioning features or use of this software must display the
// following acknowledgement: This product includes software developed by the
// Autonomous Systems Lab and Skybotix AG.
//
// * Neither the name of the Autonomous Systems Lab and Skybotix AG 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 AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''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 AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG 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.
//
// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// 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 ETH Zurich, Wyss Zurich, Zurich Eye 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 ETH Zurich, Wyss Zurich, Zurich Eye 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.
//
// Derived from https://github.com/ethz-asl/kalibr/ (2016)
//
// Changes wrt. original:
//  _ remove dependency on Schweizer-Messer toolbox
//  _ remove dependency on sparse_block_matrix (drop support for sparse
//    initialization)
//
// WARNING: Do NOT use as-is in production code.
// @todo The code itself was initially designed purely for research and before
//       actually using it in production code, we should properly verify its
//       capabilities. One major part is the efficiency which isn't perfect at
//       all (pure Eigen::Dynamic implementations, a bunch of duplicate
//       evaluations for readability and some debuggng stuff)

#pragma once

#include <vector>
#include <Eigen/Core>
#include <ze/common/logging.hpp>
#include <ze/common/types.hpp>

namespace ze_splines {

// Define a generic expression of a matrix where all coefficients
// are defined by a functor.
// A BiVector is a vector that holds the values of cumulative basis functions
// and extends the definition to the outside of its definition:
// if before the segment where it is defined: 0
// after the segment: fixed to end_value
class BiVector;
}  // namespace ze_splines

namespace Eigen {
namespace internal {
template<>

struct functor_traits<ze_splines::BiVector>
{
  enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
};
}  // namespace internal
}  // namespace Eigen

namespace ze_splines {

class BiVector {
  enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
private:
  const int startIndex_;
  const ze::real_t endValue_;
  const ze::VectorX localBi_;

public:
  BiVector(int startIndex, const ze::VectorX& localBi, ze::real_t endValue)
    : startIndex_(startIndex)
    , endValue_(endValue)
    , localBi_(localBi)
  {};

  ze::real_t operator() (int i, int j = 0) const {
    i -= startIndex_;
    if(i < 0)
    {
      return endValue_;
    }
    if(i >= (localBi_.rows()))
    {
      return 0;
    }
    return  i >= 0 ? localBi_(i): 0;
  }
};

} // namespace ze_splines

namespace ze {

/**
 * @class BSpline
 *
 * A class to facilitate state estimation for vechicles in 3D space using B-Splines
 *
 */
class BSpline
{
public:

  /**
   * Create a spline of the specified order. The resulting B-spline will
   * be a series of piecewise polynomials of degree splineOrder - 1.
   *
   * @param splineOrder The order of the spline.
   */
  BSpline(int spline_order);

  /**
   * A destructor.
   *
   */
  ~BSpline();

  /**
   *
   * @return The order of the spline
   */
  int spline_order() const;

  /**
   *
   * @return The degree of polynomial used by the spline.
   */
  int polynomialDegree() const;

  /**
   *
   * @return the minimum number of knots required to have at least one valid
   * time segment.
   */
  int minimumKnotsRequired() const;

  /**
   *
   * @return the dimension of the bspline (only valid after initialization)
   */
  int dimension() const;

  /**
   * @param numTimeSegments the number of time segments required
   * @return the number of coefficients required for a specified number of
   * valid time segments.
   */
  int numCoefficientsRequired(int num_time_segments) const;

  /**
   * @param numTimeSegments the number of time segments required
   * @return the number of knots required for a specified number of valid
   * time segments.
   */
  int numKnotsRequired(int num_time_segments) const;

  /**
   *
   * @param numKnots the number of knots.
   * @return the number of valid time segments for a given number of knots.
   */
  int numValidTimeSegments(int num_knots) const;

  /**
   *
   * @return the number of valid time segments for a given for the current
   * knot sequence.
   */
  int numValidTimeSegments() const;

  /**
   * Return the basis matrix active on the \f$i^{\textup{th}}\f$ time segment.
   *
   * @param i The index of the time segment.
   *
   * @return The basis matrix active on the time segment.
   */
  const MatrixX& basisMatrix(int i) const;

  /**
   *
   * @return the time interval that the spline is well-defined on
   * [t_min(), t_max()]
   */
  std::pair<real_t, real_t> timeInterval() const;

  /**
   * Return the time interval of a single spline segment.
   *
   * @param i The index of the time segment
   *
   * @return the time interval of the ith spline segment.
   */
  std::pair<real_t, real_t> timeInterval(int i) const;

  /**
   * Set the knots and coefficients of the spline. Each column of the
   * coefficient matrix
   * is interpreted as a single, vector-valued spline coefficient.
   *
   * @param knots        A non-decreasing knot sequence.
   * @param coefficients A set of spline coefficients.
   */
  void setKnotsAndCoefficients(const std::vector<real_t>& knots,
                               const MatrixX& coefficients);

  /**
   * Set the knots and coefficients of the spline. Each column of the
   * coefficient matrix is interpreted as a single, vector-valued spline
   * coefficient.
   *
   * @param knots        A non-decreasing knot sequence.
   * @param coefficients A set of spline coefficients.
   */
  void setKnotVectorAndCoefficients(const VectorX& knots,
                                    const MatrixX& coefficients);

  /**
   * Sets the coefficient matrix from the stacked vector of coefficients.
   *
   * @param coefficients The stacked vector of coefficients.
   */
  void setCoefficientVector(const VectorX& coefficients);

  /**
   *
   * @return The stacked vector of coefficients.
   */
  VectorX coefficientVector();

  /**
   * Sets the matrix of coefficients.
   *
   * @param coefficients
   */
  void setCoefficientMatrix(const MatrixX& coefficients);

  /**
   * @return the current knot vector.
   */
  const std::vector<real_t> knots() const;

  /**
   * @return the current knot vector.
   */
  VectorX knotVector() const;

  /**
   * @return the current spline coefficient matrix. Each column of
   * the coefficient matrix is interpreted as a single, vector-valued
   * spline coefficient.
   */
  const MatrixX& coefficients() const;

  /**
   *
   * @return The number of total coefficients the spline currently uses
   */
  int numCoefficients() const;

  /**
   * This is equivalent to spline.coefficients().cols()
   *
   * @return The number of vector-valued coefficient columns the spline
   * currently uses
   */
  int numVvCoefficients() const;

  /**
   *
   * @return The minimum time that the spline is well-defined on.
   */
  real_t t_min() const;

  /**
   *
   * @return The maximum time that the spline is well-defined on. Because B-splines
   *         are defined on half-open intervals, the spline curve is well defined up
   *         to but not including this time.
   */
  real_t t_max() const;

  /**
   * Evaluate the spline curve at the time t.
   *
   * @param t The time to evaluate the spline curve
   *
   * @return The value of the spline curve at the time t.
   */
  VectorX eval(real_t t) const;

  /**
   * Evaluate the derivative of the spline curve at time t.
   *
   * @param t The time to evaluate the spline derivative.
   * @param derivativeOrder The order of the derivative. This must be >= 0
   *
   * @return The value of the derivative of the spline curve evaluated at t.
   */
  VectorX evalD(real_t t, int derivative_order) const;

  /**
   * Evaluate the derivative of the spline curve at time t and retrieve the Jacobian
   * of the value with respect to small changes in the paramter vector. The Jacobian
   * only refers to the local parameter vector. The indices of the local parameters with
   * respect to the full paramter vector can be retrieved using localCoefficientVectorIndices().
   *
   * @param t The time to evaluate the spline derivative.
   * @param derivativeOrder The order of the derivative. This must be >= 0
   *
   * @return The value of the derivative of the spline curve evaluated at t and the Jacobian.
   */
  std::pair<VectorX, MatrixX> evalDAndJacobian(real_t t,
                                               int derivative_order) const;

  /**
   * Evaluate the derivative of the spline curve at time t and retrieve the Jacobian
   * of the value with respect to small changes in the paramter vector. The Jacobian
   * only refers to the local parameter vector.
   *
   * @param t The time to evaluate the spline derivative.
   * @param derivativeOrder The order of the derivative. This must be >= 0
   * @param a pointer to the Jacobian matrix to fill in
   * @param a pointer to an int vector that will be filled with the local coefficient indices
   *
   * @return The value of the derivative of the spline curve evaluated at t.
   */
  VectorX evalDAndJacobian(real_t t,
                           int derivative_order,
                           MatrixX * Jacobian,
                           VectorXi * coefficient_indices) const;

   /**
   * Get the local basis matrix evaluated at the time \f$ t \f$.
   * For vector-valued spline coefficients of dimension \f$ D \f$
   * and a B-spline of order $S$, this matrix will be \f$ D \times SD \f$
   *
   * @param t The time to evaluate the local basis matrix.
   * @param derivativeOrder The derivative order to return (0 is no derivative)
   *
   * @return The local basis matrix evaluated at time \f$ t \f$
   */
  MatrixX Phi(real_t t, int derivative_order = 0) const;

  /**
   * Get the local basis matrix evaluated at the time \f$ t \f$.
   * For vector-valued spline coefficients of dimension \f$ D \f$
   * and a B-spline of order $S$, this matrix will be \f$ D \times SD \f$
   *
   * @param t The time to evaluate the local basis matrix.
   * @param derivativeOrder The derivative order to return (0 is no derivative)
   *
   * @return The local basis matrix evaluated at time \f$ t \f$
   */
  MatrixX localBasisMatrix(real_t t, int derivative_order = 0) const;

  /**
   * Get the local coefficient matrix evaluated at the time \f$ t \f$.
   * For vector-valued spline coefficients of dimension \f$ D \f$
   * and a B-spline of order $S$, this matrix will be \f$ D \times S \f$.
   * Each column of the resulting matrix corresponds to a single vector-valued
   * coefficient.
   *
   * @param t The time being queried
   *
   * @return The local coefficient matrix active at time \f$ t \f$
   */
  MatrixX localCoefficientMatrix(real_t t) const;

  /**
   * Return a map to a single coefficient column.
   * This allows the user to pass around what is essentially a pointer
   * to a single column in the coefficient matrix.
   *
   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
   *
   * @return A map to column i of the coefficient matrix.
   */
  Eigen::Map<VectorX> vvCoefficientVector(int i);

  /**
   * Return a map to a single coefficient column.
   * This allows the user to pass around what is essentially a pointer
   * to a single column in the coefficient matrix.
   *
   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
   *
   * @return A map to column i of the coefficient matrix.
   */
  Eigen::Map<const VectorX> vvCoefficientVector(int i) const;

  /**
   * Return a map to a single coefficient column.
   * This allows the user to pass around what is essentially a pointer
   * to a single column in the coefficient matrix.
   *
   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
   *
   * @return A map to column i of the coefficient matrix.
   */
  template<int D>
  Eigen::Map<Eigen::Matrix<real_t, D, 1> > fixedSizeVvCoefficientVector(int i)
  {
    CHECK_EQ(D,coefficients_.rows())
        << "Size mismatch between requested vector size and actual vector size";
    CHECK_LE(0, coefficients_.cols()) << "Index out of range";
    CHECK_LE(i, coefficients_.cols()) << "Index out of range";

    return Eigen::Map<Eigen::Matrix<real_t, D, 1> >(&coefficients_(0,i),coefficients_.rows());
  }

  /**
   * Return a map to a single coefficient column.
   * This allows the user to pass around what is essentially a pointer
   * to a single column in the coefficient matrix.
   *
   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
   *
   * @return A map to column i of the coefficient matrix.
   */
  template<int D>
  Eigen::Map<const Eigen::Matrix<real_t, D, 1> > fixedSizeVvCoefficientVector(int i) const
  {
    CHECK_EQ(D,coefficients_.rows())
        << "Size mismatch between requested vector size and actual vector size";
    CHECK_LE(0, coefficients_.cols()) << "Index out of range";
    CHECK_LE(i, coefficients_.cols()) << "Index out of range";

    return Eigen::Map< const Eigen::Matrix<real_t, D, 1> >(
          &coefficients_(0,i), coefficients_.rows());
  }

  /**
   * Get the local coefficient vector evaluated at the time \f$ t \f$.
   * For vector-valued spline coefficients of dimension \f$ D \f$
   * and a B-spline of order $S$, this vector will be \f$ SD \times 1 \f$
   * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating
   * Phi(t,O) * localCoefficientVector(t)
   *
   * @param t The time being queried
   *
   * @return The local coefficient vector active at time \f$ t \f$
   */
  VectorX localCoefficientVector(real_t t) const;

  /**
   * Update the local coefficient vector
   *
   * @param t The time used to select the local coefficients.
   * @param c The local coefficient vector.
   */
  void setLocalCoefficientVector(real_t t, const VectorX& c);

  /**
   * Get the indices of the local coefficients active at time t.
   *
   * @param t The time being queried.
   *
   * @return The indices of the local coefficients active at time t.
   */
  VectorXi localCoefficientVectorIndices(real_t t) const;

  /**
   * Get the indices of the local vector-valued coefficients active at time t.
   *
   * @param t The time being queried.
   *
   * @return The indices of the local vector-valued coefficients active at time t.
   */
  VectorXi localVvCoefficientVectorIndices(real_t t) const;

  int coefficientVectorLength() const;

  /**
   * Initialize a spline from two times and two positions. The spline will be initialized to
   * have one valid time segment \f$[t_0, t_1)\f$ such that \f$\mathbf b(t_0) = \mathbf p_0\f$,
   * \f$\mathbf b(t_1) = \mathbf p_1\f$,
   * \f$\dot{\mathbf b}(t_0) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$, and
   * \f$\dot{\mathbf b}(t_1) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$.
   *
   * @param t_0 The start of the time interval.
   * @param t_1 The end of the time interval
   * @param p_0 The position at the start of the time interval.
   * @param p_1 The position at the end of the time interval.
   */
  void initSpline(real_t t_0,
                  real_t t_1,
                  const VectorX& p_0,
                  const VectorX& p_1);

  //! Spline initialization version 2.
  void initSpline2(const VectorX& times,
                   const MatrixX& interpolation_points,
                   int num_segments,
                   real_t lambda);

  //! Spline initialization version 3.
  void initSpline3(const VectorX& times,
                   const MatrixX& interpolation_points,
                   int num_segments,
                   real_t lambda);

  /**
   * Add a curve segment that interpolates the point p, ending at time t.
   *
   * If the new time corresponds with the first knot past the end of the curve,
   * the existing curve is perfectly preserved. Otherwise, the existing curve
   * will interpolate its current position at the current endpoint and the new
   * position at the new endpoint but will not necessarily match the last segment
   * exactly.
   *
   * @param t The time of the point to interpolate. This must be greater than t_max()
   * @param p The point to interpolate at time t.
   */
  void addCurveSegment(real_t t, const VectorX& p);

  /**
   * Add a curve segment that interpolates the point p, ending at time t.
   *
   * If the new time corresponds with the first knot past the end of the curve,
   * the existing curve is perfectly preserved. Otherwise, the existing curve
   * will interpolate its current position at the current endpoint and the new
   * position at the new endpoint but will not necessarily match the last segment
   * exactly.
   *
   * @param t The time of the point to interpolate. This must be greater than t_max()
   * @param p The point to interpolate at time t.
   * @param lambda a smoothness parameter. Higher for more smooth.
   */
  void addCurveSegment2(real_t t, const VectorX& p, real_t lambda);

  /**
   * Removes a curve segment from the left by removing one knot and one coefficient vector.
   * After calling this function, the curve will have one fewer segment. The new minimum
   * time will be timeInterval(0).first
   *
   */
  void removeCurveSegment();

  /**
   * Get the \f$ \mathbf V_i \f$ matrix associated with the integral over the segment.
   *
   * @param segmentIndex
   *
   * @return the \f$ \mathbf V_i \f$ matrix
   */
  MatrixX Vi(int segmentIndex) const;

  VectorX evalIntegral(real_t t1, real_t t2) const;
  inline VectorX evalI(real_t t1, real_t t2) const
  {
    return evalIntegral(t1, t2);
  }

  MatrixX Mi(int segment_index) const;
  MatrixX Bij(int segment_index, int column_index) const;
  MatrixX U(real_t t, int derivative_order) const;
  VectorX u(real_t t, int derivative_order) const;
  int segmentIndex(real_t t) const;
  MatrixX Dii(int segment_index) const;
  MatrixX Di(int segment_index) const;

  /**
   * Get the b_i(t) for i in localVvCoefficientVectorIndices
   * (@see #localVvCoefficientVectorIndices).
   *
   * @param t The time being queried.
   *
   * @return [b_i(t) for i in localVvCoefficientVectorIndices].
   *
   */
  VectorX getLocalBiVector(real_t t, int derivative_order = 0) const;
  void getLocalBiInto(real_t t, VectorX& ret, int derivative_order = 0) const;

  /**
   * Get the cumulative (tilde) b_i(t) for i in localVvCoefficientVectorIndices
   * (@see #localVvCoefficientVectorIndices).
   *
   * @param t The time being queried.
   *
   * @return [tilde b_i(t) for i in localVvCoefficientVectorIndices].
   *
   */
  VectorX getLocalCumulativeBiVector(real_t t, int derivative_order = 0) const;

  Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>
  getBiVector(real_t t) const
  {
    return Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>(
          numValidTimeSegments(), 1,
          ze_splines::BiVector(segmentIndex(t), getLocalBiVector(t), 0));
  }
  Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>
  getCumulativeBiVector(real_t t) const
  {
    return Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>(
          numValidTimeSegments(), 1,
          ze_splines::BiVector(segmentIndex(t),getLocalCumulativeBiVector(t), 1));
  }

  MatrixX segmentQuadraticIntegral(const MatrixX& W,
                                   int segment_index,
                                   int derivative_order) const;
  MatrixX segmentQuadraticIntegralDiag(const VectorX& Wdiag,
                                       int segment_index,
                                       int derivative_order) const;
  MatrixX curveQuadraticIntegral(const MatrixX& W,int derivative_order) const;
  MatrixX curveQuadraticIntegralDiag(const VectorX& Wdiag, int derivative_order) const;

  void initConstantSpline(real_t t_min, real_t t_max,
                          int num_segments, const VectorX& constant);

protected:
  /**
   * An internal function to find the segment of the knot sequence
   * that the time t falls in. The function returns the
   * value \f$ u = \frac{t - t_i}{t_{i+1} - t_i} \f$ and the index \f$i\f$
   *
   * @param t The time being queried.
   *
   * @return A pair with the first value \f$ u = \frac{t - t_i}{t_{i+1} - t_i} \f$
   * and the second value the index \f$i\f$
   */
  std::pair<real_t,int> computeUAndTIndex(real_t t) const;

  /**
   * An internal function to find the segment of the knot sequence
   * that the time t falls in. The function returns the width of the
   * knot segment \f$ \Delta t_i = t_{i+1} - t_i \f$ and the index \f$i\f$
   *
   * @param t The time being queried.
   *
   * @return A pair with the first value \f$ \Delta t_i = t_{i+1} - t_i \f$
   * and the second value the index \f$i\f$
   */
  std::pair<real_t,int> computeTIndex(real_t t) const;

  /**
   * Compute the vector \f$ \mathbf u(t) \f$ for a spline of
   * order \f$ S \f$, this is an \f$ S \times 1 \f$ vector.
   *
   * At derivative order 0 (no derivative), this vector is
   * \f$ \mathbf u(t) = \left [ 1 \; u(t) \; u(t)^2 \; \dots \; u(t)^{S-1} \right ]^T \f$
   *
   * For higher derivative order \f$ M \f$, the vector returned is
   * \f$ \mathbf u^{(M)}(t) = \frac{\partial^{(M)}\mathbf u(t)}{\partial t^{(M)}}
   *
   * @param uval the value \f$ u(t) \f$
   * @param segmentIndex
   * @param derivativeOrder
   *
   * @return
   */
  VectorX computeU(real_t uval, int segment_index, int derivative_order) const;

  int basisMatrixIndexFromStartingKnotIndex(int starting_knot_index) const;
  int startingKnotIndexFromBasisMatrixIndex(int basis_matrix_index) const;
  const MatrixX& basisMatrixFromKnotIndex(int knot_index) const;

  /**
   * Throws an exception if the knot sequence is not non-decreasing.
   *
   * @param knots The knot sequence to verify.
   */
  void verifyKnotSequence(const std::vector<real_t>& knots);

  /**
   * Initialize the basis matrices based on the current knot sequence.
   * There is one basis matrix for each valid time segment defined by the spline.
   *
   * Implemented using the recursive basis matrix algorithm from
   * Qin, Kaihuai, General matrix representations for B-splines,
   * The Visual Computer (2000) 16:177–186
   *
   */
  void initializeBasisMatrices();

  /**
   * The recursive function used to implement the recursive basis matrix algorithm from
   * Qin, Kaihuai, General matrix representations for B-splines,
   * The Visual Computer (2000) 16:177–186
   *
   * @param k The order of the matrix requested.
   * @param i The time segment of the basis matrix
   *
   * @return
   */
  MatrixX M(int k, int i);

  /**
   * A helper function for producing the M matrices. Defined in
   * Qin, Kaihuai, General matrix representations for B-splines,
   * The Visual Computer (2000) 16:177–186
   *
   */
  real_t d_0(int k, int i, int j);

  /**
   * A helper function for producing the M matrices. Defined in
   * Qin, Kaihuai, General matrix representations for B-splines,
   * The Visual Computer (2000) 16:177–186
   *
   */
  real_t d_1(int k, int i, int j);

  /// The order of the spline.
  int spline_order_;

  /// The knot sequence used by the B-spline.
  std::vector<real_t> knots_;

  /// The coefficient matrix used by the B-Spline. Each column can be seen as a
  /// single vector-valued spline coefficient.
  /// This is stored explicityl in column major order to ensure that each column (i.e.
  /// a single vector-valued spline coefficient) is stored in contiguous memory. This
  /// allows one to, for example, map a single spline coefficient using the Eigen::Map type.
  Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> coefficients_;

  /// The basis matrices for each time segment the B-spline is defined over.
  std::vector<MatrixX> basis_matrices_;
};
} // namespace ze


/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

/****************************************************************************/

/* Paso: Pattern */

/****************************************************************************/

/* Author: Lutz Gross, l.gross@uq.edu.au */

/****************************************************************************/

#include "Pattern.h"
#include "PasoException.h"
#include "PasoUtil.h"

#include <boost/scoped_array.hpp>

using escript::IndexList;

namespace paso {

Pattern::Pattern(int ntype, dim_t numOut, dim_t numIn, index_t* inPtr,
                 index_t* idx) :
    type(ntype),
    numOutput(numOut),
    numInput(numIn),
    len(0),
    ptr(inPtr),
    index(idx),
    main_iptr(NULL),
    numColors(-1),
    coloring(NULL),
    hb_row(NULL),
    hb_col(NULL)
{
    const index_t index_offset = (ntype & MATRIX_FORMAT_OFFSET1 ? 1:0);
    index_t min_index = index_offset, max_index = index_offset-1;

    if (inPtr!=NULL && idx != NULL) {
#pragma omp parallel
      {
        index_t loc_min_index=index_offset;
        index_t loc_max_index=index_offset-1;
        if (ntype & MATRIX_FORMAT_OFFSET1) {
#pragma omp for schedule(static)
            for (dim_t i=0; i < numOut; ++i) {
                if (inPtr[i] < inPtr[i+1]) {
                    qsort(&(idx[inPtr[i]-1]),(size_t)(inPtr[i+1]-inPtr[i]),sizeof(index_t), util::comparIndex);
                    loc_min_index = std::min(loc_min_index, idx[inPtr[i]-1]);
                    loc_max_index = std::max(loc_max_index, idx[inPtr[i+1]-2]);
                }
            }
        } else {
#pragma omp for schedule(static)
            for (dim_t i=0; i < numOut; ++i) {
                if (inPtr[i] < inPtr[i+1]) {
                    qsort(&(idx[inPtr[i]]),(size_t)(inPtr[i+1]-inPtr[i]),sizeof(index_t), util::comparIndex);
                    loc_min_index = std::min(loc_min_index, idx[inPtr[i]]);
                    loc_max_index = std::max(loc_max_index, idx[inPtr[i+1]-1]);
                }
            }
        }
        #pragma omp critical
        {
            min_index=std::min(loc_min_index, min_index);
            max_index=std::max(loc_max_index, max_index);
        }
      } // parallel section

      if (min_index < index_offset || max_index >= numIn+index_offset) {
          throw PasoException("Pattern: Pattern index out of range.");
      }
      len = ptr[numOutput] - index_offset;
    }
}

/* deallocates a Pattern */
Pattern::~Pattern()
{
    delete[] ptr;
    delete[] index;
    delete[] main_iptr;
    delete[] coloring;
    delete[] hb_row;
    delete[] hb_col;
}

/* creates a pattern from a range of indices */
Pattern_ptr Pattern::fromIndexListArray(dim_t n0, dim_t n,
                                        const IndexList* index_list_array,
                                        index_t range_min, index_t range_max,
                                        index_t index_offset)
{
    dim_t* ptr = new index_t[n+1-n0];

    // get the number of connections per row
#pragma omp parallel for schedule(static)
    for (dim_t i=n0; i < n; ++i) {
        ptr[i-n0]=index_list_array[i].count(range_min, range_max);
    }
    // accumulate ptr
    dim_t s=0;
    for (dim_t i=n0; i < n; ++i) {
        const dim_t itmp=ptr[i-n0];
        ptr[i-n0]=s;
        s+=itmp;
    }
    ptr[n-n0]=s;

    // fill index
    index_t* index = new index_t[ptr[n-n0]];
#pragma omp parallel for schedule(static)
    for (dim_t i=n0; i < n; ++i) {
        index_list_array[i].toArray(&index[ptr[i-n0]], range_min, range_max,
                                    index_offset);
    }
    Pattern_ptr out(new Pattern(MATRIX_FORMAT_DEFAULT, n-n0,
                                range_max+index_offset, ptr, index));

    return out;
}

index_t* Pattern::borrowMainDiagonalPointer()
{
    if (main_iptr == NULL) {
        const dim_t n = numOutput;
        main_iptr=new index_t[n];
        bool fail = false;
        // identify the main diagonals
#pragma omp parallel for schedule(static)
        for (index_t i = 0; i < n; ++i) {
            index_t* idx = &index[ptr[i]];
            index_t* where_p=reinterpret_cast<index_t*>(bsearch(&i, idx,
                        (size_t)(ptr[i+1] - ptr[i]),
                        sizeof(index_t), util::comparIndex));
            if (where_p == NULL) {
                fail = true;
            } else {
                main_iptr[i] = ptr[i]+(index_t)(where_p-idx);
            }
        }
        if (fail) {
            delete[] main_iptr;
            main_iptr = NULL;
        }
    }
    return main_iptr;
}

index_t* Pattern::borrowColoringPointer()
{
    // is coloring available?
    if (coloring == NULL) {
        coloring = new index_t[numInput];
        index_t out = 0;
        const dim_t n = numOutput;
        index_t* mis_marker = new index_t[n];
        // get coloring
#pragma omp parallel for schedule(static)
        for (index_t i = 0; i < n; i++) {
            coloring[i] = -1;
            mis_marker[i] = -1;
        }

        while (util::isAny(n, coloring, -1)) {
            mis(mis_marker);

#pragma omp parallel for schedule(static)
            for (index_t i = 0; i < n; ++i) {
                if (mis_marker[i])
                    coloring[i] = out;
                mis_marker[i] = coloring[i];
            }
            ++out;
        }
        delete[] mis_marker;
        numColors = out;
    }
    return coloring;
}

// creates a subpattern
Pattern_ptr Pattern::getSubpattern(dim_t newNumRows, dim_t newNumCols,
                                   const index_t* row_list,
                                   const index_t* new_col_index) const
{
    const index_t index_offset=(type & MATRIX_FORMAT_OFFSET1 ? 1:0);

    index_t* newPtr = new index_t[newNumRows+1];
#pragma omp parallel
    {
#pragma omp for schedule(static)
        for (dim_t i=0; i < newNumRows+1; ++i) newPtr[i]=0;

        // find the number of column entries in each row
#pragma omp for schedule(static)
        for (dim_t i=0; i < newNumRows; ++i) {
            index_t j=0;
            const index_t subpattern_row=row_list[i];
            #pragma ivdep
            for (index_t k=ptr[subpattern_row]-index_offset;
                         k < ptr[subpattern_row+1]-index_offset; ++k) {
                if (new_col_index[index[k]-index_offset] > -1)
                    j++;
            }
            newPtr[i]=j;
        }
    } // parallel section

    // accumulate ptr
    newPtr[newNumRows]=util::cumsum(newNumRows, newPtr);
    index_t* newIndex = new index_t[newPtr[newNumRows]];

    // find the number of column entries in each row
#pragma omp parallel for schedule(static)
    for (dim_t i=0; i < newNumRows; ++i) {
        index_t j=newPtr[i];
        const index_t subpattern_row=row_list[i];
        #pragma ivdep
        for (index_t k=ptr[subpattern_row]-index_offset; k < ptr[subpattern_row+1]-index_offset; ++k) {
            const index_t tmp=new_col_index[index[k]-index_offset];
            if (tmp > -1) {
                newIndex[j]=tmp;
                ++j;
            }
        }
    }
    // create return value
    Pattern_ptr out(new Pattern(type, newNumRows, newNumCols, newPtr, newIndex));
    return out;
}

Pattern_ptr Pattern::unrollBlocks(int newType, dim_t output_block_size,
                                  dim_t input_block_size)
{
    Pattern_ptr out;
    const index_t index_offset_in=(type & MATRIX_FORMAT_OFFSET1 ? 1:0);
    const index_t index_offset_out=(newType & MATRIX_FORMAT_OFFSET1 ? 1:0);

    if (output_block_size == 1 && input_block_size == 1 &&
          (type & MATRIX_FORMAT_OFFSET1) == (newType & MATRIX_FORMAT_OFFSET1)) {
        out = shared_from_this();
    } else {
        const dim_t block_size = output_block_size*input_block_size;
        const dim_t new_len = len * block_size;
        const dim_t new_numOutput = numOutput * output_block_size;
        const dim_t new_numInput = numInput * input_block_size;

        index_t* newPtr = new index_t[new_numOutput+1];
        index_t* newIndex = new index_t[new_len];
        #pragma omp parallel
        {
            #pragma omp for schedule(static)
            for (dim_t i=0; i < new_numOutput+1; ++i)
                newPtr[i]=index_offset_out;

            #pragma omp single
            newPtr[new_numOutput]=new_len+index_offset_out;

            #pragma omp for schedule(static)
            for (dim_t i=0; i < numOutput; ++i) {
                for (dim_t k=0; k < output_block_size; ++k) {
                    newPtr[i*output_block_size+k]=(ptr[i]-index_offset_in)*block_size+
                                                       (ptr[i+1]-ptr[i])*input_block_size*k+index_offset_out;
                }
            }

#pragma omp for schedule(static)
            for (dim_t i=0; i < new_numOutput; ++i) {
                #pragma ivdep
                for (index_t iPtr=newPtr[i]-index_offset_out; iPtr<newPtr[i+1]-index_offset_out; ++iPtr) {
                    newIndex[iPtr]=index_offset_out;
                }
            }

            #pragma omp for schedule(static)
            for (dim_t i=0; i < numOutput; ++i) {
                for (index_t iPtr=ptr[i]-index_offset_in; iPtr < ptr[i+1]-index_offset_in; ++iPtr) {
                    for (dim_t k=0; k < output_block_size; ++k) {
                        #pragma ivdep
                        for (dim_t j=0; j < input_block_size; ++j) {
                            newIndex[newPtr[i*output_block_size+k]-index_offset_out+(iPtr-(ptr[i]-index_offset_in))*input_block_size+j] =
                                (index[iPtr]-index_offset_in)*input_block_size+j+index_offset_out;
                        }
                    }
                }
            }
        } // parallel section
        out.reset(new Pattern(newType, new_numOutput, new_numInput, newPtr, newIndex));
    }
    return out;
}

// computes the pattern coming from a matrix-matrix multiplication
Pattern_ptr Pattern::multiply(int type, const_Pattern_ptr B) const
{
    boost::scoped_array<IndexList> index_list(new IndexList[numOutput]);

#pragma omp parallel for schedule(static)
    for (dim_t i = 0; i < numOutput; i++) {
        for (index_t iptrA = ptr[i]; iptrA < ptr[i+1]; ++iptrA) {
            const dim_t j = index[iptrA];
            for (index_t iptrB = B->ptr[j]; iptrB < B->ptr[j+1]; ++iptrB) {
                const dim_t k = B->index[iptrB];
                index_list[i].insertIndex(k);
            }
        }
    }
    return Pattern::fromIndexListArray(0, numOutput, index_list.get(), 0,
                                       B->numInput, 0);
}

/*
 * Computes the pattern  of C = A binary operation B for CSR matrices A,B
 * Note: we do not check whether A_ij(op)B_ij=0
 */
Pattern_ptr Pattern::binop(int type, const_Pattern_ptr B) const
{
    boost::scoped_array<IndexList> index_list(new IndexList[numOutput]);
    const dim_t nRowsB = B->numOutput;

#pragma omp parallel for schedule(static)
    for (dim_t i = 0; i < nRowsB; i++) {
        index_t iptrA = ptr[i];
        index_t iptrB = B->ptr[i];

        while (iptrA < ptr[i+1] && iptrB < B->ptr[i+1]) {
            const dim_t j = index[iptrA];
            const dim_t k = B->index[iptrB];
            if (j < k) {
                index_list[i].insertIndex(j);
                iptrA++;
            } else if (j > k) {
                index_list[i].insertIndex(k);
                iptrB++;
            } else { // (j == k)
                index_list[i].insertIndex(j);
                iptrB++;
                iptrA++;
            }
        }
        while(iptrA < ptr[i+1]) {
            const dim_t j = index[iptrA];
            index_list[i].insertIndex(j);
            iptrA++;
        }
        while(iptrB < B->ptr[i+1]) {
            const dim_t k = B->index[iptrB];
            index_list[i].insertIndex(k);
            iptrB++;
        }
    }

    return Pattern::fromIndexListArray(0, numOutput, index_list.get(), 0,
                                       numInput, 0);
}

} // namespace paso


// Spear: Statistical Platform for Elucidating moleculAr Reactivity
// Copyright (C) Purdue University -- BSD license

#include "spear/FunctionalGroup.hpp"
#include "spear/Molecule_impl.hpp"

#include <boost/graph/vf2_sub_graph_iso.hpp>

using namespace Spear;

static std::string str_toupper(std::string s) {
    std::transform(s.begin(), s.end(), s.begin(),
                   [](unsigned char c){ return std::toupper(c); }
                  );
    return s;
}

FunctionalGroup::FunctionalGroup(const std::string& smiles) {
    using boost::add_vertex;
    assert(!smiles.empty());

    std::queue<size_t> paren_backs;
    std::map <size_t, size_t> rings;
    size_t current = 0;
    size_t previous = 0;
    auto bond_order = Bond::Order::UNKNOWN;
    bool first_atom = true;
    bool in_prop_list = false;

    auto add_atom =
    [this, &first_atom, &bond_order, &previous, &current](Element::Symbol atom) {
        add_vertex(atom, graph_);

        if (!first_atom) {
            boost::add_edge(previous, ++current,
            EdgeProperty(bond_order), graph_);
        }

        first_atom = false;
        previous = current;
        bond_order = Bond::Order::UNKNOWN;
        properties_.emplace_back(std::list<AtomPropertyCompare>());
    };

    auto read_number =
    [&smiles](size_t& start) {
        if (std::isdigit(smiles[start + 1]) == 0) {
            return static_cast<size_t>(1);
        }
        ++start;
        size_t number = 0;
        while (start < smiles.size() && (std::isdigit(smiles[start]) != 0)) {
            number *= 10;
            number += static_cast<size_t>(smiles[start] - '0');
            ++start;
        }
        --start;
        return number;
    };

    for (size_t i = 0; i < smiles.size(); ++i) {
        if (in_prop_list) {
            size_t bonds = 0;
            size_t i_copy = i;
            switch (smiles[i]) {
            case 'D': // Degree
                bonds = read_number(i);

                // Add a lambda function to compare the bond counts
                properties_.back().emplace_back(
                    [bonds](const AtomVertex& a1) {
                        return a1.degree() == bonds;
                    }
                );
                continue;
            case 'X': // Total bonds (explicit + implicit Hs)
                bonds = read_number(i);
                properties_.back().emplace_back(
                    [bonds](const AtomVertex& a1) {
                        return a1.degree() + a1.implicit_hydrogens() == bonds;
                    }
                );
                continue;
            case 'h': // Implicit hydrogens
                bonds = read_number(i);
                if (i != i_copy) {
                    properties_.back().emplace_back(
                        [bonds](const AtomVertex& a1) {
                            return a1.implicit_hydrogens() == bonds;
                        }
                    );
                } else {
                    properties_.back().emplace_back(
                        [bonds](const AtomVertex& a1) {
                            return a1.implicit_hydrogens() >= 1;
                        }
                    );
                }
                continue;
            case 'H': // Total hydrogens
                bonds = read_number(i);
                properties_.back().emplace_back(
                    [bonds](const AtomVertex& a1) {
                        return a1.total_hydrogens() == bonds;
                    }
                );
                continue;
            case 'R':
                bonds = read_number(i);
                if (i != i_copy) {
                    properties_.back().emplace_back(
                        [bonds](const AtomVertex& a1) {
                            auto sssrs = a1.sssrs();
                            return std::distance(sssrs.first, sssrs.second) ==
                                   static_cast<std::ptrdiff_t>(bonds);
                        }
                    );
                } else {
                    properties_.back().emplace_back(
                        [](const AtomVertex& a1) {
                            auto sssrs = a1.sssrs();
                            return std::distance(sssrs.first, sssrs.second) != 0;
                        }
                    );
                }
                continue;
            case 'r':
                bonds = read_number(i);
                if (i != i_copy) {
                    properties_.back().emplace_back(
                        [bonds](const AtomVertex& a1) {
                            auto sssrs = a1.sssrs();
                            for (auto ring = sssrs.first; ring != sssrs.second; ++ring) {
                                if (ring->second.size() == bonds) return true;
                            }
                            return false;
                        }
                    );
                } else {
                    properties_.back().emplace_back(
                        [](const AtomVertex& a1) {
                            auto sssrs = a1.sssrs();
                            return std::distance(sssrs.first, sssrs.second) != 0;
                        }
                    );
                }
                continue;
            case 'a':
                properties_.back().emplace_back(
                    [](const AtomVertex& a1) {
                        return a1.is_aromatic();
                    }
                );
                continue;
            case 'A':
                properties_.back().emplace_back(
                    [](const AtomVertex& a1) {
                        return !a1.is_aromatic();
                    }
                );
                continue;
            case '#':
                bonds = read_number(i);
                add_atom(static_cast<Element::Symbol>(bonds));
                continue;
            default:
                break;
            }
        }

        if (smiles[i] == 'a' || smiles[i] == 'A') {
            bool should_be_aromatic = std::islower(smiles[i]) != 0;
            if (!in_prop_list) {
                add_atom(Element::Symbol(0));
            }

            properties_.back().emplace_back(
                [should_be_aromatic](const AtomVertex& a1) {
                    return a1.is_aromatic() == should_be_aromatic;
                }
            );

            continue;
        }

        if (std::islower(smiles[i]) != 0) {
            auto is_found = Element::SymbolForName.find(
                str_toupper(smiles.substr(i, 1))
            );
            if (is_found != Element::SymbolForName.end()) {
                add_atom(is_found->second);
            } else {
                throw std::invalid_argument(std::string("Element not found: ") + smiles[i]);
            }
            properties_.back().emplace_back(
                [](const AtomVertex& a1) {
                    return a1.is_aromatic();
                }
            );
            continue;
        }

        // Unlike a smiles string, C does not imply aliphatic!
        if (std::isupper(smiles[i]) != 0) {
            size_t element_length =
                i + 1 < smiles.size() && (std::islower(smiles[i+1]) != 0)? 2 : 1;

            auto element_name = smiles.substr(i, element_length);
            auto is_found = Element::SymbolForName.find(element_name);

            if (is_found != Element::SymbolForName.end()) {
                add_atom(is_found->second);
            } else {
                throw std::invalid_argument("Element not found: " + element_name);
            }
            i += element_length - 1;
            continue;
        }

        if (std::isdigit(smiles[i]) != 0) {
            auto ring_id = static_cast<size_t>(smiles[i] - '0');
            auto ring_lookup = rings.find(ring_id);

            if (ring_lookup == rings.end()) {
                rings.insert({ring_id, previous});
                continue;
            }

            boost::add_edge(previous, ring_lookup->second,
                EdgeProperty(bond_order), graph_);
            rings.erase(ring_lookup);
            continue;
        }

        switch (smiles[i]) {
        case '.': first_atom = false; break;
        case '*': add_atom(Element::Symbol(0)); break;
        case '@': break;
        case '/': break;
        case '\\': break;
        case '%': break;
        case '~': bond_order = Bond::Order::UNKNOWN; break;
        case '-': bond_order = Bond::Order::SINGLE; break; // We don't support charges
        case '=': bond_order = Bond::Order::DOUBLE; break;
        case '#': bond_order = Bond::Order::TRIPLE; break;
        case ':': bond_order = Bond::Order::AROMATIC; break;
        case '[': in_prop_list = true; break;
        case ']': in_prop_list = false; break;
        case '+': break;
        case '(':
            paren_backs.push(previous);
            break;
        case ')':
            previous = paren_backs.back();
            paren_backs.pop();
            break;
        default: break;
        }
    }
}

/// This Functor is run when the graph matching algorithm finds a potential
/// FunctionalGroup. It is responsible for populating the found_groups list
/// and performing final checks for properties in the functional group.
struct FunctionalGroupFinder {
    FunctionalGroupFinder(const Molecule& mol, const FunctionalGroup& fg,
        std::list<std::vector<size_t>>& found_groups):
    mol_(mol), fg_(fg), found_groups_(found_groups) {
    }

    // Map1 should be the functional group
    // Map2 should be the molecule
    // Therefore, fg_to_mol coverts the functional group mapping to the molecule
    template <typename Map1To2, typename Map2To1>
    bool operator()(Map1To2 fg_to_mol, Map2To1 /*unused*/) {

        std::vector<size_t> group;

        // Prepare to loop through the functional group
        group.reserve(boost::num_vertices(fg_.graph()));
        auto verticies_iter = boost::vertices(fg_.graph());

        // Add the **molecule** index to the group, *not* the functional groups
        // v is an interator through the functional group
        for (auto v = verticies_iter.first; v != verticies_iter.second; ++v) {

            const auto& mol_id = boost::get(fg_to_mol, *v); // v is the functional group index
            const auto& mol_index = boost::get(boost::vertex_index, mol_.graph(), mol_id);

            // Final property check
            auto fg_index = boost::get(boost::vertex_index_t(), fg_.graph(), *v);
            const auto& props = fg_.properties(fg_index);
            for (const auto& prop : props) {
                if (!prop(mol_[mol_index])) {
                    // Don't add the group, get out!
                    return true;
                }
            }

            // All tests for this functional group atom -> molecular atom passed
            group.push_back(mol_index);
        }

        // All tests for all atoms have passed
        found_groups_.emplace_back(group);
        return true;
    }

private:
    const Molecule& mol_;
    const FunctionalGroup& fg_;
    std::list<std::vector<size_t>>& found_groups_;
};

/// This Functor compares the properties of an edge (bond) or vertex(atom).
/// The template arguments should be automatically deduced when a comparison
/// structure is initialized using the **compare_topology** function.
template <typename PropertyFirst, typename PropertySecond, typename IgnoreType>
struct CompareTopology {

    CompareTopology(const PropertyFirst property_map1,
                    const PropertySecond property_map2,
                    const IgnoreType ignore) :
    property1_(property_map1), property2_(property_map2), ignore_(ignore) {}

    template <typename ItemFirst,
              typename ItemSecond>
    bool operator()(const ItemFirst item1, const ItemSecond item2) const {
        if (boost::get(property1_, item1) == ignore_) return true;
        if (boost::get(property2_, item2) == ignore_) return true;
        return boost::get(property1_, item1) == boost::get(property2_, item2);
    }

private:
    const PropertyFirst property1_;
    const PropertySecond property2_;
    const IgnoreType ignore_;
};

/// This is a convience function for creating CompareTopology Functors.
template <typename PropertyFirst, typename PropertySecond, typename IgnoreType>
CompareTopology<PropertyFirst, PropertySecond, IgnoreType>
compare_topology(const PropertyFirst property1,
                 const PropertySecond property2,
                 const IgnoreType ignore) {
    return CompareTopology<PropertyFirst, PropertySecond, IgnoreType>
        (property1, property2, ignore);
}

std::list<std::vector<size_t>> Spear::find_functional_groups(const Molecule& mol,
                                                             const FunctionalGroup& fg) {

    // Initial objects to hold the found functional groups
    std::list<std::vector<size_t>> found_groups;
    FunctionalGroupFinder callback(mol, fg, found_groups);

    const auto& fg_graph = fg.graph();
    const auto& mol_graph = mol.graph();

    // Make comparison functions
    auto vertex_comp =
        compare_topology(boost::get(boost::vertex_name, fg_graph),
                         boost::get(boost::vertex_name, mol_graph),
                         uint64_t(0));

    auto edge_comp =
        compare_topology(boost::get(boost::edge_name, fg_graph),
                         boost::get(boost::edge_name, mol_graph),
                         uint64_t(0));

    // Find subgraphs
    boost::vf2_subgraph_iso(fg_graph, mol_graph, callback,
        boost::vertex_order_by_mult(fg_graph),
        boost::edges_equivalent(edge_comp).vertices_equivalent(vertex_comp));

    return found_groups;
}


/*
 * Copyright 2010-2016 by RomboStudios
 * 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 software's owners 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 HOLDER 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.

 * Created:	Augutst 15, 2011
 */

#ifndef CPC_POINTCONTAINER_H_INCLUDED
#define CPC_POINTCONTAINER_H_INCLUDED

#include <boost/scoped_array.hpp>

namespace cpc
{

/// ------------------------------------------------------------------------------
/// Naive octree for storing a point hierarchy
template< typename DataType, class iPointlist >
class iOctreeGI
{
public:
	/// Tree node
	/// Leaf nodes have npoints > 0,
	/// specifying the number of child points contained
	struct Node
	{
		Node()
		:	bbox_min(1e30f)
		,	bbox_max(-1e30f)

		,	aggP(0.f)
		,	aggN(0.f)
		,	aggR(0.f)
		,	aggCol(0.f)

		,	npoints(0)

		,	data()
		{
			children[0] = children[1] = children[2] = children[3] = 0;
			children[4] = children[5] = children[6] = children[7] = 0;
		}

		/// Octree node data
		iVector3	bbox_min;				// node bounding box min
		iVector3	bbox_max;				// node bounding box max

		iVector3	aggP;					// aggregated position
		iVector3	aggN;					// normal
		iVector3	aggCol;					// node radiance
		float		aggR;					// and radius

		int npoints;						// Number of child points for the leaf node case

		Node* children[8];					// Child nodes, to be indexed as children[z][y][x]

		boost::scoped_array<float> data;	// Collection of points in leaf
											// The building mechanism stores leaf points
											// directly in the octree node
	};

	/// Construct tree from array of points
	iOctreeGI():				m_root(0),	m_dataSize(0){}
	~iOctreeGI()				{};

	/// Get total node
	int getTotalNodes()			{ return m_totnodes; };

	/// Get root node of tree
	const Node* root()	const	{ return m_root; }

	/// Get number of floats representing each point.
	int dataSize()		const	{ return m_dataSize; }

	/// Clear tree
	void Clear()				{ deleteTree(m_root); }

	/// Build octree from pointlist //////////////////////////////////////////////////////////////////////
	iOctreeGI( iPointlist* pointlist )
	:	m_root(0)
	,	m_dataSize(iPointlist::NODESIZE)
	{
		this->Clear();	///	/////////////////////////////////////// <== !!?
		size_t npoints = pointlist->GetTotalNodes();

		// We make octree bound cubic rather than fitting the point cloud
		// tightly.  This improves the distribution of points in the octree
		// nodes and reduces artifacts when groups of points are aggregated
		// in the internal nodes.
		//
		// If we *don't* do this and we have a rectangular (non-cubic)
		// bound, we end up with a lot more points in one direction inside
		// a node than another.  This means the aggregated averaged point -
		// intended to represent the collection - is in the middle, but
		// with lots of room on either side:
		//
		// +-----------+   ----->    +----/^\----+
		// | o o o o o |  aggregate  |   | . |   |
		// +-----------+             +----\_/----+
		//
		//   <------->                   <--->
		// even distribution           all in middle :(
		//
		// That is, there will be large gaps between neighbouring disks,
		// which gives large transparent gaps in the microrendered surface.

		iVector3	bbox_min;							// node bounding box min
		iVector3	bbox_max;							// node bounding box max

		for(size_t i = 0; i < npoints; ++i)
		{
			iVector3 p ( pointlist->template GetNodeVectorAt<DATA_SLOT_POS>(i) );
			p.expand_bbox(bbox_min, bbox_max);
		}
		iVector3 d ( bbox_max - bbox_min );				// diagonal
		iVector3 c ( (bbox_min + bbox_max) / 2.f );		// center

		float maxDim2 = ds_MAX(ds_MAX(d.GetX(), d.GetY()), d.GetZ()) / 2;
		bbox_min = c - maxDim2;
		bbox_max = c + maxDim2;

		// Get a ref(workspace) to the actual pointlist
		iPointListSimple workspace;
		workspace.SetStorageArray( pointlist->GetStorageArray() );

		// Build octree
		m_root = makeTree(0, &workspace, npoints, m_dataSize, bbox_min, bbox_max);
	}

	/// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
private:

	/// Members
	Node* m_root;
	int m_dataSize;
	int m_totnodes;

	/// Recursively delete tree, depth first.
	static void deleteTree(Node* n)
	{
		if(!n) return;

		for(int i = 0; i < 8; ++i)
		deleteTree(n->children[i]);

		delete n;
	}

	/// Build a tree node from the given points
	template<class xPointList>
	Node* makeTree(	int					depth,
					xPointList*			pointlist,
					size_t				npoints,
					int					dataSize,
					const iVector3&		bbox_min,
					const iVector3&		bbox_max
					)
	{
		assert(npoints != 0);
		m_totnodes++;

		// Setup bounding box
		Node* node = new Node;
		node->bbox_min = bbox_min;
		node->bbox_max = bbox_max;

		iVector3 c ( (bbox_min + bbox_max) / 2.f);
		node->npoints = 0;

		// Limit max depth of tree to prevent infinite recursion
		int maxDepth = 24;
		size_t pointsPerLeaf = 8;

		if(npoints <= pointsPerLeaf || depth >= maxDepth)
		{
			// Small number of child points: make this a leaf node
			node->npoints = npoints;

			// Copy over data into node
			node->data.reset(new float[npoints*dataSize]);

			iVector3 sumP(0.f);		iVector3 sumN(0.f);
			iVector3 sumCol(0.f);	float sumA = 0.f;

			for(size_t j = 0; j < npoints; ++j)
			{
				const float* nodedata = pointlist->GetNodePtr( j );

				// copy extra data
				for(int i = 0; i < dataSize; ++i)
				node->data[ j*dataSize + i ] = nodedata[i];

				// compute averages (area weighted)
				float A = pointlist->template GetNodeScalarAt<DATA_SLOT_AREA>( nodedata );
				A *= A;
				sumA	+=	A;
				sumP	+=	A * iVector3(pointlist->template GetNodeVectorAt<DATA_SLOT_POS>( nodedata ));
				sumN	+=	A * iVector3(pointlist->template GetNodeVectorAt<DATA_SLOT_DIR>( nodedata ));
				sumCol	+=	A * iVector3(pointlist->template GetNodeVectorAt<DATA_SLOT_IRRAD>( nodedata ));
			}

			node->aggP		=	1.0f/sumA * sumP;
			node->aggN		=	sumN.normalize();
			node->aggR		=	sqrtf(sumA);
			node->aggCol	=	1.0f/sumA * sumCol;

			return node;
		}

		// allocate extra workspace for storing child points (ugh!)
		std::vector< const float*> workspace(8*npoints);
		const float** w = &workspace[0];
		const float** P[8] = {
			w,             w + npoints,   w + 2*npoints, w + 3*npoints,
			w + 4*npoints, w + 5*npoints, w + 6*npoints, w + 7*npoints
		};

		// Partition points into the eight child nodes
		size_t np[8] = {0};
		for(size_t i = 0; i < npoints; ++i)
		{
			const float* p = pointlist->GetNodePtr( i );

			int cellIndex = 4*(p[2] > c.GetZ()) + 2*(p[1] > c.GetY()) + (p[0] > c.GetX());
			P[cellIndex][np[cellIndex]++] = p;
		}

		// Recursively generate child nodes and finalize node
		float sumA = 0;
		iVector3 sumP(0.f);
		iVector3 sumN(0.f);
		iVector3 sumCol(0.f);

		for(int i = 0; i < 8; ++i)
		{
			if(np[i] == 0)
			continue;

			//Box3f bnd;
			iVector3	bnd_bbox_min;			// node bounding box min
			iVector3	bnd_bbox_max;			// node bounding box max
			bnd_bbox_min.SetX( (i     % 2 == 0) ? bbox_min.GetX() : c.GetX() );
			bnd_bbox_min.SetY( ((i/2) % 2 == 0) ? bbox_min.GetY() : c.GetY() );
			bnd_bbox_min.SetZ( ((i/4) % 2 == 0) ? bbox_min.GetZ() : c.GetZ() );
			bnd_bbox_max.SetX( (i     % 2 == 0) ? c.GetX() : bbox_max.GetX() );
			bnd_bbox_max.SetY( ((i/2) % 2 == 0) ? c.GetY() : bbox_max.GetY() );
			bnd_bbox_max.SetZ( ((i/4) % 2 == 0) ? c.GetZ() : bbox_max.GetZ() );

			// Node plist workspace
			iPointListSimple nodeplist;
			nodeplist.SetStorageArray( P[i] );

			//* Make child node
			Node* child = makeTree(depth+1, &nodeplist, np[i], dataSize, bnd_bbox_min, bnd_bbox_max);
			node->children[i] = child;

			// Weighted average with weight = disk surface area
			float A = child->aggR * child->aggR;
			sumA	+=	A;
			sumP	+=	A * child->aggP;
			sumN	+=	A * child->aggN;
			sumCol	+=	A * child->aggCol;
		}

		node->aggP		=	1.0f/sumA * sumP;
		node->aggN		=	sumN.normalize();
		node->aggR		=	sqrtf(sumA);
		node->aggCol	=	1.0f/sumA * sumCol;

		return node;
	}

};		// iOctreeGI

}		// cpc namespace
#endif	// CPC_POINTCONTAINER_H_INCLUDED


// Copyright (C) 2004-2006 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

/**************************************************************************
 * This source file implements the variation on Dijkstra's algorithm      *
 * presented by Crauser et al. in:                                        *
 *                                                                        *
 *   Andreas Crauser, Kurt Mehlhorn, Ulrich Meyer, and Peter              *
 *   Sanders. A Parallelization of Dijkstra's Shortest Path               *
 *   Algorithm. In Lubos Brim, Jozef Gruska, and Jiri Zlatuska,          *
 *   editors, Mathematical Foundations of Computer Science (MFCS),        *
 *   volume 1450 of Lecture Notes in Computer Science, pages              *
 *   722--731, 1998. Springer.                                            *
 *                                                                        *
 * This implementation is, however, restricted to the distributed-memory  *
 * case, where the work is distributed by virtue of the vertices being    *
 * distributed. In a shared-memory (single address space) context, we     *
 * would want to add an explicit balancing step.                          *
 **************************************************************************/
#ifndef BOOST_GRAPH_CRAUSER_ET_AL_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_CRAUSER_ET_AL_SHORTEST_PATHS_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/assert.hpp>
#include <boost/graph/distributed/detail/dijkstra_shortest_paths.hpp>
#include <boost/graph/parallel/algorithm.hpp>
#include <functional>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/property_map_iterator.hpp>
#include <boost/type_traits/is_same.hpp>
#include <algorithm>
#include <boost/property_map/parallel/caching_property_map.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/distributed/detail/remote_update_set.hpp>
#include <vector>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/parallel/container_traits.hpp>

#ifdef PBGL_ACCOUNTING
#  include <boost/graph/accounting.hpp>
#  include <numeric>
#endif // PBGL_ACCOUNTING

#ifdef MUTABLE_QUEUE
#    include <boost/pending/mutable_queue.hpp>
#endif

namespace boost { namespace graph { namespace distributed {

#ifdef PBGL_ACCOUNTING
struct crauser_et_al_shortest_paths_stats_t
{
  /* Total wall-clock time used by the algorithm.*/
  accounting::time_type execution_time;

  /* The number of vertices deleted in each superstep. */
  std::vector<std::size_t> deleted_vertices;

  template<typename OutputStream>
  void print(OutputStream& out)
  {
    double avg_deletions = std::accumulate(deleted_vertices.begin(),
                                           deleted_vertices.end(),
                                           0.0);
    avg_deletions /= deleted_vertices.size();

    out << "Problem = \"Single-Source Shortest Paths\"\n"
        << "Algorithm = \"Crauser et al\"\n"
        << "Function = crauser_et_al_shortest_paths\n"
        << "Wall clock time = " << accounting::print_time(execution_time)
        << "\nSupersteps = " << deleted_vertices.size() << "\n"
        << "Avg. deletions per superstep = " << avg_deletions << "\n";
  }
};

static crauser_et_al_shortest_paths_stats_t crauser_et_al_shortest_paths_stats;
#endif

namespace detail {

  /************************************************************************
   * Function objects that perform distance comparisons modified by the   *
   * minimum or maximum edge weights.                                     *
   ************************************************************************/
  template<typename Vertex, typename DistanceMap, typename MinInWeightMap,
           typename Combine, typename Compare>
  struct min_in_distance_compare
    : std::binary_function<Vertex, Vertex, bool>
  {
    min_in_distance_compare(DistanceMap d, MinInWeightMap m,
                            Combine combine, Compare compare)
      : distance_map(d), min_in_weight(m), combine(combine),
        compare(compare)
    {
    }

    bool operator()(const Vertex& x, const Vertex& y) const
    {
      return compare(combine(get(distance_map, x), -get(min_in_weight, x)),
                     combine(get(distance_map, y), -get(min_in_weight, y)));
    }

  private:
    DistanceMap distance_map;
    MinInWeightMap min_in_weight;
    Combine combine;
    Compare compare;
  };

  template<typename Vertex, typename DistanceMap, typename MinOutWeightMap,
           typename Combine, typename Compare>
  struct min_out_distance_compare
    : std::binary_function<Vertex, Vertex, bool>
  {
    min_out_distance_compare(DistanceMap d, MinOutWeightMap m,
                             Combine combine, Compare compare)
      : distance_map(d), min_out_weight(m), combine(combine),
        compare(compare)
    {
    }

    bool operator()(const Vertex& x, const Vertex& y) const
    {
      return compare(combine(get(distance_map, x), get(min_out_weight, x)),
                     combine(get(distance_map, y), get(min_out_weight, y)));
    }

  private:
    DistanceMap distance_map;
    MinOutWeightMap min_out_weight;
    Combine combine;
    Compare compare;
  };
  /************************************************************************/

  /************************************************************************
   * Dijkstra queue that implements Crauser et al.'s criteria. This queue *
   * actually stores three separate priority queues, to help expose all   *
   * vertices that can be processed in a single phase.                    *
   ************************************************************************/
  template<typename Graph, typename Combine,
           typename Compare, typename VertexIndexMap, typename DistanceMap,
           typename PredecessorMap, typename MinOutWeightMap,
           typename MinInWeightMap>
  class crauser_et_al_dijkstra_queue
    : public graph::detail::remote_update_set<
               crauser_et_al_dijkstra_queue<
                 Graph, Combine, Compare, VertexIndexMap, DistanceMap,
                 PredecessorMap, MinOutWeightMap, MinInWeightMap>,
               typename boost::graph::parallel::process_group_type<Graph>::type,
               typename dijkstra_msg_value<DistanceMap, PredecessorMap>::type,
               typename property_map<Graph, vertex_owner_t>::const_type>
  {
    typedef typename graph_traits<Graph>::vertex_descriptor
      vertex_descriptor;
    typedef crauser_et_al_dijkstra_queue self_type;
    typedef dijkstra_msg_value<DistanceMap, PredecessorMap> msg_value_creator;
    typedef typename msg_value_creator::type msg_value_type;
    typedef typename graph_traits<Graph>::vertices_size_type
      vertices_size_type;
    typedef typename property_map<Graph, vertex_owner_t>::const_type
      OwnerPropertyMap;
    typedef typename boost::graph::parallel::process_group_type<Graph>::type
      process_group_type;
    typedef graph::detail::remote_update_set<self_type, process_group_type,
                                             msg_value_type, OwnerPropertyMap>
      inherited;

    // Priority queue for tentative distances
    typedef indirect_cmp<DistanceMap, Compare> dist_queue_compare_type;

    typedef typename property_traits<DistanceMap>::value_type distance_type;

#ifdef MUTABLE_QUEUE
    typedef mutable_queue<vertex_descriptor, std::vector<vertex_descriptor>,
                          dist_queue_compare_type, VertexIndexMap> dist_queue_type;

#else
    typedef relaxed_heap<vertex_descriptor, dist_queue_compare_type,
                         VertexIndexMap> dist_queue_type;
#endif // MUTABLE_QUEUE

    // Priority queue for OUT criteria
    typedef min_out_distance_compare<vertex_descriptor, DistanceMap,
                                     MinOutWeightMap, Combine, Compare>
      out_queue_compare_type;

#ifdef MUTABLE_QUEUE
    typedef mutable_queue<vertex_descriptor, std::vector<vertex_descriptor>,
                          out_queue_compare_type, VertexIndexMap> out_queue_type;

#else
    typedef relaxed_heap<vertex_descriptor, out_queue_compare_type,
                         VertexIndexMap> out_queue_type;
#endif // MUTABLE_QUEUE

    // Priority queue for IN criteria
    typedef min_in_distance_compare<vertex_descriptor, DistanceMap,
                                    MinInWeightMap, Combine, Compare>
      in_queue_compare_type;

#ifdef MUTABLE_QUEUE
    typedef mutable_queue<vertex_descriptor, std::vector<vertex_descriptor>,
                          in_queue_compare_type, VertexIndexMap> in_queue_type;

#else
    typedef relaxed_heap<vertex_descriptor, in_queue_compare_type,
                         VertexIndexMap> in_queue_type;
#endif // MUTABLE_QUEUE

    typedef typename process_group_type::process_id_type process_id_type;

  public:
    typedef typename dist_queue_type::size_type  size_type;
    typedef typename dist_queue_type::value_type value_type;

    crauser_et_al_dijkstra_queue(const Graph& g,
                                 const Combine& combine,
                                 const Compare& compare,
                                 const VertexIndexMap& id,
                                 const DistanceMap& distance_map,
                                 const PredecessorMap& predecessor_map,
                                 const MinOutWeightMap& min_out_weight,
                                 const MinInWeightMap& min_in_weight)
      : inherited(boost::graph::parallel::process_group(g), get(vertex_owner, g)),
        dist_queue(num_vertices(g),
                   dist_queue_compare_type(distance_map, compare),
                   id),
        out_queue(num_vertices(g),
                  out_queue_compare_type(distance_map, min_out_weight,
                                         combine, compare),
                  id),
        in_queue(num_vertices(g),
                 in_queue_compare_type(distance_map, min_in_weight,
                                       combine, compare),
                 id),
        g(g),
        distance_map(distance_map),
        predecessor_map(predecessor_map),
        min_out_weight(min_out_weight),
        min_in_weight(min_in_weight),
        min_distance(0),
        min_out_distance(0)
#ifdef PBGL_ACCOUNTING
        , local_deletions(0)
#endif
    { }

    void push(const value_type& x)
    {
      msg_value_type msg_value =
        msg_value_creator::create(get(distance_map, x),
                                  predecessor_value(get(predecessor_map, x)));
      inherited::update(x, msg_value);
    }

    void update(const value_type& x) { push(x); }

    void pop()
    {
      // Remove from distance queue
      dist_queue.remove(top_vertex);

      // Remove from OUT queue
      out_queue.remove(top_vertex);

      // Remove from IN queue
      in_queue.remove(top_vertex);

#ifdef PBGL_ACCOUNTING
      ++local_deletions;
#endif
    }

    vertex_descriptor& top() { return top_vertex; }
    const vertex_descriptor& top() const { return top_vertex; }

    bool empty()
    {
      inherited::collect();

      // If there are no suitable messages, wait until we get something
      while (!has_suitable_vertex()) {
        if (do_synchronize()) return true;
      }
      // Return true only if nobody has any messages; false if we
      // have suitable messages
      return false;
    }

    bool do_synchronize()
    {
      using boost::parallel::all_reduce;
      using boost::parallel::minimum;

      inherited::synchronize();

      // TBD: could use combine here, but then we need to stop using
      // minimum<distance_type>() as the function object.
      distance_type local_distances[2];
      local_distances[0] =
        dist_queue.empty()? (std::numeric_limits<distance_type>::max)()
        : get(distance_map, dist_queue.top());

      local_distances[1] =
        out_queue.empty()? (std::numeric_limits<distance_type>::max)()
        : (get(distance_map, out_queue.top())
           + get(min_out_weight, out_queue.top()));

      distance_type distances[2];
      all_reduce(this->process_group, local_distances, local_distances + 2,
                 distances, minimum<distance_type>());
      min_distance = distances[0];
      min_out_distance = distances[1];

#ifdef PBGL_ACCOUNTING
      std::size_t deletions = 0;
      all_reduce(this->process_group, &local_deletions, &local_deletions + 1,
                 &deletions, std::plus<std::size_t>());
      if (process_id(this->process_group) == 0) {
        crauser_et_al_shortest_paths_stats.deleted_vertices.push_back(deletions);
      }
      local_deletions = 0;
      BOOST_ASSERT(deletions > 0);
#endif

      return min_distance == (std::numeric_limits<distance_type>::max)();
    }

  private:
    vertex_descriptor predecessor_value(vertex_descriptor v) const
    { return v; }

    vertex_descriptor
    predecessor_value(property_traits<dummy_property_map>::reference) const
    { return graph_traits<Graph>::null_vertex(); }

    bool has_suitable_vertex() const
    {
      if (!dist_queue.empty()) {
        top_vertex = dist_queue.top();
        if (get(distance_map, dist_queue.top()) <= min_out_distance)
          return true;
      }

      if (!in_queue.empty()) {
        top_vertex = in_queue.top();
        return (get(distance_map, top_vertex)
                - get(min_in_weight, top_vertex)) <= min_distance;
      }
      return false;
    }

  public:
    void
    receive_update(process_id_type source, vertex_descriptor vertex,
                   distance_type distance)
    {
      // Update the queue if the received distance is better than
      // the distance we know locally
      if (distance < get(distance_map, vertex)
          || (distance == get(distance_map, vertex)
              && source == process_id(this->process_group))) {
        // Update the local distance map
        put(distance_map, vertex, distance);

        bool is_in_queue = dist_queue.contains(vertex);

        if (!is_in_queue) {
          dist_queue.push(vertex);
          out_queue.push(vertex);
          in_queue.push(vertex);
        }
        else {
          dist_queue.update(vertex);
          out_queue.update(vertex);
          in_queue.update(vertex);
        }
      }
    }

    void
    receive_update(process_id_type source, vertex_descriptor vertex,
                   std::pair<distance_type, vertex_descriptor> p)
    {
      if (p.first <= get(distance_map, vertex)) {
        put(predecessor_map, vertex, p.second);
        receive_update(source, vertex, p.first);
      }
    }

  private:
    dist_queue_type           dist_queue;
    out_queue_type            out_queue;
    in_queue_type             in_queue;
    mutable value_type        top_vertex;
    const Graph&              g;
    DistanceMap               distance_map;
    PredecessorMap            predecessor_map;
    MinOutWeightMap           min_out_weight;
    MinInWeightMap            min_in_weight;
    distance_type             min_distance;
    distance_type             min_out_distance;
#ifdef PBGL_ACCOUNTING
    std::size_t               local_deletions;
#endif
  };
  /************************************************************************/

  /************************************************************************
   * Initialize the property map that contains the minimum incoming edge  *
   * weight for each vertex. There are separate implementations for       *
   * directed, bidirectional, and undirected graph.                       *
   ************************************************************************/
  template<typename Graph, typename MinInWeightMap, typename WeightMap,
           typename Inf, typename Compare>
  void
  initialize_min_in_weights(const Graph& g, MinInWeightMap min_in_weight,
                            WeightMap weight, Inf inf, Compare compare,
                            directed_tag, incidence_graph_tag)
  {
    // Send minimum weights off to the owners
    set_property_map_role(vertex_distance, min_in_weight);
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
        if (get(weight, e) < get(min_in_weight, target(e, g)))
          put(min_in_weight, target(e, g), get(weight, e));
      }
    }

    using boost::graph::parallel::process_group;
    synchronize(process_group(g));

    // Replace any infinities with zeros
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (get(min_in_weight, v) == inf) put(min_in_weight, v, 0);
    }
  }

  template<typename Graph, typename MinInWeightMap, typename WeightMap,
           typename Inf, typename Compare>
  void
  initialize_min_in_weights(const Graph& g, MinInWeightMap min_in_weight,
                            WeightMap weight, Inf inf, Compare compare,
                            directed_tag, bidirectional_graph_tag)
  {
#if 0
    typename property_map<Graph, vertex_local_t>::const_type
      local = get(vertex_local, g);

    // This code assumes that the properties of the in-edges are
    // available locally. This is not necessarily the case, so don't
    // do this yet.
    set_property_map_role(vertex_distance, min_in_weight);
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (in_edges(v, g).first != in_edges(v, g).second) {
        std::cerr << "weights(" << g.distribution().global(get(local, v))
                  << ") = ";
        BGL_FORALL_INEDGES_T(v, e, g, Graph) {
          std::cerr << get(weight, e) << ' ';
        }
        std::cerr << std::endl;
        put(min_in_weight, v,
            *std::min_element
            (make_property_map_iterator(weight, in_edges(v, g).first),
             make_property_map_iterator(weight, in_edges(v, g).second),
             compare));
      } else {
        put(min_in_weight, v, 0);
      }
      std::cerr << "miw(" << g.distribution().global(get(local, v)) << ") = "
                << get(min_in_weight, v) << std::endl;
    }
#else
    initialize_min_in_weights(g, min_in_weight, weight, inf, compare,
                              directed_tag(), incidence_graph_tag());
#endif
  }

  template<typename Graph, typename MinInWeightMap, typename WeightMap,
           typename Inf, typename Compare>
  inline void
  initialize_min_in_weights(const Graph&, MinInWeightMap, WeightMap, Inf,
                            Compare, undirected_tag, bidirectional_graph_tag)
  {
    // In weights are the same as out weights, so do nothing
  }
  /************************************************************************/

  /************************************************************************
   * Initialize the property map that contains the minimum outgoing edge  *
   * weight for each vertex.                                              *
   ************************************************************************/
  template<typename Graph, typename MinOutWeightMap, typename WeightMap,
           typename Compare>
  void
  initialize_min_out_weights(const Graph& g, MinOutWeightMap min_out_weight,
                             WeightMap weight, Compare compare)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;

    BGL_FORALL_VERTICES_T(v, g, Graph) {
      if (out_edges(v, g).first != out_edges(v, g).second) {
        put(min_out_weight, v,
            *std::min_element
            (make_property_map_iterator(weight, out_edges(v, g).first),
             make_property_map_iterator(weight, out_edges(v, g).second),
             compare));
        if (get(min_out_weight, v) < weight_type(0))
            boost::throw_exception(negative_edge());
      }
    }
  }

  /************************************************************************/

} // end namespace detail

template<typename DistributedGraph, typename DijkstraVisitor,
         typename PredecessorMap, typename DistanceMap, typename WeightMap,
         typename IndexMap, typename ColorMap, typename Compare,
         typename Combine, typename DistInf, typename DistZero>
void
crauser_et_al_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
   IndexMap index_map, ColorMap color_map,
   Compare compare, Combine combine, DistInf inf, DistZero zero,
   DijkstraVisitor vis)
{

#ifdef PBGL_ACCOUNTING
  crauser_et_al_shortest_paths_stats.deleted_vertices.clear();
  crauser_et_al_shortest_paths_stats.execution_time = accounting::get_time();
#endif

  // Property map that stores the lowest edge weight outgoing from
  // each vertex. If a vertex has no out-edges, the stored weight
  // is zero.
  typedef typename property_traits<WeightMap>::value_type weight_type;
  typedef iterator_property_map<weight_type*, IndexMap> MinOutWeightMap;
  std::vector<weight_type> min_out_weights_vec(num_vertices(g), inf);
  MinOutWeightMap min_out_weight(&min_out_weights_vec.front(), index_map);
  detail::initialize_min_out_weights(g, min_out_weight, weight, compare);

  // Property map that stores the lowest edge weight incoming to
  // each vertex. For undirected graphs, this will just be a
  // shallow copy of the version for outgoing edges.
  typedef typename graph_traits<DistributedGraph>::directed_category
    directed_category;
  const bool is_undirected =
    is_same<directed_category, undirected_tag>::value;
  typedef MinOutWeightMap MinInWeightMap;
  std::vector<weight_type>
    min_in_weights_vec(is_undirected? 1 : num_vertices(g), inf);
  MinInWeightMap min_in_weight(&min_in_weights_vec.front(), index_map);
  typedef typename graph_traits<DistributedGraph>::traversal_category
    category;
  detail::initialize_min_in_weights(g, min_in_weight, weight, inf, compare,
                                    directed_category(), category());

  // Initialize local portion of property maps
  typename graph_traits<DistributedGraph>::vertex_iterator ui, ui_end;
  for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
    put(distance, *ui, inf);
    put(predecessor, *ui, *ui);
  }
  put(distance, s, zero);

  // Dijkstra Queue
  typedef detail::crauser_et_al_dijkstra_queue
            <DistributedGraph, Combine, Compare, IndexMap, DistanceMap,
             PredecessorMap, MinOutWeightMap, MinInWeightMap>
    Queue;

  Queue Q(g, combine, compare, index_map, distance, predecessor,
          min_out_weight, is_undirected? min_out_weight : min_in_weight);

  // Parallel Dijkstra visitor
  ::boost::detail::dijkstra_bfs_visitor<
      DijkstraVisitor, Queue, WeightMap,
      boost::parallel::caching_property_map<PredecessorMap>,
      boost::parallel::caching_property_map<DistanceMap>, Combine, Compare
    > bfs_vis(vis, Q, weight,
              boost::parallel::make_caching_property_map(predecessor),
              boost::parallel::make_caching_property_map(distance),
              combine, compare, zero);

  set_property_map_role(vertex_color, color_map);
  set_property_map_role(vertex_distance, distance);

  breadth_first_search(g, s, Q, bfs_vis, color_map);

#ifdef PBGL_ACCOUNTING
  crauser_et_al_shortest_paths_stats.execution_time =
    accounting::get_time() - crauser_et_al_shortest_paths_stats.execution_time;
#endif
}

template<typename DistributedGraph, typename PredecessorMap,
         typename DistanceMap, typename WeightMap>
void
crauser_et_al_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance, WeightMap weight)
{
  typedef typename property_traits<DistanceMap>::value_type distance_type;

  std::vector<default_color_type> colors(num_vertices(g), white_color);

  crauser_et_al_shortest_paths(g, s, predecessor, distance, weight,
                               get(vertex_index, g),
                               make_iterator_property_map(&colors[0],
                                                          get(vertex_index, g)),
                               std::less<distance_type>(),
                               closed_plus<distance_type>(),
                               (std::numeric_limits<distance_type>::max)(),
                               distance_type(),
                               dijkstra_visitor<>());
}

template<typename DistributedGraph, typename PredecessorMap,
         typename DistanceMap>
void
crauser_et_al_shortest_paths
  (const DistributedGraph& g,
   typename graph_traits<DistributedGraph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance)
{
  crauser_et_al_shortest_paths(g, s, predecessor, distance,
                               get(edge_weight, g));
}

} // end namespace distributed

#ifdef PBGL_ACCOUNTING
using distributed::crauser_et_al_shortest_paths_stats;
#endif

using distributed::crauser_et_al_shortest_paths;

} } // end namespace boost::graph

#endif // BOOST_GRAPH_CRAUSER_ET_AL_SHORTEST_PATHS_HPP