// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <boost/math/tools/roots.hpp>
#include <cmath>
#include <cstddef>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "ErrorHandling/Error.hpp"
#include "ErrorHandling/Exceptions.hpp"
#include "Framework/TestHelpers.hpp"
#include "NumericalAlgorithms/RootFinding/NewtonRaphson.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeString.hpp"

namespace {
std::pair<double, double> func_and_deriv_free(double x) noexcept {
  return std::make_pair(2. - square(x), -2. * x);
}
struct FuncAndDeriv {
  std::pair<double, double> operator()(double x) const noexcept {
    return std::make_pair(2. - square(x), -2. * x);
  }
};

void test_simple() noexcept {
  /// [double_newton_raphson_root_find]
  const size_t digits = 8;
  const double correct = sqrt(2.);
  const double guess = 1.5;
  const double lower = 1.;
  const double upper = 2.;
  const auto func_and_deriv_lambda = [](double x) noexcept {
    return std::make_pair(2. - square(x), -2. * x);
  };
  const auto root_from_lambda = RootFinder::newton_raphson(
      func_and_deriv_lambda, guess, lower, upper, digits);
  /// [double_newton_raphson_root_find]
  const auto root_from_free = RootFinder::newton_raphson(
      func_and_deriv_free, guess, lower, upper, digits);
  const FuncAndDeriv func_and_deriv_functor{};
  const auto root_from_functor = RootFinder::newton_raphson(
      func_and_deriv_functor, guess, lower, upper, digits);
  CHECK(std::abs(root_from_lambda - correct) < 1.0 / std::pow(10, digits));
  CHECK(root_from_free == root_from_lambda);
  CHECK(root_from_free == root_from_functor);
}

void test_bounds() noexcept {
  const size_t digits = 8;
  const double guess = 1.5;
  double upper = 2.;
  double lower = sqrt(2.);
  const auto func_and_deriv_lambda = [](double x) noexcept {
    return std::make_pair(2. - square(x), -2. * x);
  };

  auto root = RootFinder::newton_raphson(func_and_deriv_lambda, guess, lower,
                                         upper, digits);
  const double correct = sqrt(2.);

  CHECK(std::abs(root - correct) < 1.0 / std::pow(10, digits));

  lower = 0.;
  upper = sqrt(2.);

  root = RootFinder::newton_raphson(func_and_deriv_lambda, guess, lower, upper,
                                    digits);
  CHECK(std::abs(root - correct) < 1.0 / std::pow(10, digits));
}

void test_datavector() noexcept {
  /// [datavector_newton_raphson_root_find]
  const size_t digits = 8;
  const DataVector guess{1.6, 1.9, -1.6, -1.9};
  const DataVector lower{sqrt(2.), sqrt(2.), -2., -3.};
  const DataVector upper{2., 3., -sqrt(2.), -sqrt(2.)};
  const DataVector constant{2., 4., 2., 4.};

  const auto func_and_deriv_lambda = [&constant](const double x,
                                                 const size_t i) noexcept {
    return std::make_pair(constant[i] - square(x), -2. * x);
  };

  const auto root = RootFinder::newton_raphson(func_and_deriv_lambda, guess,
                                               lower, upper, digits);
  /// [datavector_newton_raphson_root_find]

  const DataVector correct{sqrt(2.), 2., -sqrt(2.), -2.};

  for (size_t i = 0; i < guess.size(); i++) {
    CHECK(std::abs(root[i] - correct[i]) < 1.0 / std::pow(10, digits));
  }
}

void test_convergence_error_double() noexcept {
  const size_t max_iterations = 2;
  const size_t digits = 8;
  const double guess = 1.5;
  const double lower = 1.;
  const double upper = 2.;
  boost::uintmax_t max_iters = max_iterations;

  const auto func_and_deriv = [](double x) noexcept {
    return std::make_pair(2. - square(x), -2. * x);
  };

  // Exception message is expected to print the "best" result, so here
  // we obtain that result directly with boost, as RootFinder::newton_raphson
  // would throw an exception.
  // clang-tidy: internal boost warning, can't fix it.
  const auto best_result =
      boost::math::tools::newton_raphson_iterate(  // NOLINT
          func_and_deriv, guess, lower, upper,
          std::round(std::log2(std::pow(10, digits))), max_iters);
  test_throw_exception(
      [&func_and_deriv, &guess, &lower, &upper, &max_iters]() {
        RootFinder::newton_raphson(func_and_deriv, guess, lower, upper, digits,
                                   max_iters);
      },
      convergence_error(MakeString{}
                        << "newton_raphson reached max iterations of "
                        << max_iterations
                        << " without converging. Best result is: "
                        << best_result << " with residual "
                        << func_and_deriv(best_result).first));
}

void test_convergence_error_datavector() noexcept {
  const size_t max_iterations = 2;
  const size_t digits = 8;
  const auto digits_binary = std::round(std::log2(std::pow(10, digits)));
  const DataVector lower{sqrt(2.), sqrt(2.), -2., -3.};
  const DataVector upper{2., 3., -sqrt(2.), -sqrt(2.)};
  const DataVector constant{2., 4., 2., 4.};

  const auto func_and_deriv = [&constant](const double x,
                                          const size_t i) noexcept {
    return std::make_pair(constant[i] - square(x), -2. * x);
  };

  // We test with different guesses, the difference being the location of the
  // element in the DataVector expected to throw the exception.
  const std::array<DataVector, 4> guess{{{1.6, 1.9, -1.6, -1.9},
                                         {sqrt(2.), 1.9, -1.6, -1.9},
                                         {sqrt(2.), 2.0, -1.6, -1.9},
                                         {sqrt(2.), 2.0, -sqrt(2.), -1.9}}};
  for (size_t i = 0; i < guess.size(); ++i) {
    const DataVector& current_guess = gsl::at(guess, i);
    // Exception message is expected to print the "best" result, so here
    // we obtain that result directly with boost, as RootFinder::newton_raphson
    // would throw an exception.
    DataVector best_result_vector{lower.size()};
    for (size_t s = 0; s < best_result_vector.size(); ++s) {
      boost::uintmax_t max_iters = max_iterations;
      // clang-tidy: internal boost warning, can't fix it.
      best_result_vector[s] =
          boost::math::tools::newton_raphson_iterate(  // NOLINT
              [&func_and_deriv, s ](double x) noexcept {
                return func_and_deriv(x, s);
              },
              current_guess[s], lower[s], upper[s], digits_binary, max_iters);
    }
    for (size_t s = 0; s < best_result_vector.size(); ++s) {
      boost::uintmax_t max_iters = max_iterations;
      test_throw_exception(
          [&func_and_deriv, &current_guess, &lower, &upper, &max_iters]() {
            RootFinder::newton_raphson(func_and_deriv, current_guess, lower,
                                       upper, digits, max_iters);
          },
          convergence_error(MakeString{}
                            << "newton_raphson reached max iterations of "
                            << max_iterations
                            << " without converging. Best result is: "
                            << best_result_vector[i] << " with residual "
                            << func_and_deriv(best_result_vector[i], i).first));
    }
  }
}

SPECTRE_TEST_CASE("Unit.Numerical.RootFinding.NewtonRaphson",
                  "[NumericalAlgorithms][RootFinding][Unit]") {
  test_simple();
  test_bounds();
  test_datavector();
  test_convergence_error_double();
  test_convergence_error_datavector();
}

// [[OutputRegex, The desired accuracy of 100 base-10 digits must be smaller]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.Numerical.RootFinding.NewtonRaphson.Digits.Double",
    "[NumericalAlgorithms][RootFinding][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const size_t digits = 100;
  const double guess = 1.5;
  double lower = 1.;
  double upper = 2.;
  const auto func_and_deriv_lambda = [](double x) {
    return std::make_pair(2. - square(x), -2. * x);
  };

  RootFinder::newton_raphson(func_and_deriv_lambda, guess, lower, upper,
                             digits);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The desired accuracy of 100 base-10 digits must be smaller]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.Numerical.RootFinding.NewtonRaphson.Digits.DataVector",
    "[NumericalAlgorithms][RootFinding][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const size_t digits = 100;
  const DataVector guess{1.6, 1.9, -1.6, -1.9};
  const DataVector lower{sqrt(2.), sqrt(2.), -2., -3.};
  const DataVector upper{2., 3., -sqrt(2.), -sqrt(2.)};
  const DataVector constant{2., 4., 2., 4.};

  const auto func_and_deriv_lambda = [&constant](const double x,
                                                 const size_t i) noexcept {
    return std::make_pair(constant[i] - square(x), -2. * x);
  };

  const auto root = RootFinder::newton_raphson(func_and_deriv_lambda, guess,
                                               lower, upper, digits);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}
}  // namespace


#ifndef MIXMAT_HPP
#define MIXMAT_HPP

#include <vector>
#include <cstddef>
#include <iostream>
#include <wignerSymbols.h>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <cassert>
#include <exception>

#include <Eigen/Core>
#include <Eigen/Dense>

enum mixMatType
{
    TT,
    EE,
};

template<typename _realScalarType>
class MixingMatrix
{
public:
    /**
     * \typedef _realScalarType realScalarType
     * \brief real floating point type
     */
    typedef _realScalarType realScalarType;

    /**
     * \typedef typename Eigen::Matrix<realScalarType,Eigen::Dynamic,Eigen::Dynamic> mixingMatrixType
     * \brief mixing matrix type
     */
    typedef Eigen::Matrix<realScalarType,Eigen::Dynamic,Eigen::Dynamic> mixingMatrixType;

    /**
     * \typedef typename mixingMatrixType::Index indexType
     * \brief integral type
     */
    typedef typename mixingMatrixType::Index indexType;

    /**
     * \typedef Eigen::LLT<mixingMatrixType> LLTType;
     * \brief Cholesky decompostion type
     */
    typedef Eigen::LLT<mixingMatrixType> LLTType;

    typedef Eigen::Matrix<realScalarType,Eigen::Dynamic,1> powSpecType;

    MixingMatrix(powSpecType const & W,mixMatType const mt)
    :mW(W)
    {
        const indexType lMax = W.rows()-1;
        const indexType matDim(lMax+1);
        mM = mixingMatrixType::Zero(matDim,matDim);
        mLMax = lMax;
        mMT = mt;
        std::cout<<"lMax = "<<lMax<<std::endl;
    }

    void computeMixingMatrixWithRecursion()
    {
        if(mMT == TT)
        {
            std::cout<<"Computing TT mixing matrix"<<std::endl;

            const indexType matDim(mLMax+1);
            mM = mixingMatrixType::Zero(matDim,matDim);

            for(indexType l1=0;l1<=mLMax;++l1)
            {
                for(indexType l2=0;l2<=mLMax;++l2)
                {
                    std::vector<double> wig3j =
                        WignerSymbols::wigner3j((double)l1,(double)l2,
                            (double)0,(double)0,(double)0);

                    indexType l3Min = std::max(std::abs(l1-l2),indexType(0));
                    indexType l3max = std::min( l3Min + indexType(wig3j.size()-1),mLMax ) ;

                    for(indexType l3=l3Min;l3<=l3max;++l3)
                    {
                        if( selectWig3j( l3,l1,l2,indexType(0),indexType(0),indexType(0) )  )
                        {
                            mM(l1,l2) += realScalarType(2*l3+1)*mW(l3)*wig3j[l3 - l3Min]*wig3j[l3 - l3Min];
                        }
                    }

                    mM(l1,l2) *= realScalarType(2*l2+1)/(4*M_PI);
                }
            }
        }
        else if(mMT == EE)
        {
            std::cout<<"Computing EE mixing matrix"<<std::endl;

            const indexType matDim(mLMax+1);
            mM = mixingMatrixType::Zero(matDim,matDim);

            for(indexType l1=0;l1<=mLMax;++l1)
            {
                for(indexType l2=0;l2<=mLMax;++l2)
                {
                    std::vector<double> wig3j =
                        WignerSymbols::wigner3j((double)l1,(double)l2,
                            (double)0,(double)2,(double)-2);

                    indexType l3Min = std::max(std::abs(l1-l2),indexType(0));
                    indexType l3max = std::min( l3Min + indexType(wig3j.size()-1),mLMax ) ;

                    for(indexType l3=l3Min;l3<=l3max;++l3)
                    {
                        if( selectWig3j( l3,l1,l2,indexType(0),indexType(2),indexType(-2) ) && (l1+l2+l3)%2 == 0 )
                        {
                            mM(l1,l2) += realScalarType(2*l3+1)*realScalarType(2)*mW(l3)*wig3j[l3 - l3Min]*wig3j[l3 - l3Min];
                        }
                    }

                    mM(l1,l2) *= realScalarType(2*l2+1)/(8*M_PI);
                }
            }

        }

    }

    bool selectWig3j(indexType const l1,indexType const l2,indexType const l3,
        indexType const m1,indexType const m2,indexType const m3)
    {
        bool select(true);

        select = (
            std::abs(m1+m2+m3) == indexType(0)
            && l3 >= std::abs(l1-l2)
            && l3 <= l1+l2
            && std::abs(m1) <= l1
            && std::abs(m2) <= l2
            && std::abs(m3) <= l3
        );

        return ( select );
    }

    void save(std::string const fileName)
    {
        writeMatrixToFile(fileName,mM,10,",");
    }

    void applyMixingMatrix(powSpecType & C)
    {
        C = mM*C;
    }

    void applyMixingMatrixInv(powSpecType & C)
    {
        LLTType lltOfM;
        lltOfM.compute(mM);
        assert(lltOfM.info() == Eigen::Success);
        C = lltOfM.solve(C);
    }

private:

    void writeMatrixToFile(std::string const & fileName,
        mixingMatrixType const& mat,unsigned int precision,
        std::string const& separation)
    {
        std::ofstream outFile;
        outFile.open(fileName.c_str(),std::ios::trunc);

        if(outFile.is_open())
        {
            outFile<<std::scientific;
            for(indexType i=0;i<mat.rows();++i)
            {
                for(indexType j=0;j<mat.cols()-1;++j)
                {
                    outFile<<std::setprecision(precision)<<mat(i,j)<<separation;
                }
                outFile<<mat(i,(mat.cols()-1) )<<std::endl;
            }
            outFile.close();
        }
        else
        {
            std::string msg = std::string("Error in opening the file ") +
                fileName;
            throw std::runtime_error(msg);
        }
    }

    powSpecType mW;
    mixingMatrixType mM;
    indexType mLMax;
    mixMatType mMT;

};

#endif //MIXMAT_HPP


/**
 * @file k_means_clustering.hpp
 * @brief
 * @author Piotr Smulewicz, Piotr Wygocki
 * @version 1.0
 * @date 2014-06-25
 */
#ifndef PAAL_K_MEANS_CLUSTERING_HPP
#define PAAL_K_MEANS_CLUSTERING_HPP

#include "paal/clustering/k_means_clustering_engine.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/empty.hpp>

#include <vector>

namespace paal {

/**
 * @brief  return centroid that minimize within-cluster sum of squares
 */
template <typename Cluster, typename OutputIterator>
void centroid_minimalize_w_c_s_s(Cluster && cluster, OutputIterator out) {
    assert(!boost::empty(cluster));
    using point_t = range_to_elem_t<Cluster>;
    using coordinate_t = range_to_elem_t<point_t>;

    auto dim = boost::size(*std::begin(cluster));
    for(auto idx : irange(dim)) {
        coordinate_t res{};
        for (auto && point : cluster) {
            res += std::begin(point)[idx];
        }
        *out = res / boost::size(cluster);
        ++out;
   }
}

/**
 * @brief centroid minimize within cluster sum of squares
 * @param clusters
 * @param out
 * @tparam Clusters
 * @tparam OutputIterator
 */
template <typename Clusters, typename OutputIterator>
void centroids_minimalize_w_c_s_s(Clusters && clusters, OutputIterator out) {
    assert(!boost::empty(clusters));
    assert(!boost::empty(*std::begin(clusters)));

    using cluster_t = range_to_elem_t<Clusters>;
    using point_t = range_to_elem_t<cluster_t>;
    using coordinate_t = range_to_elem_t<point_t>;

    auto size = boost::size(*std::begin(*begin(clusters)));
    for (auto && cluster : clusters) {
        std::vector<coordinate_t> point(size);
        centroid_minimalize_w_c_s_s(cluster, point.begin());
        *out = point;
        ++out;
    }
}

/**
 * @brief this is solve k_means_clustering problem
 * and return vector of cluster
 * example:
 *  \snippet k_means_clustering_example.cpp K Means Clustering Example
 *
 * complete example is k_means_clustering_example.cpp
 * @param points
 * @param centers
 * @param result pairs of point and id of cluster
 * (number form 0,1,2 ...,number_of_cluster-1)
 * @param visitor
 * @tparam Points
 * @tparam OutputIterator
 * @tparam CoordinateType
 * @tparam Visitor
 */
template <typename Points, class Centers, class OutputIterator,
          class Visitor = k_means_visitor>
auto k_means(Points &&points, Centers &&centers, OutputIterator result,
             Visitor visitor = Visitor{}) {
    using point_t = range_to_elem_t<Points>;
    using center_t = range_to_elem_t<Centers>;

    center_t center{ *std::begin(centers) };

    return k_means(
        points, centers,
        [&](std::vector<point_t> const & points)->center_t const & {
            centroid_minimalize_w_c_s_s(points, std::begin(center));
            return center;
        },
        [&](point_t const &point) { return closest_to(point, centers); },
        result, utils::equal_to{}, visitor);
}

} //!paal

#endif /* PAAL_K_MEANS_CLUSTERING_HPP */


#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/bandwidth.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrs.hpp>
#include <vector>
#include <stdexcept>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

// solves the equation Ax = B, and puts the solution in B
// A is mutated by this routine
template <typename MatrA, typename MatrB>
void InPlaceSolve(MatrA& a, MatrB& b)
{
  // if the matrix has kl lower and ku upper diagonals, then we should have
  // allocated kl lower and kl+ku upper diagonals
  fortran_int_t const kl = bindings::bandwidth_lower(a);
  fortran_int_t const ku = bindings::bandwidth_upper(a) - kl;
  std::vector<fortran_int_t> piv(a.size1());
  int ret = lapack::gbtrf(/*kl, ku, */a, piv);
  if(ret < 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRF had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRF had an illegal value");
  }
  if(ret > 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF", ret, ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: the (%d,%d) diagonal element is 0 after DGBTRF");
  }

  ret = lapack::gbtrs(/*kl, ku, */a, piv, b);
  if(ret < 0)
  {
    //CStdString err;
    //err.Format("banded::Solve: argument %d in DGBTRS had an illegal value", -ret);
    //throw RuntimeError(err);
    throw std::runtime_error("banded::Solve: argument %d in DGBTRS had an illegal value");
  }
}

template<typename T>
void do_typename()
{
  using namespace boost::numeric::ublas;
  // if the matrix has kl lower and ku upper diagonals, then we should
  // allocate kl lower and kl+ku upper diagonals
  size_t sz = 1000, kl = 1, ku = 1;
  ublas::banded_matrix<T> a(sz, sz, kl, kl+ku);
  ublas::vector<T> b(sz);
  // fill values in a and b
  for(size_t i = 0; i < sz; ++i)
  {
    a(i,i) = i;
    b(i) = i;
  }
  for(size_t i = 1; i < sz; ++i)
  {
    a(i,i-1) = i;
    a(i-1,i) = 1;
  }
  InPlaceSolve(a, b);
}

int main()
{
  do_typename<float>();
  do_typename<double>();
  do_typename<std::complex<float> >();
  do_typename<std::complex<double> >();
  return 0;
}


// test_integrate.cpp
// (c) Tivole

#include <boost/test/unit_test.hpp>
#include "../src/numerary.hpp"

namespace numerary
{

    BOOST_AUTO_TEST_SUITE(TestIntegrate)

    double test_integrate_function_1(double x) {
        return 5*pow(x, 3) + 2*cos(x);
    }

    double test_integrate_function_2(double x) {
        return (cos(x) / (1.0 + exp(1.0 / x)));
    }

    BOOST_AUTO_TEST_CASE(test_integrate_simpson)
    {
        const double eps = 1e-9;
        double *I = NULL;
        double a, b;
        double expected_result;

        // Testing \int_{0}^{1}\left(\5x^3 + 2cos(x)\right) dx
        a = 0.0;
        b = 1.0;
        expected_result = 2.93294196961579301330500464;
        I = Numerary::integrate(test_integrate_function_1, a, b, "simpson", eps);
        BOOST_CHECK(fabs(I[0] - expected_result) <= I[1]);

        // Testing \int_{-1}^{1} \left( \frac{\cos x}{1 +e^{\frac{1}{x}}} \right)
        a = -1.0;
        b = 1.0;
        expected_result = 0.8414709848078965066525023;
        I = Numerary::integrate(test_integrate_function_2, a, b, "simpson", eps);
        BOOST_CHECK(fabs(I[0] - expected_result) <= I[1]);

        delete[] I;
    }

    BOOST_AUTO_TEST_SUITE_END()
}


#include <iostream>
#include <complex>
#include <cmath>
#include <Eigen/Dense>
#include "mex.h"

const std::complex<double> i1(0.0, 1.0);

Eigen::Matrix3cd T;

void update_T(double a, double p)
{
    T(0,0) = pow(cos(a/2),2);
    T(0,1) = pow(sin(a/2),2)*(cos(2*p)+1.0*i1*sin(2*p));
    T(0,2) = sin(a)*(sin(p)-1.0*i1*cos(p));
    T(1,0) = std::conj(T(0,1));
    T(1,1) = T(0,0);
    T(1,2) = std::conj(T(0,2));
    T(2,0) = -0.5*sin(a)*(sin(p)+1.0*i1*cos(p));
    T(2,1) = std::conj(T(2,0));
    T(2,2) = cos(a);
}

void EPG_GRE(double *signal_real, double *signal_imag, double *path_real, double *path_imag, double *N, double *FA, double *RF, double *TR, double *T1, double *T2, double *M0)
{
    int n = (int) N[0];

    //max order of EPG simulation
    int KMAX = n;
    if(n>25){KMAX = 25;}

    ///// MEMORY ALLOCATION
    // relaxation operators
    double E1 = exp(-TR[0]/T1[0]);
    double E2 = exp(-TR[0]/T2[0]);

    // signal vectors
    Eigen::VectorXcd F_plus(KMAX);
    F_plus.setZero();

    Eigen::VectorXcd F_minus(KMAX);
    F_minus.setZero();

    Eigen::VectorXcd Z(KMAX);
    Z.setZero();

    Eigen::Vector3cd aux_states(0, 0, 0);

    /////SIGNAL SIMULATION
    Z(0) = M0[0];

    // transition matrix
    update_T(FA[0], RF[0]);

    //apply first pulse
    aux_states(0) = F_plus(0);
    aux_states(1) = F_minus(0);
    aux_states(2) = Z(0);
    aux_states = T * aux_states;
    F_plus(0) = aux_states(0);
    F_minus(0) = aux_states(1);
    Z(0) = aux_states(2);

    //save F_minus_zero >> this is the measured signal
    signal_real[0] = F_minus(0).real();
    signal_imag[0] = F_minus(0).imag();

    //simulate rest of the sequence
    for (int j=1; j<n; j++){

	    //apply E
    	for (int k=0; k<(std::min(j,KMAX)); k++)
    	{
    		F_plus(k) = F_plus(k) * E2;
    		F_minus(k) = F_minus(k) * E2;
    		Z(k) = Z(k) * E1;
    	}
    	Z(0) += (1.0 - E1)*M0[0];

    	//apply S
    	for (int k=(std::min(KMAX-1,j)); k>0; k--){ F_plus(k) = F_plus(k-1); }
    	F_plus(0) = std::conj(F_minus(1));
    	for (int k=0; k<(std::min(KMAX-1,j)); k++){ F_minus(k) = F_minus(k+1); }
    	F_minus(std::min(KMAX-1,j)) = 0.0 + 0.0*i1;

    	//apply T
        update_T(FA[j], RF[j]);
    	for (int k=0; k<(std::min(KMAX,j+1)); k++)
    	{
    		aux_states(0) = F_plus(k);
    		aux_states(1) = F_minus(k);
    		aux_states(2) = Z(k);
    		aux_states = T * aux_states;

    		F_plus(k) = aux_states(0);
    		F_minus(k) = aux_states(1);
    		Z(k) = aux_states(2);
    	}

    	//save F_minus_zero
    	signal_real[j] = F_minus(0).real();
        signal_imag[j] = F_minus(0).imag();

        //save last states
        if(j==(n-1))
        {
            for (int k=0; k<KMAX; k++)
            {
                path_real[3*k] = F_plus(k).real();
                path_real[3*k+1] = F_minus(k).real();
                path_real[3*k+2] = Z(k).real();
                path_imag[3*k] = F_plus(k).imag();
                path_imag[3*k+1] = F_minus(k).imag();
                path_imag[3*k+2] = Z(k).imag();
            }
        }
    }
}

void mexFunction(int nlhs, mxArray *plhs[], int rhs, const mxArray *prhs[])
{
    /* Declare arrays necessary to compute signals */
    double *N;
    double *FA;
    double *RF;
    double *TR;
    double *T1;
    double *T2;
    double *M0;

    double *signal_real;
    double *signal_imag;
    double *path_real;
    double *path_imag;

    /* Check for proper number of arguments */

    /* Create pointers to the inputs */
    N = mxGetPr(prhs[0]);
    FA = mxGetPr(prhs[1]);
    RF = mxGetPr(prhs[2]);
    TR = mxGetPr(prhs[3]);
    T1 = mxGetPr(prhs[4]);
    T2 = mxGetPr(prhs[5]);
    M0 = mxGetPr(prhs[6]);

    /* Create pointers to the outputs */
    plhs[0] = mxCreateDoubleMatrix((mwSize)*N, 1, mxCOMPLEX);
    plhs[1] = mxCreateDoubleMatrix(3, 25, mxCOMPLEX);

    // Check this link to see how to return complex arrays:
    // http://matlab.izmiran.ru/help/techdoc/matlab_external/ch04cre9.html
    signal_real = mxGetPr(plhs[0]);
    signal_imag = mxGetPi(plhs[0]);
    path_real = mxGetPr(plhs[1]);
    path_imag = mxGetPi(plhs[1]);

    /* Call the computational routine */
    EPG_GRE(signal_real, signal_imag, path_real, path_imag, N, FA, RF, TR, T1, T2, M0);

}


// main.cpp -- example use of CppGPs for Gaussian process regression

//
// Eigen macros for using externel routines (may need to be moved to header file)
// ( see https://eigen.tuxfamily.org/dox/TopicUsingBlasLapack.html )
//#define EIGEN_USE_BLAS            // Must also link to BLAS library
//#define EIGEN_USE_LAPACKE_STRICT  // Must also link to LAPACKE library
//#define EIGEN_USE_MKL_ALL         // Must also link to Intel MKL library
#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include <string>
#include <memory>
#include <random>
#include <chrono>
#include <fstream>
#include <boost/range/irange.hpp>
#include "GPs.h"

#include <limits>

// Specify the target function for Gaussian process regression
double targetFunc(Eigen::MatrixXd X)
{

  if ( X.size() == 1 )
    {
      // Define the target function to be an oscillatory, non-periodic function
      double oscillation = 30.0;
      double xshifted = 0.5*(X(0) + 1.0);
      return std::sin(oscillation*(xshifted-0.1))*(0.5-(xshifted-0.1))*15.0;
    }
  else
    {
      // Define the target function to be the Marr wavelet (a.k.a "Mexican Hat" wavelet)
      // ( see https://en.wikipedia.org/wiki/Mexican_hat_wavelet )
      double sigma = 0.25;
      double pi = std::atan(1)*4;
      double radialTerm = std::pow(X.squaredNorm()/sigma,2);
      return 2.0 / (std::sqrt(pi*sigma) * std::pow(pi,1.0/4.0)) * (1.0 - radialTerm) * std::exp(-radialTerm);
    }
}

// Example use of CppGPs code for Gaussian process regression
int main(int argc, char const *argv[])
{

  // Retrieve aliases from GP namescope
  using Matrix = Eigen::MatrixXd;
  using Vector = Eigen::VectorXd;

  // Convenience using-declarations
  using std::cout;
  using std::endl;
  using GP::GaussianProcess;
  using GP::linspace;
  using GP::sampleNormal;
  using GP::sampleUnif;
  using GP::RBF;

  // Used for timing code with chrono
  using GP::high_resolution_clock;
  using GP::time;
  using GP::getTime;

  // Set random seed based on system clock
  std::srand(static_cast<unsigned int>(high_resolution_clock::now().time_since_epoch().count()));

  // Fix random seed for debugging and testing
  //std::srand(static_cast<unsigned int>(0));

  //
  //   [ Define Training Data ]
  //

  // Specify the input dimensions
  //int inputDim = 1;
  int inputDim = 2;

  // Specify observation data count
  int obsCount;
    if ( inputDim == 1 )
      obsCount = 250;
    else
      obsCount = 1000;

  // Specify observation noise level
  auto noiseLevel = 1.0;

  // Define random noise to add to target observations
  auto noise = sampleNormal(obsCount) * noiseLevel;

  // Define observations by sampling random uniform distribution
  Matrix X = sampleUnif(-1.0, 1.0, obsCount, inputDim);
  Matrix y;  y.resize(obsCount, 1);

  // Define target observations 'y' by applying 'targetFunc'
  // to the input observations 'X' and adding a noise vector
  for ( auto i : boost::irange(0,obsCount) )
    y(i) = targetFunc(X.row(i)) + noise(i);

  //
  //   [ Construct Gaussian Process Model ]
  //

  // Initialize Gaussian process model
  GaussianProcess model;

  // Specify training observations for GP model
  model.setObs(X,y);

  // Fix noise level [ noise level is fit to the training data by default ]
  //model.setNoise(0.33);

  // Initialize a RBF covariance kernel and assign it to the model
  RBF kernel;
  model.setKernel(kernel);

  // Specify hyperparameter bounds  [ including noise bounds (index 0) ]
  // Vector lbs(2); lbs << 0.0001 , 0.01;
  // Vector ubs(2); ubs << 5.0    , 100.0;
  // model.setBounds(lbs, ubs);

  // Specify hyperparameter bounds  [ excluding noise bounds ]
  Vector lbs(1);  lbs <<  0.01;
  Vector ubs(1);  ubs <<  100.0;
  model.setBounds(lbs, ubs);

  // Fit covariance kernel hyperparameters to the training data
  time start = high_resolution_clock::now();
  model.fitModel();
  time end = high_resolution_clock::now();
  auto computationTime = getTime(start, end);

  // Display computation time required for fitting the GP model
  cout << "\nComputation Time: ";
  cout << computationTime << " s" << endl;

  // Retrieve the tuned/optimized kernel hyperparameters
  auto optParams = model.getParams();
  cout << "\nOptimized Hyperparameters:" << endl << optParams.transpose() << "  ";
  auto noiseL = model.getNoise();
  cout << "(Noise = " << noiseL << ")\n" << endl;

  // Display the negative log marginal likelihood (NLML) of the optimized model
  cout << "NLML:  " << std::fixed << std::setprecision(4) << model.computeNLML() << endl << endl;

  //double eps = std::numeric_limits<double>::min();
  //cout << "Epsilon: " << eps << endl;

  //
  //   [ Posterior Predictions and Samples ]
  //

  // Define test mesh for GP model predictions
  int predCount;
  if ( inputDim == 1 )
    predCount = 100;
  else
    predCount = 1000;
  //int predRes = static_cast<int>(std::sqrt(predCount));
  int predRes = static_cast<int>(std::pow(predCount,1.0/inputDim));
  //auto testMesh = linspace(0.0, 1.0, predCount);
  auto testMesh = linspace(-1.0, 1.0, predRes, inputDim);
  model.setPred(testMesh);

  // Reset predCount to account for rounding
  predCount = std::pow(predRes,inputDim);

  // Compute predicted means and variances for the test points
  model.predict();
  Matrix pmean = model.getPredMean();
  Matrix pvar = model.getPredVar();
  Matrix pstd = pvar.array().sqrt().matrix();

  // Get sample paths from the posterior distribution of the model
  int sampleCount;
  Matrix samples;
  if ( inputDim == 1 )
    {
      sampleCount = 25;
      samples = model.getSamples(sampleCount);
    }

  //
  //   [ Save Results for Plotting ]
  //

  // Save true and predicted means/variances to file
  std::string outputFile = "predictions.csv";
  //Matrix trueSoln = testMesh.unaryExpr(std::ptr_fun(targetFunc));

  Matrix trueSoln(predCount,1);
  for ( auto i : boost::irange(0,predCount) )
    trueSoln(i,0) = targetFunc(testMesh.row(i));

  std::ofstream fout;
  fout.open(outputFile);
  for ( auto i : boost::irange(0,predCount) )
    {
      for ( auto j : boost::irange(0,inputDim) )
        fout << testMesh(i,j) << ",";

      fout << trueSoln(i) << "," << pmean(i) << "," << pstd(i) << "\n";
    }
  fout.close();

  // Save observations to file
  std::string outputObsFile = "observations.csv";
  fout.open(outputObsFile);
  for ( auto i : boost::irange(0,obsCount) )
    {
      for ( auto j : boost::irange(0,inputDim) )
        fout << X(i,j) << ",";

      fout << y(i) << "\n";
    }
  fout.close();

  if ( inputDim == 1 )
    {
      // Save samples to file
      std::string outputSampleFile = "samples.csv";
      fout.open(outputSampleFile);
      for ( auto j : boost::irange(0,sampleCount) )
        {
          for ( auto i : boost::irange(0,predCount) )
            fout << samples(i,j) << ((i<predCount-1) ? "," : "\n");
        }
      fout.close();
    }

  return 0;

}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;
void setZeroMean(mat& A)

{
    int n_rows;
    int D;
    n_rows=A.n_rows;
    cout<<"number row:"<<n_rows<<endl;
    D=A.n_cols;
    cout<<"number col:"<<D<<endl;
    mat Mean=mean(A); // should be 1 row D coln;
    cout<<"Mean ="<<Mean<<endl;
// subtract the mean - use score_out as temporary matrix
    A =A-repmat(mean(A), n_rows, 1);
    return ;
}
int main(int argc, char** argv)
{
    mat A = randu<mat>(4,5); //random matrix 4 rows 5 cols
    setZeroMean(A);
    mat Mean=mean(A);
    cout<<" centered data new Mean ="<<Mean<<endl;

    mat B = randu<mat>(4,5);

//    cout << A*B.t() << endl;

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// kernel::scalar::gaussian.hpp                                              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_GAUSSIAN_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_GAUSSIAN_HPP_ER_2009
#include <cmath>
#include <boost/statistics/detail/kernel/kernels/scalar/crtp.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{
namespace scalar{

template<typename T>
struct gaussian_kernel : scalar::crtp<gaussian_kernel<T>,T >{
    typedef gaussian_kernel<T> this_type;
    typedef scalar::crtp<this_type,T> crtp_;
    public:
    typedef typename crtp_::result_type result_type;

    gaussian_kernel();
    gaussian_kernel(const result_type& bandwidth);

    static result_type core_profile(const result_type& x);
    static result_type core_nc;
};

template<typename T> gaussian_kernel<T>::gaussian_kernel():crtp_(){}
template<typename T> gaussian_kernel<T>::gaussian_kernel(
    const result_type& bandwidth
)
:crtp_(bandwidth){}

template<typename T>
typename gaussian_kernel<T>::result_type
gaussian_kernel<T>::core_profile(const result_type& x){
    static result_type two = static_cast<T>(2);
    return exp(- x * x / two);
}

template<typename T>
typename gaussian_kernel<T>::result_type
gaussian_kernel<T>::core_nc = math::constants::root_two_pi<T>();

}// scalar
}// kernel
}// detail
}// statistics
}// boost

#endif


//
// diff.cpp
//
// Copyright (c) 2021 Izadori
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

#include <Eigen/Eigen>

template <typename Derived>
const Eigen::MatrixXd Diff(const Eigen::MatrixBase<Derived> & m, const unsigned int n = 1)
{
    Eigen::MatrixXd mr = m;

    for(unsigned int i = 0; i < n; i++)
    {
        mr = (mr.topRows(mr.rows() - 1) - mr.bottomRows(mr.rows() - 1)).eval();
    }

    return mr;
}

template <typename Derived>
const Eigen::SparseMatrix<double> Diff(const Eigen::SparseMatrixBase<Derived> & m, const unsigned int n = 1)
{
    Eigen::SparseMatrix<double> mr = m;

    for(unsigned int i = 0; i < n; i++)
    {
        mr = (mr.topRows(mr.rows() - 1) - mr.bottomRows(mr.rows() - 1)).eval();
    }

    return mr;
}


/**************************************
 * Class Name: PRNG, PrimeGen
 * Function: 伪随机数与伪随机素数比特生成器
 * Introduction: PrimeGen 继承于 PRNG 类
 * ***********************************/

#include "Random.h"
#include "TDES.h"
#include <iostream>
#include <ctime>
#include <cstring>
#include <NTL/ZZ.h>

using namespace std;
using namespace NTL;

typedef unsigned long long size_ul;

/*---------------------------------
 * Class: PRNG
 *-------------------------------*/

// 构造函数，进行初始化操作
PRNG::PRNG(int n) {
    m = n;
    x = new bitset<64>[m];
    s = "";
}

// 析构函数，释放内存空间
PRNG::~PRNG() {
    delete [] x;
}

// 通过外界修改 m 的数值
void PRNG::SetM(int n) {
    m = n;
    delete [] x;
    x = new bitset<64>[m];
}

// 伪随机数比特生成函数
ZZ PRNG::GenerateRandom() {
    // 获取本地时间日期
    time_t now = time(0);
    bitset<64> Date(now);

    // 产生 64 比特随机种子 s
    size_ul ss = size_ul(random()) << 32 + random();
    bitset<64> s(ss);

    TDES td;
    td.plain = Date;
    bitset<64> Im = td.TDESEncrypt();
    for (int i = 0; i < m; i++) {
        td.plain = Im ^ s;
        x[i] = td.TDESEncrypt();
        td.plain = x[i] ^ Im;
        s = td.TDESEncrypt();
    }

    // 生成字符串类型保存
    bitsToString();
    ZZ res = bitsToNumber();
    return res;
}

// 将比特数转化为 ZZ 型数字
ZZ PRNG::bitsToNumber() {
    ZZ res(0);
    for (int i = 0; i < m; i++) {
        string str = x[i].to_string();
        for (int j = 0; j < 64; j++) {
            res = res * 2;
            if (str[j] == '1') {
                res = res + 1;
            }
        }
    }
    return res;
}

// 比特位转为字符串类型（01字符串）
void PRNG::bitsToString() {
    s = "";
    for (int i = 0; i < m; i++) {
        s += x[i].to_string();
    }
}

// 以十六进制打印比特数值
void PRNG::PrintInDER() {
    cout << "modulus:";
    string pairstr = "";
    for (size_t i = 0; i < s.length(); i += 4) {
        int index = 0;

        for (size_t j = i; j < i + 4; j++) {
            index = index << 1;
            if (s[j] == '1') {
                index += 1;
            }
        }
        pairstr += DERTable[index];
        if (pairstr.length() == 2) {
            cout << pairstr;
            if (i + 4 < s.length()) {
                cout << ":";
            }
            pairstr = "";
        }
        if (i % 120 == 0) {
            cout << endl << '\t';
        }
    }
    cout << endl;
}

/*---------------------------------
 * Class: PrimeGen
 *-------------------------------*/

PrimeGen::PrimeGen(int n) : PRNG(n) {
    cout << "Create a pseudorandom number generator.\n";
}

// 生成随机素数，使用 NTL 中库函数检验
// 素性检测算法为 Miller-Rabin 检测
ZZ PrimeGen::GeneratePrime() {
    ZZ res(0);
    // 此处进行 srandom，为产生 64 比特随机种子
    srandom((unsigned)time(NULL));
    do {
        res = GenerateRandom();
    } while(!ProbPrime(res, m * 32));

    return res;
}


#include <boost/test/unit_test.hpp>
#include <shz/math/matrix.hpp>

BOOST_AUTO_TEST_CASE(matrixConstructors)
{

	auto m = shz::math::matrix<shz::math::f64, 2, 2>::identity();
	auto n = shz::math::matrix<shz::math::f64, 2, 2>::identity();

	BOOST_CHECK(true == m.is_identity());
	BOOST_CHECK(true == n.is_identity());

	BOOST_CHECK(1.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(1.f == m.data[3]);
}

BOOST_AUTO_TEST_CASE(matrixSizeConstructors)
{
	auto m = shz::math::matrix<shz::math::f64, 20, 30>::identity();
	auto n = shz::math::matrix<shz::math::f64, 20, 30>::identity();

	BOOST_CHECK(true == m.is_identity());
	BOOST_CHECK(true == n.is_identity());

	BOOST_CHECK(20*30 == m.size);
	BOOST_CHECK(20 == m.mindimension);
	BOOST_CHECK(20 == m.columns);
	BOOST_CHECK(30 == m.rows);
}

BOOST_AUTO_TEST_CASE(matrixAdds)
{
	auto m = shz::math::matrix<shz::math::f64, 2, 2>::identity();
	auto n = shz::math::matrix<shz::math::f64, 2, 2>::identity();

	auto j = m+n;
	BOOST_CHECK(true != j.is_identity());
	BOOST_CHECK(2*2 == j.size);
	BOOST_CHECK(2.f == j.data[0]);
	BOOST_CHECK(0.f == j.data[1]);
	BOOST_CHECK(0.f == j.data[2]);
	BOOST_CHECK(2.f == j.data[3]);

	m += n;
	BOOST_CHECK(2.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(2.f == m.data[3]);

	n += 2.f;
	BOOST_CHECK(3.f == n.data[0]);
	BOOST_CHECK(2.f == n.data[1]);
	BOOST_CHECK(2.f == n.data[2]);
	BOOST_CHECK(3.f == n.data[3]);
}

BOOST_AUTO_TEST_CASE(matrixSubs)
{
	auto m = shz::math::matrix<shz::math::f64, 2, 2>::identity();
	auto n = shz::math::matrix<shz::math::f64, 2, 2>::identity();

	auto j = m-n;
	BOOST_CHECK(true != j.is_identity());
	BOOST_CHECK(2*2 == j.size);
	BOOST_CHECK(0.f == j.data[0]);
	BOOST_CHECK(0.f == j.data[1]);
	BOOST_CHECK(0.f == j.data[2]);
	BOOST_CHECK(0.f == j.data[3]);

	m -= n;
	BOOST_CHECK(0.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(0.f == m.data[3]);

	n -= 2.f;
	BOOST_CHECK(-1.f == n.data[0]);
	BOOST_CHECK(-2.f == n.data[1]);
	BOOST_CHECK(-2.f == n.data[2]);
	BOOST_CHECK(-1.f == n.data[3]);
}

BOOST_AUTO_TEST_CASE(matrixMuls)
{
	auto m = shz::math::matrix<shz::math::f64, 2, 2>::identity();
	auto n = shz::math::matrix<shz::math::f64, 2, 2>::identity();

	auto j = m*n;
	BOOST_CHECK(true == j.is_identity());
	BOOST_CHECK(2*2 == j.size);
	BOOST_CHECK(1.f == j.data[0]);
	BOOST_CHECK(0.f == j.data[1]);
	BOOST_CHECK(0.f == j.data[2]);
	BOOST_CHECK(1.f == j.data[3]);

	auto v = shz::math::vector<shz::math::f64, 2>();
	v.data[0] = 2.f;
	v.data[1] = 2.f;
	auto b = m*v;
	BOOST_CHECK(2.f == b.data[0]);
	BOOST_CHECK(2.f == b.data[1]);

	m = n*2;
	BOOST_CHECK(true != m.is_identity());
	b = m*v;
	BOOST_CHECK(4.f == b.data[0]);
	BOOST_CHECK(4.f == b.data[1]);

	m *= n;
	BOOST_CHECK(true != m.is_identity());
	BOOST_CHECK(2.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(2.f == m.data[3]);

	m *= 2;
	BOOST_CHECK(true != m.is_identity());
	BOOST_CHECK(4.f == m.data[0]);
	BOOST_CHECK(0.f == m.data[1]);
	BOOST_CHECK(0.f == m.data[2]);
	BOOST_CHECK(4.f == m.data[3]);

}

/*
BOOST_AUTO_TEST_CASE(matrixPerformance)
{
	auto m = shz::math::matrix<shz::math::f64, 200, 200>::identity();
	auto n = shz::math::matrix<shz::math::f64, 200, 200>::identity();

	for(size_t i=0; i<10000; i++)
		auto r = m*n;
}*/


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

using namespace std;
int main() {
  Eigen::Matrix2d mat;
  mat << 1, 2,
      3, 4;
  cout << "Here is mat.sum():       " << mat.sum() << endl;
  cout << "Here is mat.prod():      " << mat.prod() << endl;
  cout << "Here is mat.mean():      " << mat.mean() << endl;
  cout << "Here is mat.minCoeff():  " << mat.minCoeff() << endl;
  cout << "Here is mat.maxCoeff():  " << mat.maxCoeff() << endl;
  cout << "Here is mat.trace():     " << mat.trace() << endl;
}


/****************************************
 * Creator : Léo Gaspard                *
 * Mail    : leo.gaspard@outlook.fr     *
 * Git     : https://github.com/NehZio  *
 *                                      *
 * Date    : 26/08/2019                 *
 *                                      *
 * File    : main.cpp                   *
 ****************************************/

#include <iostream>
#include <iomanip>
#include <complex>
#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/MatrixFunctions>

int main()
{
	int norb(3);
	int nvec(3);

	Eigen::MatrixXcd c(nvec,nvec);
	Eigen::MatrixXcd d(norb,nvec);
	Eigen::MatrixXcd p(nvec,nvec);
	Eigen::MatrixXcd s(nvec,nvec);
	Eigen::MatrixXcd invSqrtS(nvec,nvec);
	Eigen::MatrixXcd pON(nvec,nvec);
	Eigen::MatrixXcd heff(nvec,nvec);
	Eigen::MatrixXd e(nvec,nvec);

	Eigen::FullPivLU<Eigen::MatrixXcd> lu;
	Eigen::ComplexEigenSolver<Eigen::MatrixXcd> eig;

	std::cout << std::fixed << std::setprecision(6);
	std::cout << std::endl << "This program will perform Heff calculations using the method described in : \n Chem. Rev. 2014, 114, 1, 429-492" << std::endl;

//*************************************************
/*
 * Manual initialization of c
 */

	c(0,1) = {-0.4867551E+00,0.0000E-10};
	c(0,0) = {-0.2653160E+00,0.0000E-10};
	c(0,2) = { 0.7520712E+00,0.0000E-10};

	c(1,1) = { 0.6126111E+00,0.0000E-10};
	c(1,0) = {-0.7456646E+00,0.0000E-10};
	c(1,2) = { 0.1330535E+00,0.0000E-10};

	c(2,1) = {-0.5748099E+00,0.0000E-10};
	c(2,0) = {-0.5748095E+00,0.0000E-10};
	c(2,2) = {-0.5748079E+00,0.0000E-10};

/*
 * Manual initializaion of e
 */
	e(0,0) =-715.10E+00;
	e(1,1) =-231.20E+00;
	e(2,2) =   0.00E+00;
//*************************************************

	std::cout << std::endl << "c Matrix : " << std::endl << c << std::endl;
	std::cout << std::endl << "e Matrix :  " <<  std::endl << e << std::endl << std::endl;

	/***********************
	 * Calculation of the  *
	 * projection operator *
	 ***********************/

	std::cout << std::endl << "Projection matrix P = |I>*<I| :  " << std::endl;
        d = Eigen::MatrixXcd::Identity(norb,nvec);
	std::cout << std::endl << d << std::endl;

	/**********************
	 * Calculation of the *
	 * projected vectors  *
	 **********************/

	std::cout << std::endl << "Projected vectors into the model space :" << std::endl;
	p = c*d;
	std::cout << std::endl << p << std::endl;

	/***********************
	 * Calculation of the  *
	 * overlap matrix      *
	 ***********************/

	std::cout << std::endl << "Overlap Matrix S = c*c^T: " << std::endl;
	s = p*p.transpose();
	std::cout << std::endl << s << std::endl;

	/***********************
	 * Calculation of the  *
	 * S^(-1/2) matrix     *
	 ***********************/

	lu.compute(s.sqrt());

	if(lu.isInvertible())
	{
		invSqrtS =  lu.inverse();
	}
	else
	{
		std::cout << std::endl << "Error : The square root of the overlap matrix is not invertible" << std::endl;
		return 0;
	}

	std::cout << std::endl << "S^(-1/2) Matrix : " << std::endl;

	std::cout << std::endl << invSqrtS << std::endl;

	/***********************
	 * Calculation of the  *
	 * otrhonormalized     *
	 * projected vectors   *
	 ***********************/

	pON =  invSqrtS*p;
	std::cout << std::endl << "Orthonormalized projected vectors : " << std::endl;
	std::cout << std::endl << pON << std::endl;

	/***********************
	 * Calculation of the  *
	 * effective           *
	 * Hamiltonian         *
	 ***********************/

	heff = pON.transpose()*e*pON.conjugate();

	std::cout << std::endl << "Effective Hamiltonian |S,Ms> basis : " << std::endl;
	std::cout << std::endl << heff << std::endl;

	/***********************
	 * Diagonalization of  *
	 * the effective       *
	 * Hamiltonian         *
	 ***********************/

	eig.compute(heff);

	std::cout << std::endl << "Eigenvalues of the effective Hamiltonian :" << std::endl;
	std::cout << std::endl << eig.eigenvalues() << std::endl;

	std::cout << std::endl << "Eigenvectors of the effective Hamiltonian :" << std::endl;
	std::cout << std::endl << eig.eigenvectors() << std::endl;
	return 0;
}


#include <chrono>
#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <cmath>

using Eigen::Vector3d;
using std::runtime_error;

namespace lambert {
    struct LambertResults {
        Vector3d v0;
        Vector3d v;
    };

    double c2(double psi) {
        double res;
        auto eps = 1.0;
        if (psi > eps) {
            res = (1 - cos(sqrt(psi))) / psi;
        } else if (psi < -eps) {
            res = (cosh(sqrt(-psi)) - 1) / (-psi);
        } else {
            res = 1.0 / 2.0;
            auto delta = (-psi) / tgamma(2 + 2 + 1);
            auto k = 1;
            while (res + delta != res) {
                res += delta;
                k += 1;
                delta = pow(-psi, k) / tgamma(2*k + 2 + 1);
            }
        }
        return res;
    }

    double c3(double psi) {
        double res;
        auto eps = 1.0;
        if (psi > eps) {
            res = (sqrt(psi) - sin(sqrt(psi))) / (psi * sqrt(psi));
        } else if (psi < -eps) {
            res = (sinh(sqrt(-psi)) - sqrt(-psi)) / (-psi * sqrt(-psi));
        } else {
            res = 1.0 / 6.0;
            auto delta = (-psi) / tgamma(2 + 3 + 1);
            int k = 1;
            while (res + delta != res) {
                res += delta;
                k += 1;
                delta = pow(-psi, k) / tgamma(2*k + 3 + 1);
            }
        }
        return res;
    }

    LambertResults lambert(double k, Vector3d r0, Vector3d r, double tof,
            bool shortway=true, int numiter=35, double rtol=1e-8) {
        int t_m;
        if (shortway) {
            t_m = 1;
        } else {
            t_m = -1;
        }
        auto norm_r0 = r0.norm();
        auto norm_r = r.norm();
        auto cos_dnu = r0.dot(r) / (norm_r * norm_r0);

        auto A = t_m * sqrt(norm_r * norm_r0 * (1 + cos_dnu));

        if (A == 0.0) {
            throw runtime_error("Cannot compute orbit, phase angle is 180 degrees");
        }

        auto psi = 0.0;
        auto psi_low = -4*M_PI;
        auto psi_up = 4*M_PI;

        auto count = 0;
        double y;
        while (count < numiter) {
            y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / sqrt(c2(psi));
            if (A > 0.0 & y < 0.0) {
                while (y < 0.0) {
                    psi_low = psi;
                    psi = (0.8 * (1.0 / c3(psi)) *
                            (1.0 - (norm_r0 + norm_r) * sqrt(c2(psi)) / A));
                    y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / sqrt(c2(psi));
                }
            }
            auto xi = sqrt(y / c2(psi));
            auto tof_new = (pow(xi, 3) * c3(psi) + A * sqrt(y)) / sqrt(k);

            if (fabs((tof_new - tof) / tof) < rtol) {
                break;
            } else {
                count += 1;
                if (tof_new <= tof) {
                    psi_low = psi;
                } else {
                    psi_up = psi;
                }
                psi = (psi_up + psi_low) / 2;
            }
        }
        if (count > numiter) {
            throw runtime_error("Maximum number of iterations reached");
        }
        auto f = 1 - y / norm_r0;
        auto g = A * sqrt(y / k);
        auto gdot = 1 - y / norm_r;
        auto v0 = (r - f * r0) / g;
        auto v = (gdot * r - r0) / g;
        LambertResults results = {v, v0};
        return results;
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r0(5000.0, 10000.0, 2100.0);
        Vector3d r(-14600.0, 2500.0, 7000.0);
        auto tof = 3600.0;

        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();

            lambert(mu, r0, r, tof);

            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


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

using namespace Eigen;
using namespace std;
MatrixXf computeLinkPos(MatrixXf L, MatrixXf P, Matrix3f R, MatrixXf s, MatrixXf pb);
MatrixXf computeActuation(MatrixXf L, float lc);

MatrixXf computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
// float* computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
  // void computeSliderControl(float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi) {
  // - スライダー間距離D[mm]        : 56+(12/2)*2=68mm
  // - ロッド長さlc[mm]                  : 262+(8/2)*2=268mm
  // - スライダー設置半径rb[mm]    : sqrt(269.7^2-200.5^2)=180.38248
  // - テーブル半径re[mm]               :90mm
  // - テーブル初期高さPz[mm]       :438.17mm
  // float re = 90;
  // float Pz = 438.17;
  // float D = 68;
  // float lc = 268;
  // float rb = 180.38;

  float th = asin(D/(2*rb)); //% theta1: angle (linear actuator)
  float th2 = asin(D/(2*re)); //% theta2: angle (end effector)

  MatrixXf pb(6,3);
  pb <<
    rb*cos(th),rb*sin(th),0,
    rb*cos(-th),rb*sin(-th),0,
    rb*cos(2*M_PI/3+th),rb*sin(2*M_PI/3+th),0,
    rb*cos(2*M_PI/3-th),rb*sin(2*M_PI/3-th),0,
    rb*cos(4*M_PI/3+th),rb*sin(4*M_PI/3+th),0,
    rb*cos(4*M_PI/3-th),rb*sin(4*M_PI/3-th),0;

  MatrixXf s_local(6,3);
  s_local <<
    re*cos(th2),re*sin(th2),0,
    re*cos(-th2),re*sin(-th2),0,
    re*cos(2*M_PI/3+th2),re*sin(2*M_PI/3+th2),0,
    re*cos(2*M_PI/3-th2),re*sin(2*M_PI/3-th2),0,
    re*cos(4*M_PI/3+th2),re*sin(4*M_PI/3+th2),0,
    re*cos(4*M_PI/3-th2),re*sin(4*M_PI/3-th2),0;

  MatrixXf sliders(6,3);
  sliders <<
    rb*cos(th),rb*sin(th),Pz,
    rb*cos(-th),rb*sin(-th),Pz,
    rb*cos(2*M_PI/3+th),rb*sin(2*M_PI/3+th),Pz,
    rb*cos(2*M_PI/3-th),rb*sin(2*M_PI/3-th),Pz,
    rb*cos(4*M_PI/3+th),rb*sin(4*M_PI/3+th),Pz,
    rb*cos(4*M_PI/3-th),rb*sin(4*M_PI/3-th),Pz;

  // float phi = M_PI/24; //% rotation around X axis
  // float theta = M_PI/12; //% rotation around Y axis
  // float psi = M_PI/16; //% rotation around Z axis

  Matrix3f R;
  R <<
    cos(phi)*cos(theta),cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi),cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi),
    sin(phi)*cos(theta),sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi),sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi),
    -sin(theta),cos(theta)*sin(psi),cos(theta)*cos(psi);

  Vector3f a;
  a << 0,0,1;

  MatrixXf s(6,3);
  s = s_local*R;//6x3 x 3x3 => 6x3

  MatrixXf P(1,3);
  P << xd,yd,zd; //% Position Vector of the end effector
  // P << 50,50,height; //% Position Vector of the end effector

  // cout << "*** re: " << endl << s << endl;
  // cout << "*** rb: " << endl << rb << endl;
  // cout << "*** pb:  " << endl << pb << endl;
  // cout << "*** R:  " << endl<< R << endl;

  MatrixXf L(6,3);
  L << computeLinkPos(L, P, R, s, pb);
  // cout << "*** L:  " << endl << L << endl;

  MatrixXf C(1,6);
  C << computeActuation(L,lc);
  cout << "*** C:  " << endl << C << endl;

  return C;
  // float* arrayf = C.data();
}

// %% compute position vector of rod end of end effector
MatrixXf computeLinkPos(MatrixXf L, MatrixXf P, Matrix3f R, MatrixXf s, MatrixXf pb){
  // L = P + R*s - pb;
  for(int i=0;i<6;i++){
    L.block(i,0,1,3) << P + (s.block(i,0,1,3))*R - pb.block(i,0,1,3);
  }
  return L;
}

MatrixXf computeActuation(MatrixXf L, float lc){
  MatrixXf C(1,6);
  for (int i=0;i<6;i++){
    float lxi = L(i,0);
    float lyi = L(i,1);
    float lzi = L(i,2);
    C.block(0,i,1,1) << lzi - sqrt(lc*lc-lxi*lxi-lyi*lyi);
  }
  return C;
}

extern "C" {
  void solveInverseMechanism(float* arrayf, float re,float rb,float lc,float D,float Pz,float xd, float yd, float zd,float phi,float theta,float psi){
    MatrixXf C(1,6);
    C << computeSliderControl(re,rb,lc,D,Pz,xd,yd,zd,phi,theta,psi);
    memcpy(arrayf,C.data(),C.size()*sizeof(float));
  }
}


#ifndef CANNON_MATH_MULTIVARIATE_NORMAL_H
#define CANNON_MATH_MULTIVARIATE_NORMAL_H

/*!
 * \file cannon/math/multivariate_normal.hpp
 * \brief File containing MultivariateNormal class definition.
 */

#include <random>

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Class representing a multivariate normal distribution of
     * arbitrary dimensionality.
     */
    class MultivariateNormal {
      public:
        MultivariateNormal() = delete;

        /*!
         * \brief Constructor taking a covariance matrix. The distribution is
         * assumed to be centered at zero.
         */
        MultivariateNormal(const Ref<const MatrixXd>& covar) :
          MultivariateNormal(VectorXd::Zero(covar.rows()), covar) {}

        /*!
         * \brief Constructor taking a mean and covariance matrix.
         */
        MultivariateNormal(const Ref<const VectorXd> &mean, const Ref<const MatrixXd> &covar)
            : mean_(mean),
              transform_(MatrixXd::Zero(mean.size(), mean.size())) {
          SelfAdjointEigenSolver<MatrixXd> eigen_solver(covar);
          transform_ = eigen_solver.eigenvectors() *
            eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
        }

        /*!
         * \brief Sample a point from this distribution.
         *
         * \returns A sample from the multivariate normal distribution
         * represented by this object.
         */
        VectorXd sample() const {
          return mean_ + (transform_ * VectorXd::NullaryExpr(mean_.size(),
                [&](){return dist(gen);}));
        }

        /*!
         * \brief Set the seed for all normal distributions.
         *
         * \param seed The new seed to use.
         */
        static void set_seed(int seed);

        static std::random_device rd; //!< Physical random number generator.
        static std::mt19937 gen; //!< Pseudo-random number generator.
        static std::normal_distribution<double> dist; //!< 1D normal distribution for sampling

      private:
        VectorXd mean_; //!< Mean of this normal distribution
        MatrixXd transform_; //!< Transform used to compute multivariate samples

    };

  }
}

#endif /* ifndef CANNON_MATH_MULTIVARIATE_NORMAL_H */


#include <Eigen/Dense>

#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

int main() {
  Vector3d x = Vector3d::Zero();
  Vector3d y = Vector3d::Zero();

  x[1] = 1.0;
  x[2] = 2.0;

  y[0] = 1.0;
  y[1] = 2.0;
  y[2] = 3.0;

  log_info(x.array() > 0.0);
  log_info((x.array() <= y.array()).all());
  log_info(x.array().max(1));

  Matrix3d A;
  A << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;

  Vector3d a = Vector3d::Zero();
  for (unsigned int i = 0; i < 3; i++) {
    a[i] = *(A.row(1).data() + i);
  }
  log_info(a);

}


#pragma once
#include "neumann_boundary.hpp"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>
//----------------AssembleNeumannBegin----------------
//! Adds the contribution of the boundary to the stiffness matrix.
//!
//! @note This should only be called for the *boundary* edges
//!
//! @param[in] edges a list of boundary edges
//! @param[in] vertices a list of vertices
//! @param[in] gamma the boundary parameter gamma
SparseMatrix assembleBoundaryMatrix(
    const Eigen::MatrixXd &vertices,
    const Eigen::MatrixXi &edges,
    double                 gamma) {
	SparseMatrix         B(vertices.rows(), vertices.rows());
	std::vector<Triplet> triplets;
	const int            numberOfElements = edges.rows();

	// (write your solution here)
	for (int i = 0; i < numberOfElements; ++i) {
		auto indexSet = edges.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));

		Eigen::Matrix2d boundaryMatrix;
		computeBoundaryMatrix(boundaryMatrix, a, b, gamma);

		for (int n = 0; n < 2; ++n) {
			for (int m = 0; m < 2; ++m) {
				triplets.emplace_back(indexSet(n), indexSet(m), boundaryMatrix(n, m));
			}
		}
	}

	B.setFromTriplets(triplets.begin(), triplets.end());
	return B;
}
//----------------AssembleMatrixEnd----------------


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

#ifndef SMOOTH__INTERNAL__SO3_HPP_
#define SMOOTH__INTERNAL__SO3_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief SO(3) Lie group represented as S^3
 *
 * Memory layout
 * -------------
 * Group:    qx qy qz qw  (same as Eigen quaternion)
 * Tangent:  Ωx Ωy Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * 3x3 rotation matrix
 *
 * Lie algebra Matrix form
 * -----------------------
 * [  0 -Ωz  Ωy ]
 * [  Ωz  0 -Ωx ]
 * [ -Ωy Ωx   0 ]
 *
 * Constraints
 * -----------
 * Group:   qx * qx + qy * qy + qz * qz + qw * qw = 1
 * Tangent: -pi < Ωx, Ωy, Ωz <= pi
 */
template<typename _Scalar>
class SO3Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 4;
  static constexpr Eigen::Index Dim     = 3;
  static constexpr Eigen::Index Dof     = 3;
  static constexpr bool IsCommutative   = false;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(0), Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    g_out = Eigen::Quaternion<Scalar>::UnitRandom().coeffs();
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    m_out = q.toRotationMatrix();
  }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q1(g_in1.data());
    Eigen::Map<const Eigen::Quaternion<Scalar>> q2(g_in2.data());
    g_out = (q1 * q2).coeffs();
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    g_out = q.inverse().coeffs();
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::atan2, std::sqrt;
    const Scalar xyz2 = g_in[0] * g_in[0] + g_in[1] * g_in[1] + g_in[2] * g_in[2];

    const auto phi = [&]() -> Scalar {
      if (xyz2 < Scalar(eps2)) {
        // https://www.wolframalpha.com/input/?i=series+atan%28y%2Fx%29+%2F+y+at+y%3D0
        return Scalar(2) / g_in[3] - Scalar(2) * xyz2 / (Scalar(3) * g_in[3] * g_in[3] * g_in[3]);
      } else {
        const Scalar xyz = sqrt(xyz2);
        return Scalar(2) * atan2(xyz, g_in[3]) / xyz;
      }
    }();

    a_out << g_in[0], g_in[1], g_in[2];
    a_out *= phi;
  }

  static void Ad(GRefIn g_in, TMapRefOut A_out)
  {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(g_in.data());
    A_out = q.toRotationMatrix();
  }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::sqrt, std::cos, std::sin;

    const Scalar th2 = a_in.squaredNorm();

    const auto [A, B] = [&]() -> std::array<Scalar, 2> {
      if (th2 < Scalar(eps2)) {
        return {
          // https://www.wolframalpha.com/input/?i=series+sin%28x%2F2%29%2Fx+at+x%3D0
          Scalar(1) / Scalar(2) - th2 / Scalar(48),
          // https://www.wolframalpha.com/input/?i=series+cos%28x%2F2%29+at+x%3D0
          Scalar(1) - th2 / Scalar(8),
        };
      } else {
        const Scalar th = sqrt(th2);
        return {
          sin(th / Scalar(2)) / th,
          cos(th / Scalar(2)),
        };
      }
    }();

    g_out << A * a_in.x(), A * a_in.y(), A * a_in.z(), B;
    if (g_out[3] < Scalar(0)) { g_out *= Scalar(-1); }
  }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out << Scalar(0), -a_in(2), a_in(1), a_in(2), Scalar(0), -a_in(0), -a_in(1), a_in(0),
      Scalar(0);
  }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    a_out << (A_in(2, 1) - A_in(1, 2)) / Scalar(2), (A_in(0, 2) - A_in(2, 0)) / Scalar(2),
      (A_in(1, 0) - A_in(0, 1)) / Scalar(2);
  }

  static void ad(TRefIn a_in, TMapRefOut A_out) { hat(a_in, A_out); }

  static void dr_exp(TRefIn a_in, TMapRefOut A_out)
  {
    using std::sqrt, std::sin, std::cos;
    const Scalar th2 = a_in.squaredNorm();

    const auto [A, B] = [&]() -> std::array<Scalar, 2> {
      if (th2 < Scalar(eps2)) {
        return {
          // https://www.wolframalpha.com/input/?i=series+%281-cos+x%29+%2F+x%5E2+at+x%3D0
          Scalar(1) / Scalar(2) - th2 / Scalar(24),
          // https://www.wolframalpha.com/input/?i=series+%28x+-+sin%28x%29%29+%2F+x%5E3+at+x%3D0
          Scalar(1) / Scalar(6) - th2 / Scalar(120),
        };
      } else {
        const Scalar th = sqrt(th2);
        return {
          (Scalar(1) - cos(th)) / th2,
          (th - sin(th)) / (th2 * th),
        };
      }
    }();

    using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out.noalias() = TangentMap::Identity() - A * ad_a + B * ad_a * ad_a;
  }

  static void dr_expinv(TRefIn a_in, TMapRefOut A_out)
  {
    using std::sqrt, std::sin, std::cos;
    const Scalar th2 = a_in.squaredNorm();

    const auto A = [&]() -> Scalar {
      if (th2 < Scalar(eps2)) {
        // https://www.wolframalpha.com/input/?i=series+1%2Fx%5E2-%281%2Bcos+x%29%2F%282*x*sin+x%29+at+x%3D0
        return Scalar(1) / Scalar(12) + th2 / Scalar(720);
      } else {
        const Scalar th = sqrt(th2);
        return Scalar(1) / th2 - (Scalar(1) + cos(th)) / (Scalar(2) * th * sin(th));
      }
    }();

    using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
    TangentMap ad_a;
    ad(a_in, ad_a);
    A_out.noalias() = TangentMap::Identity() + ad_a / Scalar(2) + A * ad_a * ad_a;
  }

  static void d2r_exp(TRefIn a_in, THessRefOut H_out)
  {
    const auto [A, B, dA_over_th, dB_over_th] = [&]() -> std::array<Scalar, 4> {
      using std::sqrt;

      const Scalar th2 = a_in.squaredNorm();
      const Scalar th  = sqrt(th2);

      if (th2 < Scalar(eps2)) {
        return {
          Scalar(0.5) - th2 / 24,
          Scalar(1. / 6) - th2 / 120,
          -Scalar(1) / 48,
          -Scalar(1) / 60,
        };
      } else {
        const Scalar sTh = sin(th);
        const Scalar cTh = cos(th);
        const Scalar th3 = th2 * th;
        const Scalar th4 = th2 * th2;
        const Scalar th5 = th3 * th2;
        return {
          (Scalar(1) - cTh) / th2,
          (th - sTh) / th3,
          sTh / th3 + 2 * cTh / th4 - 2 / th4,
          -cTh / th4 - 2 / th4 + 3 * sTh / th5,
        };
      }
    }();

    // -A * d(ad) + B * d(ad^2)
    // clang-format off
    H_out <<
      0., -2 * B * a_in.y(), -2 * B * a_in.z(), B * a_in.y(), B * a_in.x(), -A, B * a_in.z(), A, B * a_in.x(),
      B * a_in.y(), B * a_in.x(), A, -2 * B * a_in.x(), 0., -2 * B * a_in.z(), -A, B * a_in.z(), B * a_in.y(),
      B * a_in.z(), -A, B * a_in.x(), A, B * a_in.z(), B * a_in.y(), -2 * B * a_in.x(), -2 * B * a_in.y(), 0.;
    // clang-format on

    // add -dA * ad + dB * ad^2
    Eigen::Matrix3<Scalar> ad_a;
    ad(a_in, ad_a);
    const Eigen::Matrix3<Scalar> ad_a2 = ad_a * ad_a;

    for (auto i = 0u; i < 3; ++i) {
      const Scalar dA_dxi = dA_over_th * a_in(i);
      const Scalar dB_dxi = dB_over_th * a_in(i);
      for (auto j = 0u; j < 3; ++j) {
        H_out.col(i + 3 * j) -= dA_dxi * ad_a.row(j).transpose();
        H_out.col(i + 3 * j) += dB_dxi * ad_a2.row(j).transpose();
      }
    }
  }

  static void d2r_expinv(TRefIn a_in, THessRefOut H_out)
  {
    const auto [A, dA_over_th] = [&]() -> std::array<Scalar, 2> {
      using std::sqrt;

      const Scalar th2 = a_in.squaredNorm();
      const Scalar th  = sqrt(th2);
      if (th2 < Scalar(eps2)) {
        return {
          Scalar(1) / Scalar(12) + th2 / Scalar(720),
          Scalar(1) / Scalar(360),
        };
      } else {
        const Scalar th3 = th2 * th;
        const Scalar th4 = th2 * th2;
        const Scalar sTh = sin(th);
        const Scalar cTh = cos(th);
        return {
          Scalar(1) / th2 - (Scalar(1) + cTh) / (Scalar(2) * th * sTh),
          1 / (2 * th2) + cTh * cTh / (2 * th2 * sTh * sTh) + cTh / (2 * th2 * sTh * sTh)
            + cTh / (2 * th3 * sTh) + 1 / (2 * th3 * sTh) - 2 / th4,
        };
      }
    }();

    // A * d(ad^2)
    // clang-format off
    H_out <<
      0, -2 * A * a_in.y(), -2 * A * a_in.z(), A * a_in.y(), A * a_in.x(), 0.5, A * a_in.z(), -0.5, A * a_in.x(),
      A * a_in.y(), A * a_in.x(), -0.5, -2 * A * a_in.x(), 0, -2 * A * a_in.z(), 0.5, A * a_in.z(), A * a_in.y(),
      A * a_in.z(), 0.5, A * a_in.x(), -0.5, A * a_in.z(), A * a_in.y(), -2 * A * a_in.x(), -2 * A * a_in.y(), 0;
    // clang-format on

    // add dA * ad^2
    Eigen::Matrix3<Scalar> ad_a;
    ad(a_in, ad_a);
    const Eigen::Matrix3<Scalar> ad_a2 = ad_a * ad_a;
    for (auto i = 0u; i < 3; ++i) {
      const Scalar dA_dxi = dA_over_th * a_in(i);
      for (auto j = 0u; j < 3; ++j) { H_out.col(i + 3 * j) += dA_dxi * ad_a2.row(j).transpose(); }
    }
  }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__SO3_HPP_


// inverse_chi_squared_distribution_example.cpp

// Copyright Paul A. Bristow 2010.
// Copyright Thomas Mang 2010.

// 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)

// Example 1 of using inverse chi squared distribution
#include <boost/math/distributions/inverse_chi_squared.hpp>
using boost::math::inverse_chi_squared_distribution;  // inverse_chi_squared_distribution.
using boost::math::inverse_chi_squared; //typedef for nverse_chi_squared_distribution double.

#include <iostream>
using std::cout;    using std::endl;
#include <iomanip>
using std::setprecision;
using std::setw;
#include <cmath>
using std::sqrt;

template <class RealType>
RealType naive_pdf1(RealType df, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Inverse-chi-square_distribution
  // definition 1 using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(2., -df2) * pow(x, (-df2 -1)) * exp(-1/(2 * x) ) )
      / tgamma(df2);  //
   return result;
}

template <class RealType>
RealType naive_pdf2(RealType df, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Inverse-chi-square_distribution
  // Definition 2, using tgamma for simplicity as a check.
  // Not scaled, so assumes scale = 1 and only uses nu aka df.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(df2, df2) * pow(x, (-df2 -1)) * exp(-df/(2*x) ) )
     / tgamma(df2);
   return result;
}

template <class RealType>
RealType naive_pdf3(RealType df, RealType scale, RealType x)
{ // Formula from Wikipedia http://en.wikipedia.org/wiki/Scaled-inverse-chi-square_distribution
  // *Scaled* version, definition 3, df aka nu, scale aka sigma^2
  // using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType df2 = df / 2;
   RealType result = (pow(scale * df2, df2) * exp(-df2 * scale/x) )
     / (tgamma(df2) * pow(x, 1+df2));
   return result;
}

template <class RealType>
RealType naive_pdf4(RealType df, RealType scale, RealType x)
{ // Formula from http://mathworld.wolfram.com/InverseChi-SquaredDistribution.html
  // Weisstein, Eric W. "Inverse Chi-Squared Distribution." From MathWorld--A Wolfram Web Resource.
  // *Scaled* version, definition 3, df aka nu, scale aka sigma^2
  // using tgamma for simplicity as a check.
   using namespace std; // For ADL of std functions.
   using boost::math::tgamma;
   RealType nu = df; // Wolfram uses greek symbols nu,
   RealType xi = scale; // and xi.
   RealType result =
     pow(2, -nu/2) *  exp(- (nu * xi)/(2 * x)) * pow(x, -1-nu/2) * pow((nu * xi), nu/2)
     / tgamma(nu/2);
   return result;
}

int main()
{

  cout << "Example (basic) using Inverse chi squared distribution. " << endl;

  // TODO find a more practical/useful example.  Suggestions welcome?

#ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  int max_digits10 = 2 + (boost::math::policies::digits<double, boost::math::policies::policy<> >() * 30103UL) / 100000UL;
  cout << "BOOST_NO_CXX11_NUMERIC_LIMITS is defined" << endl;
#else
  int max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = "
    << max_digits10 << endl;
  cout.precision(max_digits10); //

  inverse_chi_squared ichsqdef; // All defaults  - not very useful!
  cout << "default df = " << ichsqdef.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef.scale() << endl; //  default df = 1, default scale = 0.5

   inverse_chi_squared ichsqdef4(4); // Unscaled version, default scale = 1 / degrees_of_freedom
   cout << "default df = " << ichsqdef4.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef4.scale() << endl; //  default df = 4, default scale = 2

   inverse_chi_squared ichsqdef32(3, 2); // Scaled version, both degrees_of_freedom and scale specified.
   cout << "default df = " << ichsqdef32.degrees_of_freedom()
    << ", default scale = " <<  ichsqdef32.scale() << endl; // default df = 3, default scale = 2

  {
    cout.precision(3);
    double nu = 5.;
    //double scale1 = 1./ nu; // 1st definition sigma^2 = 1/df;
    //double scale2 = 1.; // 2nd definition sigma^2 = 1
    inverse_chi_squared ichsq(nu, 1/nu); // Not scaled
    inverse_chi_squared sichsq(nu, 1/nu); // scaled

    cout << "nu = " << ichsq.degrees_of_freedom() << ", scale = " << ichsq.scale() << endl;

    int width = 8;

    cout << "     x        pdf      pdf1   pdf2  pdf(scaled)    pdf       pdf      cdf     cdf" << endl;
    for (double x = 0.0; x < 1.; x += 0.1)
    {
      cout
        << setw(width) << x
        << ' ' << setw(width) << pdf(ichsq, x) // unscaled
        << ' ' << setw(width) << naive_pdf1(nu,  x) // Wiki def 1 unscaled matches graph
        << ' ' << setw(width) << naive_pdf2(nu,  x) // scale = 1 - 2nd definition.
        << ' ' << setw(width) << naive_pdf3(nu, 1/nu, x) // scaled
        << ' ' << setw(width) << naive_pdf4(nu, 1/nu, x) // scaled
        << ' ' << setw(width) << pdf(sichsq, x)  // scaled
        << ' ' << setw(width) << cdf(sichsq, x)  // scaled
        << ' ' << setw(width) << cdf(ichsq, x)  // unscaled
       << endl;
    }
  }

  cout.precision(max_digits10);

  inverse_chi_squared ichisq(2., 0.5);
  cout << "pdf(ichisq, 1.) = " << pdf(ichisq, 1.) << endl;
  cout << "cdf(ichisq, 1.) = " << cdf(ichisq, 1.) << endl;

  return 0;
}  // int main()

/*

Output is:
 Example (basic) using Inverse chi squared distribution.
  Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = 17
  default df = 1, default scale = 1
  default df = 4, default scale = 0.25
  default df = 3, default scale = 2
  nu = 5, scale = 0.2
       x        pdf      pdf1   pdf2  pdf(scaled)    pdf       pdf      cdf     cdf
         0        0    -1.#J    -1.#J    -1.#J    -1.#J        0        0        0
       0.1     2.83     2.83 3.26e-007     2.83     2.83     2.83   0.0752   0.0752
       0.2     3.05     3.05  0.00774     3.05     3.05     3.05    0.416    0.416
       0.3      1.7      1.7    0.121      1.7      1.7      1.7    0.649    0.649
       0.4    0.941    0.941    0.355    0.941    0.941    0.941    0.776    0.776
       0.5    0.553    0.553    0.567    0.553    0.553    0.553    0.849    0.849
       0.6    0.345    0.345    0.689    0.345    0.345    0.345    0.893    0.893
       0.7    0.227    0.227    0.728    0.227    0.227    0.227    0.921    0.921
       0.8    0.155    0.155    0.713    0.155    0.155    0.155     0.94     0.94
       0.9     0.11     0.11    0.668     0.11     0.11     0.11    0.953    0.953
         1   0.0807   0.0807     0.61   0.0807   0.0807   0.0807    0.963    0.963
  pdf(ichisq, 1.) = 0.30326532985631671
  cdf(ichisq, 1.) = 0.60653065971263365

*/


#include "core/BezierCurve.h"

#include "core/Util.h"
#include <boost/math/quadrature/trapezoidal.hpp>

BezierCurve::BezierCurve(const vector_t& control0, const vector_t& control1, const vector_t& control2, const vector_t& control3)
    : _control0(control0), _control1(control1), _control2(control2), _control3(control3) {

}

vector_t BezierCurve::Position(float t) const {
  if (t <= 0) {
    return _control0;
  } else if (t >= 1) {
    return _control3;
  } else {
    return (1 - t) * (1 - t) * (1 - t) * _control0 +
      3 * (1 - t) * (1 - t) * t * _control1 +
      3 * (1 - t) * t * t * _control2 +
      t * t * t * _control3;
  }
}

vector_t BezierCurve::Heading(float t) const {
  if (t <= 0) {
    return (_control1 - _control0).normalized();
  } else if (t >= 1) {
    return (_control3 - _control2).normalized();
  } else {
    return (3 * (1 - t) * (1 - t) * (_control1 - _control0) +
      6 * (1 - t) * t * (_control2 - _control1) +
      3 * t * t * (_control3 - _control2)).normalized();
  }
}

float BezierCurve::Length(float start, float end) const {
  if (start >= end) {
    return 0;
  } else {
    return boost::math::quadrature::trapezoidal(
        [this](float t) {
          vector_t grad = -3 * (1 - t) * (1 - t) * _control0 +
              3 * (1 - t) * (1 - 3 * t) * _control1 +
              3 * t * (2 - 3 * t) * _control2 +
              3 * t * t * _control3;
          return grad.norm();
        },
        std::max(start, 0.0f), std::min(end, 1.0f), 1e-12f);
  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

//[length_with_strategy
//`The following example shows the length measured over a sphere, expressed in kilometers. To do that the radius of the sphere must be specified in the constructor of the strategy.

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>

int main()
{
    using namespace boost::geometry;
    typedef model::point<float, 2, cs::spherical_equatorial<degree> > P;
    model::linestring<P> line;
    line.push_back(P(2, 41));
    line.push_back(P(2, 48));
    line.push_back(P(5, 52));
    double const mean_radius = 6371.0; /*< [@http://en.wikipedia.org/wiki/Earth_radius Wiki]  >*/
    std::cout << "length is "
        << length(line, strategy::distance::haversine<P>(mean_radius) )
        << " kilometers " << std::endl;

    return 0;
}

//]

//[length_with_strategy_output
/*`
Output:
[pre
length is 1272.03 kilometers
]
*/
//]


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschrÃ¤nkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/timer.hpp>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
typename mtl::Collection<Matrix>::value_type
inline determinant(const Matrix& A, double eps= 0)
{
    mtl::dense_vector<std::size_t> P(num_rows(A));
    mtl::dense2D<typename mtl::Collection<Matrix>::value_type>   LU(A);  // A is copied in a dense matrix because lu overwrites its argument
    lu(LU, P, eps);
    return product(diagonal(LU));
}

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    laplacian_setup(A, 3, 4);
    // boost::timer t;
    cout << "\n" << name << ", determinant is " << determinant(A) << "\n";
    // cout << "It took " << t.elapsed() << "s\n";
}

int main(int, char**)
{
    using namespace mtl;
    dense2D<double>                                      dr;
    dense2D<complex<double> >                            dz;
    dense2D<double, mat::parameters<col_major> >      dc;
    compressed2D<double>                                 cr;

    test(dr, "Row-major dense");
    test(dz, "Row-major dense with complex numbers");
    test(dc, "Column-major dense");
    test(cr, "Row-major compressed");

    return 0;
}


#include <Eigen/Core>

#include <vector>
#include <memory>

#include "SDOT/Polygon.h"

using namespace sdot;

void MakePolygonConvex(){

  // First, construct a non-convex polygon
  /*
      v4     v2
      | \   /|
      |  \ / |
      |  v3  |
      |      |
      v0-----v1
 */
  Eigen::Matrix2Xd pts(2,5);
  pts << 0, 1, 1, 0.5, 0,
         0, 0, 1, 0.5, 1;

  Polygon poly(pts);

  // Test the IsConvex() function.
  std::cout << "Is non-convex base polygon being detected? ";
  if(poly.IsConvex()){
    std::cout << " NO!"<< std::endl;
  }else{
    std::cout << " YES!" << std::endl;
  }

  // Test the ability to construct a convex partition
  std::vector<std::shared_ptr<Polygon>> polys = poly.ConvexPartition();

  std::cout << "Convex Polygons:" << std::endl;
  for(int i=0; i<polys.size(); ++i){
    std::cout << "  Poly"<< i+1 << " IsConvex()? " << polys.at(i)->IsConvex() << std::endl;
  }
}

int main(){

  MakePolygonConvex();
  return 0;
}


/* MonteCarlo.hpp
-Description:
	*Declare and define MonteCarlo template class that calculates the price of a financial option using the Euler-Maruyama scheme.
-State Objects:
	*variate_generator rng: Boost implemented normal random number generator for Monte Carlo path generation.
-Class Methods:
	// Constructors/Destructors:
	*MonteCarlo(double t, double s, double k, double b, double r, double sigma, unsigned long NSIM, unsigned long NT, bool isCall): Overloaded Constructor.
	*MonteCarlo(const MonteCarlo&): Copy constructor.
	*~MonteCarlo(): Destructor.
	// Accessors:
	*unsigned long NSIM() const: Return the number of trials to be run in Monte Carlo.
	*unsigned long NT() const: Return number of sub-intervals in time to be used in generating each path.
	// Mutators:
	*void NSIM(unsigned long): Set number of trials.
	*void NT(unsigned long): Set number of time sub-intervals.
	// Misc. Methods:
	*double Price() const: Calculate price of associated option using Monte Carlo method with Euler-Maruyama scheme.
*/

#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <ctime>
#include <cmath>
#include <string>
#include <vector>
#include "MonteCarlo.hpp"
#include "MonteCarloExcept.hpp"
#include "Option.hpp"
#include "OptionExcept.hpp"

namespace Options
{
	////////////////////////////
	// Constructors/Destructor:
	////////////////////////////
	MonteCarlo::MonteCarlo(double t, double s, double k, double b, double r, double sigma, bool isCall, unsigned long NSIM_in, unsigned long NT_in) : /* Overloaded Constructor. Set all parameters. */
		Option(t, s, k, b, r, sigma, isCall), nT(NT_in), nSIM(NSIM_in)
	{
		mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::MonteCarlo(const MonteCarlo &in) : Option(in), nT(in.nT), nSIM(in.nSIM) /* Copy Constructor. */
	{
			mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::MonteCarlo(const Option &in, unsigned long NSIM, unsigned long NT): 							/* Copy Constructor 2, with NT and NSIM setting*/
		Option(in), nSIM(NSIM), nT(NT)
	{
		mainRNG = new boost::variate_generator<boost::mt19937, boost::normal_distribution<double> >(boost::mt19937(time(0)), boost::normal_distribution<>(0, 1));
	}
	MonteCarlo::~MonteCarlo()													/* Destructor. */
	{
		delete mainRNG;
	}
	////////////////////////////
	// Accessors:
	////////////////////////////
	unsigned long MonteCarlo::NSIM() const							/* Return number of simulations run in the Monte Carlo. */
	{
		return nSIM;
	}
	unsigned long MonteCarlo::NT() const								/* Return number of time intervals run in the Monte Carlo path generation. */
	{
		return nT;
	}
	////////////////////////////
	// Mutators:
	////////////////////////////
	void MonteCarlo::NSIM(unsigned long NSIM_in)						/* Set the number of simulations to run in the Monte Carlo. */
	{
		nSIM = NSIM_in;
	}
	void MonteCarlo::NT(unsigned long NT_in)							/* Set the number of time intervals to run in the Monte Carlo path generation. */
	{
		nT = NT_in;
	}
	////////////////////////////
	// Misc. Methods:
	////////////////////////////
	double MonteCarlo::Price() const		/* Get price of option using Monte Carlo method with Euler-Maruyama pdf. */
	{
		// NSIM is number of paths to generate, 1/NT is the time divisor (discretizes "t" into number of subintervals).
		// As NSIM AND NT approach infinity, GeneratePrice approaches the exact price given by the Black-Scholes equation or the American Perpetual Option pricing method.
		double S, output = 0, payoff = 0;
		for (unsigned long currTrial = 0; currTrial < nSIM; currTrial++)
		{
			// Reset the path:
			S = Param("s");
			// Generate path using Euler-Maruyama partial differentiation scheme:
			for (double currTimeInterval = 0; currTimeInterval < Param("t"); currTimeInterval += 1.0 / (double) nT)
			{
				S += (Param("r") * S * (((double) 1.0 / (double) nT))) + (Param("sigma") * S * std::sqrt((double) 1.0 / (double) nT) * (*mainRNG)());
			}
			// Calculate payoff of option at expiry depending on type:
			if (Option::Type())
			{
				payoff = std::max(S - Param("k"), 0.0);
			}
			else
			{
				payoff = std::max(Param("k") - S, 0.0);
			}
			// Discount the payoff back to present and accumulate:
			output += payoff / (double) nSIM;
		}
		// Return the average discounted payoff:
		return output * exp(-Param("r") * Param("t"));
	}
	////////////////////////////
	// Overloaded Operators:
	////////////////////////////
	MonteCarlo& MonteCarlo::operator=(const MonteCarlo &in)									/* Assignment Operator. */
	{
		if (this != &in)
		{
			Option::operator=(in);
		}
		return *this;
	}
}


// EigenConsoleApplication1.cpp : Defines the entry point for the console application.
//

//#include "stdafx.h"
#include <ctime>
#include <Eigen/Dense>
#include <iostream>
#include <complex>      // std::complex

//using Eigen::MatrixXd;
//using Eigen::Matrix3cd;

void rf_pulse_matrix(Eigen::Matrix3cd *rfpulse, double alpha, double phi)
{
	const double PI = 3.1415926535897932384626433832795;

	double alpha_half = alpha / 2.0;
	double c_a_h = cos(alpha_half);
	double c_a_h2 = c_a_h * c_a_h;
	double s_a_h = sin(alpha_half);
	double s_a_h2 = s_a_h * s_a_h;
	double c_a = cos(alpha);
	double s_a = sin(alpha);

	std::complex<double> i_phi = std::complex<double>(0.0, phi);
	std::complex<double> i_half = std::complex<double>(0.0, 0.5);

	(*rfpulse)(0, 0) = c_a_h2;
	(*rfpulse)(0, 1) = exp(i_phi*2.0)*s_a_h2;
	(*rfpulse)(0, 2) = -1.0*std::complex<double>(0.0, 1.0)*exp(i_phi)*s_a;

	(*rfpulse)(1, 0) = exp(-2.0 * i_phi)*s_a_h2;
	(*rfpulse)(1, 1) = c_a_h2;
	(*rfpulse)(1, 2) = std::complex<double>(0.0, 1.0) * exp(-1.0*i_phi)*s_a;

	(*rfpulse)(2, 0) = -1.0*i_half*exp(-1.0*i_phi)*s_a;
	(*rfpulse)(2, 1) = 1.0*i_half*exp(1.0*i_phi)*s_a;
	(*rfpulse)(2, 2) = c_a;

	//std::cout << *rfpulse << std::endl;
}

void epg_grad(Eigen::MatrixXcd *FpFmZ, int num_cols)
{

	for (int i = 0; i < num_cols - 1; i++)
	{
		//std::cout << i << "\t" << num_cols - 1 - i << "\t" << num_cols - 1 - i - 1 << "\t" << (*FpFmZ)(0,i) << std::endl;
		(*FpFmZ)(0, num_cols - 1 - i) = (*FpFmZ)(0, num_cols - 1 - i - 1);
	}

	(*FpFmZ)(0, 0) = std::complex<double>(0.0, 0.0);

	for (int i = 0; i < num_cols - 1; i++)
		(*FpFmZ)(1, i) = (*FpFmZ)(1, i + 1);

	(*FpFmZ)(1, num_cols - 1) = std::complex<double>(0.0, 0.0);

	(*FpFmZ)(0, 0) = conj((*FpFmZ)(1, 0));
}

void  cpmg_epg(double *signal, int Nechos, double rf_180, double T1, double T2, double Techo)
{
	const double PI = 3.1415926535897932384626433832795;
	int    num_cols = 2 * Nechos;

	Eigen::MatrixXcd FpFmZ(3, 34);
	Eigen::Matrix3cd rfpulse90y = Eigen::Matrix3cd::Zero();
	Eigen::Matrix3cd rfpulse180x = Eigen::Matrix3cd::Zero();
	Eigen::Matrix3d ee = Eigen::Matrix3d::Zero();
	//Eigen::MatrixXd signal(1, 17);

	FpFmZ = FpFmZ.Zero(3, 34);

	// ****************************************
	// Initialize diagonal of relaxation matrix
	// ****************************************

	ee(0, 0) = exp(-Techo / 2.0 / T2);
	ee(1, 1) = exp(-Techo / 2.0 / T2);
	ee(2, 2) = exp(-Techo / 2.0 / T1);

	// ********************************
	// Set Z magnetization to 1.0 +0.0j
	// ********************************

	FpFmZ(0, 0) = 0.0;
	FpFmZ(1, 0) = 0.0;
	FpFmZ(2, 0) = 1.0;

	// *********************************
	// Create 90(y) rotation matrix
	// *********************************

	rf_pulse_matrix(&rfpulse90y, PI / 2.0, PI / 2.0);

	// *********************************
	// Create 180(x) rotation matrix
	// *********************************

	rf_pulse_matrix(&rfpulse180x, rf_180*PI / 180.0, 0.0);

	// *********************************
	// Apply 90 degree pulse
	// *********************************

	FpFmZ = rfpulse90y * FpFmZ;

	// ****************************************
	// Apply 180 pulses of CPMG sequence
	// ****************************************

	for (int i = 0; i < Nechos; i++)
	{

		FpFmZ = ee * FpFmZ;
		FpFmZ(2, 0) = FpFmZ(2, 0) + 1.0 - ee(2, 2);
		epg_grad(&FpFmZ, 34);

		FpFmZ = rfpulse180x * FpFmZ;

		FpFmZ = ee * FpFmZ;
		FpFmZ(2, 0) = FpFmZ(2, 0) + 1.0 - ee(2, 2);
		epg_grad(&FpFmZ, 34);

		signal[i] = real(FpFmZ(0, 0));
	}

}

//int main()
//{
//	const double PI = 3.1415926535897932384626433832795;
//	std::clock_t    start;
//
//	double T1 = 3000.0;
//	double T2 = 50.0;
//	double Techo = 10.0;
//	int    Nechos = 17;
//	int    num_cols = 2 * Nechos;
//
//	Eigen::MatrixXcd FpFmZ(3, num_cols);
//	Eigen::Matrix3cd rfpulse90y = Eigen::Matrix3cd::Zero();
//	Eigen::Matrix3cd rfpulse180x = Eigen::Matrix3cd::Zero();
//	Eigen::Matrix3d ee = Eigen::Matrix3d::Zero();
//
//	double *signal = new  double [Nechos] { 0.0 };
//
//	start = std::clock();
//
//	for (int j = 0; j < 10000; j++)
//		cpmg_epg(signal, Nechos, PI, T1, T2, Techo);
//
//	std::cout << "Time: " << (std::clock() - start) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl;
//
//	for (int i = 0; i<Nechos; i++)
//		std::cout << signal[i] << std::endl;
//
//
//
//}


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

using namespace Eigen;
using namespace std;

int main()
{
  Matrix3f m = Matrix3f::Random();
  m = (m + Matrix3f::Constant(1.2)) * 50;
  cout << "m =" << endl << m << endl;
  Vector3f v(1,2,3);

  cout << "m * v =" << endl << m * v << endl;
}