/**
 * @file   BSpline.hpp
 * @author Paul Furgale <paul.furgale@utoronto.ca>
 * @date   Fri Feb 11 13:51:57 2011
 *
 * @brief  A class to facilitate state estimation for vehicles in 3D
 *         space using B-splines.
 *
 *
 */

#ifndef _BSPLINE_HPP
#define _BSPLINE_HPP
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <Eigen/Core>
#include <sm/assert_macros.hpp>
#include <vector>

namespace bsplines {
class BiVector;
}

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

namespace bsplines {

class BiVector {
    enum { Cost = 1, PacketAccess = false, IsRepeatable = true };

  private:
    const int startIndex_;
    const double endValue_;
    const Eigen::VectorXd localBi_;

  public:
    BiVector(int startIndex, const Eigen::VectorXd& localBi, const double& endValue)
        : startIndex_(startIndex), endValue_(endValue), localBi_(localBi){};

    double operator()(int i, int j = 0) const {
        // kill unused parameter warning
        static_cast<void>(j);
        i -= startIndex_;
        if (i < 0) {
            return endValue_;
        }
        if (i >= (localBi_.rows())) {
            return 0;
        }
        return i >= 0 ? localBi_(i) : 0;
    }
};

/**
 * @brief A class to facilitate state estimation for vehicles in 3D space using B-Splines
 *
 * NOTE by CC:
 * 1. The B-Spline definition in the class is some different to wiki, "The NURBS Book", and MTU course
 *  (a) Basic function N[i,0](u) = 1 in "The NURBS Book", B[i,1](t) = 1 in this definition
 *  (b) Basic function N[i,p](u) is a degree p polynomial in u, but B[i,k](t) is k-1 degree polynomial in this
 * definition, where k is the order of B-Spline, and k = p + 1.
 *  (c) m = n + p + 1 for "The NURBS Book", m = n + k for this. And control points(coefficient) number is n+1, knots
 * number is m+1, spline degree is p
 * 2. Some terminology in this class
 *  (a) B-Spline order: k
 *  (b) B-Spline(Polynomial) degree: k - 1
 *  (c) For clamped(nonperiodic) curve(see Ref[2]), the knots is defined(see Ref[1]) as U = {a,...,a,
 * u[p+1],...,u[m-p-1],b,...,b} = {0,...,0,u[p+1],...,u[m-p-1],1,...,1}. And the first knot and last knot has the
 * multiplicity of p+1, the valid knots is U1 = {0,u[p+1],...,u[m-p-1],1}. I denote the number of knots is m1+1, and the
 * valid number of knots is m+1. The relation meets m1 = n + p + 1 = n + k, and m = m1 - 2p
 *  (d) Valid time segment: m
 *  (e) Knots numbers: m1 + 1 = m + 2(k - 1) + 1 = m + 2k - 1
 *  (f) Coefficient matrix, the same mean of control points matrix
 *  (g) Coefficient number: n + 1
 * 3. When evaluating the B-Spline value at t, only part of control points(ie, coefficient matrix) will active in the
 * calculation, Vk = {V[i-k+1], V[i-k+2],..., V[i]}. The matrix of Vk in code is called local parameters.
 *
 * QUESTIONS:
 * 1. The class use clamped curve in previous discussion, and the knots sequences in class (knots_) should be U, but in
 * the initSpline() function, it seems that the use opened curve.
 *  ANSWER: There are some confuse in the code implementation, this class is implements the clamped curve indeed, but
 * some variable don't seems like this.
 *  (1) The knots should be U = {a,...,a,u[p+1],...,u[m-p-1],b,...,b}, where a = tMin != 0, b = tMax != 1, but in
 * initialization, the first and last k element not the same, like {a - (k-1) * dt, a - (k-2) * dt, ..., a}.
 *  (2) The method `tMin()` and `tMax()` return `a` and `b`
 *  (3) The matrix M(or the basis matrix) don't save the first and last k element because they are zero.
 *
 * I am more familiar with the definition in "The NURBS Book". So, although I have add some comment to this class, but
 * maybe they are not quite correct.
 * Some function about integral I could understand yet.
 *
 * Ref:
 *  [1] L. Piegl and W. Tiller, The NURBS Book, Second Edition. Berlin, Heidelberg: Springer-Verlag, 1997.
 *  [2] K. Qin, “General Matrix Representations for B-Splines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.
 *  [3] MTU, “CS3621 Introduction to Computing with Geometry Notes.” https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/.
 */
class BSpline {
  public:
    /**
     * @brief A base class for BSpline exceptions
     */
    SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

    /**
     * @brief 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
     */
    explicit BSpline(int splineOrder);

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

    /**
     * @brief Get the order of the spline, k
     * @return The order of the spline, k
     */
    inline int splineOrder() const { return splineOrder_; }

    /**
     * @brief Get the degree of polynomial used by the spline, p = k - 1
     * @return The degree of polynomial used by the spline, p = k - 1
     */
    inline int polynomialDegree() const { return splineOrder_ - 1; }

    /**
     * @brief Get the number of coefficients(control points) required for a specified number of valid time segments
     * @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 numTimeSegments) const;

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

    /**
     * @brief Get the number of knots required for a specified number of valid time segments
     * @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 numTimeSegments) const;

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

    /**
     * @brief Get the number of valid time segments for the current knot sequence
     * @return The number of valid time segments for the current knot sequence
     */
    int numValidTimeSegments() const;

    /**
     * @brief Get the basic matrix active on the i-th time segment
     * @param i The index of the time segment
     * @return The basic matrix active on the i-th time segment
     */
    const Eigen::MatrixXd& basisMatrix(int i) const;

    /**
     * @brief Get the minimum time that the spline is well-defined on
     * @return The minimum time that the spline is well-defined on
     */
    const double& tMin() const;

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

    /**
     * @brief Get the time interval that the spline is well-defined on [tMin, tMax)
     * @return The time interval that the spline is well-defined on [tMin, tMax)
     */
    std::pair<double, double> timeInterval() const;

    /**
     * @brief Get the time interval of a single spline segment
     * @param i The index of the time segment
     * @return The time interval of the i-th spline segment
     */
    std::pair<double, double> timeInterval(int i) const;

    /**
     * @brief Set the knots and coefficients of the spline, each column of the coefficient(control points) matrix is
     * interpreted as a single, vector-valued spline coefficient, and then calculate the basic matrix M in each segment
     * @param knots         A non-decreasing knot sequence
     * @param coefficients  A set of spline coefficients(control points)
     */
    void setKnotsAndCoefficients(const std::vector<double>& knots, const Eigen::MatrixXd& coefficients);

    /**
     * @brief Set the knots and coefficients of the spline, each column of the coefficient(control points) matrix is
     * interpreted as a single, vector-valued spline coefficient, and then calculate the basic matrix M in each segment
     * @param knots         A non-decreasing knot vector
     * @param coefficients  A set of spline coefficients(control points)
     */
    void setKnotVectorAndCoefficients(const Eigen::VectorXd& knots, const Eigen::MatrixXd& coefficients);

    /**
     * @brief Set the coefficient(control points) matrix
     * @param coefficients  Coefficient matrix(control points)
     */
    void setCoefficientMatrix(const Eigen::MatrixXd& coefficients);

    /**
     * @brief Sets the coefficient(control points) matrix from the stacked vector of coefficients
     * @param coefficients  The stacked vector of coefficients(control points)
     */
    void setCoefficientVector(const Eigen::VectorXd& coefficients);

    /**
     * @brief Get the knot vector
     * @return Knot vector
     */
    inline const std::vector<double>& knots() const { return knots_; }

    /**
     * @brief Get the knot vector with a column matrix
     * @return Knot vector with a column matrix
     */
    Eigen::VectorXd knotVector() const;

    /**
     * @brief Get the coefficient(control points) matrix, each column of the coefficient matrix is interpreted as a
     * single, vector-valued spline coefficient.
     * @return Coefficient(control points) matrix
     */
    inline const Eigen::MatrixXd& coefficients() const { return coefficients_; }

    /**
     * @brief Get the stacked vector of coefficients(control points) matrix
     * @return Stacked vector of coefficients (control points) matrix
     */
    Eigen::VectorXd coefficientVector();

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

    /**
     * @brief Get the length(number) of total coefficients the spline currently uses
     * @return The length(number) of total coefficients the spline currently uses
     */
    int coefficientVectorLength() const;

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

    /**
     * @brief Get 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. 0 < i < coefficients().cols() = n
     * @return A map to column i of the coefficient matrix.
     */
    Eigen::Map<const Eigen::VectorXd> vvCoefficientVector(int i) const;

    /**
     * @brief Get 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. 0 < i < coefficients().cols() = n
     * @return A map to column i of the coefficient matrix.
     */
    Eigen::Map<Eigen::VectorXd> vvCoefficientVector(int i);

    /**
     * @brief Evaluate the spline curve at time t
     * @param t The time to evaluate the spline curve
     * @return The value of the spline curve at the time t
     */
    Eigen::VectorXd eval(double t) const;

    /**
     * @brief 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
     */
    Eigen::VectorXd evalD(double t, int derivativeOrder) const;

    /**
     * @brief Evaluate the derivative of the spline curve at time t and retrieve the Jacobian of the value with respect
     * to small changed in the parameter vector(control points, coefficient matrix). The Jacobian only refers to the
     * local parameter vector (part of coefficient matrix). The indices of the local parameters with respect to the full
     * parameter 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<Eigen::VectorXd, Eigen::MatrixXd> evalDAndJacobian(double t, int derivativeOrder) const;

    /**
     * @brief Evaluate the derivative of the spline curve at time t and retrieve the Jacobian of the value with respect
     * to small changed in the parameter vector(control points, coefficient matrix). The Jacobian only refers to the
     * local parameter vector (part of coefficient matrix)
     * @param t                     The time to evaluate the spline derivative
     * @param derivativeOrder       The order of the derivative. This must be >= 0
     * @param jacobian              A pointer to the Jacobian matrix to fill in
     * @param coefficientIndices    A pointer to an int vector that will be filled with the local coefficient indices
     * (which index is activated for calculating Jacobian matrix)
     * @return The value of the derivative of the spline curve evaluated at t
     */
    Eigen::VectorXd evalDAndJacobian(double t, int derivativeOrder, Eigen::MatrixXd* jacobian,
                                     Eigen::VectorXi* coefficientIndices) const;

    /**
     * @brief Get the local basic matrix evaluated at the time t. For vector-valued spline coefficients of dimension n1,
     * and a B-Spline of order k, this matrix will be n1 x (kn1).
     *
     * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating Phi(t,O) * localCoefficientVector(t).
     * It's similar to the Jacobian calculation.
     *      C(u) = U * M(i) * V(i) = J * V
     * where V is the stacked vector representation of V(i)
     *
     * @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 t
     */
    Eigen::MatrixXd Phi(double t, int derivativeOrder = 0) const;

    /**
     * @brief Get the local basic matrix evaluated at the time t. For vector-valued spline coefficients of dimension n1,
     * and a B-Spline of order k, this matrix will be n1 x (kn1).
     *
     * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating Phi(t,O) * localCoefficientVector(t)
     *
     * @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 t
     */
    Eigen::MatrixXd localBasisMatrix(double t, int derivativeOrder = 0) const;

    /**
     * @brief Get the local coefficient matrix evaluated at the time t. For vector-valued spline coefficients of
     * dimension n1, and a B-Spline of order k, this matrix will be n1 x k
     * @param t The time being queried
     * @return The local coefficient matrix active at time t
     */
    Eigen::MatrixXd localCoefficientMatrix(double t) const;

    /**
     * @brief Get the local coefficient vector evaluated at the time t. For vector-valued spline coefficients of
     * dimension n1, and a B-Spline of order k, this vector will be k*n1 x 1. 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 t
     */
    Eigen::VectorXd localCoefficientVector(double t) const;

    /**
     * @brief Get the local coefficient vector for segment i
     * @param segmentIdx The segment index, should less than number of valid time segments
     * @return The local coefficient vector active on time segment i
     */
    Eigen::VectorXd segmentCoefficientVector(int segmentIdx) const;

    /**
     * @brief 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
     * @tparam D    The dimension of vector-valued spline coefficients n1
     * @param i     The column of the coefficient matrix to return. 0 <= i < n = coefficients().cols()
     * @return  A map to column i of the coefficient matrix
     */
    template <int D>
    Eigen::Map<Eigen::Matrix<double, D, 1> > fixedSizeVvCoefficientVector(int i) {
        SM_ASSERT_EQ_DBG(Exception, D, coefficients_.rows(),
                         "Size mismatch between requested vector size and actual vector size");
        SM_ASSERT_GE_LT(Exception, i, 0, coefficients_.cols(), "Index out of range");
        return Eigen::Map<Eigen::Matrix<double, D, 1> >(&coefficients_(0, i), coefficients_.rows());
    }

    /**
     * @brief Return a map to a single coefficient column, const version.
     *
     * This allows the user to pass around what is essentially a pointer to a single column in the coefficient matrix
     * @tparam D    The dimension of vector-valued spline coefficients n1
     * @param i     The column of the coefficient matrix to return. 0 <= i < n = coefficients().cols()
     * @return  A map to column i of the coefficient matrix
     */
    template <int D>
    Eigen::Map<const Eigen::Matrix<double, D, 1> > fixedSizeVvCoefficientVector(int i) const {
        SM_ASSERT_EQ_DBG(Exception, D, coefficients_.rows(),
                         "Size mismatch between requested vector size and actual vector size");
        SM_ASSERT_GE_LT(Exception, i, 0, coefficients_.cols(), "Index out of range");
        return Eigen::Map<const Eigen::Matrix<double, D, 1> >(&coefficients_(0, i), coefficients_.rows());
    }

    /**
     * @brief Get the indices of the local coefficients active at time t. Only part of control points(coefficient
     * matrix) is activated in evaluating B-Spline at t, i.e., Vk = {V[i-k+1], V[i-k+2], ... , V[i]}, the indices is the
     * index of every element of Vk with respect to the coefficient matrix, ie, [(i-k+1) * n1, i * n1]
     * @param t The time being queried
     * @return The indices of the local coefficients active at time t
     */
    Eigen::VectorXi localCoefficientVectorIndices(double t) const;

    /**
     * @brief Get the indices of the local coefficients active on segment i
     * @param segmentIdx The segment being queried
     * @return The indices of the local coefficients active on this segment
     */
    Eigen::VectorXi segmentCoefficientVectorIndices(int segmentIdx) const;

    /**
     * @brief Get the indices of the local vector-valued coefficients active at time t. Only part of control
     * points(coefficient matrix) is activated in evaluating B-Spline at t, ie, Vk = {V[i-k+1], V[i-k+2], ... , V[i]},
     * the indices is the row index of Vj with respect to the coefficient matrix. ie, [i-k+1, i]
     * @param t The time being queried
     * @return The indices of the local vector-valued coefficients active at time t
     */
    Eigen::VectorXi localVvCoefficientVectorIndices(double t) const;

    /**
     * @brief Get the indices of the local vector-valued coefficients active on segment i
     * @param segmentIdx The segment being queried
     * @return The indices of the local vector-valued coefficients active at time t
     */
    Eigen::VectorXi segmentVvCoefficientVectorIndices(int segmentIdx) const;

    /**
     * @brief Update the local coefficient vector
     * @param t The time used to select the local coefficients.
     * @param c The local coefficient vector
     */
    void setLocalCoefficientVector(double t, const Eigen::VectorXd& c);

    /**
     * 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$.
     *
     * This solve method is same as "The NURBS Book", but use the matrix representation form in Ref[2]. It construct the
     * linear system Ax = b. Where x is the control points matrix but is implemented in stacked vector, The linear
     * system include the position equation(no derivatives), if k > 2, then add the velocity(first order derivatives)
     * equation. If k > 4, then set all higher order derivatives to zero, and add it to linear system.
     *
     * @param t0 The start of the time interval.
     * @param t1 The end of the time interval
     * @param p0 The position at the start of the time interval.
     * @param p1 The position at the end of the time interval.
     */
    void initSpline(double t0, double t1, const Eigen::VectorXd& p0, const Eigen::VectorXd& p1);

    /**
     * @brief Initialize spline with <t, v> list, where t is the timestamp, v is the vector value evaluted at t. The
     * initialization using global interpolation method(see Ref[1] Section 9.2)
     *
     * The BSpline could be represented in a matrix form like below
     *      C(t) = U(t) * M * V
     * where U(t) * M is implemented in `Phi(t)`, and V is the control(coefficient) matrix. The initialize method
     * construct a linear system Ax = b, where x is the stacked control vector from V; A is U(t) * M, the size is
     * adjusted to meet the size of x; b is the interpolation value C(t).
     *
     * But for this method, the size of <t, v> is less than the knots number, so the number of equation Ax = b is not
     * enough to solve x. The author add other equations to constrain the spline, i.e., assume the second derivativate
     * of spline at each knot to zero.
     *      C''(t) = U''(t) * M * V
     * where U''(t) * M is also implemented in `Phi(t, 2)`. Then the equation is Phi * x = 0, but in implementation, the
     * author add a value `lambda` to set the weight, i.e., lambda * Phi * x = 0. In my option, the lambda could regard
     * as the weight for the whole equation systems, like the information matrix in SLAM optimization.
     *
     * Please see the difference to above equation, the argument is spline at each time, this is spline at each knot.
     *
     * @param times                 Timestamp list
     * @param interpolationPoints   Interpolation points
     * @param numSegments           Number of time segments, it's used to set the knots number.
     * @param lambda                The adjust weight for second derivatives
     */
    void initSpline2(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                     double lambda);

    /**
     * @brief
     *
     * The method is only few difference to `initSpline2`. The second derivatives is assuming to zero in `initSpline2`
     * while the quadratic integral is assume to zero in this method. It's seems like to the bias in IMU model.
     *
     * @param times
     * @param interpolationPoints
     * @param numSegments
     * @param lambda
     * @return
     */
    void initSpline3(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                     double lambda);

    /**
     * @brief Same to `initSpline3` but using sparse calculation
     *
     * @param times
     * @param interpolationPoints
     * @param numSegments
     * @param lambda
     * @return
     */
    void initSplineSparse(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints, int numSegments,
                          double lambda);

    void initSplineSparseKnots(const Eigen::VectorXd& times, const Eigen::MatrixXd& interpolationPoints,
                               const Eigen::VectorXd knots, double 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(const double& t, const Eigen::VectorXd& 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(const double& t, const Eigen::VectorXd& p, const double& lambda);

    /**
     * @brief 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();

    /**
     * @brief Evaluate the integral from t1 to t2
     *
     * NOTE, TODO: some calculation detail don't match to my option, try later
     *
     * @param t1
     * @param t2
     * @return
     */
    Eigen::VectorXd evalIntegral(double t1, double t2) const;

    /**
     * @brief Same as evalIntegral
     *
     * @param t1
     * @param t2
     * @return
     */
    inline Eigen::VectorXd evalI(double t1, double t2) const { return evalIntegral(t1, t2); }

    /**
     * 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 of size k x k
     */
    Eigen::MatrixXd Vi(int segmentIndex) const;

    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Mi(int segmentIndex) const;
    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Bij(int segmentIndex, int columnIndex) const;
    // return matrix of size kn1 x n1
    Eigen::MatrixXd U(double t, int derivativeOrder) const;
    // return matrix of size 1 x k
    Eigen::VectorXd u(double t, int derivativeOrder) const;
    // the index of basic matrix for input time t
    int segmentIndex(double t) const;
    // return matrix of size k x k
    Eigen::MatrixXd Dii(int segmentIndex) const;
    // return matrix of size kn1 x kn1
    Eigen::MatrixXd Di(int segmentIndex) 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] of size k x 1
     *
     */
    Eigen::VectorXd getLocalBiVector(double t) const;
    void getLocalBiInto(double t, Eigen::VectorXd& ret) 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].
     *
     */
    Eigen::VectorXd getLocalCumulativeBiVector(double t) const;

    Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd> getBiVector(double t) const {
        return Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd>(numValidTimeSegments(), 1,
                                                                BiVector(segmentIndex(t), getLocalBiVector(t), 0));
    }

    Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd> getCumulativeBiVector(double t) const {
        return Eigen::CwiseNullaryOp<BiVector, Eigen::VectorXd>(
            numValidTimeSegments(), 1, BiVector(segmentIndex(t), getLocalCumulativeBiVector(t), 1));
    }

    Eigen::MatrixXd segmentIntegral(int segmentIdx, const Eigen::MatrixXd& W, int derivativeOrder) const;

    Eigen::MatrixXd segmentQuadraticIntegral(const Eigen::MatrixXd& W, int segmentIdx, int derivativeOrder) const;
    Eigen::MatrixXd segmentQuadraticIntegralDiag(const Eigen::VectorXd& Wdiag, int segmentIdx,
                                                 int derivativeOrder) const;
    Eigen::MatrixXd curveQuadraticIntegral(const Eigen::MatrixXd& W, int derivativeOrder) const;
    Eigen::MatrixXd curveQuadraticIntegralDiag(const Eigen::VectorXd& Wdiag, int derivativeOrder) const;

    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> curveQuadraticIntegralSparse(const Eigen::MatrixXd& W,
                                                                                int derivativeOrder) const;
    sparse_block_matrix::SparseBlockMatrix<Eigen::MatrixXd> curveQuadraticIntegralDiagSparse(
        const Eigen::VectorXd& Wdiag, int derivativeOrder) const;

    void initConstantSpline(double t_min, double t_max, int numSegments, const Eigen::VectorXd& constant);

  private:
    /**
     * @brief Check the knot sequence is valid, it will throw an exception if it's not valid
     * @param knots The knot sequence to verify
     */
    void verifyKnotSequence(const std::vector<double>& knots);

    /**
     * @brief Initialize the basis matrices M based on the current knot sequence. There is one basis matrix for each
     * valid time segment defined by the spline. See Ref[2]
     *
     * 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();

    /**
     * @brief The recursive function to calculate basis matrix M.
     *           [ M_{k-1}(i) ]        [      0       ]
     *  M_k(i) = [            ] * A +  [              ] * B = M1 * A + M2 * B
     *           [     0      ]        [ [ M_{k-1}(i) ]
     *
     *  M_1(i) = [1]
     *  size of A and B: (k-1) X k
     *
     * Ref: Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k The degree(order) of the matrix
     * @param i The time segment index
     * @return  Basic matrix
     */
    Eigen::MatrixXd M(int k, int i);

    /**
     * @brief A helper function to calculate d0 for producing th basic matrix M.
     *  d(0,j) = (t[i] - t[j]) / (t[j+k-1] - t[j]).
     * Defined in Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k Spline degree(order)
     * @param i Time segment index
     * @param j Time segment index
     * @return  Value d0
     */
    double d0(int k, int i, int j);

    /**
     * @brief A helper function to calculate d1 for producing the M matrices.
     *  d(1,j) = (t[i+1] - t[i]) / (t[j+k-1] - t[j]).
     * Defined in Qin, Kaihuai, General matrix representations for B-splines, The Visual Computer (2000) 16:177–186.
     *
     * @param k Spline degree(order)
     * @param i Time segment index
     * @param j Time segment index
     * @return  Value d1
     */
    double d1(int k, int i, int j);

    /**
     * @brief An internal function to find the segment of the knot sequence that the time t falls in. The function
     * returns the value u = (t - t[i]) / (t[i+1] - t[i]) and the index i
     * @param t The time being queried
     * @return A pair with the first value u = (t - t[i]) / (t[i+1] - t[i]) and the second value the index i
     */
    std::pair<double, int> computeUAndTIndex(double t) const;

    /**
     * @brief 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 dt = t[i+1] - t[i] and the index i
     * @param t The time being queried
     * @return  The pair with the first value dt = t[i+1] -t[i] and the second value the index i
     */
    std::pair<double, int> computeTIndex(double t) const;

    /**
     * @brief Compute the vector U w.r.t t at given derivative order l, this is an k X 1 vector.
     *
     * At derivative order 0 (no derivative), this vector is U(t) = [1, u(t), u(t)^2, ... , u(t)^{k-1}]^T.
     * For higher derivative order n, the vector is U^{(l)}(t) = d^l U(t) / dt^{l}
     *
     * @param u                 The value u(t)
     * @param segmentIndex      The index of t
     * @param derivativeOrder   Derivative order l
     * @return The vector of dU
     */
    Eigen::VectorXd computeU(double u, int segmentIndex, int derivativeOrder) const;

    int basisMatrixIndexFromStartingKnotIndex(int startingKnotIndex) const;
    int startingKnotIndexFromBasisMatrixIndex(int basisMatrixIndex) const;
    const Eigen::MatrixXd& basisMatrixFromKnotIndex(int knotIndex) const;

  private:
    int splineOrder_;            // The order of the spline
    std::vector<double> knots_;  // The knot sequence used by the B-spline
    // The basis matrices M for each time segment, see Ref[2]. The basis matrix don't save all the matrix, only N =
    // valid time segments elements are saved, the first and last k-1 elements are 0
    std::vector<Eigen::MatrixXd> basisMatrices_;

    // The coefficient matrix(control points) used by the B-Spline. Each column can be seen as a single vector-valued
    // spline coefficient. This is stored explicitly 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.
    // If the control points is V = [V_0, V_1,.., V_n], consider Vj is size of n1 x 1, then the size of V is
    // n1 x (n+1)
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> coefficients_;
};

}  // namespace bsplines

#endif /* _BSPLINE_HPP */


/*
BSD 2-Clause License

Copyright (c) 2019, Oscar Riveros - www.peqnp.science.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

2. 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.

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.
 */

#include <iostream>
#include <random>

#include <boost/mpi/environment.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/collectives.hpp>

namespace mpi = boost::mpi;

typedef unsigned long long int natural;
typedef long double real;

template<class T, typename F>
class cluster {
public:
    cluster(const mpi::communicator &world, F functor) : __world(world), __functor(functor), __n(world.rank()), __m(world.size()) {}

    T operator()() {
        __n += __m;
        return __functor(__n - __m);
    }

private:
    const mpi::communicator &__world;
    const F __functor;
    T __n;
    T __m;
};

template<typename T>
T scale(const mpi::communicator &world, const T length) {
    return length / world.size() + world.size();
}

template<typename T, typename F>
std::vector<T> parallelize(const mpi::communicator &world, std::vector<T> &vs, F functor) {
    std::generate(vs.begin(), vs.end(), cluster<T, F>(world, functor));
    return vs;
}

template<typename T>
T accumulate(const mpi::communicator &world, std::vector<T> &vs, const T size) {
    real pi(0);
    mpi::all_reduce(world, 4 * std::accumulate(vs.begin(), vs.end(), real(0), std::plus<>()) / size, pi, std::plus<>());
    return pi;
}

int main() {
    mpi::environment env;
    mpi::communicator world;

    const natural size = 1000000;

    std::vector<real> vs(scale<natural>(world, size));

    std::random_device device;
    std::uniform_real_distribution<> distribution(0, 1);

    parallelize<real>(world, vs, [&](auto) {
        auto x = 2 * distribution(device) - 1;
        auto y = 2 * distribution(device) - 1;
        return std::pow(x, 2) + std::pow(y, 2) < 1;
    });

    auto pi = accumulate<real>(world, vs, size);

    if (!world.rank()) {
        std::cout << pi << std::endl;
    }

    return EXIT_SUCCESS;
}


// Copyright Paul A. Bristow 2017
// Copyright John Z. Maddock 2017

// Distributed under 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).

/*! \brief Graph showing use of Lambert W function.

\details

Both Lambert W0 and W-1 branches can be shown on one graph.
But useful to have another graph for larger values of argument z.
Need two separate graphs for Lambert W0 and -1 prime because
the sensible ranges and axes are too different.

One would get too small LambertW0 in top right and W-1 in bottom left.

*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;
using boost::math::lambert_wm1;
using boost::math::lambert_w0_prime;
using boost::math::lambert_wm1_prime;

#include <boost/math/special_functions.hpp>
using boost::math::isfinite;
#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;
#include <boost/svg_plot/show_2d_settings.hpp>
using boost::svg::show_2d_plot_settings;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>
#include <utility>
using std::pair;
#include <map>
using std::map;
#include <set>
using std::multiset;
#include <limits>
using std::numeric_limits;
#include <cmath> //

  /*!
  */
int main()
{
  try
  {
    std::cout << "Lambert W graph example." << std::endl;

//[lambert_w_graph_1
//] [/lambert_w_graph_1]
    {
      std::map<const double, double> wm1s;   // Lambert W-1 branch values.
      std::map<const double, double> w0s;   // Lambert W0 branch values.

      std::cout.precision(std::numeric_limits<double>::max_digits10);

      int count = 0;
      for (double z = -0.36787944117144232159552377016146086744581113103176804; z < 2.8; z += 0.001)
      {
        double w0 = lambert_w0(z);
        w0s[z] = w0;
   //     std::cout << "z " << z << ", w = " << w0 << std::endl;
        count++;
      }
      std::cout << "points " << count << std::endl;

      count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < -0.001; z += 0.001)
      {
        double wm1 = lambert_wm1(z);
        wm1s[z] = wm1;
        count++;
      }
      std::cout << "points " << count << std::endl;

      svg_2d_plot data_plot;
      data_plot.title("Lambert W function.")
        .x_size(400)
        .y_size(300)
        .legend_on(true)
        .legend_lines(true)
        .x_label("z")
        .y_label("W")
        .x_range(-1, 3.)
        .y_range(-4., +1.)
        .x_major_interval(1.)
        .y_major_interval(1.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        //.x_values_on(true)
        //.y_values_on(true)
        .y_values_rotation(horizontal)
        //.plot_window_on(true)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        //.background_border_color(black);
        ;
      data_plot.plot(w0s, "W0 branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plot.plot(wm1s, "W-1 branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plot.write("./lambert_w_graph");

      show_2d_plot_settings(data_plot); // For plot diagnosis only.

    } // small z Lambert W

    {  // bigger argument z Lambert W

      std::map<const double, double> w0s_big;   // Lambert W0 branch values for large z and W.
      std::map<const double, double> wm1s_big;   // Lambert W-1 branch values for small z and large -W.
      int count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < 10000.; z += 50.)
      {
        double w0 = lambert_w0(z);
        w0s_big[z] = w0;
        count++;
      }
      std::cout << "points " << count << std::endl;

      count = 0;
      for (double z = -0.3678794411714423215955237701614608727; z < -0.001; z += 0.001)
      {
        double wm1 = lambert_wm1(z);
        wm1s_big[z] = wm1;
        count++;
      }
     std::cout << "Lambert W0 large z argument points = " << count << std::endl;

     svg_2d_plot data_plot2;
     data_plot2.title("Lambert W0 function for larger z.")
      .x_size(400)
      .y_size(300)
      .legend_on(false)
      .x_label("z")
      .y_label("W")
      //.x_label_on(true)
      //.y_label_on(true)
      //.xy_values_on(false)
      .x_range(-1, 10000.)
      .y_range(-1., +8.)
      .x_major_interval(2000.)
      .y_major_interval(1.)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      //.x_values_on(true)
      //.y_values_on(true)
      .y_values_rotation(horizontal)
      //.plot_window_on(true)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2018")
      //.background_border_color(black);
    ;

    data_plot2.plot(w0s_big, "W0 branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
    // data_plot2.plot(wm1s_big, "W-1 branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
    // This wouldn't show anything useful.
    data_plot2.write("./lambert_w_graph_big_w");
   } // Big argument z Lambert W

    { //  Lambert W0 Derivative plots

    //  std::map<const double, double> wm1ps;   // Lambert W-1 prime branch values.
      std::map<const double, double> w0ps;   // Lambert W0 prime branch values.

      std::cout.precision(std::numeric_limits<double>::max_digits10);

      int count = 0;
      for (double z = -0.36; z < 3.; z += 0.001)
      {
        double w0p = lambert_w0_prime(z);
        w0ps[z] = w0p;
        // std::cout << "z " << z << ", w0 = " << w0 << std::endl;
        count++;
      }
      std::cout << "points " << count << std::endl;

      //count = 0;
      //for (double z = -0.36; z < -0.1; z += 0.001)
      //{
      //  double wm1p = lambert_wm1_prime(z);
      //  std::cout << "z " << z << ", w-1 = " << wm1p << std::endl;
      //  wm1ps[z] = wm1p;
      //  count++;
      //}
      //std::cout << "points " << count << std::endl;

      svg_2d_plot data_plotp;
      data_plotp.title("Lambert W0 prime function.")
        .x_size(400)
        .y_size(300)
        .legend_on(false)
        .x_label("z")
        .y_label("W0'")
        .x_range(-0.3, +1.)
        .y_range(0., +5.)
        .x_major_interval(0.2)
        .y_major_interval(2.)
        .x_major_grid_on(true)
        .y_major_grid_on(true)
        .y_values_rotation(horizontal)
        .x_values_precision(3)
        .y_values_precision(3)
        .coord_precision(4) // Needed to avoid stepping on curves.
        .copyright_holder("Paul A. Bristow")
        .copyright_date("2018")
        ;

      // derivative of N[productlog(0, x), 55]  at x=0 to 10
      // Plot[D[N[ProductLog[0, x], 55], x], {x, 0, 10}]
      // Plot[ProductLog[x]/(x + x ProductLog[x]), {x, 0, 10}]
      data_plotp.plot(w0ps, "W0 prime branch").line_color(red).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plotp.write("./lambert_w0_prime_graph");
  } // Lambert W0 Derivative plots

    { //  Lambert Wm1 Derivative plots

    std::map<const double, double> wm1ps;   // Lambert W-1 prime branch values.

    std::cout.precision(std::numeric_limits<double>::max_digits10);

    int count = 0;
    for (double z = -0.3678; z < -0.00001; z += 0.001)
    {
      double wm1p = lambert_wm1_prime(z);
      // std::cout << "z " << z << ", w-1 = " << wm1p << std::endl;
      wm1ps[z] = wm1p;
      count++;
    }
    std::cout << "Lambert W-1 prime points = " << count << std::endl;

    svg_2d_plot data_plotp;
    data_plotp.title("Lambert W-1 prime function.")
      .x_size(400)
      .y_size(300)
      .legend_on(false)
      .x_label("z")
      .y_label("W-1'")
      .x_range(-0.4, +0.01)
      .x_major_interval(0.1)
      .y_range(-20., -5.)
      .y_major_interval(5.)
      .x_major_grid_on(true)
      .y_major_grid_on(true)
      .y_values_rotation(horizontal)
      .x_values_precision(3)
      .y_values_precision(3)
      .coord_precision(4) // Needed to avoid stepping on curves.
      .copyright_holder("Paul A. Bristow")
      .copyright_date("2018")
      ;

      // derivative of N[productlog(0, x), 55]  at x=0 to 10
      // Plot[D[N[ProductLog[0, x], 55], x], {x, 0, 10}]
      // Plot[ProductLog[x]/(x + x ProductLog[x]), {x, 0, 10}]
      data_plotp.plot(wm1ps, "W-1 prime branch").line_color(blue).shape(none).line_on(true).bezier_on(false).line_width(1);
      data_plotp.write("./lambert_wm1_prime_graph");
    } // Lambert W-1 prime graph
 } // try
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}  // int main()

   /*

   //[lambert_w_graph_1_output

   //] [/lambert_w_graph_1_output]
   */


//  Copyright 2015 Stephan Menzel. Distributed under 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)

#include <tools/Random.hpp>

#include <boost/thread.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>

#include <iostream>

struct door {
	door() : m_win(false), m_open(false) {};
	bool m_win;
	bool m_open;
};

enum strategy {
	STUBBORN = 0,  // will stick to original choice upon Monty revealing the goat
	MINDCHANGER,   // will always change his mind to the remaining door
	UNDECIDED      // reacts randomly
};

//! \return win or loose
bool round(const strategy n_strategy) {

	struct door doors[3];

	// First assign the door to win the car
	doors[moose::tools::urand(2)].m_win = true;

	// Now our player selects a door randomly
	unsigned int choice = moose::tools::urand(2);

	// Monty selects another random door with a goat to open
	unsigned int idx = 0;
	do {
		idx = moose::tools::urand(2);
	} while (doors[idx].m_win || (idx == choice));
						// we need a random index that looses
						// and was not the player's choice
	doors[idx].m_open = true;

	switch (n_strategy) {
		case STUBBORN:
			// player does not change his mind but sticks with his choice
			break;
		case UNDECIDED:
			// undecided player has a 50% chance to change his mind, otherwise
			// bail too and stick with his choice
			if (moose::tools::urand(100) < 50) {
				break;
			}
		case MINDCHANGER:
			// player always changes his mind and changes
			// his choice to the remaining door
			for (unsigned int i = 0; i < 3; ++i) {
				if (!doors[i].m_open && (i != choice)) {
					choice = i;
					break;
				}
			}
			break;
	}

	// player does not change his mind but sticks with his choice
	return doors[choice].m_win;
}

// return ratio of wins in percent
void player(double &n_result, const strategy n_strategy, const unsigned int n_rounds) {

	using namespace boost::accumulators;

	// accumulate all the results
	accumulator_set<double, stats<tag::mean> > acc;

	// play as many rounds and accumulate wins and loosses
	for (unsigned int i = 0; i < n_rounds; ++i) {

		bool result = round(n_strategy);
		acc(result ? 100.0 : 0.0);
	}

	n_result = mean(acc);
}

int main(int argc, char **argv) {

	unsigned int rounds = 1000000;

	double stubborn_wins = 0.0;
	double mindchanger_wins = 0.0;
	double undecided_wins = 0.0;

	std::cout << "players going to work..." << std::endl;

	boost::thread sp = boost::thread(boost::bind(&player, boost::ref(stubborn_wins),    STUBBORN,    rounds));
	boost::thread mp = boost::thread(boost::bind(&player, boost::ref(mindchanger_wins), MINDCHANGER, rounds));
	boost::thread up = boost::thread(boost::bind(&player, boost::ref(undecided_wins),   UNDECIDED,   rounds));

	sp.join();
	mp.join();
	up.join();

	std::cout << "Stubborn player won " << stubborn_wins << " percent of his " << rounds << " rounds." << std::endl;
	std::cout << "Mindchanger player won " << mindchanger_wins << " percent of his " << rounds << " rounds." << std::endl;
	std::cout << "Undecided player won " << undecided_wins << " percent of his " << rounds << " rounds." << std::endl;
}


#include <array>
#include <chrono>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include "bicycle/whipple.h"
#include "parameters.h"

namespace {
    const double fs = 200; // sample rate [Hz]
    const double dt = 1.0/fs; // sample time [s]
    const double v0 = 4.0; // forward speed [m/s]
    const size_t N = 1000; // length of simulation in samples

    std::array<model::BicycleWhipple::state_t, N> continuous_time_system_state_n;
    std::array<model::BicycleWhipple::state_t, N> continuous_time_system_state_0;
    std::array<model::BicycleWhipple::state_t, N> discrete_time_system_state_n;
    std::array<model::BicycleWhipple::state_t, N> discrete_time_system_state_0;
} // namespace

int main(int argc, char* argv[]) {
    (void)argc;
    (void)argv;

    model::BicycleWhipple bicycle(v0, dt);

    std::chrono::time_point<std::chrono::system_clock> cont_start, cont_stop;
    std::chrono::time_point<std::chrono::system_clock> disc_start, disc_stop;

    cont_start = std::chrono::system_clock::now();
    bicycle.set_v_dt(v0, 0);
    cont_stop = std::chrono::system_clock::now();

    disc_start = std::chrono::system_clock::now();
    bicycle.set_v_dt(v0, dt);
    disc_stop = std::chrono::system_clock::now();

    std::chrono::duration<double> cont_time = cont_stop - cont_start;
    std::chrono::duration<double> disc_time = disc_stop - disc_start;
    disc_time -= cont_time;
    std::cout << "time for continuous state space computation: " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time).count() <<
        " us" << std::endl;
    std::cout << "(additional) time for discrete state space computation: " <<
        std::chrono::duration_cast<std::chrono::microseconds>(disc_time).count() <<
        " us" << std::endl;

    std::cout << "M: " << std::endl << bicycle.M() << std::endl;
    std::cout << "C1: " << std::endl << bicycle.C1() << std::endl;
    std::cout << "K0: " << std::endl << bicycle.K0() << std::endl;
    std::cout << "K2: " << std::endl << bicycle.K2() << std::endl << std::endl;

    std::cout << "for v = " << bicycle.v() << " m/s" << std::endl;
    std::cout << "A: " << std::endl << bicycle.A() << std::endl;
    std::cout << "B: " << std::endl << bicycle.B() << std::endl << std::endl;

    std::cout << "for fs = "  << fs << "  Hz" << std::endl;
    std::cout << "Ad: " << std::endl << bicycle.Ad() << std::endl;
    std::cout << "Bd: " << std::endl << bicycle.Bd() << std::endl << std::endl;

    model::BicycleWhipple::state_t x, x0;
    x0 << 0, 0, 10, 10, 0; // define in degrees
    x0 *= constants::as_radians;

    std::cout << "initial state: [" << x0.transpose() << "]' rad" << std::endl;
    std::cout << "states are: [yaw angle, roll angle, steer angle, roll rate, steer rate]'" << std::endl << std::endl;

    std::cout << "simulating (no input) continuous time system at constant speed..." << std::endl;
    x = x0;
    for (auto& state: continuous_time_system_state_n) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }

    std::cout << "simulating (zero input) continuous time system at constant speed..." << std::endl;
    cont_start = std::chrono::system_clock::now();
    x = x0;
    for (auto& state: continuous_time_system_state_0) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }
    cont_stop = std::chrono::system_clock::now();

    std::cout << "simulating (no input) discrete time system at constant speed..." << std::endl;
    x = x0;
    for (auto& state: discrete_time_system_state_n) {
        state = bicycle.update_state(x);
        x = state;
    }

    std::cout << "simulating (zero input) discrete time system at constant speed..." << std::endl;
    disc_start = std::chrono::system_clock::now();
    x = x0;
    for (auto& state: discrete_time_system_state_0) {
        state = bicycle.update_state(x, model::BicycleWhipple::input_t::Zero());
        x = state;
    }
    disc_stop = std::chrono::system_clock::now();

    std::cout << std::endl;
    std::cout << "state at end of simulation (" << N << " steps)" << std::endl;
    std::cout << "continuous time (no input):   " << continuous_time_system_state_n.back().transpose() << std::endl;
    std::cout << "continuous time (zero input): " << continuous_time_system_state_0.back().transpose() << std::endl;
    std::cout << "discrete time (no input):     " << discrete_time_system_state_n.back().transpose() << std::endl;
    std::cout << "discrete time (zero input):   " << discrete_time_system_state_0.back().transpose() << std::endl;
    std::cout << std::endl;

    cont_time = cont_stop - cont_start;
    disc_time = disc_stop - disc_start;
    std::cout << "simulation time (zero input form)" << std::endl;
    std::cout << "continuous: Tc = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time).count() <<
        " us" << std::endl;
    std::cout << "discrete: Td = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(disc_time).count() <<
        " us" << std::endl;
    std::cout << "Tc - Td = " <<
        std::chrono::duration_cast<std::chrono::microseconds>(cont_time - disc_time).count() <<
        " us" << std::endl;

    return EXIT_SUCCESS;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

int main(int argc, char** argv)
{

  // This program illustrates a simple use of boyer_myrvold_planar_embedding
  // as a simple yes/no test for planarity.

  using namespace boost;

  typedef adjacency_list<vecS,
                         vecS,
                         undirectedS,
                         property<vertex_index_t, int>
                         > graph;

  graph K_4(4);
  add_edge(0, 1, K_4);
  add_edge(0, 2, K_4);
  add_edge(0, 3, K_4);
  add_edge(1, 2, K_4);
  add_edge(1, 3, K_4);
  add_edge(2, 3, K_4);

  if (boyer_myrvold_planarity_test(K_4))
    std::cout << "K_4 is planar." << std::endl;
  else
    std::cout << "ERROR! K_4 should have been recognized as planar!"
          << std::endl;

  graph K_5(5);
  add_edge(0, 1, K_5);
  add_edge(0, 2, K_5);
  add_edge(0, 3, K_5);
  add_edge(0, 4, K_5);
  add_edge(1, 2, K_5);
  add_edge(1, 3, K_5);
  add_edge(1, 4, K_5);
  add_edge(2, 3, K_5);
  add_edge(2, 4, K_5);

  // We've almost created a K_5 - it's missing one edge - so it should still
  // be planar at this point.

  if (boyer_myrvold_planarity_test(K_5))
    std::cout << "K_5 (minus an edge) is planar." << std::endl;
  else
    std::cout << "ERROR! K_5 with one edge missing should"
              << " have been recognized as planar!" << std::endl;

  // Now add the final edge...
  add_edge(3, 4, K_5);

  if (boyer_myrvold_planarity_test(K_5))
    std::cout << "ERROR! K_5 was recognized as planar!" << std::endl;
  else
    std::cout << "K_5 is not planar." << std::endl;

  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3Q.cpp
 * @brief   Rotation (internal: quaternion representation*)
 * @author  Richard Roberts
 */

#ifdef GTSAM_DEFAULT_QUATERNIONS

#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>

using namespace std;

namespace gtsam {

	static const Matrix I3 = eye(3);

  /* ************************************************************************* */
	Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}

  /* ************************************************************************* */
	Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
      quaternion_((Eigen::Matrix3d() <<
          r1.x(), r2.x(), r3.x(),
          r1.y(), r2.y(), r3.y(),
          r1.z(), r2.z(), r3.z()).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(double R11, double R12, double R13,
      double R21, double R22, double R23,
      double R31, double R32, double R33) :
        quaternion_((Eigen::Matrix3d() <<
            R11, R12, R13,
            R21, R22, R23,
            R31, R32, R33).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix& R) :
      quaternion_(Eigen::Matrix3d(R)) {}

//  /* ************************************************************************* */
//   Rot3::Rot3(const Matrix3& R) :
//       quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}

  /* ************************************************************************* */
  Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

  /* ************************************************************************* */
  Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }

  /* ************************************************************************* */
  Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }

  /* ************************************************************************* */
  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
      Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
      Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
      Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
  }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w, double theta) {
    return Quaternion(Eigen::AngleAxisd(theta, w)); }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w) {
    double t = w.norm();
    if (t < 1e-10) return Rot3();
    return rodriguez(w/t, t);
  }

  /* ************************************************************************* */
  bool Rot3::equals(const Rot3 & R, double tol) const {
    return equal_with_abs_tol(matrix(), R.matrix(), tol);
  }

  /* ************************************************************************* */
  Rot3 Rot3::compose(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = R2.transpose();
    if (H2) *H2 = I3;
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Point3 Rot3::operator*(const Point3& p) const {
    Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
    return Point3(r(0), r(1), r(2));
  }

  /* ************************************************************************* */
  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
    if (H1) *H1 = -matrix();
    return Rot3(quaternion_.inverse());
  }

  /* ************************************************************************* */
  Rot3 Rot3::between(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = -(R2.transpose()*matrix());
    if (H2) *H2 = I3;
    return between_default(*this, R2);
  }

  /* ************************************************************************* */
  Rot3 Rot3::operator*(const Rot3& R2) const {
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Point3 Rot3::rotate(const Point3& p,
        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
    Matrix R = matrix();
    if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    if (H2) *H2 = R;
    Eigen::Vector3d r = R * p.vector();
    return Point3(r.x(), r.y(), r.z());
  }

  /* ************************************************************************* */
  // see doc/math.lyx, SO(3) section
  Point3 Rot3::unrotate(const Point3& p,
      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    const Matrix Rt(transpose());
    Point3 q(Rt*p.vector()); // q = Rt*p
    if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
    if (H2) *H2 = Rt;
    return q;
  }

  /* ************************************************************************* */
  // Log map at identity - return the canonical coordinates of this rotation
  Vector3 Rot3::Logmap(const Rot3& R) {
    Eigen::AngleAxisd angleAxis(R.quaternion_);
    if(angleAxis.angle() > M_PI)      // Important:  use the smallest possible
      angleAxis.angle() -= 2.0*M_PI;  // angle, e.g. no more than PI, to keep
    if(angleAxis.angle() < -M_PI)     // error continuous.
      angleAxis.angle() += 2.0*M_PI;
    return angleAxis.axis() * angleAxis.angle();
  }

  /* ************************************************************************* */
	Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
		return compose(Expmap(omega));
	}

	/* ************************************************************************* */
	Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
		return Logmap(between(t2));
	}

  /* ************************************************************************* */
  Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); }

  /* ************************************************************************* */
  Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }

  /* ************************************************************************* */
  Point3 Rot3::column(int index) const{
    if(index == 3)
      return r3();
    else if(index == 2)
      return r2();
    else if(index == 1)
      return r1(); // default returns r1
    else
      throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
  }

  /* ************************************************************************* */
  Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

  /* ************************************************************************* */
  Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }

  /* ************************************************************************* */
  Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }

  /* ************************************************************************* */
  Vector3 Rot3::xyz() const {
    Matrix I;Vector3 q;
    boost::tie(I,q)=RQ(matrix());
    return q;
  }

  /* ************************************************************************* */
  Vector3 Rot3::ypr() const {
  	Vector3 q = xyz();
    return Vector3(q(2),q(1),q(0));
  }

  /* ************************************************************************* */
  Vector3 Rot3::rpy() const {
  	Vector3 q = xyz();
    return Vector3(q(0),q(1),q(2));
  }

  /* ************************************************************************* */
  Quaternion Rot3::toQuaternion() const { return quaternion_; }

  /* ************************************************************************* */
  pair<Matrix3, Vector3> RQ(const Matrix3& A) {

    double x = -atan2(-A(2, 1), A(2, 2));
    Rot3 Qx = Rot3::Rx(-x);
    Matrix3 B = A * Qx.matrix();

    double y = -atan2(B(2, 0), B(2, 2));
    Rot3 Qy = Rot3::Ry(-y);
    Matrix3 C = B * Qy.matrix();

    double z = -atan2(-C(1, 0), C(1, 1));
    Rot3 Qz = Rot3::Rz(-z);
    Matrix3 R = C * Qz.matrix();

    Vector xyz = Vector3(x, y, z);
    return make_pair(R, xyz);
  }

} // namespace gtsam

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the linear model predictive
    control tool from the dlib C++ Library.  To explain what it does, suppose
    you have some process you want to control and the process dynamics are
    described by the linear equation:
        x_{i+1} = A*x_i + B*u_i + C
    That is, the next state the system goes into is a linear function of its
    current state (x_i) and the current control (u_i) plus some constant bias or
    disturbance.

    A model predictive controller can find the control (u) you should apply to
    drive the state (x) to some reference value, which is what we show in this
    example.  In particular, we will simulate a simple vehicle moving around in
    a planet's gravity.  We will use MPC to get the vehicle to fly to and then
    hover at a certain point in the air.

*/

#include <dlib/gui_widgets.h>
#include <dlib/control.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

//  ----------------------------------------------------------------------------

int main()
{
    const int STATES = 4;
    const int CONTROLS = 2;

    // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
    // Our state space (the x) will have 4 dimensions, the 2D vehicle position
    // and also the 2D velocity.  The control space (u) will be just 2 variables
    // which encode the amount of force we apply to the vehicle along each axis.
    // Therefore, the A matrix defines a simple constant velocity model.
    matrix<double,STATES,STATES> A;
    A = 1, 0, 1, 0,  // next_pos = pos + velocity
        0, 1, 0, 1,  // next_pos = pos + velocity
        0, 0, 1, 0,  // next_velocity = velocity
        0, 0, 0, 1;  // next_velocity = velocity

    // Here we say that the control variables effect only the velocity. That is,
    // the control applies an acceleration to the vehicle.
    matrix<double,STATES,CONTROLS> B;
    B = 0, 0,
        0, 0,
        1, 0,
        0, 1;

    // Let's also say there is a small constant acceleration in one direction.
    // This is the force of gravity in our model.
    matrix<double,STATES,1> C;
    C = 0,
        0,
        0,
        0.1;

    const int HORIZON = 30;
    // Now we need to setup some MPC specific parameters.  To understand them,
    // let's first talk about how MPC works.  When the MPC tool finds the "best"
    // control to apply it does it by simulating the process for HORIZON time
    // steps and selecting the control that leads to the best performance over
    // the next HORIZON steps.
    //
    // To be precise, each time you ask it for a control, it solves the
    // following quadratic program:
    //
    //     min     sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i
    //    x_i,u_i
    //
    //     such that: x_0     == current_state
    //                x_{i+1} == A*x_i + B*u_i + C
    //                lower <= u_i <= upper
    //                0 <= i < HORIZON
    //
    // and reports u_0 as the control you should take given that you are currently
    // in current_state.  Q and R are user supplied matrices that define how we
    // penalize variations away from the target state as well as how much we want
    // to avoid generating large control signals.  We also allow you to specify
    // upper and lower bound constraints on the controls.  The next few lines
    // define these parameters for our simple example.

    matrix<double,STATES,1> Q;
    // Setup Q so that the MPC only cares about matching the target position and
    // ignores the velocity.
    Q = 1, 1, 0, 0;

    matrix<double,CONTROLS,1> R, lower, upper;
    R = 1, 1;
    lower = -0.5, -0.5;
    upper =  0.5,  0.5;

    // Finally, create the MPC controller.
    mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);

    // Let's tell the controller to send our vehicle to a random location.  It
    // will try to find the controls that makes the vehicle just hover at this
    // target position.
    dlib::rand rnd;
    matrix<double,STATES,1> target;
    target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
    controller.set_target(target);

    // Now let's start simulating our vehicle.  Our vehicle moves around inside
    // a 400x400 unit sized world.
    matrix<rgb_pixel> world(400,400);
    image_window win;
    matrix<double,STATES,1> current_state;
    // And we start it at the center of the world with zero velocity.
    current_state = 200,200,0,0;

    int iter = 0;
    while(!win.is_closed())
    {
        // Find the best control action given our current state.
        matrix<double,CONTROLS,1> action = controller(current_state);
        cout << "best control: " << trans(action);

        // Now draw our vehicle on the world.  We will draw the vehicle as a
        // black circle and its target position as a green circle.
        assign_all_pixels(world, rgb_pixel(255,255,255));
        const dpoint pos = point(current_state(0),current_state(1));
        const dpoint goal = point(target(0),target(1));
        draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
        draw_solid_circle(world, pos, 7, 0);
        // We will also draw the control as a line showing which direction the
        // vehicle's thruster is firing.
        draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
        win.set_image(world);

        // Take a step in the simulation
        current_state = A*current_state + B*action + C;
        dlib::sleep(100);

        // Every 100 iterations change the target to some other random location.
        ++iter;
        if (iter > 100)
        {
            iter = 0;
            target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
            controller.set_target(target);
        }
    }
}

//  ----------------------------------------------------------------------------


// Copyright 2020 Xanadu Quantum Technologies Inc.

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
 * @file
 * \rst
 * Contains tensor representations of supported gates in ``lightning.qubit``.
 * \endrst
 */
#pragma once

#define _USE_MATH_DEFINES

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

using Eigen::MatrixXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;
using Eigen::Tensor;

using State_1q = Eigen::Tensor<std::complex<double>, 1>;
using State_2q = Eigen::Tensor<std::complex<double>, 2>;
using State_3q = Eigen::Tensor<std::complex<double>, 3>;

using Gate_1q = Eigen::Tensor<std::complex<double>, 2>;
using Gate_2q = Eigen::Tensor<std::complex<double>, 4>;
using Gate_3q = Eigen::Tensor<std::complex<double>, 6>;

using Pairs = Eigen::IndexPair<int>;
using Pairs_1q = Eigen::array<Pairs, 1>;
using Pairs_2q = Eigen::array<Pairs, 2>;

const double SQRT_2 = sqrt(2);
const std::complex<double> IMAG(0, 1);
const std::complex<double> NEGATIVE_IMAG(0, -1);

/**
* Generates the identity gate.
*
* @return the identity tensor
*/
Gate_1q Identity() {
    Gate_1q X(2, 2);
    X.setValues({{1, 0}, {0, 1}});
    return X;
}

/**
* Generates the X gate.
*
* @return the X tensor
*/
Gate_1q X() {
    Gate_1q X(2, 2);
    X.setValues({{0, 1}, {1, 0}});
    return X;
}

/**
* Generates the Y gate.
*
* @return the Y tensor
*/
Gate_1q Y() {
    Gate_1q Y(2, 2);
    Y.setValues({{0, NEGATIVE_IMAG}, {IMAG, 0}});
    return Y;
}

/**
* Generates the Z gate.
*
* @return the Z tensor
*/
Gate_1q Z() {
    Gate_1q Z(2, 2);
    Z.setValues({{1, 0}, {0, -1}});
    return Z;
}

/**
* Generates the H gate.
*
* @return the H tensor
*/
Gate_1q H() {
    Gate_1q H(2, 2);
    H.setValues({{1/SQRT_2, 1/SQRT_2}, {1/SQRT_2, -1/SQRT_2}});
    return H;
}

/**
* Generates the S gate.
*
* @return the S tensor
*/
Gate_1q S() {
    Gate_1q S(2, 2);
    S.setValues({{1, 0}, {0, IMAG}});
    return S;
}

/**
* Generates the T gate.
*
* @return the T tensor
*/
Gate_1q T() {
    Gate_1q T(2, 2);

    const std::complex<double> exponent(0, M_PI/4);
    T.setValues({{1, 0}, {0, std::pow(M_E, exponent)}});
    return T;
}

/**
* Generates the X rotation gate.
*
* @param parameter the rotation angle
* @return the RX tensor
*/
Gate_1q RX(const double& parameter) {
    Gate_1q RX(2, 2);

    const std::complex<double> c (std::cos(parameter / 2), 0);
    const std::complex<double> js (0, std::sin(-parameter / 2));

    RX.setValues({{c, js}, {js, c}});
    return RX;
}

/**
* Generates the Y rotation gate.
*
* @param parameter the rotation angle
* @return the RY tensor
*/
Gate_1q RY(const double& parameter) {
    Gate_1q RY(2, 2);

    const double c = std::cos(parameter / 2);
    const double s = std::sin(parameter / 2);

    RY.setValues({{c, -s}, {s, c}});
    return RY;
}

/**
* Generates the Z rotation gate.
*
* @param parameter the rotation angle
* @return the RZ tensor
*/
Gate_1q RZ(const double& parameter) {
    Gate_1q RZ(2, 2);

    const std::complex<double> exponent(0, -parameter/2);
    const std::complex<double> exponent_second(0, parameter/2);
    const std::complex<double> first = std::pow(M_E, exponent);
    const std::complex<double> second = std::pow(M_E, exponent_second);

    RZ.setValues({{first, 0}, {0, second}});
    return RZ;
}

/**
* Generates the phase-shift gate.
*
* @param parameter the phase shift
* @return the phase-shift tensor
*/
Gate_1q PhaseShift(const double& parameter) {
    Gate_1q PhaseShift(2, 2);

    const std::complex<double> exponent(0, parameter);
    const std::complex<double> shift = std::pow(M_E, exponent);

    PhaseShift.setValues({{1, 0}, {0, shift}});
    return PhaseShift;
}

/**
* Generates the arbitrary single qubit rotation gate.
*
* The rotation is achieved through three separate rotations:
* \f$R(\phi, \theta, \omega)= RZ(\omega)RY(\theta)RZ(\phi)\f$.
*
* @param phi the first rotation angle
* @param theta the second rotation angle
* @param omega the third rotation angle
* @return the rotation tensor
*/
Gate_1q Rot(const double& phi, const double& theta, const double& omega) {
    Gate_1q Rot(2, 2);

    const std::complex<double> e00(0, (-phi - omega)/2);
    const std::complex<double> e10(0, (-phi + omega)/2);
    const std::complex<double> e01(0, (phi - omega)/2);
    const std::complex<double> e11(0, (phi + omega)/2);

    const std::complex<double> exp00 = std::pow(M_E, e00);
    const std::complex<double> exp10 = std::pow(M_E, e10);
    const std::complex<double> exp01 = std::pow(M_E, e01);
    const std::complex<double> exp11 = std::pow(M_E, e11);

    const double c = std::cos(theta / 2);
    const double s = std::sin(theta / 2);

    Rot.setValues({{exp00 * c, -exp01 * s}, {exp10 * s, exp11 * c}});

    return Rot;
}

/**
* Generates the CNOT gate.
*
* @return the CNOT tensor
*/
Gate_2q CNOT() {
    Gate_2q CNOT(2,2,2,2);
    CNOT.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{1, 0}}}});
    return CNOT;
}

/**
* Generates the SWAP gate.
*
* @return the SWAP tensor
*/
Gate_2q SWAP() {
    Gate_2q SWAP(2,2,2,2);
    SWAP.setValues({{{{1, 0},{0, 0}},{{0, 0},{1, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 1}}}});
    return SWAP;
}

/**
* Generates the CZ gate.
*
* @return the CZ tensor
*/
Gate_2q CZ() {
    Gate_2q CZ(2,2,2,2);
    CZ.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{1, 0}},{{0, 0},{0, -1}}}});
    return CZ;
}

/**
* Generates the Toffoli gate.
*
* @return the Toffoli tensor
*/
Gate_3q Toffoli() {
    Gate_3q Toffoli(2,2,2,2,2,2);
    Toffoli.setValues({{{{{{1, 0},{0, 0}},{{0, 0},{0, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 0}}}},
            {{{{0, 0},{1, 0}},{{0, 0},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{0, 0}}}}
        },
        {   {{{{0, 0},{0, 0}},{{1, 0},{0, 0}}},{{{0, 0},{0, 0}},{{0, 1},{0, 0}}}},
            {{{{0, 0},{0, 0}},{{0, 0},{0, 1}}},{{{0, 0},{0, 0}},{{0, 0},{1, 0}}}}
        }});
    return Toffoli;
}

/**
* Generates the CSWAP gate.
*
* @return the CSWAP tensor
*/
Gate_3q CSWAP() {
    Gate_3q CSWAP(2,2,2,2,2,2);
    CSWAP.setValues({{{{{{1, 0},{0, 0}},{{0, 0},{0, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 0}}}},
            {{{{0, 0},{1, 0}},{{0, 0},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{0, 0}}}}
        },
        {   {{{{0, 0},{0, 0}},{{1, 0},{0, 0}}},{{{0, 0},{0, 0}},{{0, 0},{1, 0}}}},
            {{{{0, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{0, 0}},{{0, 0},{0, 1}}}}
        }});
    return CSWAP;
}

/**
* Generates the controlled-X rotation gate.
*
* @param parameter the rotation angle
* @return the CRX tensor
*/
Gate_2q CRX(const double& parameter) {
    Gate_2q CRX(2, 2, 2, 2);

    const std::complex<double> c (std::cos(parameter / 2), 0);
    const std::complex<double> js (0, std::sin(-parameter / 2));

    CRX.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{c, js}},{{0, 0},{js, c}}}});
    return CRX;
}

/**
* Generates the controlled-Y rotation gate.
*
* @param parameter the rotation angle
* @return the CRY tensor
*/
Gate_2q CRY(const double& parameter) {
    Gate_2q CRY(2, 2, 2, 2);

    const double c = std::cos(parameter / 2);
    const double s = std::sin(parameter / 2);

    CRY.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{c, -s}},{{0, 0},{s, c}}}});
    return CRY;
}

/**
* Generates the controlled-Z rotation gate.
*
* @param parameter the rotation angle
* @return the CRZ tensor
*/
Gate_2q CRZ(const double& parameter) {
    Gate_2q CRZ(2, 2, 2, 2);

    const std::complex<double> exponent(0, -parameter/2);
    const std::complex<double> exponent_second(0, parameter/2);
    const std::complex<double> first = std::pow(M_E, exponent);
    const std::complex<double> second = std::pow(M_E, exponent_second);

    CRZ.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{first, 0}},{{0, 0},{0, second}}}});
    return CRZ;
}

/**
* Generates the controlled rotation gate.
*
* This gate implements a rotation on a target qubit depending on a control qubit. The rotation
* on the target qubit is achieved through three separate rotations:
* \f$R(\phi, \theta, \omega)= RZ(\omega)RY(\theta)RZ(\phi)\f$.
*
* @param phi the first rotation angle
* @param theta the second rotation angle
* @param omega the third rotation angle
* @return the controlled rotation tensor
*/
Gate_2q CRot(const double& phi, const double& theta, const double& omega) {
    Gate_2q CRot(2,2,2,2);

    const std::complex<double> e00(0, (-phi - omega)/2);
    const std::complex<double> e10(0, (-phi + omega)/2);
    const std::complex<double> e01(0, (phi - omega)/2);
    const std::complex<double> e11(0, (phi + omega)/2);

    const std::complex<double> exp00 = std::pow(M_E, e00);
    const std::complex<double> exp10 = std::pow(M_E, e10);
    const std::complex<double> exp01 = std::pow(M_E, e01);
    const std::complex<double> exp11 = std::pow(M_E, e11);

    const double c = std::cos(theta / 2);
    const double s = std::sin(theta / 2);

    CRot.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{exp00 * c, -exp01 * s}},
            {{0, 0},{exp10 * s, exp11 * c}}
        }});
    return CRot;
}

// Creating aliases based on the function signatures of each operation
typedef Gate_1q (*pfunc_1q)();
typedef Gate_1q (*pfunc_1q_one_param)(const double&);
typedef Gate_1q (*pfunc_1q_three_params)(const double&, const double&, const double&);

typedef Gate_2q (*pfunc_2q)();
typedef Gate_2q (*pfunc_2q_one_param)(const double&);
typedef Gate_2q (*pfunc_2q_three_params)(const double&, const double&, const double&);

typedef Gate_3q (*pfunc_3q)();

// Defining the operation maps
const std::map<std::string, pfunc_1q> OneQubitOps = {
    {"Identity", Identity},
    {"PauliX", X},
    {"PauliY", Y},
    {"PauliZ", Z},
    {"Hadamard", H},
    {"S", S},
    {"T", T}
};

const std::map<std::string, pfunc_1q_one_param> OneQubitOpsOneParam = {
    {"RX", RX},
    {"RY", RY},
    {"RZ", RZ},
    {"PhaseShift", PhaseShift}
};

const std::map<std::string, pfunc_1q_three_params> OneQubitOpsThreeParams = {
    {"Rot", Rot}
};

const std::map<std::string, pfunc_2q> TwoQubitOps = {
    {"CNOT", CNOT},
    {"SWAP", SWAP},
    {"CZ", CZ}
};

const std::map<std::string, pfunc_2q_one_param> TwoQubitOpsOneParam = {
    {"CRX", CRX},
    {"CRY", CRY},
    {"CRZ", CRZ}
};

const std::map<std::string, pfunc_2q_three_params> TwoQubitOpsThreeParams = {
    {"CRot", CRot}
};

const std::map<std::string, pfunc_3q> ThreeQubitOps = {
    {"Toffoli", Toffoli},
    {"CSWAP", CSWAP}
};


// Boost.Geometry

// Copyright (c) 2020-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/precise_math.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation
\details This predicate determines at which side of a segment a point lies
*/
template
<
    typename CalculationType = void
>
struct side_non_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type CoordinateType;
        typedef typename select_most_precise
            <
                CoordinateType,
                double
            >::type PromotedType;

        CoordinateType const x = get<0>(p);
        CoordinateType const y = get<1>(p);

        CoordinateType const sx1 = get<0>(p1);
        CoordinateType const sy1 = get<1>(p1);
        CoordinateType const sx2 = get<0>(p2);
        CoordinateType const sy2 = get<1>(p2);

        //non-robust 1
        //the following is 2x slower in some generic cases when compiled with g++
        //(tested versions 9 and 10)
        //
        //auto detleft = (sx1 - x) * (sy2 - y);
        //auto detright = (sy1 - y) * (sx2 - x);
        //return detleft > detright ? 1 : (detleft < detright ? -1 : 0 );

        //non-robust 2
        PromotedType const dx = sx2 - sx1;
        PromotedType const dy = sy2 - sy1;
        PromotedType const dpx = x - sx1;
        PromotedType const dpy = y - sy1;

        PromotedType sv = geometry::detail::determinant<PromotedType>
                (
                    dx, dy,
                    dpx, dpy
                );
        PromotedType const zero = PromotedType();

        return sv == 0 ? 0 : sv > zero ? 1 : -1;
    }

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Marc Glisse
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

#include <boost/range/iterator_range.hpp>

#include <wasserstein.h> // Hera

#include <array>

namespace py = pybind11;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> Dgm;

double wasserstein_distance(
    Dgm d1, Dgm d2,
    double wasserstein_power, double internal_p,
    double delta)
{
  py::buffer_info buf1 = d1.request();
  py::buffer_info buf2 = d2.request();
  // shape (n,2) or (0) for empty
  if((buf1.ndim!=2 || buf1.shape[1]!=2) && (buf1.ndim!=1 || buf1.shape[0]!=0))
    throw std::runtime_error("Diagram 1 must be an array of size n x 2");
  if((buf2.ndim!=2 || buf2.shape[1]!=2) && (buf2.ndim!=1 || buf2.shape[0]!=0))
    throw std::runtime_error("Diagram 2 must be an array of size n x 2");
  typedef std::array<double, 2> Point;
  auto p1 = (Point*)buf1.ptr;
  auto p2 = (Point*)buf2.ptr;
  auto diag1 = boost::make_iterator_range(p1, p1+buf1.shape[0]);
  auto diag2 = boost::make_iterator_range(p2, p2+buf2.shape[0]);

  hera::AuctionParams<double> params;
  params.wasserstein_power = wasserstein_power;
  // hera encodes infinity as -1...
  if(std::isinf(internal_p)) internal_p = hera::get_infinity<double>();
  params.internal_p = internal_p;
  params.delta = delta;
  // The extra parameters are purposedly not exposed for now.
  return hera::wasserstein_dist(diag1, diag2, params);
}

PYBIND11_MODULE(hera, m) {
      m.def("wasserstein_distance", &wasserstein_distance,
          py::arg("X"), py::arg("Y"),
          py::arg("order") = 1,
          py::arg("internal_p") = std::numeric_limits<double>::infinity(),
          py::arg("delta") = .01,
          R"pbdoc(
        Compute the Wasserstein distance between two diagrams.
        Points at infinity are supported.

        Parameters:
            X (n x 2 numpy array): First diagram
            Y (n x 2 numpy array): Second diagram
            order (float): Wasserstein exponent W_q
            internal_p (float): Internal Minkowski norm L^p in R^2
            delta (float): Relative error 1+delta

        Returns:
            float: Approximate Wasserstein distance W_q(X,Y)
    )pbdoc");
}


/** @file GammaDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief GammaDist class definition
 *
 */
#include "PriorHessian/GammaDist.h"
#include "PriorHessian/util.h"
#include "PriorHessian/PriorHessianError.h"

#include <cmath>
#include <sstream>
#include <limits>

#include <boost/math/special_functions/gamma.hpp>

namespace prior_hessian {

const StringVecT GammaDist::_param_names = { "scale", "shape" };
const GammaDist::NparamsVecT GammaDist::_param_lbound = {0, 0}; //Lower bound on valid parameter values
const GammaDist::NparamsVecT GammaDist::_param_ubound = {INFINITY, INFINITY}; //Upper bound on valid parameter values

/* Constructors */
GammaDist::GammaDist(double scale, double shape)
    : UnivariateDist(),
      _scale(checked_scale(scale)),
      _shape(checked_shape(shape)),
      llh_const_initialized(false)
{ }

/* Non-static member functions */

void GammaDist::set_scale(double val)
{
    _scale = checked_scale(val);
    llh_const_initialized = false;
}

void GammaDist::set_shape(double val)
{
    _shape = checked_shape(val);
    llh_const_initialized = false;
}

void GammaDist::set_params(double scale, double shape)
{
    _scale = checked_scale(scale);
    _shape = checked_shape(shape);
    llh_const_initialized = false;
}

double GammaDist::cdf(double x) const
{
   return boost::math::gamma_p(_shape, x / _scale);
}

double GammaDist::icdf(double u) const
{
    if(u == 0) return 0;
    if(u == 1) return INFINITY;
    return boost::math::gamma_p_inv(_shape, u) * _scale;
}

double GammaDist::pdf(double x) const
{
    if(x==0) return 0;
    double inv_scale = 1/_scale;
    return boost::math::gamma_p_derivative(_shape, x*inv_scale) * inv_scale;
}

double GammaDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const();
    return rllh(x) + llh_const;
}

void GammaDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(shape(),scale());
    llh_const_initialized = true;
}

double GammaDist::compute_llh_const(double shape, double scale)
{
    return -shape*log(scale) - std::lgamma(shape);
}

double GammaDist::checked_scale(double val)
{
    if(!std::isfinite(val) || val <= 0) {
        std::ostringstream msg;
        msg<<"GammaDist: got bad scale value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double GammaDist::checked_shape(double val)
{
    if(!std::isfinite(val) || val <= 0) {
        std::ostringstream msg;
        msg<<"GammaDist: got bad shape value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

} /* namespace prior_hessian */


///////////////////////////////////////////////////////////////////////////////
// vector_space::functional::l2_distance_squared.hpp                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l2_DISTANCE_SQUARED_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l2_DISTANCE_SQUARED_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <functional>
#include <boost/type_traits.hpp>
#include <boost/function.hpp>
#include <boost/call_traits.hpp>
#include <boost/range.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace vector_space{

    // Stores a range r, and when passed another range r1, computes the squared
    // Euclidian distance of r-r1.
    //
    // Examples:
    // R == const std::vector<double>&
    // R == iterator_range<std::vector<double> >
    template<typename R>
    class l2_distance_squared{
        typedef typename is_reference<R>::type is_ref_;
    public:
        typedef typename remove_reference<R>::type const_range_type;
        typedef typename remove_const<const_range_type>::type range_type;
        typedef typename range_value<range_type>::type result_type;

        // Constructor
        l2_distance_squared(){} // Warning, x_ not initialized
        l2_distance_squared(typename call_traits<R>::param_type x);
        l2_distance_squared(const l2_distance_squared& that);
        l2_distance_squared& operator=(const l2_distance_squared& that);

        // Call
        template<typename R1>
        result_type operator()(const R1& y)const;

        private:
        typename call_traits<R>::value_type x_;
    };

    template<typename R>
    l2_distance_squared<R>
    make_l2_distance_squared(const R& x){
        return l2_distance_squared<R>(x);
    }

    // Definitions
    template<typename R>
    l2_distance_squared<R>::l2_distance_squared(
        typename call_traits<R>::param_type x
    ):x_(x){}

    template<typename R>
    l2_distance_squared<R>::l2_distance_squared(
        const l2_distance_squared& that
    ):x_(that.x_){}

    template<typename R>
    l2_distance_squared<R>&
    l2_distance_squared<R>::operator=(const l2_distance_squared& that){
        if(&that != this){
            x_ = that.x_;
        }
        return *this;
    }

    template<typename R>
    template<typename R1>
    typename l2_distance_squared<R>::result_type
    l2_distance_squared<R>::operator()(const R1& y)const{
        BOOST_ASSERT(
            size(x_) == size(y)
        );
        return std::inner_product(
            begin(y),
            end(y),
            begin(x_),
            static_cast<result_type>(0),
            std::plus<result_type>(),
            (lambda::_1 - lambda::_2) * (lambda::_1 - lambda::_2)
        );
    };

}// vector_space
}// detail
}// statistics
}// boost

#endif


#ifndef NODEUTILS_HPP
#define NODEUTILS_HPP

#include <type_traits>

#include <geometry_msgs/PointStamped.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <cmath>
#include <tuple>

#include <ros/ros.h>

namespace igvc
{
/**
Calculates euclidian distance between two points

@tparam T the data type of the input points to calculate the euclidian distance for
@param[in] x1 x value of first point
@param[in] y1 y value of first point
@param[in] x2 x value of second point
@param[in] y2 y value of second point
@return the euclidian distance between both points
*/
template <typename T>
inline T get_distance(T x1, T y1, T x2, T y2)
{
  return std::hypot(x2 - x1, y2 - y1);
}

/**
Calculates euclidian distance between two points

@tparam T the data type of the input points to calculate the euclidian distance for
@param[in] p1 the <x,y> coords of the first point
@param[in] p2 the <x,y> coords of the second point
@return the euclidian distance between both points
*/
inline double get_distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2)
{
  return igvc::get_distance(p1.x, p1.y, p2.x, p2.y);
}

/**
Calculates euclidian distance between two points, taking tuples for each
(x,y) point as arguments

@tparam T the data type contained within each input tuple
@param[in] p1 the first point
@param[in] p2 the second point
@return the euclidian distance between both points
*/
template <typename T>
inline T get_distance(const std::tuple<T, T>& p1, const std::tuple<T, T>& p2)
{
  return igvc::get_distance(std::get<0>(p1), std::get<1>(p1), std::get<0>(p2), std::get<1>(p2));
}

/**
symmetric round up
Bias: away from zero

@tparam T the data type to round up
@param[in] the value to round up
@return the value rounded away from zero
*/
template <typename T>
T ceil0(const T& value)
{
  return (value < 0.0) ? std::floor(value) : std::ceil(value);
}

/**
Adjust angle to lie within the polar range [-PI, PI]
*/
inline void fit_to_polar(double& angle)
{
  angle = std::fmod(angle, 2 * M_PI);
  if (angle > M_PI)
  {
    angle -= 2 * M_PI;
  }
  else if (angle < -M_PI)
  {
    angle += 2 * M_PI;
  }
}

/**
Computes the egocentric polar angle of vec2 wrt vec1 in 2D, that is:
  - clockwise: negative
  - counter-clockwise: positive

source: https://stackoverflow.com/questions/14066933/direct-way-of-computing-clockwise-angle-between-2-vectors

@param[in] angle the double variable to assign the computed angle to
@param[in] vec2 the vector the angle is computed with respect to
@param[in] vec1 the reference vector
*/
inline void compute_angle(double& angle, Eigen::Vector3d vec2, Eigen::Vector3d vec1)
{
  double dot = vec2[0] * vec1[0] + vec2[1] * vec1[1];  // dot product - proportional to cos
  double det = vec2[0] * vec1[1] - vec2[1] * vec1[0];  // determinant - proportional to sin

  angle = atan2(det, dot);
}

}  // namespace igvc
#endif