#include <Eigen/Dense>
#include <iostream>
#include "timer.h"

using namespace Eigen;
using namespace std;

// WARNING: Global! Timer variable
timer<> tm_slow, tm_fast;

//! \brief Compute lmin from the vector d, naive implementation
//! \param[in] d VectorXd of size $n$
//! \param[in] double tol, tolerance
//! \param[out] double lmin

void rankoneinvit(const VectorXd & d, const double & tol, double & lmin)
{
    VectorXd ev=d;
    lmin=0;
    double lnew=d.cwiseAbs().minCoeff();
    while (abs(lnew-lmin)>tol*lmin) {
        tm_slow.start();
        lmin=lnew;
        MatrixXd M=d.asDi agonal();
        M+=ev*ev.transpose();
        ev = M.lu().solve(ev);
        ev.normalize();
        lnew=ev.transpose()*M*ev;
        tm_slow.stop();
    }
    lmin=lnew;
}

//! \brief Compute lmin from the vector d, optimized implementation
//! \param[in] d VectorXd of size $n$
//! \param[in] double tol, tolerance
//! \param[out] double lmin

void rankoneinvit_fast(const VectorXd & d, const double & tol, double & lmin)
{
    VectorXd ev=d;
    lmin=0;
    double lnew=d.cwiseAbs().minCoeff();

    VectorXd dinv=(1/d.array()).matrix();
    while (abs(lnew-lmin)>tol*lmin) {
        tm_fast.start();
        lmin=lnew;
        VectorXd ev0=ev;

        VectorXd Aib=dinv.cwiseProduct(ev); // In these three lines we solve the linear system with the
        double temp=ev.transpose()*Aib;     // Sherman-Morrison-Woodbury formula, in the case of rank-1
        ev=Aib*(1-temp/(1+temp));    // perturbations

        ev.normalize();
        lnew=ev.transpose()*d.cwiseProduct(ev)+pow(ev.transpose()*ev0,2); //better implementation
        tm_fast.stop();
    }
    lmin=lnew;
}

int main(){
    srand((unsigned int) time(0));
    double tol=1e-3;
    double lmin;
    int n=10;

    // Check correctedness of the fast version
    VectorXd d=VectorXd::Random(n);
    rankoneinvit(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;
    rankoneinvit_fast(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;

    //Compare runtimes
    unsigned int repeats = 3;

    for(unsigned int p = 2; p <= 9; p++) {
        tm_slow.reset();
        tm_fast.reset();
        unsigned int n = pow(2,p);

        for(unsigned int r = 0; r < repeats; ++r) {

//             d = VectorXd::Random(n);
            d = VectorXd::LinSpaced(n,1,2);
            rankoneinvit(d,tol,lmin);

            rankoneinvit_fast(d,tol,lmin);
        }

        cout << "The slow method took:    " <<  tm_slow.min().count() / 1000000. << " ms for n = " <<n<<endl;
        cout << "The fast method took:    " <<  tm_fast.min().count() / 1000000. << " ms for n = " <<n<<endl;
    }
}

Nummerical Methods for CSE/PS3/solutions_ps3/rankoneinvit.cpp


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Operations.cpp
/// \brief Implementation file for the SE3 Lie Group math functions.
/// \details These namespace functions provide implementations of the special Euclidean (SE)
///          Lie group functions that we commonly use in robotics.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <lgmath/se3/Operations.hpp>

#include <Eigen/Dense>
#include <stdexcept>
#include <stdio.h>

#include <lgmath/so3/Operations.hpp>

namespace lgmath {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 4x4 "skew symmetric matrix"
///
/// The hat (^) operator, builds the 4x4 skew symmetric matrix from the 3x1 axis angle
/// vector and 3x1 translation vector.
///
/// hat(rho, aaxis) = [aaxis^ rho] = [0.0  -a3   a2  rho1]
///                   [  0^T    0]   [ a3  0.0  -a1  rho2]
///                                  [-a2   a1  0.0  rho3]
///                                  [0.0  0.0  0.0   0.0]
///
/// See eq. 4 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix4d hat(const Eigen::Vector3d& rho, const Eigen::Vector3d& aaxis) {

  Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
  mat.topLeftCorner<3,3>() = so3::hat(aaxis);
  mat.topRightCorner<3,1>() = rho;
  return mat;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 4x4 "skew symmetric matrix"
///
/// The hat (^) operator, builds the 4x4 skew symmetric matrix from
/// the 6x1 se3 algebra vector, xi:
///
/// xi^ = [rho  ] = [aaxis^ rho] = [0.0  -a3   a2  rho1]
///       [aaxis]   [  0^T    0]   [ a3  0.0  -a1  rho2]
///                                [-a2   a1  0.0  rho3]
///                                [0.0  0.0  0.0   0.0]
///
/// See eq. 4 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix4d hat(const Eigen::Matrix<double,6,1>& xi) {

  return hat(xi.head<3>(), xi.tail<3>());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 "curly hat" matrix (related to the skew symmetric matrix)
///
/// The curly hat operator builds the 6x6 skew symmetric matrix from the 3x1 axis angle
/// vector and 3x1 translation vector.
///
/// curlyhat(rho, aaxis) = [aaxis^   rho^]
///                        [     0 aaxis^]
///
/// See eq. 12 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> curlyhat(const Eigen::Vector3d& rho, const Eigen::Vector3d& aaxis) {

  Eigen::Matrix<double,6,6> mat = Eigen::Matrix<double,6,6>::Zero();
  mat.topLeftCorner<3,3>() = mat.bottomRightCorner<3,3>() = so3::hat(aaxis);
  mat.topRightCorner<3,3>() = so3::hat(rho);
  return mat;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 "curly hat" matrix (related to the skew symmetric matrix)
///
/// The curly hat operator builds the 6x6 skew symmetric matrix
/// from the 6x1 se3 algebra vector, xi:
///
/// curlyhat(xi) = curlyhat([rho  ]) = [aaxis^   rho^]
///                        ([aaxis])   [     0 aaxis^]
///
/// See eq. 12 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> curlyhat(const Eigen::Matrix<double,6,1>& xi) {

  return curlyhat(xi.head<3>(), xi.tail<3>());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Turns a homogeneous point into a special 4x6 matrix
///
/// See eq. 72 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,4,6> point2fs(const Eigen::Vector3d& p, double scale) {

  Eigen::Matrix<double,4,6> mat = Eigen::Matrix<double,4,6>::Zero();
  mat.topLeftCorner<3,3>() = scale*Eigen::Matrix3d::Identity();
  mat.topRightCorner<3,3>() = -so3::hat(p);
  return mat;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Turns a homogeneous point into a special 6x4 matrix
///
/// See eq. 72 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,4> point2sf(const Eigen::Vector3d& p, double scale) {

  Eigen::Matrix<double,6,4> mat = Eigen::Matrix<double,6,4>::Zero();
  mat.bottomLeftCorner<3,3>() = -so3::hat(p);
  mat.topRightCorner<3,1>() = p;
  return mat;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds a transformation matrix using the analytical exponential map
///
/// This function builds a transformation matrix, T_ab, using the analytical exponential map,
/// from the se3 algebra vector, xi_ba,
///
///   T_ab = exp(xi_ba^) = [ C_ab r_ba_ina],   xi_ba = [  rho_ba]
///                        [  0^T        1]            [aaxis_ba]
///
/// where C_ab is a 3x3 rotation matrix from 'b' to 'a', r_ba_ina is the 3x1 translation
/// vector from 'a' to 'b' expressed in frame 'a', aaxis_ba is a 3x1 axis-angle vector,
/// the magnitude of the angle of rotation can be recovered by finding the norm of the vector,
/// and the axis of rotation is the unit-length vector that arises from normalization.
/// Note that the angle around the axis, aaxis_ba, is a right-hand-rule (counter-clockwise
/// positive) angle from 'a' to 'b'.
///
/// The parameter, rho_ba, is a special translation-like parameter related to 'twist' theory.
/// It is most inuitively described as being like a constant linear velocity (expressed in
/// the smoothly-moving frame) for a fixed duration; for example, consider the curve of a
/// car driving 'x' meters while turning at a rate of 'y' rad/s.
///
/// For more information see Barfoot-TRO-2014 Appendix B1.
///
/// Alternatively, we that note that
///
///   T_ba = exp(-xi_ba^) = exp(xi_ab^).
///
/// Both the analytical (numTerms = 0) or the numerical (numTerms > 0) may be evaluated.
//////////////////////////////////////////////////////////////////////////////////////////////
void vec2tran_analytical(const Eigen::Vector3d& rho_ba, const Eigen::Vector3d& aaxis_ba,
                         Eigen::Matrix3d* out_C_ab, Eigen::Vector3d* out_r_ba_ina) {

  // Check pointers
  if (out_C_ab == NULL) {
    throw std::invalid_argument("Null pointer out_C_ab in vec2tran_analytical");
  }
  if (out_r_ba_ina == NULL) {
    throw std::invalid_argument("Null pointer out_r_ba_ina in vec2tran_analytical");
  }

  if(aaxis_ba.norm() < 1e-12) {

    // If angle is very small, rotation is Identity
    *out_C_ab = Eigen::Matrix3d::Identity();
    *out_r_ba_ina = rho_ba;
  } else {

    // Normal analytical solution
    Eigen::Matrix3d J_ab;

    // Use rotation identity involving jacobian, as we need it to
    // convert rho_ba to the proper translation
    so3::vec2rot(aaxis_ba, out_C_ab, &J_ab);

    // Convert rho_ba (twist-translation) to r_ba_ina
    *out_r_ba_ina = J_ab*rho_ba;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds a transformation matrix using the first N terms of the infinite series
///
/// Builds a transformation matrix numerically using the infinite series evalation
/// of the exponential map.
///
/// For more information see eq. 96 in Barfoot-TRO-2014
//////////////////////////////////////////////////////////////////////////////////////////////
void vec2tran_numerical(const Eigen::Vector3d& rho_ba, const Eigen::Vector3d& aaxis_ba,
                        Eigen::Matrix3d* out_C_ab, Eigen::Vector3d* out_r_ba_ina,
                        unsigned int numTerms) {

  // Check pointers
  if (out_C_ab == NULL) {
    throw std::invalid_argument("Null pointer out_C_ab in vec2tran_numerical");
  }
  if (out_r_ba_ina == NULL) {
    throw std::invalid_argument("Null pointer out_r_ba_ina in vec2tran_numerical");
  }

  // Init 4x4 transformation
  Eigen::Matrix4d T_ab = Eigen::Matrix4d::Identity();

  // Incremental variables
  Eigen::Matrix<double,6,1> xi_ba; xi_ba << rho_ba, aaxis_ba;
  Eigen::Matrix4d x_small = se3::hat(xi_ba);
  Eigen::Matrix4d x_small_n = Eigen::Matrix4d::Identity();

  // Loop over sum up to the specified numTerms
  for (unsigned int n = 1; n <= numTerms; n++) {
    x_small_n = x_small_n*x_small/double(n);
    T_ab += x_small_n;
  }

  // Fill output
  *out_C_ab = T_ab.topLeftCorner<3,3>();
  *out_r_ba_ina = T_ab.topRightCorner<3,1>();
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 rotation and 3x1 translation using the exponential map, the
///        default parameters (numTerms = 0) use the analytical solution.
//////////////////////////////////////////////////////////////////////////////////////////////
void vec2tran(const Eigen::Matrix<double,6,1>& xi_ba, Eigen::Matrix3d* out_C_ab,
              Eigen::Vector3d* out_r_ba_ina, unsigned int numTerms) {

  if (numTerms == 0) {

    // Analytical solution
    vec2tran_analytical(xi_ba.head<3>(), xi_ba.tail<3>(), out_C_ab, out_r_ba_ina);
  } else {

    // Numerical solution (good for testing the analytical solution)
    vec2tran_numerical(xi_ba.head<3>(), xi_ba.tail<3>(), out_C_ab, out_r_ba_ina, numTerms);
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds a 4x4 transformation matrix using the exponential map, the
///        default parameters (numTerms = 0) use the analytical solution.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix4d vec2tran(const Eigen::Matrix<double,6,1>& xi_ba, unsigned int numTerms) {

  // Get rotation and translation
  Eigen::Matrix3d C_ab;
  Eigen::Vector3d r_ba_ina;
  vec2tran(xi_ba, &C_ab, &r_ba_ina, numTerms);

  // Fill output
  Eigen::Matrix4d T_ab = Eigen::Matrix4d::Identity();
  T_ab.topLeftCorner<3,3>() = C_ab;
  T_ab.topRightCorner<3,1>() = r_ba_ina;
  return T_ab;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute the matrix log of a transformation matrix (from the rotation and trans)
///
/// Compute the inverse of the exponential map (the logarithmic map). This lets us go from
/// a the 3x3 rotation and 3x1 translation vector back to a 6x1 se3 algebra vector (composed
/// of a 3x1 axis-angle vector and 3x1 twist-translation vector). In some cases, when the
/// rotation in the transformation matrix is 'numerically off', this involves some
/// 'projection' back to SE(3).
///
///   xi_ba = ln(T_ab)
///
/// where xi_ba is the 6x1 se3 algebra vector. Alternatively, we that note that
///
///   xi_ab = -xi_ba = ln(T_ba) = ln(T_ab^{-1})
///
/// See Barfoot-TRO-2014 Appendix B2 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,1> tran2vec(const Eigen::Matrix3d& C_ab,
                                   const Eigen::Vector3d& r_ba_ina) {

  // Init
  Eigen::Matrix<double,6,1> xi_ba;

  // Get axis angle from rotation matrix
  Eigen::Vector3d aaxis_ba = so3::rot2vec(C_ab);

  // Get twist-translation vector using Jacobian
  Eigen::Vector3d rho_ba = so3::vec2jacinv(aaxis_ba)*r_ba_ina;

  // Return se3 algebra vector
  xi_ba << rho_ba, aaxis_ba;
  return xi_ba;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute the matrix log of a transformation matrix
///
/// Compute the inverse of the exponential map (the logarithmic map). This lets us go from
/// a 4x4 transformation matrix back to a 6x1 se3 algebra vector (composed of a 3x1 axis-angle
/// vector and 3x1 twist-translation vector). In some cases, when the rotation in the
/// transformation matrix is 'numerically off', this involves some 'projection' back to SE(3).
///
///   xi_ba = ln(T_ab)
///
/// where xi_ba is the 6x1 se3 algebra vector. Alternatively, we that note that
///
///   xi_ab = -xi_ba = ln(T_ba) = ln(T_ab^{-1})
///
/// See Barfoot-TRO-2014 Appendix B2 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,1> tran2vec(const Eigen::Matrix4d& T_ab) {

  return tran2vec(T_ab.topLeftCorner<3,3>(), T_ab.topRightCorner<3,1>());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 adjoint transformation matrix from the 3x3 rotation matrix and 3x1
///        translation vector.
///
/// Builds the 6x6 adjoint transformation matrix from the 3x3 rotation matrix and 3x1
///        translation vector.
///
///  Adjoint(T_ab) = Adjoint([C_ab r_ba_ina]) = [C_ab r_ba_ina^*C_ab] = exp(curlyhat(xi_ba))
///                         ([ 0^T        1])   [   0           C_ab]
///
/// See eq. 101 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> tranAd(const Eigen::Matrix3d& C_ab,
                                 const Eigen::Vector3d& r_ba_ina) {

  Eigen::Matrix<double,6,6> adjoint_T_ab = Eigen::Matrix<double,6,6>::Zero();
  adjoint_T_ab.topLeftCorner<3,3>() = adjoint_T_ab.bottomRightCorner<3,3>() = C_ab;
  adjoint_T_ab.topRightCorner<3,3>() = so3::hat(r_ba_ina)*C_ab;
  return adjoint_T_ab;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 adjoint transformation matrix from a 4x4 one
///
/// Builds the 6x6 adjoint transformation matrix from a 4x4 transformation matrix
///
///  Adjoint(T_ab) = Adjoint([C_ab r_ba_ina]) = [C_ab r_ba_ina^*C_ab] = exp(curlyhat(xi_ba))
///                         ([ 0^T        1])   [   0           C_ab]
///
/// See eq. 101 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> tranAd(const Eigen::Matrix4d& T_ab) {

  return tranAd(T_ab.topLeftCorner<3,3>(), T_ab.topRightCorner<3,1>());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Construction of the 3x3 "Q" matrix, used in the 6x6 Jacobian of SE(3)
///
/// See eq. 102 in Barfoot-TRO-2014 for more information
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2Q(const Eigen::Vector3d& rho_ba, const Eigen::Vector3d& aaxis_ba) {

  // Construct scalar terms
  const double ang = aaxis_ba.norm();
  const double ang2 = ang*ang;
  const double ang3 = ang2*ang;
  const double ang4 = ang3*ang;
  const double ang5 = ang4*ang;
  const double cang = cos(ang);
  const double sang = sin(ang);
  const double m2 = (ang - sang)/ang3;
  const double m3 = (1.0 - 0.5*ang2 - cang)/ang4;
  const double m4 = 0.5*(m3 - 3*(ang - sang - ang3/6)/ang5);

  // Construct matrix terms
  Eigen::Matrix3d rx = so3::hat(rho_ba);
  Eigen::Matrix3d px = so3::hat(aaxis_ba);
  Eigen::Matrix3d pxrx = px*rx;
  Eigen::Matrix3d rxpx = rx*px;
  Eigen::Matrix3d pxrxpx = pxrx*px;

  // Construct Q matrix
  return 0.5 * rx + m2 * (pxrx + rxpx + pxrxpx)
         - m3 * (px*pxrx + rxpx*px - 3*pxrxpx) - m4 * (pxrxpx*px + px*pxrxpx);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Construction of the 3x3 "Q" matrix, used in the 6x6 Jacobian of SE(3)
///
/// See eq. 102 in Barfoot-TRO-2014 for more information
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2Q(const Eigen::Matrix<double,6,1>& xi_ba) {
  return vec2Q(xi_ba.head<3>(), xi_ba.tail<3>());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 Jacobian matrix of SE(3) using the analytical expression
///
/// Build the 6x6 left Jacobian of SE(3).
///
/// For the sake of a notation, we assign subscripts consistence with the transformation,
///
///   J_ab = J(xi_ba)
///
/// Where applicable, we also note that
///
///   J(xi_ba) = Adjoint(exp(xi_ba^)) * J(-xi_ba),
///
/// and
///
///   Adjoint(exp(xi_ba^)) = identity + curlyhat(xi_ba) * J(xi_ba).
///
/// For more information see eq. 100 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> vec2jac(const Eigen::Vector3d& rho_ba,
                                  const Eigen::Vector3d& aaxis_ba) {

  // Init
  Eigen::Matrix<double,6,6> J_ab = Eigen::Matrix<double,6,6>::Zero();

  if(aaxis_ba.norm() < 1e-12) {

    // If angle is very small, so3 jacobian is Identity
    J_ab.topLeftCorner<3,3>() = J_ab.bottomRightCorner<3,3>() = Eigen::Matrix3d::Identity();
    J_ab.topRightCorner<3,3>() = 0.5*so3::hat(rho_ba);
  } else {

    // General analytical scenario
    J_ab.topLeftCorner<3,3>() = J_ab.bottomRightCorner<3,3>() = so3::vec2jac(aaxis_ba);
    J_ab.topRightCorner<3,3>() = se3::vec2Q(rho_ba, aaxis_ba);
  }
  return J_ab;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 Jacobian matrix of SE(3) from the se(3) algebra; note that the
///        default parameter (numTerms = 0) will call the analytical solution, but the
///        numerical solution can also be evaluating to some number of terms.
///
/// For more information see eq. 100 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> vec2jac(const Eigen::Matrix<double,6,1>& xi_ba,
                                  unsigned int numTerms) {

  if (numTerms == 0) {

    // Analytical solution
    return vec2jac(xi_ba.head<3>(), xi_ba.tail<3>());
  } else {

    // Numerical solution (good for testing the analytical solution)
    Eigen::Matrix<double,6,6> J_ab = Eigen::Matrix<double,6,6>::Identity();

    // Incremental variables
    Eigen::Matrix<double,6,6> x_small = se3::curlyhat(xi_ba);
    Eigen::Matrix<double,6,6> x_small_n = Eigen::Matrix<double,6,6>::Identity();

    // Loop over sum up to the specified numTerms
    for (unsigned int n = 1; n <= numTerms; n++) {
      x_small_n = x_small_n*x_small/double(n+1);
      J_ab += x_small_n;
    }
    return J_ab;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 inverse Jacobian matrix of SE(3) using the analytical expression
///
/// Build the 6x6 inverse left Jacobian of SE(3).
///
/// For the sake of a notation, we assign subscripts consistence with the transformation,
///
///   J_ab_inverse = J(xi_ba)^{-1},
///
/// Please note that J_ab_inverse is not equivalent to J_ba:
///
///   J(xi_ba)^{-1} != J(-xi_ba)
///
/// For more information see eq. 103 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> vec2jacinv(const Eigen::Vector3d& rho_ba,
                                     const Eigen::Vector3d& aaxis_ba) {

  // Init
  Eigen::Matrix<double,6,6> J66_ab_inv = Eigen::Matrix<double,6,6>::Zero();

  if(aaxis_ba.norm() < 1e-12) {

    // If angle is very small, so3 jacobian is Identity
    J66_ab_inv.topLeftCorner<3,3>()  = J66_ab_inv.bottomRightCorner<3,3>()
                                     = Eigen::Matrix3d::Identity();
    J66_ab_inv.topRightCorner<3,3>() = -0.5*so3::hat(rho_ba);
  } else {

    // General analytical scenario
    Eigen::Matrix3d J33_ab_inv = so3::vec2jacinv(aaxis_ba);
    J66_ab_inv.topLeftCorner<3,3>() = J66_ab_inv.bottomRightCorner<3,3>() = J33_ab_inv;
    J66_ab_inv.topRightCorner<3,3>() = -J33_ab_inv*se3::vec2Q(rho_ba, aaxis_ba)*J33_ab_inv;
  }
  return J66_ab_inv;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 6x6 inverse Jacobian matrix of SE(3) from the se(3) algebra; note that
///        the default parameter (numTerms = 0) will call the analytical solution, but the
///        numerical solution can also be evaluating to some number of terms.
///
/// For more information see eq. 103 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix<double,6,6> vec2jacinv(const Eigen::Matrix<double,6,1>& xi_ba,
                                     unsigned int numTerms) {

  if (numTerms == 0) {

    // Analytical solution
    return vec2jacinv(xi_ba.head<3>(), xi_ba.tail<3>());
  } else {

    // Logic error
    if (numTerms > 20) {
      throw std::invalid_argument("Numerical vec2jacinv does not support numTerms > 20");
    }

    // Numerical solution (good for testing the analytical solution)
    Eigen::Matrix<double,6,6> J_ab = Eigen::Matrix<double,6,6>::Identity();

    // Incremental variables
    Eigen::Matrix<double,6,6> x_small = se3::curlyhat(xi_ba);
    Eigen::Matrix<double,6,6> x_small_n = Eigen::Matrix<double,6,6>::Identity();

    // Boost has a bernoulli package... but we shouldn't need more than 20
    Eigen::Matrix<double,21,1> bernoulli;
    bernoulli << 1.0, -0.5, 1.0/6.0, 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30.0,
                 0.0, 5.0/66.0, 0.0, -691.0/2730.0, 0.0, 7.0/6.0, 0.0, -3617.0/510.0,
                 0.0, 43867.0/798.0, 0.0, -174611.0/330.0;

    // Loop over sum up to the specified numTerms
    for (unsigned int n = 1; n <= numTerms; n++) {
      x_small_n = x_small_n*x_small/double(n);
      J_ab += bernoulli(n)*x_small_n;
    }
    return J_ab;
  }
}

} // se3
} // lgmath


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2020, Aditya Mohan

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

//[cross_product
//` Calculate the cross product of two points

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
     //Example 1 2D Vector

     bg::model::point<double, 2, bg::cs::cartesian> p1(7.0, 2.0);
     bg::model::point<double, 2, bg::cs::cartesian> p2(4.0, 5.0);

     bg::model::point<double, 2, bg::cs::cartesian> r1;

     r1 = bg::cross_product(p1,p2);

     std::cout << "Cross Product 1: "<< r1.get<0>() << std::endl;  //Note that the second point (r1.get<1>) would be undefined in this case

     //Example 2 - 3D Vector

     bg::model::point<double, 3, bg::cs::cartesian> p3(4.0, 6.0, 5.0);
     bg::model::point<double, 3, bg::cs::cartesian> p4(7.0, 2.0, 3.0);

     bg::model::point<double, 3, bg::cs::cartesian> r2;

     r2 = bg::cross_product(p3,p4);

     std::cout << "Cross Product 2: ("<< r2.get<0>() <<","<< r2.get<1>() <<","<< r2.get<2>() << ")"<< std::endl;

     return 0;
}

//]

//[cross_product_output
/*`
Output:
[pre
Cross Product 1: 27
Cross Product 2: (8,23,-34)
]
*/
//]


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// Define a 2nd order dual type using HigherOrderDual<N> construct.
using dual2nd = HigherOrderDual<2>;

// The scalar function for which the gradient is needed
dual2nd f(const VectorXdual2nd& x, dual2nd p)
{
    return x.cwiseProduct(x).sum() * exp(p); // sum([x(i) * x(i) for i = 1:5]) * exp(p)
}

int main()
{
    VectorXdual2nd x(5);    // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    dual2nd p = 3;    // the input parameter vector p with 3 variables

    dual2nd u;  // the output scalar u = f(x, p) evaluated together with gradient below
    VectorXd g; // gradient of f(x, p) evaluated together with Hessian below

    VectorXd H = hessian(f, wrtpack(p, x), at(x, p), u, g);  // evaluate the function value u and its gradient vector gp = [du/dp, du/dx]

    cout << "u = " << u << endl;    // print the evaluated output u
    cout << "Hessian = \n" << H << endl;  // print the evaluated gradient vector gp = [du/dp, du/dx]
    cout << "g =\n" << g << endl; // print the evaluated gradient vector gp = [du/dp, du/dx]
}


#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数
    Sophus::SO3 SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"旋转向量a属于so3(R3，是旋转矩阵对应的李代数)，a = "<<so3.transpose()<<endl;
    // hat 为向量到反对称矩阵
    cout<<"so3 hat, 旋转向量a的反对称矩阵a^ = \n"<<Sophus::SO3::hat(so3)<<endl;
    cout<<"SO3 matrix, 旋转向量a的指数映射为R = exp(a^) = "<<endl<<SO3_R.matrix()<<endl; //matrix函数返回的是旋转矩阵R
    // 相对的，vee为反对称到向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些

    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated, 旋转向量a属于so3(R3)，a = "<<SO3_updated<<endl;
    cout<<"SO3 updated, 旋转向量a的反对称矩阵a^ = \n"<<Sophus::SO3::hat(SO3_updated.log())<<endl;
    cout<<"SO3 updated, 旋转向量a的指数映射为旋转矩阵R(R属于特殊正交群) = exp(a^) = "<<endl<<SO3_updated.matrix()<<endl; //matrix函数返回的是旋转矩阵R

    /********************萌萌的分割线*****************************/
    cout<<"************我是分割线*************"<<endl;
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"b属于se3([t', 旋转向量a]属于R6，是变换矩阵对应的李代数), b= "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat, b的反对称矩阵b^ = \n"<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 matrix, b的指数映射为变换矩阵R(R属于特殊欧式群) = exp(b^) = "<<endl<<SE3_Rt.matrix()<<endl; //matrix函数返回的是旋转矩阵R
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated, T = "<<endl<<SE3_updated.matrix()<<endl; //matrix函数返回的是变换矩阵T

    return 0;
}


#include <iostream>
#include <vector>
#include <iomanip>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <eigen3/unsupported/Eigen/NumericalDiff>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

/*

https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm
https://en.wikipedia.org/wiki/Non-linear_least_squares
https://lingojam.com/SuperscriptGenerator
https://lingojam.com/SubscriptGenerator

consider m data points:
(x₁,y₁),(x₂,y₂),...(xₘ,yₘ)

and a functions which has n parameters:
β=(β₁, β₂,..., βₙ)

where m>=n

this gives us m residuals:
rᵢ=yᵢ-f(xᵢ,β)

our objective is minimize the:

S=Σ(rᵢ)²

The minimum value of S occurs when the gradient is zero

Lets say we have the following dataset:

i	    1	    2	    3	    4	    5	    6	    7
x	    0.038	0.194	0.425	0.626	1.253	2.500	3.740
y    	0.050	0.127	0.094	0.2122	0.2729	0.2665	0.3317

and we have the folliwng function:

y=(β₁*x)/(β₂+x)

so our r is
r₁=y₁ - (β₁*x₁)/(β₂+x₁)
r₂=y₂ - (β₁*x₂)/(β₂+x₂)
r₃=y₃ - (β₁*x₃)/(β₂+x₃)
r₄=y₄ - (β₁*x₄)/(β₂+x₄)
r₅=y₅ - (β₁*x₅)/(β₂+x₅)
r₆=y₆ - (β₁*x₆)/(β₂+x₆)
r₇=y₇ - (β₁*x₇)/(β₂+x₇)

r(β)₂ₓ₇  -> J₇ₓ₂

σrᵢ/σβ₁=-xᵢ/(β₂+xᵢ)

σrᵢ/σβ₂=(β₁*xᵢ)/(β₂xᵢ)²

βᵏ⁺¹=βᵏ- (JᵀJ)⁻¹Jᵀr(βᵏ)
*/

// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
};
// Tell the caller the matrix sizes associated with the input, output, and jacobian
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

// Local copy of the number of inputs
int m_inputs, m_values;

// Two constructors:
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }

};

typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > Point2DVector;

//Point2DVector GeneratePoints(const unsigned int numberOfPoints)

struct SubstrateConcentrationFunctor : Functor<double>
{

    SubstrateConcentrationFunctor(Eigen::MatrixXd points): Functor<double>(points.cols(),points.rows())
    {
        this->Points = points;
    }

    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &r) const
    {
        double x_i,y_i,beta1,beta2;
        for(unsigned int i = 0; i < this->Points.rows(); ++i)
        {
            y_i=this->Points.row(i)(1);
            x_i=this->Points.row(i)(0);
            beta1=z(0);
            beta2=z(1);
            r(i) =y_i-(beta1*x_i) /(beta2+x_i);
        }

        return 0;
    }
    Eigen::MatrixXd Points;

    int inputs() const { return 2; } // There are two parameters of the model, beta1, beta2
    int values() const { return this->Points.rows(); } // The number of observations
};

void SubstrateConcentrationLeastSquare()
{
/*
our data:

x     y
0.038,0.050;
0.194,0.127;
0.425,0.094;
0.626,0.2122;
1.253,0.2729;
2.500,0.2665;
3.740,0.3317;

the last column in the matrix should be "y"

*/

    Eigen::MatrixXd points(7,2);

    points.row(0)<< 0.038,0.050;
    points.row(1)<<0.194,0.127;
    points.row(2)<<0.425,0.094;
    points.row(3)<<0.626,0.2122;
    points.row(4)<<1.253,0.2729;
    points.row(5)<<2.500,0.2665;
    points.row(6)<<3.740,0.3317;

    SubstrateConcentrationFunctor functor(points);
    Eigen::NumericalDiff<SubstrateConcentrationFunctor,Eigen::NumericalDiffMode::Central> numDiff(functor);

    std::cout<<"functor.Points.size(): " <<functor.Points.size()<<std::endl;
    Eigen::VectorXd beta(2);
    beta<<0.9,0.2;

    Eigen::MatrixXd J(7,2);
    numDiff.df(beta,J);

    std::cout << "jacobian of matrix at "<< beta(0)<<","<<beta(1)  <<" is:\n " << J << std::endl;

    Eigen::VectorXd r(7);
    functor(beta,r);
    std::cout<<r<<std::endl;

    //βᵏ⁺¹=βᵏ- (JᵀJ)⁻¹Jᵀr(βᵏ)
    for(int i=0;i<10;i++)
    {
        numDiff.df(beta,J);
        std::cout<<"J: \n" << J<<std::endl;
        functor(beta,r);
        std::cout<<"r: \n" << r<<std::endl;
        beta=beta-(J.transpose()*J).inverse()*J.transpose()*r ;

    }
    std::cout<<"beta: \n" << beta<<std::endl;

    //optimal beta 0.36,0.556;

}

int main()
{
    SubstrateConcentrationLeastSquare();
}


/*
Refs: https://joelcfd.com/automatic-differentiation/
*/

#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>

template<typename T>
T MultiVarFunction(const T &x,const  T& y)
{
    T z=x*cos(y);
    return z;
}

void MultiVarFunctionDerivativesExample()
{
    Eigen::AutoDiffScalar<Eigen::VectorXd> x,y,z_derivative;

    x.value()=2  ;
    y.value()=M_PI/4;

/*
    Eigen::VectorXd::Unit(2, 0) means a unit vector with first element be 1

    0
    1

    which means we want compute the derivates relative to the first element
*/
    //we telling we want dz/dx
    x.derivatives() = Eigen::VectorXd::Unit(2, 0);

    //we telling we want dz/dy
    y.derivatives() = Eigen::VectorXd::Unit(2, 1);

    z_derivative = MultiVarFunction(x, y);

    std::cout << "AutoDiff:" << std::endl;
    std::cout << "Function output: " << z_derivative.value() << std::endl;
    std::cout << "Derivative: \n" << z_derivative.derivatives()<< std::endl;

}

template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct VectorFunction {

    typedef _Scalar Scalar;

    enum
    {
      InputsAtCompileTime = NX,
      ValuesAtCompileTime = NY
    };

    // Also needed by Eigen
    typedef Eigen::Matrix<Scalar, NX, 1> InputType;
    typedef Eigen::Matrix<Scalar, NY, 1> ValueType;

    // Vector function
    template <typename Scalar>
    void operator()(const Eigen::Matrix<Scalar, NX, 1>& x, Eigen::Matrix<Scalar, NY, 1>* y ) const
    {
        (*y)(0,0) = 10.0*pow(x(0,0)+3.0,2) +pow(x(1,0)-5.0,2) ;
        (*y)(1,0) = (x(0,0)+1)*x(1,0);
        (*y)(2,0) = sin(x(0,0))*x(1,0);
    }
};

void JacobianDerivativesExample()
{

    Eigen::Matrix<double, 2, 1> x;
    Eigen::Matrix<double, 3, 1> y;
    Eigen::Matrix<double, 3,2> fjac;

    Eigen::AutoDiffJacobian< VectorFunction<double,2, 3> > JacobianDerivatives;

    // Set values in x, y and fjac...

    x(0,0)=2.0;
    x(1,0)=3.0;

    JacobianDerivatives(x, &y, &fjac);

    std::cout << "jacobian of matrix at "<< x(0,0)<<","<<x(1,0)  <<" is:\n " << fjac << std::endl;

}

int main()
{
    MultiVarFunctionDerivativesExample();
    JacobianDerivativesExample();
}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  // Rotation matrix with 90 degrees along Z axis
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();

  // or quaternion
  Quaterniond q(R);

  // Sophus::SO3d can be constructed from rotation matrix
  Sophus::SO3d SO3_R(R);

  // or a from a quaternion
  Sophus::SO3d SO3_q(q);

  // They are equivalent
  cout << " SO(3) from matrix:\n" << SO3_R.matrix() << "\n";
  cout << " SO(3) from quaternion:\n" << SO3_q.matrix() << "\n";
  cout << "They are equal!" << "\n***************\n";

  // Use logarithic map to get the Lie Algebra
  Vector3d so3 = SO3_R.log();
  cout << "so3 = \n" << so3.transpose() << "\n\n";
  // hat is from vector to skew-symmetric matrix
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << "\n";
  // inversely from matrix to vector
  cout << "so3 hat vee=" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << "\n";

  // update by perturbation model
  // this is a small update
  Vector3d update_so3(1e-4, 0, 0);
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << "\n";
  cout << "*********************\n";

  // Similar for SE(3)
  // translation 1 along X
  Vector3d t(1, 0, 0);
  // construct SE3 from R, t
  Sophus::SE3d SE3_Rt(R, t);
  // or SE3 from q, t
  Sophus::SE3d SE3_qt(q, t);
  cout << "SE3 from R, t= \n" << SE3_Rt.matrix() << "\n";
  cout << "SE3 from q, t= \n" << SE3_qt.matrix() << "\n";

  // Lie Algebra is a 6d vector - we provide a typedef
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << "\n";

  return 0;
}


#include "catmull_rom_interpolation.h"
#include <Eigen/Dense>
#include<Eigen/Geometry>

Eigen::Vector3d catmull_rom_interpolation(
  const std::vector<std::pair<double, Eigen::Vector3d> > & keyframes,
  double t)
{
  /////////////////////////////////////////////////////////////////////////////
  /**
   * The solution below is based on the textbook pg. 383.
   */
  // Check if there are keyframes
  if (keyframes.size() == 0) {
    return Eigen::Vector3d(0, 0, 0);
  }

  // Loop the query times if t is greater than the largest keyframe time
  t = std::fmod(t, keyframes.back().first);

  // Step 1: Find index i for query time t. This is curve segment i
  int i = 0;
  for (i = 0; i < keyframes.size() - 1; i++) {
    // find t_i < t <= t_i+1
    if (keyframes[i].first < t && t <= keyframes[i+1].first) {
      break;
    }
  }

  // Set up some variables
  Eigen::Vector3d p0, p1, p2, p3;                       // points
  double t0, t1, t2, t3;                                // times
  double tension = 0.0;

  /**
   * Since Catmull-Rom doesn't interpolate the first or last point, we will use
   * cosine interpolation for these two points.
   */

  // Case 1: first point or last point
  if (i == 0 || i == keyframes.size() - 2) {
    // Source: http://paulbourke.net/miscellaneous/interpolation/
    p0 = keyframes[i].second;
    p1 = keyframes[i+1].second;

    t0 = keyframes[i].first;
    t1 = keyframes[i+1].first;

    double u = (t - t0) / (t1 - t0);
    double mu2 = (1 - cos(u * M_PI)) / 2;

    // Interpolate
    return (1 - mu2) * p0 + mu2 * p1;
  }

  // Case 2: some middle point
  /**
   * Find our four points. We will interpolate between p1 and p2.
   * We want:
   * f(0) = p1
   * f(1) = p2
   * f'(0) = (p2 - p0) / (t2 - t0)
   * f'(1) = (p3 - p1) / (t3 - t1)
   */
  p0 = keyframes[i-1].second;
  p1 = keyframes[i].second;
  p2 = keyframes[i+1].second;
  p3 = keyframes[i+2].second;

  t0 = keyframes[i-1].first;
  t1 = keyframes[i].first;
  t2 = keyframes[i+1].first;
  t3 = keyframes[i+2].first;

  // Calculate u --- unit parameter in [0, 1]
  double _u = (t - t1) / (t2 - t1);
  Eigen::RowVector4d u = Eigen::RowVector4d(1, _u, pow(_u, 2), pow(_u, 3));

  // Calculate derivatives/tangents at P1 and P2
  // Source: https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Catmull%E2%80%93Rom_spline
  Eigen::Vector3d v1 = (p2 - p0) / (t2 - t0);
  Eigen::Vector3d v2 = (p3 - p1) / (t3 - t1);

  // Calculate constraint matrix. See iPad for calculations
  Eigen::Matrix4d C;
  C <<
    1, 1 - (t2 - t0), 1, 1,
    1, 0, 0, 0,
    1, 1, 1, 1,
    1, (t3 - t1), 2 * (t3 - t1), 3 * (t3 - t1);

  // Calculate the cardinal matrix B and blending functions b(u)
  Eigen::Matrix4d B = C.inverse();
  // Eigen::Vector4d b_of_u = (u * B);

  // Create a matrix for the control points
  Eigen::Matrix<double, 4, 3> p;
  p.row(0) = p0;
  p.row(1) = p1;
  p.row(2) = p2;
  p.row(3) = p3;

  // Interpolate
  return u * B * p;
  /////////////////////////////////////////////////////////////////////////////
}

/**
 * This solution below is based on the wikipedia page:
 * https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Catmull%E2%80%93Rom_spline
 * This solution also works and should be doing the exact same thing as above.
 * It's probably a bit faster as well, but the solution above is more general.
 */
// #include "catmull_rom_interpolation.h"
// #include <Eigen/Dense>
// #include<Eigen/Geometry>

// Eigen::Vector3d catmull_rom_interpolation(
//   const std::vector<std::pair<double, Eigen::Vector3d> > & keyframes,
//   double t)
// {
//   /////////////////////////////////////////////////////////////////////////////
//   // Check if there are keyframes
//   if (keyframes.size() == 0) {
//     return Eigen::Vector3d(0, 0, 0);
//   }

//   // Loop the query times if t is greater than the largest keyframe time
//   t = std::fmod(t, keyframes.back().first);

//   // Step 1: Find index i for query time t. This is curve segment i
//   int i = 0;
//   for (i = 0; i < keyframes.size() - 1; i++) {
//     // find t_i < t <= t_i+1
//     if (keyframes[i].first < t && t <= keyframes[i+1].first) {
//       break;
//     }
//   }

//   // Set up some variables
//   Eigen::Vector3d p0, p1, p2, p3;                       // points
//   double t0, t1, t2, t3;                                // times
//   double tension = 0.0;

//   /**
//    * Since Catmull-Rom doesn't interpolate the first or last point, we will use
//    * cosine interpolation for these two points.
//    */

//   // Case 1: first point or last point
//   if (i == 0 || i == keyframes.size() - 2) {
//     // Source: http://paulbourke.net/miscellaneous/interpolation/
//     p0 = keyframes[i].second;
//     p1 = keyframes[i+1].second;

//     t0 = keyframes[i].first;
//     t1 = keyframes[i+1].first;

//     double u = (t - t0) / (t1 - t0);
//     double mu2 = (1 - cos(u * M_PI)) / 2;

//     // Interpolate
//     return (1 - mu2) * p0 + mu2 * p1;
//   }

//   // Case 2: some middle point
//   /**
//    * Find our four points. We will interpolate between p1 and p2.
//    * We want:
//    * f(0) = p1
//    * f(1) = p2
//    * f'(0) = (p2 - p0) / (t2 - t0)
//    * f'(1) = (p3 - p1) / (t3 - t1)
//    */
//   p0 = keyframes[i-1].second;
//   p1 = keyframes[i].second;
//   p2 = keyframes[i+1].second;
//   p3 = keyframes[i+2].second;

//   t0 = keyframes[i-1].first;
//   t1 = keyframes[i].first;
//   t2 = keyframes[i+1].first;
//   t3 = keyframes[i+2].first;

//   // Calculate u --- unit parameter in [0, 1]
//   double u = (t - t1) / (t2 - t1);

//   // Calculate derivatives/tangents at P1 and P2
//   // Source: https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Catmull%E2%80%93Rom_spline
//   Eigen::Vector3d m1 = (p2 - p0) / (t2 - t0);
//   Eigen::Vector3d m2 = (p3 - p1) / (t3 - t1);

//   // Set up our blending functions
//   double h_00 = 2 * pow(u, 3) - 3 * pow(u, 2) + 1;
//   double h_10 = pow(u, 3) - 2 * pow(u, 2) + u;
//   double h_01 = -2 * pow(u, 3) + 3 * pow(u, 2);
//   double h_11 = pow(u, 3) - pow(u, 2);

//   Eigen::Vector3d interp = h_00 * p1 + h_10 * m1 + h_01 * p2 + h_11 * m2;

//   return interp;
//   /////////////////////////////////////////////////////////////////////////////
// }


//
// Copyright 2021 Prathamesh Tagore <prathameshtagore@gmail.com>
//
// 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)
//
#include <boost/core/lightweight_test.hpp>
#include <boost/gil.hpp>
#include <array>
#include <cmath>
#include <vector>

namespace gil = boost::gil;

// This function utilizes the fact that sum of distances of a point on an ellipse from its foci
// is equal to the length of major axis of the ellipse.
// Parameters b and a represent half of lengths of vertical and horizontal axis respectively.
void test_rasterizer_follows_equation(
    std::vector<std::array<std::ptrdiff_t, 2>> trajectory_points, float a, float b)
{
    float focus_x, focus_y;
    if (a > b) // For horizontal ellipse
    {
        focus_x = a * std::sqrt(1 - b * b / (a * a));
        focus_y = 0;
    }
    else // For vertical ellipse
    {
        focus_x = 0;
        focus_y = b * std::sqrt(1 - a * a / (b * b));
    }

    for (auto trajectory_point : trajectory_points)
    {
        // To suppress conversion warnings from compiler
        std::array<float, 2> point {
            static_cast<float>(trajectory_point[0]), static_cast<float>(trajectory_point[1])};

        double dist_sum = std::sqrt(std::pow(focus_x - point[0], 2) +
            std::pow(focus_y - point[1], 2)) + std::sqrt(std::pow( - focus_x - point[0], 2) +
            std::pow( - focus_y - point[1], 2));
        if (a > b)
        {
            BOOST_TEST(std::abs(dist_sum - 2 * a) < 1);
        }
        else
        {
            BOOST_TEST(std::abs(dist_sum - 2 * b) < 1);
        }
    }
}

// This function verifies that the difference between x co-ordinates and y co-ordinates for two
// successive trajectory points is less than or equal to 1. This ensures that the curve is connected.
void test_connectivity(std::vector<std::array<std::ptrdiff_t, 2>> points)
{
    for (std::size_t i = 1; i < points.size(); ++i)
    {
        std::ptrdiff_t diff_x = points[i][0] - points[i - 1][0];
        std::ptrdiff_t diff_y = points[i][1] - points[i - 1][1];
        BOOST_TEST_LE(diff_x, 1);
        BOOST_TEST_LE(diff_y, 1);
    }
}

// We verify all test cases for the portion of ellipse in first quadrant, since all other portions
// can be constructed with simple reflection, they tend to be correct if first quadrant is verified.
int main()
{
    for (float a = 1; a < 101; ++a)
    {
        for (float b = 1; b < 101; ++b)
        {
            auto rasterizer = gil::midpoint_elliptical_rasterizer{};
            std::vector<std::array<std::ptrdiff_t, 2>> points = rasterizer.obtain_trajectory(
                {static_cast<unsigned int>(a), static_cast<unsigned int>(b)});
            test_rasterizer_follows_equation(points, a, b);
            test_connectivity(points);
        }
    }
    return boost::report_errors();
}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <chrono>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <functional>
#include <sstream>

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include "psm.h"

using namespace std;
using namespace std::chrono;
using namespace boost::multiprecision;

typedef number<backends::cpp_bin_float<11, backends::digit_base_2, void, std::int16_t, -14,   15>, et_off> cpp_bin_float_half;
typedef number<backends::cpp_bin_float<8,  backends::digit_base_2, void, std::int16_t, -126, 127>, et_off> cpp_bin_float_bfloat;

ofstream out;

template <class T>
T absval(T v) {
    return (v < T(0)) ? -v : v;
}

template <class T>
T nthCoefficientProduct(vector<T> &x, vector<T> &y, int n){
    /* This method computes the nth coefficient of x*y, runs in O(n)
     */
    T sum = 0;
    for(int i  = 0; i<=n; i++){
        sum+=x[i]*y[n-i];
    }
    return sum;
}

template <class T>
void updateNthCoefficient(vector<T> &x, vector<T> &xx, vector<T> &xxx, int n){
    /* This method asumes we have the n-th coefficient for x, it then updates the rest
     *
     * This method runs in O(N)
     */
    xx[n] = nthCoefficientProduct(x,x,n);
    xxx[n] = nthCoefficientProduct(xx,x,n);
 }

template <class T>
vector<T> coefficients(T x0, int nn){
    /* Inputs
     * n : order of the polynomial approximation
     * x0: initial conditions
     * Output:
     * coefficients of the nth polynomial approximation in increasing order
     *
     * Runs in O(n^2), memory O(n)
     */
    vector<T> x(nn,0);
    vector<T> xx(nn,0);
    vector<T> xxx(nn,0);
    vector<T> xprime(nn,0);
    x[0] = x0; //The initial condition is x0
    updateNthCoefficient<T>(x,xx,xxx,0);

    for(int n = 0; n<nn-1; n++){
        xprime[n] = xx[n]-xxx[n];
        x[n+1] = xprime[n]/(n+1);
        updateNthCoefficient<T>(x,xx,xxx,n+1);
    }
    return x;
}

template<class T>
pair<T, T> computeNext(T x0, T step, bool forward, int n){
    /*Inputs:
     * x0  : parameters of the ODE
     * step: Size of the step
     * forward: if you want to go forward or backwards
     * Outputs:
     * x(t) where t = x0+step and x solves the equation x' = x^2-x^3
     */
    vector<T> coeff = coefficients(x0,n);
    vector<T> deriv = derivative(coeff);
    if(forward){
        return make_pair(eval<T>(coeff,step), eval<T>(deriv,step));
    }
    else{
        return make_pair(eval<T>(coeff,-step), eval<T>(deriv,-step));
    }
}

template<class T>
pair<vector<T>, vector<T> > generateSolutionStepper(T x0, T step, bool forward, T end, int n){
    /*Inputs:
     * x0  : parameters of the ODE
     * step: Size of the step
     * end : End point to solve the solution (solves in [0,end])
     * n   : Degree of the approximation
     * forward: if you want to go forward or backwards
     * Output:
     * vector of solutions to the equation x' = x^2-x^3
     */
    step = absval(step);
    vector<T> sol;
    sol.push_back(x0);
    vector<T> deriv;
    T xx = x0 * x0;
    deriv.push_back(xx-x0*xx);
    T newx0 = x0;
    for(int k = 1; step*k<=end; k++){
        //cout << "step " << k << endl;
        pair<T, T> cond = computeNext<T>(newx0,step,forward,n);
        sol.push_back(cond.first);
        deriv.push_back(cond.second);
        newx0 = cond.first;
    }
    return make_pair(sol,deriv);
}

template<class T>
void solve_p4(T x0, T end, T step, int n, string fn)
{
    // generate solutions
    //cout << "size = " << sizeof(x0) << endl;
    vector<T> coeff = coefficients(x0,n);
    auto stepperB = generateSolutionStepper<T>(x0, step, 0, end, n);
    vector<T> solutionStepperB = stepperB.first;
    vector<T> derivSolutionB = stepperB.second;
    auto stepperF = generateSolutionStepper<T>(x0, step, 1, end, n);
    vector<T> solutionStepperF = stepperF.first;
    vector<T> derivSolutionF = stepperF.second;

    // print backward solution
    out.open(fn);
    for(int i = solutionStepperB.size()-1; i>=0; i--) {
        int dig = 1000;
        out.precision(5);
        out << fixed << setw(15) << -i*step << " ";
        T stepper = solutionStepperB[i];
        T deriv = derivSolutionB[i];
        if (absval(stepper) > dig || absval(deriv) > dig) {
            out << scientific << setw(15) << stepper << " " << setw(15) << deriv << "\n";
        }
        else{
            out << fixed << setw(15) << stepper << " " << setw(15) << deriv << "\n";
        }
    }

    // print forward solution
    for(int i = 1; i<solutionStepperF.size(); i++) {
        int dig = 1000;
        out.precision(5);
        out << fixed << setw(15) << i*step << " ";
        T stepper = solutionStepperF[i];
        T deriv = derivSolutionF[i];
        if (absval(stepper) > dig || absval(deriv) > dig) {
            out << scientific << setw(15) << stepper << " " << setw(15) << deriv << "\n";
        }
        else{
            out << fixed << setw(15) << stepper << " " << setw(15) << deriv << "\n";
        }
    }
    out.close();
}

int main(int argc, const char* argv[])
{
    // check parameters
    if (argc != 5) {
        cout << "Usage: " << argv[0] << " <x0> <tn> <dt> <rtype>" << endl;
        return EXIT_FAILURE;
    }

    // parse parameters
    double x0  = stod(argv[1], NULL);
    double end = stod(argv[2], NULL);
    double step = stod(argv[3], NULL);
    string rtype = string(argv[4]);

    if (rtype == "float") {
        solve_p4((float)x0, (float)end, (float)step, 10, "out.dat");
    } else if (rtype == "quad") {
        solve_p4(cpp_bin_float_quad(x0), cpp_bin_float_quad(end), cpp_bin_float_quad(step), 10, "out.dat");
    } else if (rtype == "half") {
        solve_p4(cpp_bin_float_half(x0), cpp_bin_float_half(end), cpp_bin_float_half(step), 10, "out.dat");
    } else if (rtype == "bfloat") {
        solve_p4(cpp_bin_float_bfloat(x0), cpp_bin_float_bfloat(end), cpp_bin_float_bfloat(step), 10, "out.dat");
    } else if (rtype == "rational") {
        solve_p4(cpp_rational(x0), cpp_rational(end), cpp_rational(step), 10, "out.dat");
    } else {
        solve_p4(x0, end, step, 10, "out.dat");
    }

    return EXIT_SUCCESS;
}


#ifndef ALM_HERMITE_QUATERNION_CURVE_H
#define ALM_HERMITE_QUATERNION_CURVE_H

#include <Eigen/Dense>
#include <iostream>
#include <math.h>
#include <algorithm>

// Hermite Quaternion curve for global frame quaternion trajectory given boundary conditions
// also computes global frame angular velocity and angular acceleration for s \in [0,1]

class HermiteQuaternionCurve{
public:
  HermiteQuaternionCurve();
  HermiteQuaternionCurve(const Eigen::Quaterniond & quat_start, const Eigen::Vector3d & angular_velocity_start,
                         const Eigen::Quaterniond & quat_end, const Eigen::Vector3d & angular_velocity_end);
  ~HermiteQuaternionCurve();

  void initialize(const Eigen::Quaterniond & quat_start, const Eigen::Vector3d & angular_velocity_start,
                  const Eigen::Quaterniond & quat_end, const Eigen::Vector3d & angular_velocity_end);

  // All values are expressed in "world frame"
  void evaluate(const double & s_in, Eigen::Quaterniond & quat_out);
  void getAngularVelocity(const double & s_in, Eigen::Vector3d & ang_vel_out);
  void getAngularAcceleration(const double & s_in, Eigen::Vector3d & ang_acc_out);

private:
  Eigen::Quaterniond qa; // Starting quaternion
  Eigen::Vector3d omega_a; // Starting Angular Velocity
  Eigen::Quaterniond qb; // Ending quaternion
  Eigen::Vector3d omega_b; // Ending Angular velocity

  Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
  Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

  void initialize_data_structures();

  void computeBasis(const double & s_in); // computes the basis functions
  void computeOmegas();

  Eigen::Quaterniond q0; // quat0
  Eigen::Quaterniond q1; // quat1
  Eigen::Quaterniond q2; // quat1
  Eigen::Quaterniond q3; // quat1

  double b1; // basis 1
  double b2; // basis 2
  double b3; // basis 3

  double bdot1; // 1st derivative of basis 1
  double bdot2; // 1st derivative of basis 2
  double bdot3; // 1st derivative of basis 3

  double bddot1; // 2nd derivative of basis 1
  double bddot2; // 2nd derivative of basis 2
  double bddot3; // 2nd derivative of basis 3

  Eigen::Vector3d omega_1;
  Eigen::Vector3d omega_2;
  Eigen::Vector3d omega_3;

  Eigen::AngleAxisd omega_1aa;
  Eigen::AngleAxisd omega_2aa;
  Eigen::AngleAxisd omega_3aa;

  // Allocate memory for quaternion operations
  Eigen::Quaterniond qtmp1;
  Eigen::Quaterniond qtmp2;
  Eigen::Quaterniond qtmp3;

  // progression variable
  double s_;
  // by default clamps within 0 and 1.
  double clamp(const double & s_in, double lo = 0.0, double hi = 1.0);

  void printQuat(const Eigen::Quaterniond & quat);

};

#endif


#include "plugins/evaluate/convexhull.h"
#include <Eigen/Core>

// Implementation of Andrew's monotone chain 2D convex hull algorithm.
// Asymptotic complexity: O(n log n).
// Practical performance: 0.5-1.0 seconds for n=1000000 on a 1GHz machine.
#include <algorithm>
#include <vector>

// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
// Returns a positive value, if OAB makes a counter-clockwise turn,
// negative for clockwise turn, and zero if the points are collinear.
float cross(const Eigen::Vector2f O, const Eigen::Vector2f A, const Eigen::Vector2f B) {
    return (long)(A.x() - O.x()) * (B.y() - O.y()) - (long)(A.y() - O.y()) * (B.x() - O.x());
}

// Returns a list of points on the convex hull in counter-clockwise order.
// Note: the last point in the returned list is the same as the first one.
std::vector<int> convex_hull(std::vector<int> idxs, std::function<Eigen::Vector2f(int)> & getPoint){
    int n = idxs.size(), k = 0;
    std::vector<int> hull(2*n);

    // Sort points lexicographically
    sort(idxs.begin(), idxs.end(), [&getPoint](int a_, int b_){
        Eigen::Vector2f a = getPoint(a_);
        Eigen::Vector2f b = getPoint(b_);
        return a.x() < b.x() || (a.x() == b.x() && a.y() < b.y());
    });

    // Build lower hull
    for (int i = 0; i < n; ++i) {
        while (k >= 2 && cross(getPoint(hull[k-2]), getPoint(hull[k-1]), getPoint(idxs[i])) <= 0) k--;
        hull[k++] = idxs[i];
    }

    // Build upper hull
    for (int i = n-2, t = k+1; i >= 0; i--) {
        while (k >= t && cross(getPoint(hull[k-2]), getPoint(hull[k-1]), getPoint(idxs[i])) <= 0) k--;
        hull[k++] = idxs[i];
    }

    hull.resize(k);
    return hull;
}


#include <fstream>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Dense>
#include <math.h>

#include "readfile.h"

using namespace Eigen;

VectorXd stochasticGD(const MatrixXd& X, const VectorXd& Y)
{
    const size_t allRows = X.rows();
    VectorXd theta = VectorXd::Zero(X.cols());

    for (size_t i = 0; i < allRows; ++i) {
        VectorXd hTheta = X.row(i) * theta;
        VectorXd diff = hTheta - Y.row(i);

        for (size_t j = 0; j < theta.rows(); ++j) {
            theta.row(j) -= (diff * X.row(i).col(j));
        }
    }

    return theta;
}

template <class OutputIterator>
VectorXd batchGD(const MatrixXd& X, const VectorXd& Y, OutputIterator costs, const size_t maxIter = 1000)
{
    const size_t allRows = X.rows();
    VectorXd theta = VectorXd::Zero(X.cols());

    const double acceptableCost = 0.000001;
    double cost = std::numeric_limits<double>::infinity();
    for (size_t i = 0; i < maxIter && cost > acceptableCost; ++i) {
        VectorXd hTheta = X * theta;
        VectorXd diff = hTheta - Y;

        auto diff_T = diff.transpose();

        for (size_t j = 0; j < theta.rows(); ++j) {
            theta.row(j) -= (diff_T * X.col(j)) / allRows;
        }

        costs = cost = (diff_T * diff / (2 * allRows))(0);
    }

    return theta;
}

void plotVector(const std::vector<double>& vec)
{
    FILE* gp = popen("gnuplot", "w");
    fprintf(gp, "reset \n");
    // fprintf(gp, "set terminal qt \n");
    fprintf(gp, "set grid \n");
    fprintf(gp, "set title 'Cost Function' \n");
    fprintf(gp, "set xlabel 'Number of Iterations' \n");
    fprintf(gp, "set ylabel 'Cost J' \n");
    fprintf(gp, "set style data lines \n");
    fprintf(gp, "plot '-' lw 1 \n");

    size_t i = 0;
    for (const auto& val : vec) {
        fprintf(gp, "%zu %f \n", i++, val);
    }
    fprintf(gp, "e\n");

    fflush(gp);

    puts("Hit enter to continue");
    getchar();
    pclose(gp);
}

int main(int argc, char* argv[])
{
    constexpr int rows = 13;
    constexpr int columns = 3;
    MatrixXd data(rows, columns);

    if (argc < 2) {
        printf("usage: %s <data>\n", argv[0]);
        return 0;
    }

    if (readData(argv[1], data) < 0) {
        fprintf(stderr, "Error reading data\n");
        return 0;
    }

    data.conservativeResize(NoChange, data.cols() + 1);
    data.col(columns) = data.col(columns - 1);
    data.col(columns - 1).setOnes();            // Set intercept to column to 1

    std::vector<double> costs;
    VectorXd theta = batchGD(data.block<rows, columns>(0, 0), data.col(columns), std::back_inserter(costs));
    std::cout << "theta: " << theta.transpose() << std::endl;
    std::cout << "iterations: " << costs.size() << std::endl;

    plotVector(costs);

    // std::cout << "Answer ->\n" << (X.transpose() * X).ldlt().solve(X.transpose() * Y) << std::endl;
    // std::cout << "Answer ->\n" << X.completeOrthogonalDecomposition().solve(Y) << std::endl;

    return 0;
}


#pragma once
#include "coordinate_transform.hpp"
#include "grad_shape.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>

//----------------compMatrixBegin----------------
//! Evaluate the stiffness matrix on the triangle spanned by
//! the points (a, b, c).
//!
//! Here, the stiffness matrix A is a 3x3 matrix
//!
//! $$A_{ij} = \int_{K} (\nabla \lambda_i^K(x, y) \cdot  \nabla \lambda_j^K(x, y) dV$$
//!
//! where $K$ is the triangle spanned by (a, b, c).
//!
//! @param[out] stiffnessMatrix should be a 3x3 matrix
//!                        At the end, will contain the integrals above.
//!
//! @param[in] a the first corner of the triangle
//! @param[in] b the second corner of the triangle
//! @param[in] c the third corner of the triangle
//! @param[in] sigma the function sigma as in the exercise
//! @param[in] r the parameter r as in the exercise
template <class MatrixType, class Point>
void computeStiffnessMatrix(MatrixType & stiffnessMatrix,
                            const Point &a,
                            const Point &b,
                            const Point &c) {
	Eigen::Matrix2d coordinateTransform = makeCoordinateTransform(b - a, c - a);
	double          volumeFactor        = std::abs(coordinateTransform.determinant());

	Eigen::Matrix2d elementMap = coordinateTransform.inverse().transpose();
	// (write your solution here)
	stiffnessMatrix.resize(3, 3);

	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			auto f = [&](double x, double y) -> double {
				Eigen::Vector2d gradLambdaI = elementMap * gradientLambda(i, x, y);
				Eigen::Vector2d gradLambdaJ = elementMap * gradientLambda(j, x, y);

				return gradLambdaI.dot(gradLambdaJ);
			};

			stiffnessMatrix(i, j) = volumeFactor * integrate2d(f);
		}
	}
}
//----------------compMatrixEnd----------------


#pragma once
//! c/c++ headers
#include <limits>
//! dependency headers
#include <armadillo>
//! project headers

/**
 * @brief Construct roll-pitch-yaw rotation matrix from angles
 *
 * @param [in] yaw rotation angle about z-axis
 * @param [in] pitch rotation angle about y-axis
 * @param [in] roll rotation angle about x-axis
 * @param [out] R rpy rotation matrix
 * @return
 */
void make_euler(double const & yaw, double const & pitch, double const & roll, arma::mat33 & R) {
  //! yaw
  auto const & psi = yaw;
  double const s_psi = std::sin(psi);
  double const c_psi = std::cos(psi);
  arma::mat33 const Ry = {{c_psi, -s_psi, 0}, {s_psi, c_psi, 0}, {0, 0, 1}};
  //! pitch
  auto const & theta = pitch;
  double const s_theta = std::sin(theta);
  double const c_theta = std::cos(theta);
  arma::mat33 const Rp = {{c_theta, 0, s_theta}, {0, 1, 0}, {-s_theta, 0, c_theta}};
  //! roll
  auto const & phi = roll;
  double const s_phi = std::sin(phi);
  double const c_phi = std::cos(phi);
  arma::mat33 const Rr = {{1, 0, 0}, {0, c_phi, -s_phi}, {0, s_phi, c_phi}};

  //! rpy sequence
  R = Ry * Rp * Rr;
}

/**
 * @brief Compute Euler angles from rotation matrix
 *
 * @param [in] R rpy rotation matrix
 * @param [out] angles column vector with (roll, pitch, yaw) angles of rpy rotation sequence
 * @param [in] flip_pitch (optional, default=false) use alternate convention for pitch angle
 * @return
 *
 * @note this routine assumes input matrix is a valid rotation matrix
 */
void find_euler_angles(arma::mat33 const & R, arma::vec3 & angles, bool flip_pitch = false) {
  auto const & r_31 = R(2, 0);
  if (std::abs(r_31 - 1.0) <= std::numeric_limits<double>::epsilon() ||
      std::abs(r_31 + 1.0) <= std::numeric_limits<double>::epsilon()) {
    //! gimbal lock condition
    angles(2) = static_cast<double>(0);
    if (r_31 < 0) {
      angles(1) = 0.5 * M_PI;
      angles(0) = angles(2) + std::atan2(R(0, 1), R(0, 2));
    } else {
      angles(1) = -0.5 * M_PI;
      angles(0) = -angles(2) + std::atan2(-R(0, 1), -R(0, 2));
    }
  } else {
    if (flip_pitch) {
      angles(1) = M_PI + std::asin(r_31);
    } else {
      angles(1) = -std::asin(r_31);
    }
    angles(0) = std::atan2(R(2, 1) / std::cos(angles(0)), R(2, 2) / std::cos(angles(0)));
    angles(2) = std::atan2(R(1, 0) / std::cos(angles(0)), R(0, 0) / std::cos(angles(0)));
  }
}


/* Main routine to run an implicit midpoint finite-difference method for solution
   of a second-order, scalar-valued BVP:

      u'' = p(t)*u' + q(t)*u + r(t),  a<t<b,
      u(a) = ua,  u(b) = ub

   where the problem has stiffness that may be adjusted using
   the real-valued parameter lambda<0 [read from the command line]

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#include <iostream>
#include <iomanip>
#include <vector>
#include <armadillo>
#include "bvp.hpp"

using namespace std;
using namespace arma;

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

  // get lambda from the command line, otherwise set to -10
  double lambda;
  if (argc > 1)
    lambda = atof(argv[1]);
  if (lambda >= 0.0)
    lambda = -10.0;

  // define BVP object
  BVP bvp(lambda);

  // loop over spatial resolutions for tests
  vector<int> N = {100, 1000, 10000};
  for (int i=0; i<N.size(); i++) {

    // output problem information
    cout << "\nImplicit Midpoint FD method for BVP with lambda = "
         << lambda << ":" << ",  N = " << N[i] << endl;

    // compute/store analytical solution
    vec t = zeros(N[i]+1);
    t(0) = 0.0;
    t(N[i]) = 1.0;
    for (int j=1; j<=N[i]-1; j++)
      t(j) = 0.5*(1.0-cos((2*j-1)*M_PI/(2*(N[i]-1))));
    vec utrue(N[i]+1);
    for (int j=0; j<=N[i]; j++)
      utrue(j) = bvp.utrue(t(j));

    // create matrix and right-hand side vectors
#ifdef USE_SPARSE
    sp_mat A(2*(N[i]+1),2*(N[i]+1));
#else
    mat A(2*(N[i]+1),2*(N[i]+1));
    A.fill(0.0);
#endif
    vec b(2*(N[i]+1));
    b.fill(0.0);

    // set up linear system:
    //   note: y = [y_{0,1} y_{0,2} y_{1,1} y_{1,2} ... y_{N,1} y_{N,2}]
    A(0,0) = 1.0;
    b(0) = bvp.ua;
    A(1,2*N[i]) = 1.0;
    b(1) = bvp.ub;
    for (int j=1; j<=N[i]; j++) {

      // setup interval-specific information
      double h = t(j)-t(j-1);
      double thalf = 0.5*(t(j)+t(j-1));
      double alpha = -h*bvp.q(thalf);
      double beta = h*bvp.p(thalf);
      double gamma = 2.0*h*bvp.r(thalf);

      // setup eqn in row 2*j:
      //    2*y_{j,1} - 2*y_{j-1,1} - h*y_{j,2} - h*y_{j-1,2} = 0
      A(2*j,2*j) = 2.0;
      A(2*j,2*j-2) = -2.0;
      A(2*j,2*j+1) = -h;
      A(2*j,2*j-1) = -h;
      b(2*j) = 0.0;

      // setup eqn in row 2*j+1:
      //     alpha*y_{j,1} + alpha*y_{j-1,1} + (2-beta)*y_{j,2} - (2+beta)*y_{j-1,2} = gamma_j
      A(2*j+1,2*j) = alpha;
      A(2*j+1,2*j-2) = alpha;
      A(2*j+1,2*j+1) = 2.0-beta;
      A(2*j+1,2*j-1) = -(2.0+beta);
      b(2*j+1) = gamma;
    }

    // solve linear system for BVP solution
#ifdef USE_SPARSE
    vec y = arma::spsolve(A,b);
#else
    vec y = arma::solve(A,b);
#endif

    // output maximum error
    vec u(N[i]+1);
    for (int j=0; j<=N[i]; j++)
       u(j) = y(2*j);
    vec uerr = abs(u-utrue);
    cout << "  Maximum BVP solution error = " << std::setprecision(4)
       << uerr.max() << "\n";
  }

  return 0;
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd A(3,3);
A << 4,-1,2, -1,6,0, 2,0,5;
cout << "The matrix A is" << endl << A << endl;

LLT<MatrixXd> lltOfA(A); // compute the Cholesky decomposition of A
MatrixXd L = lltOfA.matrixL(); // retrieve factor L  in the decomposition
// The previous two lines can also be written as "L = A.llt().matrixL()"

cout << "The Cholesky factor L is" << endl << L << endl;
cout << "To check this, let us compute L * L.transpose()" << endl;
cout << L * L.transpose() << endl;
cout << "This should equal the matrix A" << endl;

  return 0;
}


#include <iostream>
#include <NTL/ZZ.h>
#include <string>
#include <sstream>
#include <fstream>
#include <cstdlib>
#include <iomanip>
using namespace NTL;
using namespace std;

class RSA{
private:
    ZZ p;
    ZZ q;
    ZZ d;
    ZZ N;
    ZZ e;
    ZZ o;
    string abc="abcdefghijklmnopqrstuvwxyz ";
public:
    RSA();
    RSA(int n_digi);
    string RSA_encripta();
    string RSA_descifra();
    ZZ aleatorio(int num_digits2);
    ZZ modulo(ZZ a,ZZ b);
    ZZ eucl_ext(ZZ a,ZZ b);
    ZZ mcd(ZZ a,ZZ b);
    ZZ exp(ZZ base,ZZ exponente);
    ZZ exp_mod(ZZ base, ZZ exponente, ZZ mod);
    ZZ resto_chino(ZZ bloque_);
    bool Fermat(ZZ n,ZZ t);
    ZZ generar_primos(int num_digits);
    ZZ get_N(){return N;}
    string ZZto_string(ZZ a);
    ZZ STRto_ZZ(string str);
    string abrir_archivo();
    void guardar_archivo(string mensaje);
    string agregar_0(ZZ num,int K);
    string mensaje_abc(string mensaje,int K);
    string abc_mensaje(string mensaje);
};

/*Valores por defecto*/
RSA::RSA(){
    p=17;
    q=59;
    N=p*q;
    o=(p-1)*(q-1);
    e=3;
}

/*Generar p y q de n digitos*/
RSA::RSA(int n_digi){
    p=generar_primos(n_digi);
    q=generar_primos(n_digi);
    N=p*q;
    o=(p-1)*(q-1);
    e=aleatorio(n_digi/2);
    while(mcd(e,o)!=1){
        e++;
    }
    cout<<"p:"<<p<<"\nq:"<<q<<"\nN:"<<N<<"\no:"<<o<<"\ne:"<<e<<endl;
}

string RSA::RSA_encripta(){
    int K;
    string mensaje,mensaje_;
    cout<<"Palabra a encriptar: "<<endl;
    getline(cin,mensaje);
    K=(ZZto_string(get_N())).size(); //numero de digitos de N
    int i=0;
    string mensaje_cifrado;
    mensaje_=mensaje_abc(mensaje,K);
    while(i<mensaje_.size()){
        string bloque;
        ZZ bloque_,bloque__;
        bloque=mensaje_.substr(i,K-1); //sacamos subcadenas del tamaño de k-1
        bloque_=STRto_ZZ(bloque);
        bloque__=exp_mod(bloque_,e,N);
        mensaje_cifrado=mensaje_cifrado+agregar_0(bloque__,K);
        i=i+K-1;
    }
    guardar_archivo(mensaje_cifrado);
    return mensaje_cifrado;
}

string RSA::RSA_descifra(){
    int K;
    K=(ZZto_string(get_N())).size();
    string mensaje=abrir_archivo();
    d=eucl_ext((p-1)*(q-1),e);
    int i=0;
    string mensaje_descifrado;
    while(i<mensaje.size()){
        string bloque;
        ZZ bloque_,bloque__;
        bloque=mensaje.substr(i,K);
        bloque_=STRto_ZZ(bloque);
        bloque__=exp_mod(bloque_,d,N);
        //bloque__=resto_chino(bloque_);
        mensaje_descifrado=mensaje_descifrado+agregar_0(bloque__,K-1);
        i=i+K;
    }
    return abc_mensaje(mensaje_descifrado);
}

/*Generación de un número aleatorio*/
ZZ RSA::aleatorio(int num_digits2){
    ZZ num_digits;num_digits=num_digits2;
    ZZ flag; flag=1;
    ZZ rep;rep=num_digits/(num_digits/num_digits);
    int i=0;
    ZZ rand1;rand1=RandomBnd(rep+rep) + rep;
    while(i<rep){
        ZZ rand2;rand2=RandomBnd(rand1) + 1;
        flag=flag*rand2;
        i++;
    }
    string num2;num2=ZZto_string(flag);
    num2=num2.substr(0,num_digits2);
    flag=STRto_ZZ(num2);
    return flag;
}

/*Teorema de Fermat, usado para comprobar si un numero
aleatorio es primo o no*/
bool RSA::Fermat(ZZ n,ZZ t){
    ZZ i;
    ZZ a;
    for(i=1;i<t;i++){
        a=RandomBnd(n-1)+2;
        if(exp_mod(a,n-1,n)!=1){
            return false;
        }
    }
    return true;
}
/*Generación de números primos aleatorios*/
ZZ RSA::generar_primos(int num_digits){
    ZZ t;t=2;
    ZZ uno;uno=1;
    /*Se generan numeros aleatorios*/
    ZZ n;n=aleatorio(num_digits);
    n=n+uno;
    /*Se comprueba si dicho numero aleatorio es primo o no.
    Si no lo es, aumentará en una unidad hasta que este
    sea primo*/
    while(Fermat(n,t)==false){
        n=n+uno;
    }
    return n;
}

string RSA::mensaje_abc(string mensaje,int K){
    ZZ num;
    bool o=true;
    string mensaje_;
    for(int j=0;j<mensaje.size();j++){
        num=abc.find(mensaje[j]);
        mensaje_=mensaje_+agregar_0(num,2);
    }
    if(mensaje_.size()>(K-1)){
        while(mensaje_.size()%(K-1)!=0){
        mensaje_=mensaje_+"26";
        }
    }
    else{
        stringstream ss;
        ss<<setfill('0')<<setw(K-1)<<mensaje_;
        mensaje_=ss.str();
        return mensaje_;
    }
    return mensaje_;
}

string RSA::abc_mensaje(string mensaje){
    int num;
    int k=mensaje.size();
    string mensaje_;
    int i=0;
    string bloque;
    while(i<k){
        bloque=mensaje.substr(i,2);
        num=atoi(bloque.c_str());
        mensaje_=mensaje_+abc[num];
        i=i+2;
    }
    return mensaje_;
}

/*Guarda el mensaje encriptado en el archivo "mensaje_cifrado.txt"*/
void RSA::guardar_archivo(string mensaje_){
    ofstream guardar_mensaje;
    string nombre_archivo="mensaje_cifrado.txt";
    guardar_mensaje.open(nombre_archivo.c_str());
    guardar_mensaje<<mensaje_;
    guardar_mensaje.close();
}

/*Abre el archivo "mensaje_cifrado.txt" y retorna su contenido*/
string RSA::abrir_archivo(){
    ifstream archivo;
    string mensaje;
    archivo.open("mensaje_cifrado.txt",ios::in);
    if(archivo.fail()){
        cout<<"No se pudo abrir el archivo"<<endl;
    }
    while(!archivo.eof()){
        getline(archivo,mensaje);
    }
    return mensaje;
}

/*Convierte de entero a string*/
string RSA::ZZto_string(ZZ a){
    stringstream buffer;
    buffer << a;
    return buffer.str();
}

/*Convierte de string a entero*/
ZZ RSA::STRto_ZZ(string str){
    ZZ num(INIT_VAL,str.c_str());
    return num;
}

/*Agrega ceros a la izquierda*/
string RSA::agregar_0(ZZ num,int K){
    stringstream ss;
    string num_=ZZto_string(num);
    ss<<setfill('0')<<setw(K)<<num_;
    return ss.str();
}

/*Reemplaza al operador % (modulo)*/
ZZ RSA::modulo(ZZ a,ZZ b){
    ZZ r;r=a-((a/b)*b);
    if(r<0){
        return r+b;
    }
    else{
        return r;
    }
}

/*MCD*/
ZZ RSA::mcd(ZZ a, ZZ b){
    if (a==0){
        return b;
    }
    else{
        ZZ r;
        while(b>0){
           r=modulo(a,b);
           a=b;
           b=r;
        }
    }
    return a;
}

/*Euclides extendido*/
ZZ RSA::eucl_ext(ZZ a,ZZ b){
    ZZ a_inicial;a_inicial=a;
    ZZ t1,t2,r,q;t1=0,t2=1,r=1,q=0;
    ZZ t;t=0;
    while(r>0){
        q=a/b;
        r=a-q*b;
        a=b;
        b=r;
        t=t2;
        t2=t1-(q*t2);
        t1=t;
    }
    if(t1<0){
        t1=t1+a_inicial;
    }
    return t1;
}

/*Funcion exponencial*/
ZZ RSA::exp(ZZ base,ZZ exponente){
    ZZ resultado;resultado=1;
    while(exponente!=0){
    if(exponente-((exponente>>1)<<1)==1){
        resultado=base*resultado;
    }
    base=base*base;
    exponente=exponente>>1;
    }
    return resultado;

}

/*Exponenciacion modular*/
ZZ RSA::exp_mod(ZZ base,ZZ exponente,ZZ mod){
  ZZ resultado;resultado=1;
  while(exponente!=0){
    base=modulo(base,mod);
    if(exponente-((exponente>>1)<<1)==1){
      resultado=modulo(base*resultado,mod);
    }
    base=base*base;
    exponente=exponente>>1;
  }
  return resultado;
}

/*Teorema del resto chino*/
ZZ RSA::resto_chino(ZZ bloque_){
    d=eucl_ext((p-1)*(q-1),e);
    ZZ dp,dq,p1,p2,q1,q2;
    dp=modulo(d,(p-1));
    dq=modulo(d,(q-1));
    p1=N/p;
    q1=eucl_ext(p1,p);
    p2=N/q;
    q2=eucl_ext(p2,q);
    ZZ a;a=exp(bloque_,dp);
    ZZ b;b=exp(bloque_,dq);
    //cout<<"dp "<<dp<<" dq "<<dq<<" p1 "<<p1<<" q1 "<<q1<<" p2 "<<p2<<" q2 "<<q2<<" a "<<a<<" b "<<b<<endl;
    ZZ Do=modulo(((modulo(a*p1*q1,N))+(modulo(b*p2*q2,N))),N);
    return Do;
}

int main(){
    RSA A;
    A.RSA_encripta();
    cout<<A.RSA_descifra();
    return 0;
}


#ifndef MATHTOOLBOX_RBF_INTERPOLATION_HPP
#define MATHTOOLBOX_RBF_INTERPOLATION_HPP

#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <memory>
#include <vector>

namespace mathtoolbox
{
    /// \brief An abstract class for helping define custom kernel functor classes
    class AbstractRbfKernel
    {
    public:
        AbstractRbfKernel() {}
        virtual ~AbstractRbfKernel(){};

        /// \brief Perform RBF evaluation
        virtual double operator()(const double r) const = 0;
    };

    class GaussianRbfKernel final : public AbstractRbfKernel
    {
    public:
        GaussianRbfKernel(const double theta = 1.0) : m_theta(theta) {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            return std::exp(-m_theta * r * r);
        }

    private:
        const double m_theta;
    };

    /// \details This kernel is also known as the polyharmonic kernel with k = 1
    class LinearRbfKernel final : AbstractRbfKernel
    {
    public:
        LinearRbfKernel() {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            return r;
        }
    };

    /// \details This kernel is also known as the polyharmonic kernel with k = 2
    class ThinPlateSplineRbfKernel final : public AbstractRbfKernel
    {
    public:
        ThinPlateSplineRbfKernel() {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            const double value = r * r * std::log(r);
            return std::isnan(value) ? 0.0 : value;
        }
    };

    /// \details This kernel is also known as the polyharmonic kernel with k = 3
    class CubicRbfKernel final : public AbstractRbfKernel
    {
    public:
        CubicRbfKernel() {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            return r * r * r;
        }
    };

    class RbfInterpolator
    {
    public:
        RbfInterpolator(const std::function<double(const double)>& rbf_kernel          = ThinPlateSplineRbfKernel(),
                        const bool                                 use_polynomial_term = true);

        /// \brief Set data points and their values
        void SetData(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);

        /// \brief Calculate the interpolation weights
        ///
        /// \details This method should be called after setting the data
        void CalcWeights(const bool use_regularization = false, const double lambda = 0.001);

        /// \brief Calculate the interpolatetd value at the specified data point
        ///
        /// \details This method should be called after calculating the weights
        double CalcValue(const Eigen::VectorXd& x) const;

    private:
        /// \brief The RBF kernel
        const std::function<double(double)> m_rbf_kernel;

        /// \brief The polynomial term setting
        const bool m_use_polynomial_term;

        /// \brief Data locations
        Eigen::MatrixXd m_X;

        /// \brief Data values
        Eigen::VectorXd m_y;

        /// \brief Weights for the RBF kernel values
        Eigen::VectorXd m_w;

        /// \brief Weights for the polynomial terms
        Eigen::VectorXd m_v;
    };
} // namespace mathtoolbox

#endif // MATHTOOLBOX_RBF_INTERPOLATION_HPP


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file CommonMath.hpp
/// \brief Header file for some common math functions
/// \details defines some constants, angle-based functions, and comparison functions.
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_COMMON_MATH_HPP
#define LGM_COMMON_MATH_HPP

#include <Eigen/Core>

namespace lgmath {

//////////////////////////////////////////////////////////////////////////////////////////////
/// Define various pi functions -- found from matlab w long precision
//////////////////////////////////////////////////////////////////////////////////////////////
namespace constants {
  const double TWO_PI         = 6.283185307179586;  // 2*pi
  const double PI             = 3.141592653589793;  // pi
  const double PI_DIV_TWO     = 1.570796326794897;  // pi/2
  const double PI_DIV_FOUR    = 0.785398163397448;  // pi/4
  const double ONE_DIV_PI     = 0.318309886183791;  // 1/pi
  const double ONE_DIV_TWO_PI = 0.159154943091895;  // 1/2*pi
  const double DEG2RAD	      = 0.017453292519943;  // pi/180
  const double RAD2DEG	      = 57.295779513082323;  // 180/pi
} // constants

//////////////////////////////////////////////////////////////////////////////////////////////
/// Common math functions
//////////////////////////////////////////////////////////////////////////////////////////////
namespace common {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief moves a radian value to the range -pi, pi
//////////////////////////////////////////////////////////////////////////////////////////////
double angleMod(double radians);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief converts from degrees to radians
//////////////////////////////////////////////////////////////////////////////////////////////
double deg2rad(double degrees);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief converts from radians to degrees
//////////////////////////////////////////////////////////////////////////////////////////////
double rad2deg(double radians);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief compares if doubles are near equal
//////////////////////////////////////////////////////////////////////////////////////////////
bool nearEqual(double a, double b, double tol = 1e-6);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief compares if (double) Eigen matrices are near equal
//////////////////////////////////////////////////////////////////////////////////////////////
bool nearEqual(Eigen::MatrixXd A, Eigen::MatrixXd B, double tol = 1e-6);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief compares if radian angles are near equal
//////////////////////////////////////////////////////////////////////////////////////////////
bool nearEqualAngle(double radA, double radB, double tol = 1e-6);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief compares if axis angles are near equal
//////////////////////////////////////////////////////////////////////////////////////////////
bool nearEqualAxisAngle(Eigen::Matrix<double,3,1> aaxis1,
                        Eigen::Matrix<double,3,1> aaxis2, double tol = 1e-6);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief compares if lie algebra is near equal
//////////////////////////////////////////////////////////////////////////////////////////////
bool nearEqualLieAlg(Eigen::Matrix<double,6,1> vec1,
                     Eigen::Matrix<double,6,1> vec2, double tol = 1e-6);

} // common
} // lgmath

#endif // LGM_COMMON_MATH_HPP


//
// Created by egordm on 26-7-2018.
//

#ifndef PROJECT_MATH_UTILS_HPP
#define PROJECT_MATH_UTILS_HPP

#include "defines.h"
#include <sigpack.h>
#include <armadillo>

using namespace sp;
using namespace arma;

namespace libtempo { namespace utils { namespace math {

    inline int fix(double x) {
        return static_cast<int>(x >= 0 ? floor(x) : ceil(x));
    }

    inline vec sinc_fac(const vec &x) {
        vec ret(x.size());
        for(uword i = 0; i < x.size(); ++i) {
            if (fabs(x.at(i)) < EPSILON) ret.at(i) = 1;
            else ret.at(i) = sin(M_PI * x.at(i)) / (M_PI * x.at(i));
        }

        return ret;
    }

    inline arma::vec my_hanning( const arma::uword N ) {
        arma::vec h(N);
        for(arma::uword i=0; i<N; i++) {
            h[i] = 0.5-0.5*std::cos(sp::PI_2*(i)/(N-1));
        }
        return h;
    }

    inline int calc_gcd(int n1, int n2) {
        int tmp = 0;
        while (n1 > 0) {
            tmp = n1;
            n1 = n2 % n1;
            n2 = tmp;
        }
        return n2;
    }

    inline int quotient_ceil(int n1, int n2) {
        if (n1 % n2 != 0) return n1 / n2 + 1;
        return n1 / n2;
    }
}}}

#endif //PROJECT_MATH_UTILS_HPP


/** @file 20.cpp Problem 20: Factorial digit sum
 *
 * n! means n x (n - 1) x ... x 3 x 2 x 1
 *
 * For example, 10! = 10 x 9 x ... x 3 x 2 x 1 = 3628800, and the sum of the
 * digits in the number 10! is 3 + 6 + 2 + 8 + 8 + 0 + 0 = 27.
 *
 * Find the sum of the digits in the number 100!
 */

#include "cpp_int_util.hpp"                 // sum_digits

/// @cond
#include <boost/multiprecision/cpp_int.hpp> // cpp_int

#include <cassert>  // assert
#include <iostream> // cout
/// @endcond

using boost::multiprecision::cpp_int;

int sum(int n)
{
    cpp_int p = 1;
    for (int i = 2; i <= n; ++i)
        p *= i;
    return cpp_int_util::sum_digits(p);
}

int main()
{
    assert(sum(10) == 27);
    std::cout << sum(100) << std::endl;
}


#include <cstdlib>
#include <armadillo>
#include <iostream>
#include <cmath>
#include <iomanip>
#include <vector>
#include "time.h"

using namespace std; // note use of namespace
using namespace arma; // note use of namespace
int main (int argc, char* argv[])
{
  // read in dimension of square matrix
  int n = atoi(argv[1]);
  auto s = 1/sqrt( (double) n);
  // Start timing
  clock_t start, finish;
  // Allocate space for the three matrices
  mat A(n,n), B(n,n), C(n,n);
  // Set up values for matrix A and B
  for (auto i = 0; i < n; i++){
    for (auto j = 0; j < n; j++) {
      double angle = 2.0*M_PI*i*j/ (( double ) n);
      A(i,j) = s * ( sin ( angle ) + cos ( angle ) );
      B(j,i) =  A(i,j);
    }
  }
  C = 0.0;
  // Then perform the matrix-matrix multiplication using DGEMM
  start = clock();
  C = A*B;
  auto TraceMat = trace(C);
  finish = clock();
  double timeused = (double) (finish - start)/(CLOCKS_PER_SEC );
  cout << setiosflags(ios::showpoint | ios::uppercase);
  cout << setprecision(10) << setw(20) << "Time used  for matrix-matrix multiplication=" << timeused  << endl;
  cout << setprecision(10) << setw(20) << "Trace of Matrix=" << TraceMat  << endl;
  return 0;
}


#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
   Matrix2f A, b;
   LLT<Matrix2f> llt;
   A << 2, -1, -1, 3;
   b << 1, 2, 3, 1;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the right hand side b:\n" << b << endl;
   cout << "Computing LLT decomposition..." << endl;
   llt.compute(A);
   cout << "The solution is:\n" << llt.solve(b) << endl;
   A(1,1)++;
   cout << "The matrix A is now:\n" << A << endl;
   cout << "Computing LLT decomposition..." << endl;
   llt.compute(A);
   cout << "The solution is now:\n" << llt.solve(b) << endl;
}


#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;

//!
//! \brief buildDistanceLSQMatrix Efficiently build the system matrix
//! mapping positions to distances.
//! \param n Number of points (including $x_1$).
//! \return The system matrix $A$.
//!

SparseMatrix<double> buildDistanceLSQMatrix(int n) {
	SparseMatrix<double> A(n*(n-1)/2, n-1);

	std::vector<Triplet<double>> triplets;

	int row = 0;
	for(int i = 0; i < n - 1; i++) {
		for(int j = i; j < n - 1; j++) {
			triplets.push_back(Triplet<double>(row, j, 1));
			if(i > 0) {
				triplets.push_back(Triplet<double>(row, i - 1, -1));
			}
			row++;
		}
	}

	A.setFromTriplets(triplets.begin(), triplets.end());
	A.makeCompressed();
	return A;
}

//!
//! \brief estimatePointsPositions Return positions (without $x_1$).
//! The coordinate $x_1$ is assumed $x_1 = 0$.
//! \param D An $n \times n$ anti-symmetric matrix of distances.
//! \return Vector of positions $x_2, \dots, x_n$.
//!

VectorXd estimatePointsPositions(const MatrixXd& D) {

	VectorXd x;
	int n = D.rows();
	VectorXd b(n * (n - 1) / 2);

	// get matrix A
	SparseMatrix<double> A = buildDistanceLSQMatrix(n);

	// transform matrix D into vector with distance entries
	int idx = 0;
	for(int i = 0; i < n; i++) {
		for(int j = i + 1; j < n; j++) {
			b(idx++) = -D(i, j);
		}
	}

	// Solve sparse LSQ with normal equations
	SparseLU<SparseMatrix<double>> lu(A.transpose() * A);
	x = lu.solve(A.transpose() * b);

	return x;
}

int main() {

    int n = 5;

    // PART 1: build and print system matrix A
    std::cout << "**** PART 1 ****" << std::endl;
    std::cout << "The Matrix A is:"
              << std::endl
              << buildDistanceLSQMatrix(n)
              << std::endl;

    // PART 2: solve the LSQ system and find positions
    std::cout << "**** PART 2 ****" << std::endl;
    // Vector of positions
    n = 5;

    // Build D
    MatrixXd D(n,n);
    D << 0,   -2.1, -3,   -4.2, -5,
         2.1,  0,   -0.9, -2.2, -3.3,
         3,    0.9,  0,   -1.3, -1.1,
         4.2,  2.2,  1.3,  0,   -1.1,
         5,    3.3,  1.1,  1.1,  0;
    std::cout << "The matrix D is:"
              << std::endl
              << D
              << std::endl;

    // Find out positions
    VectorXd x_recovered = estimatePointsPositions(D);
    std::cout << "The positions [x_2, ..., x_n] obtained from the LSQ system are:"
              << std::endl
              << x_recovered
              << std::endl;
}


// advent2019_22.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <vector>
#include <fstream>
#include <string>

#include <boost/multiprecision/cpp_int.hpp>

using namespace std;

// C function for extended Euclidean Algorithm
__int64 gcdExtended(__int64 a, __int64 b, __int64* x, __int64* y);

// Function to find modulo inverse of a
__int64 modInverse(__int64 a, __int64 m)
{
    __int64 x, y;
    __int64 g = gcdExtended(a, m, &x, &y);
    if (g != 1)
        throw exception("Inverse doesn't exist");
    else
    {
        // m is added to handle negative x
        return (x % m + m) % m;
    }
}

// C function for extended Euclidean Algorithm
__int64 gcdExtended(__int64 a, __int64 b, __int64* x, __int64* y)
{
    // Base Case
    if (a == 0)
    {
        *x = 0, * y = 1;
        return b;
    }

    __int64 x1, y1; // To store results of recursive call
    __int64 gcd = gcdExtended(b % a, a, &x1, &y1);

    // Update x and y using results of recursive
    // call
    *x = y1 - (b / a) * x1;
    *y = x1;

    return gcd;
}

typedef vector<string> Commands;

struct Linear {
    __int64 mult;
    __int64 add;
    Linear() : mult(1), add(0) {}
    Linear(__int64 m, __int64 a) : mult(m), add(a) {}
};

void NewStack(Linear& linear, __int64 m)
{
    linear.mult = -linear.mult + m;
    linear.add = -linear.add - 1;
    while (linear.add < 0) linear.add += m;
}

void Cut(Linear& linear, __int64 n, __int64 m)
{
    linear.add = linear.add - n;
    if (linear.add >= m) linear.add -= m;
    if (linear.add < 0) linear.add += m;
}

__int64 MultMod(__int64 a, __int64 b, __int64 m)
{
    boost::multiprecision::int128_t a128(a);
    boost::multiprecision::int128_t b128(b);
    boost::multiprecision::int128_t r = a128 * b128;
    r %= m;
    return static_cast<__int64>(r);
}

void DealIncr(Linear& linear, __int64 n, __int64 m)
{
    linear.mult = MultMod(linear.mult, n, m);
    linear.add = MultMod(linear.add, n, m);
}

__int64 NewPos(Linear& linear, __int64 p, __int64 m)
{
    p = MultMod(linear.mult, p, m);
    p += linear.add;
    if (p >= m) p -= m;
    if (p < 0) p += m;
    return p;
}

Linear Invert(const Linear& linear, __int64 m)
{
    __int64 d = modInverse(linear.mult, m);
    Linear r( d, -MultMod(d, linear.add, m) );
    if (r.add < 0) r.add += m;
    return r;
}

__int64 Power(__int64 b, __int64 e, __int64 m)
{
    if (e == 0) return 1;
    if (e == 1) return b % m;

    __int64 t = Power(b, e / 2, m);
    t = MultMod(t, t, m);

    if (e % 2 == 0) return t;
    else return MultMod(b, t, m);
}

Linear Repeat(const Linear& linear, __int64 rpt, __int64 m)
{
    // A^n*x + (A^n-1) / (A-1) * B, from https://www.reddit.com/r/adventofcode/comments/ee0rqi/2019_day_22_solutions/fbnifwk/
    // Yes, I had to look up the solution.  So annoyed because I was so close and should
    // have been able to figure this out.
    __int64 mult = Power(linear.mult, rpt, m);
    __int64 add = mult - 1;
    add = MultMod(add, modInverse(linear.mult - 1, m), m);
    add = MultMod(add, linear.add, m);
    Linear r(mult, add);
    return r;
}

Linear Convert(const Commands& commands, __int64 m)
{
    Linear lin;
    for (auto c : commands) {
        if (c == "deal into new stack"s) NewStack(lin, m);
        else if (c.substr(0, 3) == "cut"s) {
            int n = atoi(c.substr(4).c_str());
            Cut(lin, n, m);
        }
        else if (c.substr(0, 19) == "deal with increment"s) {
            int n = atoi(c.substr(20).c_str());
            DealIncr(lin, n, m);
        }
    }

    return lin;
}

__int64 Part1(const Commands& commands, __int64 pos, __int64 m)
{
    Linear lin = Convert(commands, m);
    pos = NewPos(lin, pos, m);
    return pos;
}

__int64 Part2(const Commands& commands, __int64 pos, __int64 m, __int64 rpt)
{
    Linear lin = Convert(commands, m);
    lin = Invert(lin, m);
    lin = Repeat(lin, rpt, m);
    pos = NewPos(lin, pos, m);
    return pos;
}

int main()
{
    Commands commands;
    ifstream f("Data.txt");
    while (!f.eof()) {
        string s;
        getline(f, s);
        commands.push_back(s);
    }

    __int64 pos;

    pos = Part1(commands, 2019, 10007);
    cout << "Part 1: " << pos << endl;

    pos = Part2(commands, pos, 10007, 1);
    cout << "Reverse Check: " << pos << endl  << endl;

    pos = Part1(commands, 2019, 10007);
    pos = Part1(commands, pos, 10007);
    pos = Part1(commands, pos, 10007);
    pos = Part1(commands, pos, 10007);
    pos = Part1(commands, pos, 10007);
    cout << "Part 1*5: " << pos << endl;

    pos = Part2(commands, pos, 10007, 5);
    cout << "Reverse Check: " << pos << endl;

    pos = Part2(commands, 2020, 119315717514047i64, 101741582076661i64);
    cout << "Part 2: " << pos << endl;
}