/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

/* This is an old version of FIRI for temporary usage here. */

#ifndef FIRI_HPP
#define FIRI_HPP

#include "lbfgs.hpp"
#include "sdlp.hpp"

#include <Eigen/Eigen>

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cfloat>
#include <cmath>
#include <vector>

namespace firi
{

    inline void chol3d(const Eigen::Matrix3d &A,
                       Eigen::Matrix3d &L)
    {
        L(0, 0) = sqrt(A(0, 0));
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = 0.5 * (A(0, 1) + A(1, 0)) / L(0, 0);
        L(1, 1) = sqrt(A(1, 1) - L(1, 0) * L(1, 0));
        L(1, 2) = 0.0;
        L(2, 0) = 0.5 * (A(0, 2) + A(2, 0)) / L(0, 0);
        L(2, 1) = (0.5 * (A(1, 2) + A(2, 1)) - L(2, 0) * L(1, 0)) / L(1, 1);
        L(2, 2) = sqrt(A(2, 2) - L(2, 0) * L(2, 0) - L(2, 1) * L(2, 1));
        return;
    }

    inline bool smoothedL1(const double &mu,
                           const double &x,
                           double &f,
                           double &df)
    {
        if (x < 0.0)
        {
            return false;
        }
        else if (x > mu)
        {
            f = x - 0.5 * mu;
            df = 1.0;
            return true;
        }
        else
        {
            const double xdmu = x / mu;
            const double sqrxdmu = xdmu * xdmu;
            const double mumxd2 = mu - 0.5 * x;
            f = mumxd2 * sqrxdmu * xdmu;
            df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu);
            return true;
        }
    }

    inline double costMVIE(void *data,
                           const Eigen::VectorXd &x,
                           Eigen::VectorXd &grad)
    {
        const int *pM = (int *)data;
        const double *pSmoothEps = (double *)(pM + 1);
        const double *pPenaltyWt = pSmoothEps + 1;
        const double *pA = pPenaltyWt + 1;

        const int M = *pM;
        const double smoothEps = *pSmoothEps;
        const double penaltyWt = *pPenaltyWt;
        Eigen::Map<const Eigen::MatrixX3d> A(pA, M, 3);
        Eigen::Map<const Eigen::Vector3d> p(x.data());
        Eigen::Map<const Eigen::Vector3d> rtd(x.data() + 3);
        Eigen::Map<const Eigen::Vector3d> cde(x.data() + 6);
        Eigen::Map<Eigen::Vector3d> gdp(grad.data());
        Eigen::Map<Eigen::Vector3d> gdrtd(grad.data() + 3);
        Eigen::Map<Eigen::Vector3d> gdcde(grad.data() + 6);

        double cost = 0;
        gdp.setZero();
        gdrtd.setZero();
        gdcde.setZero();

        Eigen::Matrix3d L;
        L(0, 0) = rtd(0) * rtd(0) + DBL_EPSILON;
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = cde(0);
        L(1, 1) = rtd(1) * rtd(1) + DBL_EPSILON;
        L(1, 2) = 0.0;
        L(2, 0) = cde(2);
        L(2, 1) = cde(1);
        L(2, 2) = rtd(2) * rtd(2) + DBL_EPSILON;

        const Eigen::MatrixX3d AL = A * L;
        const Eigen::VectorXd normAL = AL.rowwise().norm();
        const Eigen::Matrix3Xd adjNormAL = (AL.array().colwise() / normAL.array()).transpose();
        const Eigen::VectorXd consViola = (normAL + A * p).array() - 1.0;

        double c, dc;
        Eigen::Vector3d vec;
        for (int i = 0; i < M; ++i)
        {
            if (smoothedL1(smoothEps, consViola(i), c, dc))
            {
                cost += c;
                vec = dc * A.row(i).transpose();
                gdp += vec;
                gdrtd += adjNormAL.col(i).cwiseProduct(vec);
                gdcde(0) += adjNormAL(0, i) * vec(1);
                gdcde(1) += adjNormAL(1, i) * vec(2);
                gdcde(2) += adjNormAL(0, i) * vec(2);
            }
        }
        cost *= penaltyWt;
        gdp *= penaltyWt;
        gdrtd *= penaltyWt;
        gdcde *= penaltyWt;

        cost -= log(L(0, 0)) + log(L(1, 1)) + log(L(2, 2));
        gdrtd(0) -= 1.0 / L(0, 0);
        gdrtd(1) -= 1.0 / L(1, 1);
        gdrtd(2) -= 1.0 / L(2, 2);

        gdrtd(0) *= 2.0 * rtd(0);
        gdrtd(1) *= 2.0 * rtd(1);
        gdrtd(2) *= 2.0 * rtd(2);

        return cost;
    }

    // Each row of hPoly is defined by h0, h1, h2, h3 as
    // h0*x + h1*y + h2*z + h3 <= 0
    // R, p, r are ALWAYS taken as the initial guess
    // R is also assumed to be a rotation matrix
    inline bool maxVolInsEllipsoid(const Eigen::MatrixX4d &hPoly,
                                   Eigen::Matrix3d &R,
                                   Eigen::Vector3d &p,
                                   Eigen::Vector3d &r)
    {
        // Find the deepest interior point
        const int M = hPoly.rows();
        Eigen::MatrixX4d Alp(M, 4);
        Eigen::VectorXd blp(M);
        Eigen::Vector4d clp, xlp;
        const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm();
        Alp.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm;
        Alp.rightCols<1>().setConstant(1.0);
        blp = -hPoly.rightCols<1>().array() / hNorm;
        clp.setZero();
        clp(3) = -1.0;
        const double maxdepth = -sdlp::linprog<4>(clp, Alp, blp, xlp);
        if (!(maxdepth > 0.0) || std::isinf(maxdepth))
        {
            return false;
        }
        const Eigen::Vector3d interior = xlp.head<3>();

        // Prepare the data for MVIE optimization
        uint8_t *optData = new uint8_t[sizeof(int) + (2 + 3 * M) * sizeof(double)];
        int *pM = (int *)optData;
        double *pSmoothEps = (double *)(pM + 1);
        double *pPenaltyWt = pSmoothEps + 1;
        double *pA = pPenaltyWt + 1;

        *pM = M;
        Eigen::Map<Eigen::MatrixX3d> A(pA, M, 3);
        A = Alp.leftCols<3>().array().colwise() /
            (blp - Alp.leftCols<3>() * interior).array();

        Eigen::VectorXd x(9);
        const Eigen::Matrix3d Q = R * (r.cwiseProduct(r)).asDiagonal() * R.transpose();
        Eigen::Matrix3d L;
        chol3d(Q, L);

        x.head<3>() = p - interior;
        x(3) = sqrt(L(0, 0));
        x(4) = sqrt(L(1, 1));
        x(5) = sqrt(L(2, 2));
        x(6) = L(1, 0);
        x(7) = L(2, 1);
        x(8) = L(2, 0);

        double minCost;
        lbfgs::lbfgs_parameter_t paramsMVIE;
        paramsMVIE.mem_size = 18;
        paramsMVIE.g_epsilon = 0.0;
        paramsMVIE.min_step = 1.0e-32;
        paramsMVIE.past = 3;
        paramsMVIE.delta = 1.0e-7;
        *pSmoothEps = 1.0e-2;
        *pPenaltyWt = 1.0e+3;

        int ret = lbfgs::lbfgs_optimize(x,
                                        minCost,
                                        &costMVIE,
                                        nullptr,
                                        nullptr,
                                        optData,
                                        paramsMVIE);

        if (ret < 0)
        {
            printf("FIRI WARNING: %s\n", lbfgs::lbfgs_strerror(ret));
        }

        p = x.head<3>() + interior;
        L(0, 0) = x(3) * x(3);
        L(0, 1) = 0.0;
        L(0, 2) = 0.0;
        L(1, 0) = x(6);
        L(1, 1) = x(4) * x(4);
        L(1, 2) = 0.0;
        L(2, 0) = x(8);
        L(2, 1) = x(7);
        L(2, 2) = x(5) * x(5);
        Eigen::JacobiSVD<Eigen::Matrix3d, Eigen::FullPivHouseholderQRPreconditioner> svd(L, Eigen::ComputeFullU);
        const Eigen::Matrix3d U = svd.matrixU();
        const Eigen::Vector3d S = svd.singularValues();
        if (U.determinant() < 0.0)
        {
            R.col(0) = U.col(1);
            R.col(1) = U.col(0);
            R.col(2) = U.col(2);
            r(0) = S(1);
            r(1) = S(0);
            r(2) = S(2);
        }
        else
        {
            R = U;
            r = S;
        }

        delete[] optData;

        return ret >= 0;
    }

    inline bool firi(const Eigen::MatrixX4d &bd,
                     const Eigen::Matrix3Xd &pc,
                     const Eigen::Vector3d &a,
                     const Eigen::Vector3d &b,
                     Eigen::MatrixX4d &hPoly,
                     const int iterations = 4,
                     const double epsilon = 1.0e-6)
    {
        const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0);
        const Eigen::Vector4d bh(b(0), b(1), b(2), 1.0);

        if ((bd * ah).maxCoeff() > 0.0 ||
            (bd * bh).maxCoeff() > 0.0)
        {
            return false;
        }

        const int M = bd.rows();
        const int N = pc.cols();

        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        Eigen::Vector3d p = 0.5 * (a + b);
        Eigen::Vector3d r = Eigen::Vector3d::Ones();
        Eigen::MatrixX4d forwardH(M + N, 4);
        int nH = 0;

        for (int loop = 0; loop < iterations; ++loop)
        {
            const Eigen::Matrix3d forward = r.cwiseInverse().asDiagonal() * R.transpose();
            const Eigen::Matrix3d backward = R * r.asDiagonal();
            const Eigen::MatrixX3d forwardB = bd.leftCols<3>() * backward;
            const Eigen::VectorXd forwardD = bd.rightCols<1>() + bd.leftCols<3>() * p;
            const Eigen::Matrix3Xd forwardPC = forward * (pc.colwise() - p);
            const Eigen::Vector3d fwd_a = forward * (a - p);
            const Eigen::Vector3d fwd_b = forward * (b - p);

            const Eigen::VectorXd distDs = forwardD.cwiseAbs().cwiseQuotient(forwardB.rowwise().norm());
            Eigen::MatrixX4d tangents(N, 4);
            Eigen::VectorXd distRs(N);

            for (int i = 0; i < N; i++)
            {
                distRs(i) = forwardPC.col(i).norm();
                tangents(i, 3) = -distRs(i);
                tangents.block<1, 3>(i, 0) = forwardPC.col(i).transpose() / distRs(i);
                if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon)
                {
                    const Eigen::Vector3d delta = forwardPC.col(i) - fwd_a;
                    tangents.block<1, 3>(i, 0) = fwd_a - (delta.dot(fwd_a) / delta.squaredNorm()) * delta;
                    distRs(i) = tangents.block<1, 3>(i, 0).norm();
                    tangents(i, 3) = -distRs(i);
                    tangents.block<1, 3>(i, 0) /= distRs(i);
                }
                if (tangents.block<1, 3>(i, 0).dot(fwd_b) + tangents(i, 3) > epsilon)
                {
                    const Eigen::Vector3d delta = forwardPC.col(i) - fwd_b;
                    tangents.block<1, 3>(i, 0) = fwd_b - (delta.dot(fwd_b) / delta.squaredNorm()) * delta;
                    distRs(i) = tangents.block<1, 3>(i, 0).norm();
                    tangents(i, 3) = -distRs(i);
                    tangents.block<1, 3>(i, 0) /= distRs(i);
                }
                if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon)
                {
                    tangents.block<1, 3>(i, 0) = (fwd_a - forwardPC.col(i)).cross(fwd_b - forwardPC.col(i)).normalized();
                    tangents(i, 3) = -tangents.block<1, 3>(i, 0).dot(fwd_a);
                    tangents.row(i) *= tangents(i, 3) > 0.0 ? -1.0 : 1.0;
                }
            }

            Eigen::Matrix<uint8_t, -1, 1> bdFlags = Eigen::Matrix<uint8_t, -1, 1>::Constant(M, 1);
            Eigen::Matrix<uint8_t, -1, 1> pcFlags = Eigen::Matrix<uint8_t, -1, 1>::Constant(N, 1);

            nH = 0;

            bool completed = false;
            int bdMinId, pcMinId;
            double minSqrD = distDs.minCoeff(&bdMinId);
            double minSqrR = distRs.minCoeff(&pcMinId);
            for (int i = 0; !completed && i < (M + N); ++i)
            {
                bdMinId = bdMinId;
                pcMinId = pcMinId;
                if (minSqrD < minSqrR)
                {
                    forwardH.block<1, 3>(nH, 0) = forwardB.row(bdMinId);
                    forwardH(nH, 3) = forwardD(bdMinId);
                    bdFlags(bdMinId) = 0;
                }
                else
                {
                    forwardH.row(nH) = tangents.row(pcMinId);
                    pcFlags(pcMinId) = 0;
                }

                completed = true;
                minSqrD = INFINITY;
                for (int j = 0; j < M; ++j)
                {
                    if (bdFlags(j))
                    {
                        completed = false;
                        if (minSqrD > distDs(j))
                        {
                            bdMinId = j;
                            minSqrD = distDs(j);
                        }
                    }
                }
                minSqrR = INFINITY;
                for (int j = 0; j < N; ++j)
                {
                    if (pcFlags(j))
                    {
                        if (forwardH.block<1, 3>(nH, 0).dot(forwardPC.col(j)) + forwardH(nH, 3) > -epsilon)
                        {
                            pcFlags(j) = 0;
                        }
                        else
                        {
                            completed = false;
                            if (minSqrR > distRs(j))
                            {
                                pcMinId = j;
                                minSqrR = distRs(j);
                            }
                        }
                    }
                }
                ++nH;
            }

            hPoly.resize(nH, 4);
            for (int i = 0; i < nH; ++i)
            {
                hPoly.block<1, 3>(i, 0) = forwardH.block<1, 3>(i, 0) * forward;
                hPoly(i, 3) = forwardH(i, 3) - hPoly.block<1, 3>(i, 0).dot(p);
            }

            if (loop == iterations - 1)
            {
                break;
            }

            maxVolInsEllipsoid(hPoly, R, p, r);
        }

        return true;
    }

}

#endif


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

#ifndef SMOOTH__FEEDBACK__FLATTEN_OCP_HPP_
#define SMOOTH__FEEDBACK__FLATTEN_OCP_HPP_

/**
 * @file
 * @brief Reformulate an optimal control problem on a Lie group as an optimal control problem in the
 * tangent space around a reference trajectory.
 *
 * @todo More efficient implementation of Hessian in FlatDyn.
 * @todo Accept dxl as a template argument to avoid double differentiation.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <smooth/bundle.hpp>
#include <smooth/diff.hpp>

#include "ocp.hpp"
#include "smooth/lie_group_sparse.hpp"
#include "utils/sparse.hpp"

namespace smooth::feedback {

// \cond
namespace detail {

/// @brief The first Bernoulli numbers
static constexpr std::array<double, 23> kBn{
  1,               // 0
  -1. / 2,         // 1
  1. / 6,          // 2
  0.,              // 3
  -1. / 30,        // 4
  0,               // 5
  1. / 42,         // 6
  0,               // 7
  -1. / 30,        // 8
  0,               // 9
  5. / 66,         // 10
  0,               // 11
  -691. / 2730,    // 12
  0,               // 13
  7. / 6,          // 14
  0,               // 15
  -3617. / 510,    // 16
  0,               // 17
  43867. / 798,    // 18
  0,               // 19
  -174611. / 330,  // 20
  0,               // 21
  854513. / 138,   // 22
};

/**
 * @brief Sparse matrices containing reordered rows of algebra generators.
 */
template<LieGroup G>
inline auto generators_sparse_reordered =
  []() -> std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> {
  std::array<Eigen::SparseMatrix<double, Eigen::RowMajor>, Dof<G>> ret;
  for (auto k = 0u; k < Dof<G>; ++k) {
    ret[k].resize(Dof<G>, Dof<G>);
    for (auto i = 0u; i < Dof<G>; ++i) { ret[k].row(i) = ::smooth::generators_sparse<G>[i].row(k); }
    ret[k].makeCompressed();
  }
  return ret;
}();

/**
 * @brief Derivative of the adjoint as sparse matrix
 */
template<LieGroup G>
inline Eigen::SparseMatrix<double> d_ad = []() -> Eigen::SparseMatrix<double> {
  Eigen::SparseMatrix<double> ret;
  ret.resize(Dof<G>, Dof<G> * Dof<G>);
  for (auto i = 0u; i < Dof<G>; ++i) {
    for (auto j = 0u; j < Dof<G>; ++j) {
      ret.col(j * Dof<G> + i) = smooth::generators_sparse<G>[i].row(j).transpose();
    }
  }
  ret.makeCompressed();
  return ret;
}();

/**
 * @brief Flattening of dynamics function (t, x, u) -> Tangent, and its derivatives.
 *
 * @note Do not use numeric differentiation of operator() (it differentiates inside)
 * @note Only considers first derivative of xl and ul
 */
template<LieGroup X, Manifold U, typename F, typename Xl, typename Ul>
class FlatDyn
{
private:
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nu    = Dof<U>;
  static constexpr auto Nouts = Nx;
  static constexpr auto Nvars = 1 + Nx + Nu;

  static constexpr auto t_B = 0;
  static constexpr auto x_B = t_B + 1;
  static constexpr auto u_B = x_B + Nx;

  using E = Tangent<X>;
  using V = Tangent<U>;

  F f;
  Xl xl;
  Ul ul;

  Eigen::SparseMatrix<double> ad_e_ = smooth::ad_sparse_pattern<X>;
  Eigen::SparseMatrix<double> ad_vi = smooth::ad_sparse_pattern<X>;

  Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> dexpinv_e_  = smooth::d_exp_sparse_pattern<X>;
  Eigen::SparseMatrix<double> d2expinv_e_ = smooth::d2_exp_sparse_pattern<X>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  // Would ideally like to remove these temporaries...
  Eigen::SparseMatrix<double> ji_{Nouts, Nvars}, ji_tmp_{Nouts, Nvars}, hi_{Nvars, Nouts *Nvars},
    hi_tmp_{Nvars, Nouts *Nvars};

  /// @brief Calculate jacobian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());

    block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
    block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());

    // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
    const TangentMap<X> Adexp_X  = Ad<X>(smooth::exp<X>(-e));
    const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
    const TangentMap<U> Adexp_U  = Ad<U>(smooth::exp<U>(-v));
    const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (x_B + nx);
      block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
    }
    for (auto nu = 0u; nu < Nu; ++nu) {
      const auto b0 = Nvars * (u_B + nu);
      block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2, typename A3>
  FlatDyn(A1 && a1, A2 && a2, A3 && a3)
      : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
  {}

  template<typename T>
  CastT<T, E> operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
  {
    using XT = CastT<T, X>;

    // can not double-differentiate, so we hide derivative of xl w.r.t. t
    const double tdbl           = static_cast<double>(t);
    const auto [unused, dxlval] = diff::dr(xl, wrt(tdbl));

    return dr_expinv<XT>(e) * (f(t, rplus(xl(t), e), rplus(ul(t), v)) - dxlval.template cast<T>())
         + ad<XT>(e) * dxlval.template cast<T>();
  }

  // First derivative
  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
  {
    const double tdbl          = static_cast<double>(t);
    const auto [xlval, dxlval] = diff::dr(xl, wrt(tdbl));
    const auto [ulval, dulval] = diff::dr(ul, wrt(tdbl));
    const auto x               = rplus(xlval, e);
    const auto u               = rplus(ulval, v);

    dr_expinv_sparse<X>(dexpinv_e_, e);
    d2r_expinv_sparse<X>(d2expinv_e_, e);
    update_joplus(e, v, dxlval, dulval);

    // value and derivative of f
    const auto fval = f(t, x, u);
    const auto & Jf = f.jacobian(t, x, u);

    // Want to differentiate  drexpinv * (f o plus - dxl) + ad dxl

    // Start with drexpinv * d (f \circ (+))
    J_ = dexpinv_e_ * Jf * Joplus_;
    // Add d ( drexpinv ) * (f \circ (+) - dxl)
    for (auto i = 0u; i < d2expinv_e_.outerSize(); ++i) {
      for (Eigen::InnerIterator it(d2expinv_e_, i); it; ++it) {
        J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) +=
          (fval(it.row()) - dxlval(it.row())) * it.value();
      }
    }
    // Add d ( ad ) * dxl
    for (auto i = 0u; i < d_ad<X>.outerSize(); ++i) {
      for (Eigen::InnerIterator it(d_ad<X>, i); it; ++it) {
        J_.coeffRef(it.col() / Nx, 1 + (it.col() % Nx)) += dxlval(it.row()) * it.value();
      }
    }

    J_.makeCompressed();
    return J_;
  }

  // Second derivative
  //    \sum Bn (-1)^n / n! d2r (ad_a^n f)_aa - \sum Bn / n! d2r(ad_a^n dxl)_aa
  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
  {
    const double tdbl          = static_cast<double>(t);
    const auto [xlval, dxlval] = diff::dr(xl, wrt(tdbl));
    const auto [ulval, dulval] = diff::dr(ul, wrt(tdbl));

    const auto x    = rplus(xlval, e);
    const auto u    = rplus(ul(t), v);
    const auto & Jf = f.jacobian(t, x, u);  // nx x (1 + nx + nu)
    const auto & Hf = f.hessian(t, x, u);   // (1 + nx + nu) x (nx * (1 + nx + nu))

    ad_sparse<X>(ad_e_, e);
    update_joplus(e, v, dxlval, dulval);
    update_hoplus(e, v, dxlval, dulval);

    double coef   = 1;                    // hold (-1)^i / i!
    Tangent<X> vi = f(t, x, u) - dxlval;  // (ad_a)^i * (f - dxl)
    ji_           = Jf * Joplus_;         // dr (vi)_{t, e, v}
    set_zero(hi_);
    d2r_fog(hi_, Jf, Hf, Joplus_, Hoplus_);  // d2r (vi)_{t, e, v}

    set_zero(H_);
    for (auto iter = 0u; iter < std::tuple_size_v<decltype(kBn)>; ++iter) {
      if (kBn[iter] != 0) { block_add(H_, 0, 0, hi_, kBn[iter] * coef); }

      // update hi_
      hi_tmp_.setZero();
      for (auto i = 0u; i < ad_e_.outerSize(); ++i) {
        for (Eigen::InnerIterator it(ad_e_, i); it; ++it) {
          const auto b0 = it.row() * Nvars;
          block_add(hi_tmp_, 0, b0, hi_.middleCols(it.col() * Nvars, Nvars), it.value());
        }
      }
      for (auto k = 0u; k < Nx; ++k) {
        const auto b0 = k * Nvars;
        block_add(hi_tmp_, 1, b0, generators_sparse_reordered<X>[k] * ji_);
        block_add(hi_tmp_, 0, b0 + 1, ji_.transpose() * generators_sparse_reordered<X>[k], -1);
      }
      std::swap(hi_, hi_tmp_);

      // update ji
      ji_tmp_.setZero();
      ji_tmp_ = ad_e_ * ji_;
      ad_sparse<X>(ad_vi, vi);
      block_add(ji_tmp_, 0, 1, ad_vi, -1);
      std::swap(ji_, ji_tmp_);

      // update vi
      vi.applyOnTheLeft(ad_e_);

      coef *= (-1.) / (iter + 1);
    }

    H_.makeCompressed();
    return H_;
  }
};

/**
 * @brief Flattening of inner function (t, x, u) -> Vector, and its derivatives.
 *
 * @note Only considers first derivative of xl and ul
 */
template<LieGroup X, Manifold U, std::size_t Nouts, typename F, typename Xl, typename Ul>
class FlatInnerFun
{
private:
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, U>;

  F f;
  Xl xl;
  Ul ul;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nu    = Dof<U>;
  static constexpr auto Nvars = 1 + Nx + Nu;

  using E = Tangent<X>;
  using V = Tangent<U>;

  static constexpr auto t_B = 0;
  static constexpr auto x_B = t_B + 1;
  static constexpr auto u_B = x_B + Nx;

  Eigen::SparseMatrix<double> Joplus_ = smooth::d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = smooth::d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  /// @brief Calculate jacobian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_joplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e, v).finished());

    block_write(Joplus_, x_B, t_B, Ad<X>(smooth::exp<X>(-e)) * dxl);
    block_write(Joplus_, u_B, t_B, Ad<U>(smooth::exp<U>(-v)) * dul);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (t, x(t)+e, u(t)+v) w.r.t. (t, e, v)
  void update_hoplus(const E & e, const V & v, const E & dxl, const V & dul)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e, v).finished());

    // d (Ad_X b) = -ad_(Ad_X b) * Ad_X
    const TangentMap<X> Adexp_X  = Ad<X>(smooth::exp<X>(-e));
    const TangentMap<X> dAdexp_X = ad<X>(Adexp_X * dxl) * Adexp_X * dr_exp<X>(-e);
    const TangentMap<U> Adexp_U  = Ad<U>(smooth::exp<U>(-v));
    const TangentMap<U> dAdexp_U = ad<U>(Adexp_U * dul) * Adexp_U * dr_exp<U>(-v);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (x_B + nx);
      block_write(Hoplus_, t_B, b0 + x_B, dAdexp_X.middleRows(nx, 1));
    }
    for (auto nu = 0u; nu < Nu; ++nu) {
      const auto b0 = Nvars * (u_B + nu);
      block_write(Hoplus_, t_B, b0 + u_B, dAdexp_U.middleRows(nu, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2, typename A3>
  FlatInnerFun(A1 && a1, A2 && a2, A3 && a3)
      : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2)), ul(std::forward<A3>(a3))
  {}

  template<typename T>
  Eigen::Vector<T, Nouts>
  operator()(const T & t, const CastT<T, E> & e, const CastT<T, V> & v) const
  {
    return f.template operator()<T>(t, rplus(xl(t), e), rplus(ul(t), v));
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>>)
  {
    const auto & [xlval, dxlval] = diff::dr(xl, wrt(t));
    const auto & [ulval, dulval] = diff::dr(ul, wrt(t));
    const auto & Jf              = f.jacobian(t, rplus(xlval, e), rplus(ulval, v));

    update_joplus(e, v, dxlval, dulval);

    J_ = Jf * Joplus_;
    J_.makeCompressed();
    return J_;
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double t, const E & e, const V & v) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, U>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, U>>)
  {
    const auto & [xlval, dxlval] = diff::dr(xl, wrt(t));
    const auto & [ulval, dulval] = diff::dr(ul, wrt(t));
    const auto x                 = rplus(xlval, e);
    const auto u                 = rplus(ulval, v);
    const auto & Jf              = f.jacobian(t, x, u);
    const auto & Hf              = f.hessian(t, x, u);

    update_joplus(e, v, dxlval, dulval);
    update_hoplus(e, v, dxlval, dulval);

    set_zero(H_);
    d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
    H_.makeCompressed();
    return H_;
  }
};

/**
 * @brief Flattening of endpoint function (tf, x0, xf, q) -> Vector, and its derivatives.
 *
 * @note Only considers first derivative of xl
 */
template<LieGroup X, Manifold U, std::size_t Nq, std::size_t Nouts, typename F, typename Xl>
class FlatEndptFun
{
private:
  using E       = Tangent<X>;
  using Q       = Eigen::Vector<Scalar<X>, Nq>;
  using BundleT = smooth::Bundle<Eigen::Vector<double, 1>, X, X, Q>;

  F f;
  Xl xl;

  static constexpr auto Nx    = Dof<X>;
  static constexpr auto Nvars = 1 + 2 * Nx + Nq;

  static constexpr auto tf_B = 0;
  static constexpr auto x0_B = tf_B + 1;
  static constexpr auto xf_B = x0_B + Nx;
  static constexpr auto q_B  = xf_B + Nx;

  Eigen::SparseMatrix<double> Joplus_ = d_exp_sparse_pattern<BundleT>;
  Eigen::SparseMatrix<double> Hoplus_ = d2_exp_sparse_pattern<BundleT>;

  Eigen::SparseMatrix<double> J_{Nouts, Nvars};
  Eigen::SparseMatrix<double> H_{Nvars, Nouts * Nvars};

  /// @brief Calculate jacobian of (tf, xl(0.)+e0, xl(tf)+ef, q) w.r.t. (tf, e0, ef, q)
  void update_joplus(const E & e0, const E & ef, const E & dxlf)
  {
    dr_exp_sparse<BundleT>(Joplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());

    block_write(Joplus_, xf_B, tf_B, Ad<X>(smooth::exp<X>(-ef)) * dxlf);

    Joplus_.makeCompressed();
  }

  /// @brief Calculate hessian of (tf, xl(0.)+e0, xl(tf)+ef, q) w.r.t. (tf, e0, ef, q)
  void update_hoplus(const E & e0, const E & ef, [[maybe_unused]] const E & dxlf)
  {
    d2r_exp_sparse<BundleT>(Hoplus_, (Tangent<BundleT>() << 1, e0, ef, Q::Ones()).finished());

    // dr (Ad_X b)_X = -ad_{Ad_X b} Ad_X
    const TangentMap<X> Adexp_f  = Ad<X>(smooth::exp<X>(-ef));
    const TangentMap<X> dAdexp_f = ad<X>(Adexp_f * dxlf) * Adexp_f * dr_exp<X>(-ef);

    for (auto nx = 0u; nx < Nx; ++nx) {
      const auto b0 = Nvars * (xf_B + nx);
      block_write(Hoplus_, tf_B, b0 + xf_B, dAdexp_f.middleRows(nx, 1));
    }

    Hoplus_.makeCompressed();
  }

public:
  template<typename A1, typename A2>
  FlatEndptFun(A1 && a1, A2 && a2) : f(std::forward<A1>(a1)), xl(std::forward<A2>(a2))
  {}

  template<typename T>
  auto operator()(
    const T & tf, const CastT<T, E> & e0, const CastT<T, E> & ef, const CastT<T, Q> & q) const
  {
    return f.template operator()<T>(tf, rplus(xl(T(0.)), e0), rplus(xl(tf), ef), q);
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  jacobian(double tf, const E & e0, const E & ef, const Q & q) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>>)
  {
    const auto & [xlfval, dxlfval] = diff::dr(xl, wrt(tf));
    const auto & Jf                = f.jacobian(tf, rplus(xl(0.), e0), rplus(xlfval, ef), q);

    update_joplus(e0, ef, dxlfval);

    J_ = Jf * Joplus_;
    J_.makeCompressed();
    return J_;
  }

  std::reference_wrapper<const Eigen::SparseMatrix<double>>
  hessian(double tf, const E & e0, const E & ef, const Q & q) requires(
    diff::detail::diffable_order1<F, std::tuple<double, X, X, Q>> &&
      diff::detail::diffable_order2<F, std::tuple<double, X, X, Q>>)
  {
    const auto & [xlfval, dxlfval] = diff::dr(xl, wrt(tf));
    const auto x0                  = rplus(xl(0.), e0);
    const auto xf                  = rplus(xlfval, ef);
    const auto & Jf                = f.jacobian(tf, x0, xf, q);  // Nouts x Nx
    const auto & Hf                = f.hessian(tf, x0, xf, q);   // Nx x (Nouts * Nx)

    update_joplus(e0, ef, dxlfval);
    update_hoplus(e0, ef, dxlfval);

    set_zero(H_);
    d2r_fog(H_, Jf, Hf, Joplus_, Hoplus_);
    H_.makeCompressed();
    return H_;
  }
};

}  // namespace detail
// \endcond

/**
 * @brief Flatten a LieGroup OCP by defining it in the tangent space around a trajectory.
 *
 * @param ocp OCPType defined on a LieGroup
 * @param xl nominal state trajectory
 * @param ul nominal state trajectory
 *
 * @note The flattened problem defines analytical jacobians and hessians if \p ocp does.
 *
 * @warn The Hessian of the flattened dynamics is not implemented in an efficient manner.
 *
 * @return FlatOCPType in variables (xe, ue) obtained via variables change x = xl ⊕ xe, u = ul ⊕
 * ue,
 */
auto flatten_ocp(const OCPType auto & ocp, auto && xl, auto && ul)
{
  using ocp_t = std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;
  using U     = typename ocp_t::U;
  using Xl    = decltype(xl);
  using Ul    = decltype(ul);

  static constexpr auto Nq = ocp_t::Nq;

  return OCP<
    Tangent<X>,
    Tangent<U>,
    detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>,
    detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>,
    detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>,
    detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>,
    detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>>{
    .theta = detail::FlatEndptFun<X, U, Nq, 1, decltype(ocp.theta), Xl>{ocp.theta, xl},
    .f     = detail::FlatDyn<X, U, decltype(ocp.f), Xl, Ul>{ocp.f, xl, ul},
    .g     = detail::FlatInnerFun<X, U, Nq, decltype(ocp.g), Xl, Ul>{ocp.g, xl, ul},
    .cr    = detail::FlatInnerFun<X, U, ocp_t::Ncr, decltype(ocp.cr), Xl, Ul>{ocp.cr, xl, ul},
    .crl   = ocp.crl,
    .cru   = ocp.cru,
    .ce    = detail::FlatEndptFun<X, U, Nq, ocp_t::Nce, decltype(ocp.ce), Xl>{ocp.ce, xl},
    .cel   = ocp.cel,
    .ceu   = ocp.ceu,
  };
}

/**
 * @brief Unflatten a FlatOCPSolution
 *
 * If flat_sol is a solution to flat_ocp = flatten_ocp(ocp, xl_fun, ul_fun),
 * then unflatten_ocpsol(flat_sol, xl_fun, ul_fun) is a solution to ocp.
 */
template<LieGroup X, Manifold U>
auto unflatten_ocpsol(const auto & flatsol, auto && xl_fun, auto && ul_fun)
{
  using ocpsol_t = std::decay_t<decltype(flatsol)>;

  auto u_unflat = [ul_fun = std::forward<decltype(ul_fun)>(ul_fun),
                   usol   = flatsol.u](double t) -> U { return rplus(ul_fun(t), usol(t)); };

  auto x_unflat = [xl_fun = std::forward<decltype(xl_fun)>(xl_fun),
                   xsol   = flatsol.x](double t) -> X { return rplus(xl_fun(t), xsol(t)); };

  return OCPSolution<X, U, ocpsol_t::Nq, ocpsol_t::Ncr, ocpsol_t::Nce>{
    .t0         = flatsol.t0,
    .tf         = flatsol.tf,
    .Q          = flatsol.Q,
    .u          = std::move(u_unflat),
    .x          = std::move(x_unflat),
    .lambda_q   = flatsol.lambda_q,
    .lambda_ce  = flatsol.lambda_ce,
    .lambda_dyn = flatsol.lambda_dyn,
    .lambda_cr  = flatsol.lambda_cr,
  };
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__FLATTEN_OCP_HPP_


///////////////////////////////////////////////////////////////////////////////
// tail_quantile.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. 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_ACCUMULATORS_STATISTICS_TAIL_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TAIL_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <cmath>             // For ceil
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // tail_quantile_impl
    //  Tail quantile estimation based on order statistics
    /**
        @brief Tail quantile estimation based on order statistics (for both left and right tails)

        The estimation of a tail quantile \f$\hat{q}\f$ with level \f$\alpha\f$ based on order statistics requires the
        chaching of at least the \f$\lceil n\alpha\rceil\f$ smallest or the \f$\lceil n(1-\alpha)\rceil\f$ largest samples,
        \f$n\f$ being the total number of samples. The largest of the \f$\lceil n\alpha\rceil\f$ smallest samples or the
        smallest of the \f$\lceil n(1-\alpha)\rceil\f$ largest samples provides an estimate for the quantile:

        \f[
            \hat{q}_{n,\alpha} = X_{\lceil \alpha n \rceil:n}
        \f]

        @param quantile_probability
    */
    template<typename Sample, typename LeftRight>
    struct tail_quantile_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef Sample result_type;

        tail_quantile_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            // If n is in a valid range, return result, otherwise return NaN or throw exception
            if ( n < static_cast<std::size_t>(tail(args).size()))
            {
               // Note that the cached samples of the left are sorted in ascending order,
               // whereas the samples of the right tail are sorted in descending order
               return *(boost::begin(tail(args)) + n - 1);
            }
            else
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }
            }
        }
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::tail_quantile<>
//
namespace tag
{
    template<typename LeftRight>
    struct tail_quantile
      : depends_on<count, tail<LeftRight> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::tail_quantile_impl<mpl::_1, LeftRight> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::tail_quantile
//
namespace extract
{
    extractor<tag::quantile> const tail_quantile = {};
}

using extract::tail_quantile;

// for the purposes of feature-based dependency resolution,
// tail_quantile<LeftRight> provide the same feature as quantile
template<typename LeftRight>
struct feature_of<tag::tail_quantile<LeftRight> >
  : feature_of<tag::quantile>
{
};

// So that tail_quantile can be automatically substituted with
// weighted_tail_quantile when the weight parameter is non-void.
template<typename LeftRight>
struct as_weighted_feature<tag::tail_quantile<LeftRight> >
{
    typedef tag::weighted_tail_quantile<LeftRight> type;
};

template<typename LeftRight>
struct feature_of<tag::weighted_tail_quantile<LeftRight> >
  : feature_of<tag::tail_quantile<LeftRight> >
{};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


#include "headers/rmat.h"
#include "headers/util.h"
#include <boost/numeric/ublas/matrix.hpp>

using boost::numeric::ublas::matrix;
using will::util::hash32Prob;

RmatConfig::RmatConfig(double _a, double _b, double _c) : a(_a), b(_b), c(_c) {
  d = 1. - _a + _b + _c;
  totalA = _a;
  totalB = totalA + _b;
  totalC = totalB + _c;
  if (d < 0) {
    std::cout << "Warning: a + b + c > 1" << std::endl;
  }
}

std::list<Edge> listRmat(size_t n, size_t nEdges, const RmatConfig &cfg) {
  cilk::reducer<cilk::op_list_append<Edge>> red;
  listRmatHelper(red, nEdges, cfg, 0, 0, n, n);
  const std::list<Edge> &edgeList = red.get_value();
  // std::vector<Edge> out(edgeList.size());
  // std::copy(edgeList.begin(), edgeList.end(), out.begin());
  return std::move(edgeList);
}

void listRmatHelper(cilk::reducer<cilk::op_list_append<Edge>> &red, size_t nEdges,
                    const RmatConfig &cfg, size_t x0, size_t y0, size_t x1, size_t y1) {
  if (nEdges == 0) return;
  size_t xMid = (x0 + x1) / 2;
  size_t yMid = (y0 + y1) / 2;
  size_t nCells = (x1 - x0) * (y1 - y0);

  if (nCells == 1) {
    red->push_back({y0, x0});
    return;
  }

  cilk::reducer< cilk::op_add<size_t> > redA(0);
  cilk::reducer< cilk::op_add<size_t> > redB(0);
  cilk::reducer< cilk::op_add<size_t> > redC(0);
  cilk::reducer< cilk::op_add<size_t> > redD(0);

  cilk_for(size_t i = 0; i < nEdges; ++i) {
    double prob = hash32Prob(i);
    if (prob <= cfg.totalA)
      *redA += 1;
    else if (prob <= cfg.totalB)
      *redB += 1;
    else if (prob <= cfg.totalC)
      *redC += 1;
    else
      *redD += 1;
  }
  // Required for some reason, can't use .get_value() directly in recursive call
  size_t numA = redA.get_value();
  size_t numB = redB.get_value();
  size_t numC = redC.get_value();
  size_t numD = redD.get_value();

  if (nCells == 4) {
    if (numA > 0) red->push_back({ y0, x0 });
    if (numB > 0) red->push_back({ y0, x1 - 1 });
    if (numC > 0) red->push_back({ y1 - 1, x0 });
    if (numD > 0) red->push_back({ y1 - 1, x1 - 1 });
  } else if (nCells > 4) {
    cilk_spawn listRmatHelper(red, numA, cfg, x0, y0, xMid, yMid);
    cilk_spawn listRmatHelper(red, numB, cfg, xMid, y0, x1, yMid);
    cilk_spawn listRmatHelper(red, numC, cfg, x0, yMid, xMid, y1);
    listRmatHelper(red, numD, cfg, xMid, yMid, x1, y1);
    cilk_sync;
  }
}


/* boost random/inversive_congruential.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: inversive_congruential.hpp,v 1.9 2004/07/27 03:43:32 dgregor Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP
#define BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP

#include <iostream>
#include <cassert>
#include <boost/config.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/const_mod.hpp>

namespace boost {
namespace random {

// Eichenauer and Lehn 1986
template<class IntType, IntType a, IntType b, IntType p, IntType val>
class inversive_congruential
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = (b == 0 ? 1 : 0);
  static const result_type max_value = p-1;
#else
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
#endif
  BOOST_STATIC_CONSTANT(result_type, multiplier = a);
  BOOST_STATIC_CONSTANT(result_type, increment = b);
  BOOST_STATIC_CONSTANT(result_type, modulus = p);

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return b == 0 ? 1 : 0; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return p-1; }

  explicit inversive_congruential(IntType y0 = 1) : value(y0)
  {
    BOOST_STATIC_ASSERT(b >= 0);
    BOOST_STATIC_ASSERT(p > 1);
    BOOST_STATIC_ASSERT(a >= 1);
    if(b == 0)
      assert(y0 > 0);
  }
  template<class It> inversive_congruential(It& first, It last)
  { seed(first, last); }

  void seed(IntType y0 = 1) { value = y0; if(b == 0) assert(y0 > 0); }
  template<class It> void seed(It& first, It last)
  {
    if(first == last)
      throw std::invalid_argument("inversive_congruential::seed");
    value = *first++;
  }
  IntType operator()()
  {
    typedef const_mod<IntType, p> do_mod;
    value = do_mod::mult_add(a, do_mod::invert(value), b);
    return value;
  }

  bool validation(result_type x) const { return val == x; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, inversive_congruential x)
  { os << x.value; return os; }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, inversive_congruential& x)
  { is >> x.value; return is; }
#endif

  friend bool operator==(inversive_congruential x, inversive_congruential y)
  { return x.value == y.value; }
  friend bool operator!=(inversive_congruential x, inversive_congruential y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(inversive_congruential rhs) const
  { return value == rhs.value; }
  bool operator!=(inversive_congruential rhs) const
  { return !(*this == rhs); }
#endif
private:
  IntType value;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const bool inversive_congruential<IntType, a, b, p, val>::has_fixed_range;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::min_value;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::max_value;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::multiplier;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::increment;
template<class IntType, IntType a, IntType b, IntType p, IntType val>
const typename inversive_congruential<IntType, a, b, p, val>::result_type inversive_congruential<IntType, a, b, p, val>::modulus;
#endif

} // namespace random

typedef random::inversive_congruential<int32_t, 9102, 2147483647-36884165,
  2147483647, 0> hellekalek1995;

} // namespace boost

#endif // BOOST_RANDOM_INVERSIVE_CONGRUENTIAL_HPP


// Copyright 2020 Google LLC
//
// 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.

#include <boost/math/special_functions/beta.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <stdexcept>

// Use the Boost `ibeta` implementation to generate an array of weights for the
// Harrell-Davis quantile estimator.
//
// For an array `v` of length `l` and quantile `q`,
//   v[i] = I_x1(a, b) - I_x0(a, b)
// where
//   x0 = i / l
//   x1 = (i + 1) / l
//   a = l * q
//   b = l * (1 - q)
//   and I_x() is the regularized beta function.
//
// Harrell, F., & Davis, C. (1982). A New Distribution-Free Quantile Estimator.
// Biometrika, 69(3), 635-640. doi:10.2307/2335999
//
// By default, gives a tab-seperated-values (tsv) output on stdout, but can also
// output raw doubles as bytes, so can be piped as input to another program
// (e.g. into a JS TypedArray).

using ExtendedDouble = long double;
// Other options for checking with extended precision, but little difference
// unless comparison also in extended precision. Note: no float128.hpp in Clang.
// #include <boost/multiprecision/cpp_bin_float.hpp>
// using ExtendedDouble = boost::multiprecision::cpp_bin_float_50;

int main(int argc, char** argv) {
  int arr_length = 0;
  ExtendedDouble quantile = 0;

  // Output tab separated if true, binary if not.
  bool output_tsv = true;

  try {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()("help,h", "print help message")(
        "length,l", po::value<int>(&arr_length)->required(),
        "length of the target array")(
        "quantile,q", po::value<ExtendedDouble>(&quantile)->required(),
        "quantile to use")(
        "format,f", po::value<std::string>()->default_value("tsv"),
        "Output format: either 'tsv' or 'bin'. Defaults to 'tsv'.");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help")) {
      std::cout << "Print a Harrell-Davis quantile estimator weight array.\n"
                << desc << "\n";
      return 1;
    }

    if (vm.count("format")) {
      std::string output_format = vm["format"].as<std::string>();
      if (output_format == "bin") {
        output_tsv = false;
      } else if (output_format != "tsv") {
        throw po::validation_error(po::validation_error::invalid_option_value,
                                   "format");
      }
    }

    // Ensure we have > double precision.
    int max_digits_10 = std::numeric_limits<ExtendedDouble>::max_digits10;
    if (max_digits_10 < 21) {
      throw std::runtime_error(
          "Insuffient precision available for calculation");
    }

    // Notify about missing arg(s) after --help, checking --format, etc.
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (output_tsv) {
    // Column headers for tsv.
    std::cout << "index\tvalue"
              << "\n";
    // Make sure enough digits of the upcoming numbers are printed.
    std::cout << std::setprecision(
        std::numeric_limits<ExtendedDouble>::max_digits10);
  }

  ExtendedDouble a = arr_length * quantile;
  ExtendedDouble b = arr_length * (1 - quantile);
  ExtendedDouble total = 0;

  for (int i = 0; i < arr_length; i++) {
    ExtendedDouble start =
        boost::math::ibeta(a, b, i / (ExtendedDouble)arr_length);
    ExtendedDouble end =
        boost::math::ibeta(a, b, (i + 1) / (ExtendedDouble)arr_length);
    ExtendedDouble value = end - start;
    total += value;

    if (output_tsv) {
      std::cout << i << "\t" << value << "\n";
    } else {
      // Only write doubles (not higher precision) to stdout so they can be
      // imported in JS.
      // Uncomment for boost::multiprecision.
      // double d_value = value.convert_to<double>();
      double d_value = (double)value;
      std::cout.write(reinterpret_cast<char*>(&d_value), sizeof d_value);
    }
  }

  if (output_tsv) {
    std::cerr << std::setprecision(
                     std::numeric_limits<ExtendedDouble>::max_digits10)
              << "Complete with a total of " << std::setprecision(20) << total
              << "\n";
  }
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "videoray/Throttle.h"
#include "nav_msgs/Odometry.h"

#include <iostream>
#include <sstream>

#include <boost/numeric/odeint.hpp>

using std::cout;
using std::endl;

using namespace boost::numeric::odeint;

typedef boost::array< double , 6 > state_type;

#define PI (3.14159265359)

videoray::Throttle throttle_;
void throttleCallback(const videoray::Throttle::ConstPtr& msg)
{
     throttle_ = *msg;
     //// Left Throttle Conversion:
     //left_vel_ = saturate(msg->LeftThrottle, -100, 100);
     //left_vel_ = normalize(left_vel_, -100, 100, -1, 1);
     //
     //// Right Throttle Conversion:
     //right_vel_ = saturate(msg->RightThrottle, -100, 100);
     //right_vel_ = normalize(right_vel_, -100, 100, -1, 1);
     //
     //// Vertical Throttle Conversion:
     //vert_vel_ = saturate(msg->VerticalThrottle, -100, 100);
     //vert_vel_ = normalize(vert_vel_, -100, 100, -1, 1);
}

nav_msgs::Odometry odom_;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
     odom_ = *msg;
}

// Linear and angular velocity states
double u;
double v;
double w;
double p;
double q;
double r;

// Added Mass Terms
double X_udot = 1.94; // inertia matrix M (m11)
double Y_vdot = 6.05; // inertia matrix M (m22)
double Z_wdot = 3.95; // m33
double N_rdot = 1.18e-2; // vehicle's motion of inertia about z-axis
// (6,6) entry of the vehicle inertia Matrix M

// Linear Drag Coefficients
double Xu = -0.95;
double Yv = -5.87;
double Nr = -0.023;
double Zw = -3.70;

// Quadratic Drag Coefficients
double Xuu = -6.04;
double Yvv = -30.73;
double Nrr = -0.45;
double Zww = -26.36;

double Ct_forw = 0.026667;
double Ct_back = 0.026667;
double Ct_vert_forw = 0.026667;
double Ct_vert_back = 0.026667;

double u_sat_low = -150;
double u_sat_high = 150;

// Control inputs
double X = 0;
double N = 0;
double Z = 0;

double u_port = 0;
double u_star = 0;
double u_vert = 0;

void videoray_model( const state_type &x , state_type &dxdt , double t )
{
/// States:
/// 0:  u     : surge velocity
/// 1:  v     : sway velocity
/// 2:  w     : heave velocity
/// 3:  p     : roll rate
/// 4:  q     : pitch rate
/// 5:  r     : yaw rate
     u = x[0];
     v = x[1];
     w = x[2];
     p = x[3];
     q = x[4];
     r = x[5];

     // Calculate fixed frame velocity rates
     dxdt[0] = (-Y_vdot*v*r + Xu*u + Xuu*u*abs(u) + X) / X_udot;
     dxdt[1] = (X_udot*u*r + Yv*v + Yvv*v*abs(v)) / Y_vdot;
     dxdt[2] = (Zw*w + Zww*w*abs(w) + Z) / Z_wdot;

     // Calculate fixed frame orientation rates
     dxdt[3] = 0;
     dxdt[4] = 0;
     dxdt[5] = (Nr*r + Nrr*r*abs(r) + N) / N_rdot;
}

//
// Converts input throttle commands to simulated linear and angular velocities.
//
geometry_msgs::Twist velocity_cmd_;
geometry_msgs::Vector3 velocity_linear_;
geometry_msgs::Vector3 velocity_angular_;

double saturate(double input, const double &min, const double &max)
{
     if (min > max) {
          ROS_INFO("saturate(): Invalid Min / Max Combo");
          return 0;
     } else if (input < min) {
          input = min;
     } else if(input > max) {
          input = max;
     }
     return input;
}

// Assumes that input has already been saturated within the in_min and in_max
// boundaries. Use the saturate() function on input before calling normalize
double normalize(double input, const double &in_min, const double &in_max,
                 const double &out_min, const double &out_max)
{
     if (in_min >= in_max || out_min >= out_max) {
          ROS_INFO("normalize(): Invalid Min / Max Combo");
          return 0;
     }

     double ratio = input / (in_max - in_min);
     return ratio * (out_max - out_min);

     return input;
}

double thrust_port = 0, thrust_star = 0;
void processThrottleCmds()
{
     u_port = saturate(throttle_.PortInput, u_sat_low, u_sat_high);
     u_star = saturate(throttle_.StarInput, u_sat_low, u_sat_high);
     u_vert = saturate(throttle_.VertInput, u_sat_low, u_sat_high);

     // Ct is different for reverse and forward
     if ( u_port >= 0 ) {
          thrust_port = u_port * Ct_forw;
     } else {
          thrust_port = u_port * Ct_back;
     }

     if ( u_star >= 0 ) {
          thrust_star = u_star * Ct_forw;
     } else {
          thrust_star = u_star * Ct_back;
     }

     X = thrust_port + thrust_star;
     N = thrust_star - thrust_port;

     // Ct is different for reverse and forward
     if ( u_vert >= 0 ) {
          Z = u_vert * Ct_vert_forw;
     } else {
          Z = u_vert * Ct_vert_back;
     }

}

void quaternionToEuler(const double &q0, const double &q1,
                       const double &q2, const double &q3,
                       double &roll, double &pitch, double &yaw)
{
     roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) );
     pitch = asin(2*(q0*q2-q3*q1));
     yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) );
}

runge_kutta4< state_type > stepper;

int main(int argc, char **argv)
{
     ros::init(argc, argv, "videoray_sim");
     ros::NodeHandle n;

     ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("motion", 1);
     ros::Subscriber throttle_sub = n.subscribe("throttle_cmds", 1,
                                                throttleCallback);
     ros::Subscriber odom_sub = n.subscribe("odometry", 1,
                                            odomCallback);

     double rate = 10;
     ros::Rate loop_rate(rate);

     ros::Time begin = ros::Time::now();
     ros::Time curr_time = begin;
     ros::Time prev_time = begin;
     ros::Duration dt = curr_time - prev_time;

     while (ros::ok())
     {
          //cout << "*" << std::flush;

          curr_time = ros::Time::now();
          dt = curr_time - prev_time;
          prev_time = curr_time;

          //cout << dt.toSec() << endl << std::flush;

          // Update state vector with odometry data from morse...
          state_type x = {0,0,0,0,0,0};
          x[0] = odom_.twist.twist.linear.x;
          x[1] = odom_.twist.twist.linear.y;
          x[2] = odom_.twist.twist.linear.z;
          x[3] = odom_.twist.twist.angular.x;
          x[4] = odom_.twist.twist.angular.y;
          x[5] = odom_.twist.twist.angular.z;

          processThrottleCmds();

          //geometry_msgs::Quaternion quat = odom_.pose.pose.orientation;
          //quaternionToEuler(quat.x, quat.y, quat.z, quat.w,
          //                  roll_, pitch_, yaw_);

          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x,
          //                                  curr_time.toSec() ,
          //                                  (curr_time + dt).toSec(),
          //                                  dt.toSec());
          boost::numeric::odeint::integrate(videoray_model,
                                            x,
                                            curr_time.toSec() ,
                                            curr_time.toSec() + 1.0/rate,
                                            1.0/rate);

          //ROS_INFO("========================");
          //ROS_INFO("Current: %f, \tdt: %f", curr_time.toSec(), dt.toSec());
          //ROS_INFO("Surge: %f", x[0]);
          //ROS_INFO("Sway: %f", x[1]);
          //ROS_INFO("Heave: %f", x[2]);
          //
          //ROS_INFO("3: %f", x[3]);
          //ROS_INFO("4: %f", x[4]);
          //ROS_INFO("5: %f", x[5]);
          ////ROS_INFO("6: %f", x[6]);
          ////ROS_INFO("7: %f", x[7]);
          ////ROS_INFO("8: %f", x[8]);
          ////ROS_INFO("9: %f", x[9]);
          ////ROS_INFO("10: %f", x[10]);
          ////ROS_INFO("11: %f", x[11]);

          velocity_linear_.x = x[0];
          velocity_linear_.y = x[1];
          velocity_linear_.z = x[2];
          velocity_angular_.z = x[5];

          velocity_cmd_.linear = velocity_linear_;
          velocity_cmd_.angular = velocity_angular_;
          twist_pub.publish(velocity_cmd_);

          ros::spinOnce();

          loop_rate.sleep();
     }
     return 0;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ex::devroye::detail::step4::squeeze.hpp         			//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_STEP4_SQUEEZE_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_STEP4_SQUEEZE_HPP_ER_2010
#include <boost/random/poisson_ext/devroye/detail/q.hpp>
#include <boost/random/poisson_ext/devroye/detail/math.hpp>
#include <boost/random/poisson_ext/devroye/detail/int_mean.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{
namespace step4{

    // Poisson sampler by the method of Devroye with a squeeze step4
	//
	// Complexity O(1) as mean -> infinity, p.204
    //
    // Bug : currently does not converge.
	template<typename Int ,typename T,typename P>
    class squeeze :
        detail::q<Int,T,P>,
        public detail::int_mean<step4::squeeze<Int,T,P>,Int,T,P>
    {

		typedef detail::q<Int,T,P> q_fun_;
        typedef devroye::detail::math<Int,T,P> ma_;

		typedef detail::int_mean<step4::squeeze<Int,T,P>,Int,T,P> super_;

		public:
		typedef typename super_::result_type result_type;
		typedef typename super_::input_type input_type;

		squeeze():super_(){}
		explicit squeeze(const result_type& mean):super_(mean){}

		bool accept(){
            bool a = ma_::is_strictly_negative(this->x());

			typedef result_type int_;
			typedef input_type float_;
			int_ y1 = this->y();
			int_ yp1 = y1 + 1;
            int_ y1yp1 = y1 * yp1;

			float_ t = ma_::to_float(y1yp1) / this->m2(); 	// 1.

			if(
            	(!a) && ( this->v() < (-t) )
            ){
				// exit
            }else{
                int_ m1 = this->int_mean_val();
                int_ m6 = 6 * m1;
                int_ y2 = 2 * y1;
                int_ y2p1 = y2 + 1;
                int_ m1p_ayp1 = m1 + a ? yp1 : 0;

            	float_ qr = t * (
                	ma_::to_float(y2p1) /ma_::to_float(m6) - ma_::to_float(1)
                );
                float_ tsq = t * t;
            	float_ qa =  qr - tsq  / ma_::to_float(3 * m1p_ayp1);

				if(this->v()<qa){
					// exit
                }else{
                	if(this->v()>qr){
                    	return false;
                    }else{

                    	float_ q_val = this->q_fun(m1,this->y());

                    	if(this->v() < q_val)
                        {
                        	// exit
                        }else{
                        	return false;
                        }
                    }
                }
				return false;
            }

            // exit :
            this->y_ += this->int_mean_val();
            return true;
		}

	};

}// detail
}// step4
}// devroye
}// poisson
}// random
}// boost

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_nonlinear_generic.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/filter/filter_interface.hpp>
#include <fl/distribution/gaussian.hpp>

namespace fl
{

/**
* \defgroup generic_nonlinear_gaussian_filter Generic Nonlinear Gaussian Filter
* \ingroup filters
*/

// Forward delcaration
template <typename...> class GaussianFilter;

/**
 * \internal
 * \ingroup nonlinear_gaussian_filter
 * \ingroup generic_nonlinear_gaussian_filter
 *
 * Traits for generic GaussianFilter based on quadrature (numeric integration)
 * with customizable policies, i.e implementations of the time and measurement
 * updates.
 */
template <
    typename Transition,
    typename Sensor,
    typename Quadrature,
    typename ... Policies
>
struct Traits<
           GaussianFilter<
               Transition, Sensor, Quadrature, Policies...>>
{
    typedef typename Transition::State State;
    typedef typename Transition::Input Input;
    typedef typename Sensor::Obsrv Obsrv;
    typedef Gaussian<State> Belief;
};

/**
 * \ingroup nonlinear_gaussian_filter
 * \ingroup generic_nonlinear_gaussian_filter
 *
 * GaussianFilter represents all filters based on Gaussian distributed systems.
 * This includes the Kalman Filter and filters using non-linear models such as
 * Sigma Point Kalman Filter family.
 *
 * \tparam TransitionFunction
 * \tparam SensorFunction
 * \tparam Quadrature
 * \tparam PredictionPolicy
 * \tparam UpdatePolicy
 */
template<
    typename TransitionFunction,
    typename SensorFunction,
    typename Quadrature,
    typename PredictionPolicy,
    typename UpdatePolicy
>
class GaussianFilter<
          TransitionFunction,
          SensorFunction,
          Quadrature,
          PredictionPolicy,
          UpdatePolicy>
    :
    /* Implement the filter interface */
    public FilterInterface<
               GaussianFilter<
                   TransitionFunction,
                   SensorFunction,
                   Quadrature,
                   PredictionPolicy,
                   UpdatePolicy>>
{
public:
    typedef typename TransitionFunction::State State;
    typedef typename TransitionFunction::Input Input;
    typedef typename SensorFunction::Obsrv Obsrv;
    typedef Gaussian<State> Belief;

public:
    /**
     * Creates a Gaussian filter
     *
     * \param transition         Process model instance
     * \param sensor           Obsrv model instance
     * \param transform   Point set tranfrom such as the unscented
     *                              transform
     */
    GaussianFilter(const TransitionFunction& transition,
                   const SensorFunction& sensor,
                   const Quadrature& quadrature)
        : transition_(transition),
          sensor_(sensor),
          quadrature_(quadrature)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~GaussianFilter() { }

    /**
     * \copydoc FilterInterface::predict
     */
    virtual void predict(const Belief& prior_belief,
                         const Input& input,
                         Belief& predicted_belief)
    {
        prediction_policy_(transition(),
                           quadrature(),
                           prior_belief,
                           input,
                           predicted_belief);
    }

    /**
     * \copydoc FilterInterface::update
     */
    virtual void update(const Belief& predicted_belief,
                        const Obsrv& obsrv,
                        Belief& posterior_belief)
    {
        update_policy_(sensor(),
                       quadrature(),
                       predicted_belief,
                       obsrv,
                       posterior_belief);
    }

public: /* factory functions */
    virtual Belief create_belief() const
    {
        auto belief = Belief(transition().state_dimension());
        return belief; // RVO
    }

public: /* accessors & mutators */
    TransitionFunction& transition()
    {
        return transition_;
    }

    SensorFunction& sensor()
    {
        return sensor_;
    }

    Quadrature& quadrature()
    {
        return quadrature_;
    }

    const TransitionFunction& transition() const
    {
        return transition_;
    }

    const SensorFunction& sensor() const
    {
        return sensor_;
    }

    const Quadrature& quadrature() const
    {
        return quadrature_;
    }

    virtual std::string name() const
    {
        return "GaussianFilter<"
                + this->list_arguments(
                            transition().name(),
                            sensor().name(),
                            quadrature().name(),
                            prediction_policy_.name(),
                            update_policy_.name())
                + ">";
    }

    virtual std::string description() const
    {
        return "Sigma point based GaussianFilter with"
                + this->list_descriptions(
                            transition().description(),
                            sensor().description(),
                            quadrature().description(),
                            prediction_policy_.description(),
                            update_policy_.description());
    }

protected:
    /** \cond internal */
    TransitionFunction transition_;
    SensorFunction sensor_;
    Quadrature quadrature_;
    PredictionPolicy prediction_policy_;
    UpdatePolicy update_policy_;
    /** \endcond */
};

}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
 * de Barcelona (UAB).
 * Copyright (C) 2019 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "opendrive/geometry/Geometry.h"

#include "opendrive/types.hpp"

#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
#include <cmath>
#include <stdexcept>

namespace opendrive {
namespace geometry {

DirectedPoint::DirectedPoint()
  : location(0, 0)
  , tangent(0)
{
}
DirectedPoint::DirectedPoint(const Point &point, double t)
  : location(point)
  , tangent(t)
{
}
DirectedPoint::DirectedPoint(double x, double y, double t)
  : location(x, y)
  , tangent(t)
{
}

void DirectedPoint::ApplyLateralOffset(double lateral_offset)
{
  auto normal_x = -std::sin(tangent);
  auto normal_y = std::cos(tangent);
  location.x += lateral_offset * normal_x;
  location.y += lateral_offset * normal_y;
}

GeometryType Geometry::GetType() const
{
  return _type;
}
double Geometry::GetLength() const
{
  return _length;
}
double Geometry::GetStartOffset() const
{
  return _start_position_offset;
}
double Geometry::GetHeading() const
{
  return _heading;
}

const Point &Geometry::GetStartPosition()
{
  return _start_position;
}

Geometry::Geometry(GeometryType type, double start_offset, double length, double heading, const Point &start_pos)
  : _type(type)
  , _length(length)
  , _start_position_offset(start_offset)
  , _heading(heading)
  , _start_position(start_pos)
{
  if (_length == 0.)
  {
    throw std::invalid_argument("Geometry of length 0");
  }
}

GeometryLine::GeometryLine(double start_offset, double length, double heading, const Point &start_pos)
  : Geometry(GeometryType::LINE, start_offset, length, heading, start_pos)
{
}

const DirectedPoint GeometryLine::PosFromDist(const double dist) const
{
  DirectedPoint p(_start_position, _heading);
  p.location.x += dist * std::cos(p.tangent);
  p.location.y += dist * std::sin(p.tangent);
  return p;
}

GeometryArc::GeometryArc(double start_offset, double length, double heading, const Point &start_pos, double curv)
  : Geometry(GeometryType::ARC, start_offset, length, heading, start_pos)
  , _curvature(curv)
{
}

const DirectedPoint GeometryArc::PosFromDist(double dist) const
{
  if (std::fabs(_curvature) < 1e-15)
  {
    // case not supported given the small curvature
    return DirectedPoint(_start_position, _heading);
  }
  const double radius = 1.0 / _curvature;
  const double theta = _heading - M_PI_2;
  double x = _start_position.x - radius * (cos(theta) - cos(theta + dist * _curvature));
  double y = _start_position.y - radius * (sin(theta) - sin(theta + dist * _curvature));

  double tangent = _heading + dist * _curvature;

  DirectedPoint p(x, y, tangent);

  return p;
}

double GeometryArc::GetCurvature() const
{
  return _curvature;
}

GeometryPoly3::GeometryPoly3(
  double start_offset, double length, double heading, const Point &start_pos, double a, double b, double c, double d)
  : Geometry(GeometryType::POLY3, start_offset, length, heading, start_pos)
  , _a{a}
  , _b{b}
  , _c{c}
  , _d{d}
{
}

const DirectedPoint GeometryPoly3::PosFromDist(const double dist) const
{
  auto poly = boost::array<double, 4>{{_a, _b, _c, _d}};

  double u = dist;
  double v = boost::math::tools::evaluate_polynomial(poly, u);

  const double cos_t = std::cos(_heading);
  const double sin_t = std::sin(_heading);

  double x0 = _start_position.x;
  double y0 = _start_position.y;
  double x = u * cos_t - v * sin_t;
  double y = u * sin_t + v * cos_t;

  auto tangentPoly = boost::array<double, 4>{{_b, 2.0 * _c, 3.0 * _d}};

  double tangentV = boost::math::tools::evaluate_polynomial(tangentPoly, u);
  double theta = atan2(tangentV, 1.0);

  DirectedPoint point(x0 + x, y0 + y, _heading + theta);
  return point;
}

GeometryParamPoly3::GeometryParamPoly3(double start_offset,
                                       double length,
                                       double heading,
                                       const Point &start_pos,
                                       double aU,
                                       double bU,
                                       double cU,
                                       double dU,
                                       double aV,
                                       double bV,
                                       double cV,
                                       double dV)
  : Geometry(GeometryType::PARAMPOLY3, start_offset, length, heading, start_pos)
  , _aU{aU}
  , _bU{bU}
  , _cU{cU}
  , _dU{dU}
  , _aV{aV}
  , _bV{bV}
  , _cV{cV}
  , _dV{dV}
{
}

const DirectedPoint GeometryParamPoly3::PosFromDist(const double dist) const
{
  double p = std::min(1.0, dist / _length);

  auto polyU = boost::array<double, 4>{{_aU, _bU, _cU, _dU}};
  auto polyV = boost::array<double, 4>{{_aV, _bV, _cV, _dV}};

  double u = boost::math::tools::evaluate_polynomial(polyU, p);
  double v = boost::math::tools::evaluate_polynomial(polyV, p);

  const double cos_t = std::cos(_heading);
  const double sin_t = std::sin(_heading);
  double x0 = _start_position.x;
  double y0 = _start_position.y;
  double x = u * cos_t - v * sin_t;
  double y = u * sin_t + v * cos_t;

  auto tangentPolyU = boost::array<double, 4>{{_bU, 2.0 * _cU, 3.0 * _dU, 0.0}};
  auto tangentPolyV = boost::array<double, 4>{{_bV, 2.0 * _cV, 3.0 * _dV, 0.0}};

  double tangentU = boost::math::tools::evaluate_polynomial(tangentPolyU, p);
  double tangentV = boost::math::tools::evaluate_polynomial(tangentPolyV, p);
  double theta = atan2(tangentV, tangentU);

  DirectedPoint point(x0 + x, y0 + y, _heading + theta);
  return point;
}

} // namespace
} // namespace


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

 * 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    cholesky.cpp
 * @brief   Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
 * @author  Richard Roberts
 * @date    Nov 5, 2010
 */

#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

  static const double negativePivotThreshold = -1e-1;
  static const double zeroPivotThreshold = 1e-6;
  static const double underconstrainedPrior = 1e-5;
  static const int underconstrainedExponentDifference = 12;

/* ************************************************************************* */
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {

  const bool debug = ISDEBUG("choleskyCareful");

  // Get pivot value
  double alpha = ATA(k,k);

  // Correct negative pivots from round-off error
  if(alpha < negativePivotThreshold) {
    if(debug) {
      cout << "pivot = " << alpha << endl;
      print(ATA, "Partially-factorized matrix: ");
    }
    return -1;
  } else if(alpha < 0.0)
    alpha = 0.0;

  const double beta = sqrt(alpha);

  if(beta > zeroPivotThreshold) {
    const double betainv = 1.0 / beta;

    // Update k,k
    ATA(k,k) = beta;

    if(k < (order-1)) {
      // Update A(k,k+1:end) <- A(k,k+1:end) / beta
      typedef Matrix::RowXpr::SegmentReturnType BlockRow;
      BlockRow V = ATA.row(k).segment(k+1, order-(k+1));
      V *= betainv;

      // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
      ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V;
//      ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
//          .rankUpdate(V.adjoint(), -1);
    }
    return 1;
  } else {
    // For zero pivots, add the underconstrained variable prior
    ATA(k,k) = underconstrainedPrior;
    for(size_t j=k+1; j<order; ++j)
      ATA(k,j) = 0.0;
    if(debug) cout << "choleskyCareful:  Skipping " << k << endl;
    return 0;
  }
}

/* ************************************************************************* */
pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {

  const bool debug = ISDEBUG("choleskyCareful");

  // Check that the matrix is square (we do not check for symmetry)
  assert(ATA.rows() == ATA.cols());

  // Number of rows/columns
  const size_t n = ATA.rows();

  // Negative order means factor the entire matrix
  if(order < 0)
    order = int(n);

  assert(size_t(order) <= n);

  // The index of the row after the last non-zero row of the square-root factor
  size_t maxrank = 0;
  bool success = true;

  // Factor row-by-row
  for(size_t k = 0; k < size_t(order); ++k) {
    int stepResult = choleskyStep(ATA, k, size_t(order));
    if(stepResult == 1) {
      if(debug) cout << "choleskyCareful:  Factored through " << k << endl;
      if(debug) print(ATA, "ATA: ");
      maxrank = k+1;
    } else if(stepResult == -1) {
      success = false;
      break;
    } /* else if(stepResult == 0) Found zero pivot */
  }

  return make_pair(maxrank, success);
}

/* ************************************************************************* */
bool choleskyPartial(Matrix& ABC, size_t nFrontal) {

  gttic(choleskyPartial);

  const bool debug = ISDEBUG("choleskyPartial");

  assert(ABC.rows() == ABC.cols());
  assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));

  const size_t n = ABC.rows();

  // Compute Cholesky factorization of A, overwrites A.
  gttic(lld);
  Eigen::ComputationInfo lltResult;
  if(nFrontal > 0)
  {
    Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0, 0, nFrontal, nFrontal).selfadjointView<Eigen::Upper>().llt();
    ABC.block(0, 0, nFrontal, nFrontal).triangularView<Eigen::Upper>() = llt.matrixU();
    lltResult = llt.info();
  }
  else
  {
    lltResult = Eigen::Success;
  }
  gttoc(lld);

  if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;

  // Compute S = inv(R') * B
  gttic(compute_S);
  if(n - nFrontal > 0) {
    ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
        ABC.topRightCorner(nFrontal, n-nFrontal));
  }
  if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
  gttoc(compute_S);

  // Compute L = C - S' * S
  gttic(compute_L);
  if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
  if(n - nFrontal > 0)
    ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
        ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
  if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
  gttoc(compute_L);

  // Check last diagonal element - Eigen does not check it
  bool ok;
  if(lltResult == Eigen::Success) {
    if(nFrontal >= 2) {
      int exp2, exp1;
      (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2);
      (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1);
      ok = (exp2 - exp1 < underconstrainedExponentDifference);
    } else if(nFrontal == 1) {
      int exp1;
      (void)frexp(ABC(0,0), &exp1);
      ok = (exp1 > -underconstrainedExponentDifference);
    } else {
      ok = true;
    }
  } else {
    ok = false;
  }

  return ok;
}

}


/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.

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

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

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include <ocs2_core/Types.h>
#include <ocs2_core/integration/eigenIntegration.h>

#include "ocs2_ddp/test/bouncingmass/Reference.h"

Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) {
  Create5thOrdPol(t0, t1, p0, p1);
  polV_ = polyder(polX_);
  polU_ = polyder(polV_);

  t0_ = t0;
  t1_ = t1;
}

void Reference::getInput(scalar_t time, vector_t& input) const {
  input.setZero(1);
  for (int i = 0; i < polU_.size(); i++) {
    input[0] += polU_[i] * std::pow(time, i);
  }
}

vector_t Reference::getInput(scalar_t time) const {
  vector_t input;
  getInput(time, input);
  return input;
}

void Reference::getState(scalar_t time, vector_t& x) const {
  if (time <= t1_ && time >= t0_) {
    x.setZero(3);
    for (int i = 0; i < polU_.size(); i++) {
      x[0] += polX_[i] * std::pow(time, i);
      x[1] += polV_[i] * std::pow(time, i);
    }
  } else {
    interpolate_ext(time, x);
  }
}

void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) {
  delta_ = delta;
  boost::numeric::odeint::runge_kutta_dopri5<vector_t, scalar_t, vector_t, scalar_t, boost::numeric::odeint::vector_space_algebra> stepper;
  // Lambda for general system dynamics, assuming that the reference input is available
  auto model = [](const vector_t& x, vector_t& dxdt, const double t, vector_t uref) {
    matrix_t A(3, 3);
    A << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    matrix_t B(3, 1);
    B << 0.0, 1.0, 0.0;

    dxdt = A * x + B * uref;
  };
  // pre-part of extension
  if (refPre != nullptr) {
    // Construct Lambda to represent System Dynamics with correct reference input
    auto preModel = [&refPre, &model](const vector_t& x, vector_t& dxdt, const double t) {
      vector_t uref = refPre->getInput(t);
      model(x, dxdt, t, uref);
    };
    // Construct lambda to act as observer, which will store the time and state trajectories
    scalar_array_t* timeStorePtr = &tPre_;
    vector_array_t* stateStorePtr = &xPre_;
    auto preObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) {
      timeStorePtr->push_back(t);
      stateStorePtr->push_back(x);
    };

    vector_t x0;
    getState(t0_, x0);
    scalar_t t0 = t0_;
    scalar_t t1 = t0 - delta;
    scalar_t dt = -1e-3;

    boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, dt, preObserver);
    std::reverse(std::begin(tPre_), std::end(tPre_));
    std::reverse(std::begin(xPre_), std::end(xPre_));
  }

  // post-part of extension
  if (refPost != nullptr) {
    // Construct Lambda to represent System Dynamics with correct reference input
    auto postModel = [&refPost, &model](const vector_t& x, vector_t& dxdt, const double t) {
      vector_t uref = refPost->getInput(t);
      model(x, dxdt, t, uref);
    };
    // Construct lambda to act as observer, which will store the time and state trajectories
    scalar_array_t* timeStorePtr = &tPost_;
    vector_array_t* stateStorePtr = &xPost_;
    auto postObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) {
      timeStorePtr->push_back(t);
      stateStorePtr->push_back(x);
    };

    vector_t x0;
    getState(t1_, x0);
    scalar_t t0 = t1_;
    scalar_t t1 = t0 + delta;
    scalar_t dt = 1e-3;
    boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, dt, postObserver);
  }
}

void Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) {
  Eigen::Matrix<scalar_t, 6, 6> A;
  Eigen::Matrix<scalar_t, 6, 6> Ainv;

  A << 1, t0, std::pow(t0, 2), std::pow(t0, 3), std::pow(t0, 4), std::pow(t0, 5), 0, 1, 2 * t0, 3 * std::pow(t0, 2), 4 * std::pow(t0, 3),
      5 * std::pow(t0, 4), 0, 0, 2, 6 * t0, 12 * std::pow(t0, 2), 20 * std::pow(t0, 3), 1, t1, std::pow(t1, 2), std::pow(t1, 3),
      std::pow(t1, 4), std::pow(t1, 5), 0, 1, 2 * t1, 3 * std::pow(t1, 2), 4 * std::pow(t1, 3), 5 * std::pow(t1, 4), 0, 0, 2, 6 * t1,
      12 * std::pow(t1, 2), 20 * std::pow(t1, 3);

  Ainv = A.inverse();

  Eigen::Matrix<scalar_t, 6, 1> x;
  x << p0, p1;
  polX_ = Ainv * x;
}

void Reference::interpolate_ext(scalar_t time, vector_t& x) const {
  const scalar_array_t* tVec;
  const vector_array_t* xVec;
  if (time < t0_) {
    tVec = &tPre_;
    xVec = &xPre_;
    x = xPre_.front();
  } else {
    tVec = &tPost_;
    xVec = &xPost_;
    x = xPost_.back();
  }

  int idx;
  for (int i = 0; i < tVec->size() - 1; i++) {
    if (time > tVec->at(i) && time < tVec->at(i + 1)) {
      idx = i;
      scalar_t fac = (time - tVec->at(idx)) / (tVec->at(idx + 1) - tVec->at(idx));
      x = fac * xVec->at(idx) + (1 - fac) * xVec->at(idx + 1);
      return;
    }
  }
}

void Reference::display() {
  std::cerr << "#########################" << std::endl;
  std::cerr << "#Pre-Extended-Trajectory#" << std::endl;
  std::cerr << "#########################" << std::endl;
  for (int i = 0; i < tPre_.size(); i++) {
    std::cerr << tPre_[i] << ";" << xPre_[i][0] << ";" << xPre_[i][1] << std::endl;
  }

  std::cerr << "#########################" << std::endl;
  std::cerr << "####Normal-Trajectory####" << std::endl;
  std::cerr << "#########################" << std::endl;

  scalar_t dt = 0.01;
  for (int i = 0; i < (t1_ - t0_) / dt; i++) {
    scalar_t t = t0_ + dt * i;
    vector_t x;
    getState(t, x);

    std::cerr << t << ";" << x[0] << ";" << x[1] << std::endl;
  }

  std::cerr << "##########################" << std::endl;
  std::cerr << "#Post-Extended-Trajectory#" << std::endl;
  std::cerr << "##########################" << std::endl;

  for (int i = 0; i < tPost_.size(); i++) {
    std::cerr << tPost_[i] << ";" << xPost_[i][0] << ";" << xPost_[i][1] << std::endl;
  }
}

Eigen::Matrix<scalar_t, 6, 1> Reference::polyder(Eigen::Matrix<scalar_t, 6, 1> pol) {
  Eigen::Matrix<scalar_t, 6, 1> polOld = pol;

  for (int i = 0; i < pol.size(); i++) {
    if (i < pol.size() - 1) {
      pol[i] = (i + 1) * polOld[i + 1];
    } else {
      pol[i] = 0;
    }
  }

  return pol;
}