/**
 * @file taylorode.cc
 * @brief NPDE homework TaylorODE
 * @author ?, Philippe Peter
 * @date 24.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "taylorode.h"

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

namespace TaylorODE {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d PredPreyModel::f(const Eigen::Vector2d& y) const {
  return {(alpha1_ - beta1_ * y(1)) * y(0), (beta2_ * y(0) - alpha2_) * y(1)};
}

Eigen::Vector2d PredPreyModel::df(const Eigen::Vector2d& y,
                                  const Eigen::Vector2d& z) const {
  Eigen::Matrix2d Df;
  Df << alpha1_ - beta1_ * y(1), -beta1_ * y(0), beta2_ * y(1),
      -alpha2_ + beta2_ * y(0);
  return Df * z;
}

Eigen::Vector2d PredPreyModel::d2f(const Eigen::Vector2d& y,
                                   const Eigen::Vector2d& z) const {
  Eigen::Matrix2d H1, H2;
  H1 << 0, -beta1_, -beta1_, 0;
  H2 << 0, beta2_, beta2_, 0;
  return {z.transpose() * H1 * z, z.transpose() * H2 * z};
}

std::vector<Eigen::Vector2d> SolvePredPreyTaylor(const PredPreyModel& model,
                                                 double T,
                                                 const Eigen::Vector2d& y0,
                                                 unsigned int M) {
  std::vector<Eigen::Vector2d> res;
  res.reserve(M + 1);

  Eigen::Vector2d y = y0;
  double h = T / M;
  res.push_back(y);

  for (unsigned int k = 0; k < M; ++k) {
    // evaluate terms for taylor step.
    auto fy = model.f(y);
    auto dfyfy = model.df(y, fy);
    auto df2yfy = model.df(y, dfyfy);
    auto d2fyfy = model.d2f(y, fy);

    // evaluate taylor expansion to compute update
    y = y + h * fy + 0.5 * h * h * dfyfy +
        1.0 / 6.0 * h * h * h * (df2yfy + d2fyfy);

    // save new state:
    res.push_back(y);
  }
  return res;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double TestCvgTaylorMethod() {
  // initialize parameters for the model:
  double T = 10;               // final time
  Eigen::Vector2d y0(100, 5);  // initial condition
  Eigen::Vector2d yex(0.319465882659820,
                      9.730809352326228);  // reference solution
  double alpha1 = 3.0;
  double alpha2 = 2.0;
  double beta1 = 0.1;
  double beta2 = 0.1;
  PredPreyModel model(alpha1, alpha2, beta1, beta2);

  // Initialize parameters for the convergence study
  unsigned int M0 = 128;    // Minimum number of timesteps
  unsigned int numRef = 8;  // Number of refinements

  // Convergence study
  Eigen::ArrayXd error(numRef);
  Eigen::ArrayXd M(numRef);
  for (unsigned int i = 0; i < numRef; ++i) {
    M(i) = std::pow(2, i) * M0;
    auto res = SolvePredPreyTaylor(model, T, y0, M(i));
    error(i) = (res.back() - yex).norm();
  }

  PrintErrorTable(M, error);

  // calculate linear regression line: log(error) ~ c0 + c1*log(M)
  Eigen::MatrixXd A(numRef, 2);
  A.col(0) = Eigen::VectorXd::Ones(numRef);
  A.col(1) = M.log();
  Eigen::VectorXd logError = error.log();
  Eigen::Vector2d coeffs = A.householderQr().solve(logError);

  // estimated convergence rate: -c1
  return -coeffs(1);
}
/* SAM_LISTING_END_2 */

void PrintErrorTable(const Eigen::ArrayXd& M, const Eigen::ArrayXd& error) {
  std::cout << std::setw(15) << "M" << std::setw(15) << "error" << std::setw(15)
            << "rate" << std::endl;

  for (unsigned int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << error(i);
    if (i > 0) {
      std::cout << std::setw(15) << std::log2(error(i - 1) / error(i));
    }
    std::cout << std::endl;
  }
}

}  // namespace TaylorODE


/**
 * @file semimprk.cc
 * @brief NPDE homework SemImpRK code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "semimprk.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "polyfit.h"

namespace SemImpRK {

/* SAM_LISTING_BEGIN_0 */
double cvgRosenbrock() {
  double cvgRate = 0.0;
  // TO DO: (13-2.d) Use polyfit() to estimate the rate of convergence
  // for solveRosenbrock().
  // Final time
  const double T = 10.0;
  // Mesh sizes h=2^{-k} for k in K.
  const Eigen::ArrayXd K = Eigen::ArrayXd::LinSpaced(7, 4, 10);

  // Initial data
  Eigen::Vector2d y0(1., 1.);
  // Parameter and useful matrix for f
  const double lambda = 1;
  Eigen::Matrix2d R;
  R << 0.0, -1.0, 1.0, 0.0;

  // Function and its Jacobian
  auto f = [&R, &lambda](Eigen::Vector2d y) {
    return R * y + lambda * (1.0 - y.squaredNorm()) * y;
  };
  auto df = [&lambda](Eigen::Vector2d y) {
    double x = 1 - y.squaredNorm();
    Eigen::Matrix2d J;
    J << lambda * x - 2 * lambda * y(0) * y(0), -1 - 2 * lambda * y(1) * y(0),
        1 - 2 * lambda * y(1) * y(0), lambda * x - 2 * lambda * y(1) * y(1);
    return J;
  };

  // Reference mesh size
  const int N_ref = 10 * std::pow(2, 12);
  // Reference solution
  std::vector<Eigen::VectorXd> solref = solveRosenbrock(f, df, y0, N_ref, T);

  Eigen::ArrayXd Error(K.size());
  std::cout << std::setw(15) << "N" << std::setw(16) << "maxerr\n";
  // Main loop: loop over all meshes
  for (unsigned int i = 0; i < K.size(); ++i) {
    // h = 2^{-k} => N = T*h = T*2^k
    int N = T * std::pow(2, K[i]);
    // Get solution
    std::vector<Eigen::VectorXd> sol = solveRosenbrock(f, df, y0, N, T);
    // Compute error
    double maxerr = 0;
    for (unsigned int j = 0; j < sol.size(); ++j) {
      maxerr =
          std::max(maxerr, (sol.at(j) - solref.at((j * N_ref) / N)).norm());
    }

    Error[i] = maxerr;
    std::cout << std::setw(15) << N << std::setw(16) << maxerr << std::endl;
  }
  // Use log(N)=log(T*2^k)=log(T)+log(2)*k to get natural logarithm of N.
  cvgRate = -polyfit(std::log(2.0) * K, Error.log(), 1)(0);
  return cvgRate;
}
/* SAM_LISTING_END_0 */

}  // namespace SemImpRK


#pragma once

#include "perceive/foundation.hpp"
#include "perceive/geometry/vector.hpp"
#include <Eigen/Dense>

namespace perceive
{
// ------------------------------------------------------------------- is-finite

bool is_finite(const Vector2r& M);
bool is_finite(const Vector3r& M);
bool is_finite(const Vector4r& M);
bool is_finite(const Matrix3r& M);
bool is_finite(const Matrix34r& M);
bool is_finite(const MatrixXr& M);

// ------------------------------------------------------------------- normalize

inline Vector3r& normalize(Vector3r& X)
{
   auto norm_inv = 1.0 / X.norm();
   X *= norm_inv;
   return X;
}

inline Vector3r normalized(const Vector3r& X)
{
   auto Y(X);
   normalize(Y);
   return Y;
}

// ------------------------------------------------------------------------- str

std::string str(const Vector2r& M);
std::string str(const Vector3r& M);
std::string str(const Vector4r& M);
std::string str(const Matrix3r& M);
std::string str(const Matrix34r& M);
std::string str(const MatrixXr& M);

std::string str(std::string name, const Matrix3r& M);
std::string str(std::string name, const Matrix34r& M);
std::string str(std::string name, const MatrixXr& M);

// ------------------------------------------------------------------------- SVD

real svd_thin(const MatrixXr& M, VectorXr& out);
real svd_thin(const Matrix3r& M, Vector3r& out);
// real svd_thin(const Matrix3d& M, Vector3d& out);
// real svd_thin(const MatrixXd& M, VectorXd& out);
real svd_thin(const MatrixXr& M, Vector6r& out);
void svd_UV(const MatrixXr& M, MatrixXr& U, MatrixXr& V);
void svd_UV(const Matrix3r& M, Matrix3r& U, Matrix3r& V);
// void svd_UV(const MatrixXd& M, MatrixXd& U, MatrixXd& V);
void svd_UDV(const MatrixXr& M, MatrixXr& U, VectorXr& D, MatrixXr& V);
void svd_UDV(const Matrix3r& M, Matrix3r& U, Vector3r& D, Matrix3r& V);
void svd_UDV(const Matrix3r& M, Matrix3r& U, Matrix3r& D, Matrix3r& V);
// void svd_UDV(const MatrixXd& M, MatrixXd& U, VectorXd& D, MatrixXd& V);

void svd_UDV(const Eigen::Matrix<std::complex<long double>, 3, 3>& M,
             Eigen::Matrix<std::complex<long double>, 3, 3>& U,
             Eigen::Matrix<std::complex<long double>, 3, 1>& D,
             Eigen::Matrix<std::complex<long double>, 3, 3>& V);
void svd_UDV(const Eigen::Matrix<std::complex<double>, 3, 3>& M,
             Eigen::Matrix<std::complex<double>, 3, 3>& U,
             Eigen::Matrix<std::complex<double>, 3, 1>& D,
             Eigen::Matrix<std::complex<double>, 3, 3>& V);
void svd_UDV(const Eigen::Matrix<std::complex<float>, 3, 3>& M,
             Eigen::Matrix<std::complex<float>, 3, 3>& U,
             Eigen::Matrix<std::complex<float>, 3, 1>& D,
             Eigen::Matrix<std::complex<float>, 3, 3>& V);

void svd_UDV(
    const Eigen::
        Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>& M,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<long double>, Eigen::Dynamic, Eigen::Dynamic>&
        V);

void svd_UDV(
    const Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>&
        M,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic>& V);

void svd_UDV(
    const Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& M,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& U,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, 1>& D,
    Eigen::Matrix<std::complex<float>, Eigen::Dynamic, Eigen::Dynamic>& V);

// double bdcsvd_thin(const MatrixXd& M, VectorXd& out);
real bdcsvd_thin(const MatrixXr& M, VectorXr& out);

template<typename T> struct CentreEigenVectorResult
{
   T C;                           // centre
   vector<std::pair<T, real>> Es; // {eigenvectors, eigenvalues}
};

template<typename InputIt>
auto calc_centre_and_eigenvectors(InputIt begin, InputIt end)
    -> CentreEigenVectorResult<
        typename std::iterator_traits<InputIt>::value_type>;

// For best results, centre and scale the data before performing svd-3d-UDV
struct SVD3DRet
{
   Matrix3r U, V;
   Vector3r D;
   Matrix3r Dm() const noexcept; // `D` as a Diagonal Matrix
   string to_string() const noexcept;
   friend string str(const SVD3DRet& o) noexcept { return o.to_string(); }
   Vector3r eigen_vector(int ind) const noexcept;
   Quaternion rot_vec() const noexcept;
};

template<typename InputIt> SVD3DRet svd_3d_UDV(InputIt start, InputIt finish);

// ------------------------------------------------------------ condition-number

real condition_number(const MatrixXr& M);
real condition_number(const Matrix3r& M);

// ----------------------------------------------------------------- matrix rank

template<typename T> inline unsigned matrix_rank(const T& M)
{
   Eigen::FullPivLU<T> lu(M);
   return lu.rank();
}

// ------------------------------------------------------------------ null space
template<typename T> inline T matrix_kernel(const T& M)
{
   Eigen::FullPivLU<T> lu(M);
   T out = lu.kernel();
   return out;
}

// -------------------------------------------------------- Cross-product Matrix

void to_cross_product_matrix(const Vector3& X, Matrix3r& M);
void to_cross_product_matrix(const Vector3r& X, Matrix3r& M);
Matrix3r make_cross_product_matrix(const Vector3& X);
Matrix3r make_cross_product_matrix(const Vector3r& X);

// --------------------------------------------------- Quadratic with Constraint

// Minimize xGtGx such that Hx = h
//  * G must have more rows than columns.
//  * G.cols() == H.cols()
MatrixXr minimize_with_constraint(const MatrixXr& G, const MatrixXr& H, real h);

//
//
//
//
//
//
//
//
//                                Implementations
//
//
//
//
//
//
//

template<typename InputIt>
auto calc_centre_and_eigenvectors(InputIt begin, InputIt end)
    -> CentreEigenVectorResult<
        typename std::iterator_traits<InputIt>::value_type>
{
   using T   = typename std::iterator_traits<InputIt>::value_type;
   using Ret = CentreEigenVectorResult<T>;

   Ret out;
   const int sz = std::distance(begin, end);
   const int N  = (sz == 0) ? -1 : begin->size();
   if(sz < N) return out;

   // Calculate the centre
   T C;
   for(auto i = 0; i < sz; ++i) C(i) = 0.0;
   for(auto ii = begin; ii != end; ++ii) { C += *ii; }
   C /= sz;

   // Create matrices
   MatrixXr A(sz, N);
   int pos = 0;
   for(auto ii = begin; ii != end; ++ii, ++pos) {
      for(auto j = 0; j < N; ++j) A(pos, j) = ii->operator()(j) - C(j);
   }

   MatrixXr At  = A.transpose();
   MatrixXr AtA = At * A;

   MatrixXr U, V;
   VectorXr D;
   svd_UDV(A, U, D, V);

   Expects(V.rows() == N);
   Expects(V.cols() == N);

   out.C = C;
   out.Es.resize(N);
   for(auto i = 0; i < N; ++i) {
      auto& E  = out.Es[i];
      E.second = D(i);
      for(auto j = 0; j < N; ++j) { E.first(j) = V(j, i); }
   }

   return out;
}

// -------------------------------------------------------------------svd-3d-UDV
// For best results, center and scale the data first.
template<typename InputIt> SVD3DRet svd_3d_UDV(InputIt start, InputIt finish)
{
   using T = typename std::iterator_traits<InputIt>::value_type;

   const size_t n_rows     = size_t(std::distance(start, finish));
   constexpr size_t n_cols = 3;
   Expects(n_rows >= n_cols);

   auto M  = MatrixXr(n_rows, n_cols);
   int row = 0;
   for(auto ii = start; ii != finish; ++ii) {
      const T& o = *ii;
      Expects(o.size() == n_cols);
      for(int col = 0; col < int(n_cols); ++col) M(row, col) = real(o(col));
      ++row;
   }

   MatrixXr Mt  = M.transpose();
   MatrixXr MtM = Mt * M;

   SVD3DRet ret;
   svd_UDV(MtM, ret.U, ret.D, ret.V);

   return {ret};
}

} // namespace perceive


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>

#include <adept_source.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aMatrix;
using adept::aVector;

using adept::Vector;

extern int enzyme_const;
template<typename Return, typename... T>
Return __enzyme_autodiff(T...);

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

static double sum(const double *x, size_t n) {
    double res = 0;
    for(int i=0; i<n; i++) {
        res+=x[i];
    }
    return res;
}

static double max(double x, double y) {
    return (x > y) ? x : y;
}

static adouble amax(adouble x, adouble y) {
    return (x > y) ? x : y;
}

static double logsumexp(const double *__restrict x, size_t n) {
  double A = x[0];
  for(int i=0; i<n; i++) {
    A = max(A, x[i]);
  }
  double sema = 0;
  for(int i=0; i<n; i++) {
    sema += exp(x[i] - A);
  }
  return log(sema) + A;
}

static adouble alogsumexp(const aVector &x, size_t n) {
  adouble A = x[0];
  for(int i=0; i<n; i++) {
    A = amax(A, x[i]);
  }
  adouble ema[n];
  for(int i=0; i<n; i++) {
    ema[i] = exp(x[i] - A);
  }
  adouble sema = 0;
  for(int i=0; i<n; i++)
    sema += ema[i];
  return log(sema) + A;
}

extern "C" {
#include <adBuffer.h>
}

/*
  Differentiation of logsumexp in reverse (adjoint) mode:
   gradient     of useful results: *x logsumexp
   with respect to varying inputs: *x
   RW status of diff variables: *x:incr logsumexp:in-killed
   Plus diff mem management of: x:in
*/
static void logsumexp_b(const double *__restrict x, double *xb, size_t n, double logsumexpb) {
    double A = x[0];
    double Ab = 0.0;
    int branch;
    double logsumexp;
    for (int i = 0; i < n; ++i)
        if (A < x[i]) {
            A = x[i];
            pushControl1b(0);
        } else {
            pushControl1b(1);
            A = A;
        }
    double sema = 0;
    double semab = 0.0;
    for (int i = 0; i < n; ++i)
        sema = sema + exp(x[i] - A);
    semab = logsumexpb/sema;
    Ab = logsumexpb;
    {
      double tempb;
      for (int i = n-1; i > -1; --i) {
          tempb = exp(x[i]-A)*semab;
          xb[i] = xb[i] + tempb;
          Ab = Ab - tempb;
      }
    }
    for (int i = n-1; i > -1; --i) {
        popControl1b(&branch);
        if (branch == 0) {
            xb[i] = xb[i] + Ab;
            Ab = 0.0;
        }
    }
    xb[0] = xb[0] + Ab;
}

adouble alogsumexp2(const aVector &x, size_t n) {
  adouble A = x[0];
  for(int i=0; i<n; i++) {
    A = amax(A, x[i]);
  }
  return adept::log(adept::sum(exp(x - A))) + A;
}

static void adept_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);
  double total = 0;

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp(inp, n);
    stack.pause_recording();
    total += resa.value();
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept forward (recording) %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp(inp, n);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();
    for (int i = 0; i < n; i++) {
        inputp[i] += inp(i).get_gradient();
    }
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept forward reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}
static void adept2_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);
  double total = 0;

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp2(inp, n);
    stack.pause_recording();
    total += resa.value();
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept2 forward (recording) %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
  struct timeval start, end;
  //gettimeofday(&start, NULL);

  adept::Stack stack;

  aVector inp(n);
  for(int i=0; i<n; i++) inp(i) = input[i];
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);
  for (int iter = 0; iter < repeat; iter++) {
    stack.new_recording();
    adouble resa = alogsumexp2(inp, n);
    resa.set_gradient(1.0);
    stack.reverse();
    stack.pause_recording();
    for (int i = 0; i < n; i++) {
        inputp[i] += inp(i).get_gradient();
    }
    stack.continue_recording();
  }
  gettimeofday(&end, NULL);

  stack.pause_recording();

  printf("adept2 forward reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}

static void enzyme_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
    double realinput = input[0];
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double total = 0;
  for(int i=0; i<repeat; i++) {
    input[0] = realinput + (double)i/10000000;
    total += logsumexp(input, n);
  }

  gettimeofday(&end, NULL);
  printf("enzyme forward %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
      input[0] = realinput;
  struct timeval start, end;
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);

  for(int i=0; i<repeat; i++) {
    __enzyme_autodiff<void>(logsumexp, input, inputp, n);
  }

  gettimeofday(&end, NULL);
  printf("enzyme forward and reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}
static void tapenade_sincos(double *input, double *inputp, unsigned long n, unsigned long repeat) {
    double realinput = input[0];
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);
  double total = 0;
  for(int i=0; i<repeat; i++) {
    input[0] = realinput + (double)i/10000000;
    total += logsumexp(input, n);
  }

  gettimeofday(&end, NULL);
  printf("tapenade forward %0.6f res'=%f\n", tdiff(&start, &end), total);
  }
  {
      input[0] = realinput;
  struct timeval start, end;
  memset(inputp, 0, sizeof(double)*n);

  gettimeofday(&start, NULL);

  for(int i=0; i<repeat; i++) {
    logsumexp_b(input, inputp, n, 1.0);
  }

  gettimeofday(&end, NULL);
  printf("tapenade forward and reverse %0.6f res'=%f\n", tdiff(&start, &end), sum(inputp, n));
  }
}

int main(int argc, char** argv) {
    if (argc < 2) {
        printf("usage %s n repeat\n", argv[0]);
        return 1;
    }
  unsigned long n = atoi(argv[1]);
  unsigned long repeat = atoi(argv[2]);

  double *input = new double[n];
  double *inputp = new double[n];
  for(int i=0; i<n; i++) {
    input[i] = 3.1415926535 / (i+1);
  }

  //adept_sincos(input, inputp, n, repeat);

  adept2_sincos(input, inputp, n, repeat);

  tapenade_sincos(input, inputp, n, repeat);

  enzyme_sincos(input, inputp, n, repeat);
}


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

using namespace std;
using boost::multiprecision::cpp_int;

// Returns n!
cpp_int factorial(cpp_int n) {
	if(n == 0) {
		return 1;
	}
	return n * factorial(n - 1);
}

cpp_int sum_of_digits(cpp_int num) {
	cpp_int sum = 0;
	while(num) {
		sum += num % 10;
		num /= 10;
	}
	return sum;
}

int main(int argc, char *argv[]) {
	cout << sum_of_digits(factorial(100)) << endl;
	return 0;
}


#pragma once
#include <NTL/RR.h>
#include <utility>
#include "proper_primes.hpp"

// choice of the approximation praxis for the estimated fraction of an error
// to appear in the next iteration of a bit-flipping decoder
#define ROUNDING_PRAXIS round

/* Probability that a variable node is correct, and a parity equation involving
 * it is satisfied */
NTL::RR compute_p_cc(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t ? d_c - 1 : t;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < t ? t-(n-d_c) : 0;
    /* and it should be even, since the PC equation must be satisfied */
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound + 1 : LowerTHitBound;

    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j) ) /
                         to_RR( binomial_wrapper(n-1,t) );
    }
    return result;
}

/* Probability that a variable node is correct, and a parity equation involving
 * it is *not* satisfied */
NTL::RR compute_p_ci(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t ? d_c - 1 : t;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < t ? t-(n-d_c) : 1;
    /* and it should be odd, since the PC equation must be non satisfied */
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound : LowerTHitBound + 1;

    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j) )
                 / to_RR( binomial_wrapper(n-1,t) );
    }
    return result;
}

/* Probability that a variable node is *not* correct, and a parity equation involving
 * it is *not* satisfied */
NTL::RR compute_p_ic(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){
    NTL::RR result = NTL::RR(0);
    uint64_t UpperTBound = (d_c - 1) < t - 1 ? d_c - 1 : t - 1;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c-1) < (t-1) ? (t-1)-(n-d_c-1) : 0;
    /* and it should be even, since the PC equation must be unsatisfied (when
     * accounting for the one we are considering as already placed*/
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound + 1 : LowerTHitBound;

    for(uint64_t j = LowerTHitBound; j <= UpperTBound; j = j+2 ){
        result += NTL::to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j-1) )
                 / to_RR( binomial_wrapper(n-1,t-1) );
    }
    return result;
}

/* Probability that a variable node is *not* correct, and a parity equation involving
 * it is satisfied */
NTL::RR compute_p_ii(const uint64_t d_c,
                   const uint64_t n,
                   const uint64_t t){

    NTL::RR result = NTL::RR(0);
    uint64_t bound = (d_c - 1) < t - 1 ? d_c - 1 : t - 1;

    /* the number of errors falling in the PC equation should be at least
     * the amount which cannot be placed in a non checked place          */
    uint64_t LowerTHitBound = (n-d_c) < (t-1) ? (t-1)-(n-d_c) : 1;
    /* and it should be odd, since the PC equation must be satisfied (when
     * accounting for the one we are considering as already placed)*/
    LowerTHitBound = LowerTHitBound % 2 ? LowerTHitBound : LowerTHitBound +1;
    for(uint64_t j = LowerTHitBound; j <= bound; j = j+2 ){
        result += NTL::to_RR( binomial_wrapper(d_c-1,j) * binomial_wrapper(n-d_c,t-j-1) )
                 / to_RR( binomial_wrapper(n-1,t-1) );
    }
    return result;
}

/* note p_cc + p_ci = 1 */
/* note p_ic + p_ii = 1 */

/* Probability that a given erroneous variable is deemed as such, and is thus
 * corrected, given a threshold for the amount of unsatisfied parity check
 * equations. Called P_ic in most texts */
NTL::RR ComputePrBitCorrection( const NTL::RR p_ic,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t threshold ){
// 		Pic=0; /* p_correct */
// 		for (j=b,dv,
// 			term=binomial(dv,j)*(p_ic^j)*(1-p_ic)^(dv-j);
// 			Pic=Pic+term;
// 		);
  NTL::RR result = NTL::RR(0), success, failure;
  for (uint64_t j = threshold; j <= d_v; j++){
     NTL::pow(success, p_ic, NTL::to_RR(j));
     NTL::pow(failure, NTL::RR(1)-p_ic, NTL::to_RR(d_v-j));
     result += NTL::to_RR(binomial_wrapper(d_v,j)) * success * failure;
  }
  return result;
}

/* Probability that a given correct variable is not deemed as such, and is thus
 * fault-induced, given a threshold for the amount of unsatisfied parity check
 * equations. Called P_ci in most texts, p_induce in official comment */
NTL::RR ComputePrBitFaultInduction( const NTL::RR p_ci,
                                    const uint64_t d_v,
                                    const uint64_t t, /* unused */
                                    const uint64_t threshold ){

  NTL::RR result= NTL::RR(0), success, failure;
  for (uint64_t j = threshold; j <= d_v; j++){
     NTL::pow(success, p_ci, NTL::to_RR(j));
     NTL::pow(failure, NTL::RR(1)-p_ci, NTL::to_RR(d_v-j));
     result += NTL::to_RR(binomial_wrapper(d_v,j)) * success * failure;
  }
  return result;
}

/* computes the probability that toCorrect bits are corrected
 * known as P{N_ic = toCorrect}  */
NTL::RR ComputePrBitCorrectionMulti( const NTL::RR p_ic,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t threshold,
                                const uint64_t toCorrect){
   NTL::RR ProbCorrectOne = ComputePrBitCorrection(p_ic,d_v,t,threshold);
   return NTL::to_RR(binomial_wrapper(t,toCorrect)) *
          NTL::pow(ProbCorrectOne,NTL::RR(toCorrect)) *
          NTL::pow(1-ProbCorrectOne,NTL::RR(t-toCorrect));
}

/* computes the probability that toInduce faults are induced
 * known as P{N_ci = toInduce} or Pr{f_wrong = to_induce} */
NTL::RR ComputePrBitInduceMulti(const NTL::RR p_ci,
                                const uint64_t d_v,
                                const uint64_t t,
                                const uint64_t n,
                                const uint64_t threshold,
                                const uint64_t toInduce){
//    if(toInduce <= 1 ){
//        return NTL::RR(0);
//    }
   NTL::RR ProbInduceOne = ComputePrBitFaultInduction(p_ci,d_v,t,threshold);
   return NTL::to_RR(binomial_wrapper(n-t,toInduce)) *
          NTL::pow(ProbInduceOne,NTL::RR(toInduce)) *
          NTL::pow(1-ProbInduceOne,NTL::RR(n-t-toInduce));
}

uint64_t FindNextNumErrors(const uint64_t n_0,
                           const uint64_t p,
                           const uint64_t d_v,
                           const uint64_t t){
    NTL::RR p_ci, p_ic;
     p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
     p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    uint64_t t_next=t;
//      uint64_t best_threshold = (d_v - 1)/2;
    for(uint64_t i = (d_v - 1)/2; i <= d_v - 1; i++){
       NTL::RR t_approx=  t -
                          t * ComputePrBitCorrection(p_ic, d_v, t, i) +
                          (n_0*p - t) * ComputePrBitFaultInduction(p_ci, d_v, t, i);
       unsigned long int t_curr = NTL::conv<unsigned long int>(NTL::ROUNDING_PRAXIS(t_approx)) ;
       /*Note : we increase the threshold only if it improves strictly on the
        * predicted error correction. */
       if (t_curr < t_next){
          t_next = t_curr;
//           best_threshold = i;
       }
    }
    /* considering that any code will correct a single bit error, if
     * t_next == 1, we save a computation iteration and shortcut to t_next == 0*/
    if (t_next == 1) {
        t_next = 0;
    }
    return t_next;
}

/* computes the exact 1-iteration DFR and the best threshold on the number of
 * upcs to achieve it */
std::pair<NTL::RR,uint64_t> Find1IterDFR(const uint64_t n_0,
                                         const uint64_t p,
                                         const uint64_t d_v,
                                         const uint64_t t){
    NTL::RR p_ci, p_ic, P_correct, P_induce;
    NTL::RR DFR, best_DFR = NTL::RR(1);
    p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
    p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    uint64_t best_threshold = (d_v - 1)/2;
    for(uint64_t b = best_threshold; b <= d_v - 1; b++){
       DFR = NTL::RR(1) - ComputePrBitCorrectionMulti(p_ic, d_v, t, b, t) * ComputePrBitInduceMulti(p_ci,d_v,t,n_0*p,b,0);
       /*Note : we increase the threshold only if it improves strictly on the
        * predicted error correction. */
       if (DFR < best_DFR){
          best_DFR = DFR;
          best_threshold = b;
       }
    }
//     std::cout << best_threshold << std::endl;
    return std::make_pair(best_DFR,best_threshold);
}

/* computes the exact 1-iteration probability of leaving at most t_leftover
 * uncorrected errors out of t. */
std::pair<NTL::RR,uint64_t> Find1IterTLeftoverPr(const uint64_t n_0,
                                         const uint64_t p,
                                         const uint64_t d_v,
                                         const uint64_t t,
                                         const uint64_t t_leftover){
    NTL::RR p_ci, p_ic;
    NTL::RR DFR, best_DFR = NTL::RR(1);
    p_ci = compute_p_ci(n_0*d_v,n_0*p,t);
    p_ic = compute_p_ic(n_0*d_v,n_0*p,t);
    int n= p*n_0;
    uint64_t best_threshold = (d_v + 1)/2;

    for(uint64_t b = best_threshold; b <= d_v ; b++){
       DFR = NTL::RR(0);
       NTL::RR P_correct = ComputePrBitCorrection(p_ic, d_v, t,b);
       NTL::RR P_induce = ComputePrBitFaultInduction(p_ci,d_v, t/* unused */,b);
       for(int tau = 0 ; tau <= t_leftover; tau++){
         for(int n_to_induce = 0 ; n_to_induce <= t_leftover; n_to_induce++) {
             NTL::RR prob_induce_n = NTL::to_RR(binomial_wrapper(n-t,n_to_induce)) *
                                     NTL::pow(P_induce,NTL::to_RR(n_to_induce)) *
                                     NTL::pow(NTL::RR(1)-P_induce,NTL::to_RR(n-t-n_to_induce));
             int n_to_correct = (int)t + n_to_induce - tau;
             NTL::RR prob_correct_n = NTL::to_RR(binomial_wrapper(t,n_to_correct));
                     prob_correct_n *= NTL::pow(P_correct,NTL::to_RR(n_to_correct));

                     prob_correct_n *= NTL::pow(NTL::RR(1)-P_correct,NTL::to_RR((int)t-n_to_correct)); /*unsigned exp?*/
             DFR += prob_correct_n*prob_induce_n;
         }
       }
       DFR = NTL::RR(1) - DFR;
       if (DFR < best_DFR){
          best_DFR = DFR;
          best_threshold = b;
       }
    }
    return std::make_pair(best_DFR,best_threshold);
}

// find minimum p which, asymptotically, corrects all errors
// search performed via binary search as the DFR is decreasing monot.
// in of p
uint64_t Findpth(const uint64_t n_0,
                 const uint64_t d_v_prime,
                 const uint64_t t){

    unsigned int prime_idx = 0, prime_idx_prec;
    uint64_t p = proper_primes[prime_idx];
    while(p < d_v_prime || p < t ){
          prime_idx++;
          p=proper_primes[prime_idx];
    }

    uint64_t hi, lo;
    lo = prime_idx;
    hi = PRIMES_NO;
    prime_idx_prec = lo;

    uint64_t limit_error_num = t;
    while(hi-lo > 1){
        prime_idx_prec = prime_idx;
        prime_idx = (lo+hi)/2;
        p = proper_primes[prime_idx];
        // compute number of remaining errors after +infty iters
        limit_error_num = t;
        uint64_t current_error_num;
//        std::cout << "using p:"<< p << ", errors dropping as ";
        do {
            current_error_num = limit_error_num;
            limit_error_num = FindNextNumErrors(n_0, p, d_v_prime, current_error_num);
//           std::cout << limit_error_num << " ";
           } while (
                     (limit_error_num != current_error_num) &&
                     (limit_error_num != 0)
                   );
//        std::cout << std::endl;
        if (limit_error_num > 0){
            lo = prime_idx;
        } else {
            hi = prime_idx;
        }
    }
    if(limit_error_num == 0) {
        return proper_primes[prime_idx];
    }
    return proper_primes[prime_idx_prec];
}


/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_DH_HPP
#define INTERPOL_DH_HPP

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

#include <hand_eye_calibration/DualQuaternion.h>

namespace interpol {

template<typename T>
class DH1;

//We have a new type for (screw) tangent since multiplication with scalar is defined different there
template<typename T>
class DH1Tangent
{
public:
    DH1Tangent(const Eigen::Quaternion<T>& r,const Eigen::Quaternion<T>& d) : m_real(r), m_dual(d) {}

    // dqTangent * scalar
    DH1Tangent<T> operator*(const T& scale) const{
        DH1Tangent<T> tt(m_real,m_dual);
        tt.m_real.w() *= scale; //theta
        tt.m_dual.w() *= scale; //d
        return tt;
    }

    //implemented below
    DH1<T> expScrewBen(void) const;
    DH1<T> expScrew(void) const;
    DH1<T> expT(bool useBenKenwrightImpl=false) const{
        return useBenKenwrightImpl ? expScrewBen() : expScrew();
    }

    Eigen::Quaternion<T> m_real; // real part
    Eigen::Quaternion<T> m_dual; // dual part

    DH1<T> toDH(void){//for derivative
        return DH1<T>(m_real,m_dual);
    }
};

// scalar * dqTangent
template<typename T>
inline DH1Tangent<T> operator*(T scale, DH1Tangent<T> t){
    t.m_real.w() *= scale;
    t.m_dual.w() *= scale;
    return t;
}
typedef DH1Tangent<float> DH1Tangentf;
typedef DH1Tangent<double> DH1Tangentd;

template<typename T>
DH1<T>
operator+(const DH1<T>& dq1, const DH1<T>& dq2);

template<typename T>
class DH1
{
public:
    DH1(){}
    DH1(const Eigen::Quaternion<T>& r,
        const Eigen::Quaternion<T>& d) : DH1(px::DualQuaternion<T>(r,d)) {}
    DH1(const Eigen::Quaternion<T>& r,
        const Eigen::Matrix<T, 3, 1>& t) : DH1(px::DualQuaternion<T>(r,t)) {}

    void fromScrew(T theta, T d,
                   const Eigen::Matrix<T, 3, 1>& l,
                   const Eigen::Matrix<T, 3, 1>& m);
    void toScrew  (T& theta, T& d, //added by us
                   Eigen::Matrix<T, 3, 1>& l,
                   Eigen::Matrix<T, 3, 1>& m) const;

    DH1Tangent<T> logScrewBen(void) const;
    DH1Tangent<T> logScrew(void) const;
    DH1Tangent<T> logT(bool useBenKenwrightImpl=false) const{
        return useBenKenwrightImpl ? logScrewBen() : logScrew();
    }

    DH1<T> conjugate(void) const { return DH1<T>(dq.conjugate());}
    DH1<T> normalized(void) const {return DH1<T>(dq.normalized());}
    Eigen::Quaternion<T> real() const {return dq.real();}
    Eigen::Quaternion<T> dual() const {return dq.dual();}
    Eigen::Quaternion<T> rotation(void) const {return dq.rotation();}
    Eigen::Matrix<T, 3, 1> translation(void) const {return dq.translation();}
    DH1<T> operator*(T scale) const {return DH1<T>(dq*scale);}
    DH1<T> operator*(const DH1<T>& other) const {return DH1<T>(dq*other.dq);}
    friend DH1<T> operator+<>(const DH1<T>& dq1, const DH1<T>& dq2);

    DH1<T>& operator*=(const DH1<T>& other);//added by us

private:
    DH1(const px::DualQuaternion<T>& dq) : dq(dq) {}
    px::DualQuaternion<T> dq;
    //there must be a mistake here for the dual part....
    //DH1<T> exp(void) const;
    //DH1<T> exp(const DH1<T> base) const;
    //px::DualQuaternion<T> log(void) const;
    //px::DualQuaternion<T> log(const px::DualQuaternion<T> &b) const;
};

template<typename T>
DH1<T>&
DH1<T>::operator*=(const DH1<T>& other)
{
    dq = dq * other.dq;
    return *this;
}

template<typename T>
DH1<T>
operator+(const DH1<T>& dh1, const DH1<T>& dh2)
{
    return DH1<T>(dh1.dq+dh2.dq);
}

template<typename T>
void
DH1<T>::fromScrew(T theta, T d,
        const Eigen::Matrix<T, 3, 1>& l,
        const Eigen::Matrix<T, 3, 1>& m)
{
    //was provided in original github implementation.
    dq.fromScrew(theta,d,l,m);
    //is equal to:
    //m_real = Eigen::AngleAxis<T>(theta, l);
    //m_dual.w() = -0.5*d * std::sin(0.5*theta);
    //m_dual.vec() = std::sin(0.5*theta) * m + d / 2.0 * std::cos(theta / 2.0) * l;
}

//modeled by hand as inverse of above
template<typename T>
void
DH1<T>::toScrew(T& theta, T& d,
        Eigen::Matrix<T, 3, 1>& l,
        Eigen::Matrix<T, 3, 1>& m) const
{

    Eigen::AngleAxis<T> ar(real()) ;
    theta=ar.angle();
    l=ar.axis();

    d = -2.0 * dual().w() / (std::sin(0.5*theta));
    m = (dual().vec() - 0.5*d*std::cos(0.5*theta)*l) / std::sin(0.5*theta);
}

//First half of ScLERPGeometricCosSin
template<typename T>
DH1Tangent<T>
DH1<T>::logScrewBen(void) const
{
    Eigen::Vector3d vr = real().vec();
    Eigen::Vector3d vd = dual().vec();
    double invr = 1.0 / vr.norm();

    // Screw parameters
    double  angle = 2.0 * std::acos( real().w() );
    double  pitch = -2.0 * dual().w() * invr;
    Eigen::Vector3d direction = vr * invr;
    Eigen::Vector3d moment = (vd - direction*pitch*real().w()*0.5)*invr;

    //TODO: absorb angle into norm of axis
    Eigen::Quaterniond realTangent;
    realTangent.w()=angle;//0
    realTangent.vec()=direction;//* angle

    //absorb translation into norm of moment
    Eigen::Quaterniond dualTangent;
    dualTangent.w()=pitch;//0;
    dualTangent.vec()=moment;// * pitch;

    return DH1Tangent<T>(realTangent, dualTangent);
}

//Second half of ScLERPGeometricCosSin
template<typename T>
DH1<T>
DH1Tangent<T>::expScrewBen(void) const
{
    //TODO: real and dual tangents should have w() = 0 and the norm absorbed into their vector part
    Eigen::Vector3d vr = m_real.vec();
    Eigen::Vector3d vd = m_dual.vec();

    double angle = m_real.w();//vr.norm();
    double pitch = m_dual.w();//vd.norm();

    Eigen::Vector3d direction = vr; // / angle;
    Eigen::Vector3d moment = vd;// / pitch;

    // Convert back to dual-quaternion
    double sinAngle = std::sin(0.5*angle);
    double cosAngle = std::cos(0.5*angle);

    Eigen::Vector3d axisReal = direction * sinAngle;
    Eigen::Quaterniond real(cosAngle, axisReal.x(),axisReal.y(),axisReal.z());

    Eigen::Vector3d axisDual = sinAngle*moment+pitch*0.5* cosAngle *direction;
    Eigen::Quaterniond dual(-pitch*0.5*sinAngle, axisDual.x(),axisDual.y(),axisDual.z());

    return DH1<T>(real, dual);
}

template<typename T>
DH1Tangent<T>
DH1<T>::logScrew(void) const
{
    T theta,d;
    Eigen::Matrix<T, 3, 1> axis, moment;
    toScrew(theta,d,axis,moment); //putting amount of angle or translation in w(), so it is not 0!

    //TODO: absorb angle and translation into norm of axis and moment
    Eigen::Quaternion<T> real,dual;
    real.w() = theta; //0;
    real.vec() = axis; //*theta;
//        real.w() = 0;
//        real.vec() = axis*theta;

    dual.w() = d; //0;
    dual.vec() = moment; //*d;

//        double m = moment.norm();
//        dual.w() /=m;
//        dual.vec() /= m;
//        cout<<"axis norm="<<axis.norm()<<"  moment norm="<<m<<" d="<<d<<endl;

    return DH1Tangent<T>(real, dual);
}

template<typename T>
DH1<T>
DH1Tangent<T>::expScrew(void) const
{
    DH1<T> dualQuat;
    //TODO: get angle and translation from norm of axis and moment
    dualQuat.fromScrew(
                m_real.w(),//vec().norm(),
                m_dual.w(),//vec().norm(),
                m_real.vec(),//.normalized(),
                m_dual.vec()//.normalized()
                       );
    return dualQuat;
}

typedef DH1<float> DH1f;
typedef DH1<double> DH1d;

} // ns interpol

#endif // INTERPOL_DH_HPP


/*
 * Copyright Nick Thompson, 2019
 * 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 "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <random>
#include <cmath>
#include <boost/math/special_functions/cardinal_b_spline.hpp>
#include <boost/math/interpolators/detail/cubic_b_spline_detail.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using std::abs;
using boost::math::cardinal_b_spline;
using boost::math::cardinal_b_spline_prime;
using boost::math::forward_cardinal_b_spline;
using boost::math::cardinal_b_spline_double_prime;

template<class Real>
void test_box()
{
    Real t = cardinal_b_spline<0>(Real(1.1));
    Real expected = 0;
    CHECK_ULP_CLOSE(expected, t, 0);
    CHECK_ULP_CLOSE(expected, cardinal_b_spline_prime<0>(Real(1.1)), 0);

    t = cardinal_b_spline<0>(Real(-1.1));
    expected = 0;
    CHECK_ULP_CLOSE(expected, t, 0);

    Real h = Real(1)/Real(256);
    for (t = -Real(1)/Real(2)+h; t < Real(1)/Real(2); t += h)
    {
        expected = 1;
        CHECK_ULP_CLOSE(expected, cardinal_b_spline<0>(t), 0);
        expected = 0;
        CHECK_ULP_CLOSE(expected, cardinal_b_spline_prime<0>(Real(1.1)), 0);
    }

    for (t = h; t < 1; t += h)
    {
        expected = 1;
        CHECK_ULP_CLOSE(expected, forward_cardinal_b_spline<0>(t), 0);
    }
}

template<class Real>
void test_hat()
{
    Real t = cardinal_b_spline<1>(Real(2.1));
    Real expected = 0;
    CHECK_ULP_CLOSE(expected, t, 0);

    t = cardinal_b_spline<1>(Real(-2.1));
    expected = 0;
    CHECK_ULP_CLOSE(expected, t, 0);

    Real h = Real(1)/Real(256);
    for (t = -1; t <= 1; t += h)
    {
        expected = 1-abs(t);
        if(!CHECK_ULP_CLOSE(expected, cardinal_b_spline<1>(t), 0) )
        {
            std::cerr << "  Problem at t = " << t << "\n";
        }
        if (t == -Real(1)) {
            if (!CHECK_ULP_CLOSE(Real(1)/Real(2), cardinal_b_spline_prime<1>(t), 0)) {
                std::cout << "  Problem at t = " << t << "\n";
            }
        }
        else if (t == Real(1)) {
            CHECK_ULP_CLOSE(-Real(1)/Real(2), cardinal_b_spline_prime<1>(t), 0);
        }
        else if (t < 0) {
            CHECK_ULP_CLOSE(Real(1), cardinal_b_spline_prime<1>(t), 0);
        }
        else if (t == 0) {
            CHECK_ULP_CLOSE(Real(0), cardinal_b_spline_prime<1>(t), 0);
        }
        else if (t > 0) {
            CHECK_ULP_CLOSE(Real(-1), cardinal_b_spline_prime<1>(t), 0);
        }
    }

    for (t = 0; t < 2; t += h)
    {
        expected = 1 - abs(t-1);
        CHECK_ULP_CLOSE(expected, forward_cardinal_b_spline<1>(t), 0);
    }
}

template<class Real>
void test_quadratic()
{
    using std::abs;
    auto b2 = [](Real x) {
        Real absx = abs(x);
        if (absx >= 3/Real(2)) {
            return Real(0);
        }
        if (absx >= 1/Real(2)) {
            Real t = absx - 3/Real(2);
            return t*t/2;
        }
        Real t1 = absx - 1/Real(2);
        Real t2 = absx + 1/Real(2);
        return (2-t1*t1 -t2*t2)/2;
    };

    auto b2_prime = [&](Real x)->Real {
        Real absx = abs(x);
        Real signx  = 1;
        if (x < 0) {
            signx = -1;
        }
        if (absx >= 3/Real(2)) {
            return Real(0);
        }
        if (absx >= 1/Real(2)) {
            return (absx - 3/Real(2))*signx;
        }
        return -2*absx*signx;
    };

    Real h = 1/Real(256);
    for (Real t = -5; t <= 5; t += h) {
        Real expected = b2(t);
        CHECK_ULP_CLOSE(expected, cardinal_b_spline<2>(t), 0);
        expected = b2_prime(t);

        if (!CHECK_ULP_CLOSE(expected, cardinal_b_spline_prime<2>(t), 0))
        {
            std::cerr << "  Problem at t = " << t << "\n";
        }

    }
}

template<class Real>
void test_cubic()
{
    Real expected = Real(2)/Real(3);
    Real computed = cardinal_b_spline<3, Real>(0);
    CHECK_ULP_CLOSE(expected, computed, 0);

    expected = Real(1)/Real(6);
    computed = cardinal_b_spline<3, Real>(1);
    CHECK_ULP_CLOSE(expected, computed, 0);

    expected = Real(0);
    computed = cardinal_b_spline<3, Real>(2);
    CHECK_ULP_CLOSE(expected, computed, 0);

    Real h = 1/Real(256);
    for (Real t = -4; t <= 4; t += h) {
        expected = boost::math::detail::b3_spline_prime<Real>(t);
        computed = cardinal_b_spline_prime<3>(t);
        CHECK_ULP_CLOSE(expected, computed, 0);
        expected = boost::math::detail::b3_spline_double_prime<Real>(t);
        computed = cardinal_b_spline_double_prime<3>(t);
        if (!CHECK_ULP_CLOSE(expected, computed, 0)) {
            std::cerr << "  Problem at t = " << t << "\n";
        }
    }
}

template<class Real>
void test_quintic()
{
  Real expected = Real(11)/Real(20);
  Real computed = cardinal_b_spline<5, Real>(0);
  CHECK_ULP_CLOSE(expected, computed, 0);

  expected = Real(13)/Real(60);
  computed = cardinal_b_spline<5, Real>(1);
  CHECK_ULP_CLOSE(expected, computed, 1);

  expected = Real(1)/Real(120);
  computed = cardinal_b_spline<5, Real>(2);
  CHECK_ULP_CLOSE(expected, computed, 0);

  expected = Real(0);
  computed = cardinal_b_spline<5, Real>(3);
  CHECK_ULP_CLOSE(expected, computed, 0);

}

template<unsigned n, typename Real>
void test_b_spline_derivatives()
{
    Real h = 1/Real(256);
    Real supp = (n+Real(1))/Real(2);
    for (Real t = -supp - 1; t <= supp+1; t+= h)
    {
        Real expected = cardinal_b_spline<n-1>(t+Real(1)/Real(2)) - cardinal_b_spline<n-1>(t - Real(1)/Real(2));
        Real computed = cardinal_b_spline_prime<n>(t);
        CHECK_MOLLIFIED_CLOSE(expected, computed, std::numeric_limits<Real>::epsilon());

        expected = cardinal_b_spline<n-2>(t+1) - 2*cardinal_b_spline<n-2>(t) + cardinal_b_spline<n-2>(t-1);
        computed = cardinal_b_spline_double_prime<n>(t);
        CHECK_MOLLIFIED_CLOSE(expected, computed, 2*std::numeric_limits<Real>::epsilon());
    }
}

template<unsigned n, typename Real>
void test_partition_of_unity()
{
  std::mt19937 gen(323723);
  Real supp = (n+1.0)/2.0;
  std::uniform_real_distribution<Real> dis(-supp, -supp+1);

  for(size_t i = 0; i < 500; ++i) {
    Real x = dis(gen);
    Real one = 0;
    while (x < supp) {
        one += cardinal_b_spline<n>(x);
        x += 1;
    }
    if(!CHECK_ULP_CLOSE(Real(1), one, n)) {
      std::cerr << "  Partition of unity failure at n = " << n << "\n";
    }
  }
}

int main()
{
    test_box<float>();
    test_box<double>();
    test_box<long double>();

    test_hat<float>();
    test_hat<double>();
    test_hat<long double>();

    test_quadratic<float>();
    test_quadratic<double>();
    test_quadratic<long double>();

    test_cubic<float>();
    test_cubic<double>();
    test_cubic<long double>();

    test_quintic<float>();
    test_quintic<double>();
    test_quintic<long double>();

    test_partition_of_unity<1, double>();
    test_partition_of_unity<2, double>();
    test_partition_of_unity<3, double>();
    test_partition_of_unity<4, double>();
    test_partition_of_unity<5, double>();
    test_partition_of_unity<6, double>();

    test_b_spline_derivatives<3, double>();
    test_b_spline_derivatives<4, double>();
    test_b_spline_derivatives<5, double>();
    test_b_spline_derivatives<6, double>();
    test_b_spline_derivatives<7, double>();
    test_b_spline_derivatives<8, double>();
    test_b_spline_derivatives<9, double>();

    test_b_spline_derivatives<3, long double>();
    test_b_spline_derivatives<4, long double>();
    test_b_spline_derivatives<5, long double>();
    test_b_spline_derivatives<6, long double>();
    test_b_spline_derivatives<7, long double>();
    test_b_spline_derivatives<8, long double>();
    test_b_spline_derivatives<9, long double>();

#ifdef BOOST_HAS_FLOAT128
    test_box<float128>();
    test_hat<float128>();
    test_quadratic<float128>();
    test_cubic<float128>();
    test_quintic<float128>();
#endif

    return boost::math::test::report_errors();
}


////////////////////////////////////////////////////////////////////
//
// $Id: myEigen.hxx 2021/06/05 13:36:04 kanai Exp $
//
// Copyright (c) 2021 Takashi Kanai
// Released under the MIT license
//
////////////////////////////////////////////////////////////////////

#ifndef _MYEIGEN_HXX
#define _MYEIGEN_HXX 1

#include <cmath>
using namespace std;

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

// return (double)Math.acos(dot(v1)/v1.length()/v.length());
// Numerically, near 0 and PI are very bad condition for acos.
// In 3-space, |atan2(sin,cos)| is much stable.
template <typename T>
T angleT( Eigen::Matrix<T, 3, 1>& v1, Eigen::Matrix<T, 3, 1>& v2 ) {
  Eigen::Matrix<T, 3, 1> c = v1.cross(v2);
  T s = c.norm();

  return std::fabs(std::atan2(s, v1.dot(v2)));
};

//typedef angleT<float> anglef;

#if 0
float anglef( Eigen::Vector3f& v1, Eigen::Vector3f& v2 ) {
  // return (double)Math.acos(dot(v1)/v1.length()/v.length());
  // Numerically, near 0 and PI are very bad condition for acos.
  // In 3-space, |atan2(sin,cos)| is much stable.
  Eigen::Vector3f c = v1.cross(v2);
  float s = c.norm();

  return std::fabs(std::atan2(s, v1.dot(v2)));
};
#endif

#endif // _MYEIGEN_HXX


#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include "kron.hpp"

Eigen::SparseMatrix<double> buildC(const Eigen::MatrixXd &A){
	int n= A.cols();
	Eigen::SparseMatrix<double> C(n*n,n*n);
	Eigen::MatrixXd I= Eigen::MatrixXd::Identity(n,n);
	Eigen::MatrixXd C1,C2;
	kron(A,I,C1);kron(I,A,C2);
	C1+=C2;
	//std::cout << C1 << std::endl;
	std::vector <Eigen::Triplet<double> > triplets;
	for (int i=0; i<n*n; ++i){
		for (int j=0; j<n*n; ++j){
			if (C1(i,j)!=0){
				Eigen::Triplet<double> trpl(i,j,C1(i,j));
				triplets.push_back(trpl);
				}
		}
	}
	C.setFromTriplets(triplets.begin(),triplets.end());
	C.makeCompressed();
	return C;
	//TODO
};

void solveLyapunov(const Eigen::MatrixXd &A, Eigen::MatrixXd &X){
	int n= A.cols();
	Eigen::SparseMatrix<double> C;
	C= buildC(A);
	Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n,n);
	Eigen::VectorXd b=Eigen::MatrixXd::Map(I.data(),n*n,1);
	Eigen::VectorXd x;
	Eigen::SparseLU<Eigen::SparseMatrix<double> > solver; solver.compute(C);
	x=solver.solve(b);
	X=Eigen::MatrixXd::Map(x.data(),n,n);
};

int main(){
	int n=5;
	Eigen::MatrixXd A(n,n),X(n,n);
    A<<10, 2, 3, 4, 5, 6, 20, 8, 9, 1, 1, 2, 30, 4, 5, 6, 7, 8, 20, 0, 1, 2, 3, 4, 10;
	std::cout << A << std::endl;
	///Teilaufgabe 1g
	/*Eigen::SparseMatrix<double> C;
	C= buildC(A);
	std::cout << C << std::endl;*/
	solveLyapunov(A,X);
	std::cout << X << std::endl;

	///test
	/*Eigen::VectorXd y;
	y=Eigen::MatrixXd::Map(A.data(),4,1);
	std::cout << y << std::endl;*/
	return 0;
}

Nummerical Methods for CSE/PS2/BuildC.cpp


/*
 * Copyright Nick Thompson, 2020
 * 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 "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <random>
#include <boost/math/interpolators/pchip.hpp>
#include <boost/circular_buffer.hpp>
#include <boost/assert.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::pchip;

template<typename Real>
void test_constant()
{

    std::vector<Real> x{0,1,2,3, 9, 22, 81};
    std::vector<Real> y(x.size());
    for (auto & t : y) {
        t = 7;
    }

    auto x_copy = x;
    auto y_copy = y;
    auto pchip_spline = pchip(std::move(x_copy), std::move(y_copy));
    //std::cout << "Constant value pchip spline = " << pchip_spline << "\n";

    for (Real t = x[0]; t <= x.back(); t += 0.25) {
        CHECK_ULP_CLOSE(Real(7), pchip_spline(t), 2);
        CHECK_ULP_CLOSE(Real(0), pchip_spline.prime(t), 2);
    }

    boost::circular_buffer<Real> x_buf(x.size());
    for (auto & t : x) {
        x_buf.push_back(t);
    }

    boost::circular_buffer<Real> y_buf(x.size());
    for (auto & t : y) {
        y_buf.push_back(t);
    }

    auto circular_pchip_spline = pchip(std::move(x_buf), std::move(y_buf));

    for (Real t = x[0]; t <= x.back(); t += 0.25) {
        CHECK_ULP_CLOSE(Real(7), circular_pchip_spline(t), 2);
        CHECK_ULP_CLOSE(Real(0), pchip_spline.prime(t), 2);
    }

    circular_pchip_spline.push_back(x.back() + 1, 7);
    CHECK_ULP_CLOSE(Real(0), circular_pchip_spline.prime(x.back()+1), 2);

}

template<typename Real>
void test_linear()
{
    std::vector<Real> x{0,1,2,3};
    std::vector<Real> y{0,1,2,3};

    auto x_copy = x;
    auto y_copy = y;
    auto pchip_spline = pchip(std::move(x_copy), std::move(y_copy));

    CHECK_ULP_CLOSE(y[0], pchip_spline(x[0]), 0);
    CHECK_ULP_CLOSE(Real(1)/Real(2), pchip_spline(Real(1)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[1], pchip_spline(x[1]), 0);
    CHECK_ULP_CLOSE(Real(3)/Real(2), pchip_spline(Real(3)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[2], pchip_spline(x[2]), 0);
    CHECK_ULP_CLOSE(Real(5)/Real(2), pchip_spline(Real(5)/Real(2)), 10);
    CHECK_ULP_CLOSE(y[3], pchip_spline(x[3]), 0);

    x.resize(45);
    y.resize(45);
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = i;
    }

    x_copy = x;
    y_copy = y;
    pchip_spline = pchip(std::move(x_copy), std::move(y_copy));
    for (Real t = 0; t < x.back(); t += 0.5) {
        CHECK_ULP_CLOSE(t, pchip_spline(t), 0);
        CHECK_ULP_CLOSE(Real(1), pchip_spline.prime(t), 0);
    }

    x_copy = x;
    y_copy = y;
    // Test endpoint derivatives:
    pchip_spline = pchip(std::move(x_copy), std::move(y_copy), Real(1), Real(1));
    for (Real t = 0; t < x.back(); t += 0.5) {
        CHECK_ULP_CLOSE(t, pchip_spline(t), 0);
        CHECK_ULP_CLOSE(Real(1), pchip_spline.prime(t), 0);
    }

    boost::circular_buffer<Real> x_buf(x.size());
    for (auto & t : x) {
        x_buf.push_back(t);
    }

    boost::circular_buffer<Real> y_buf(x.size());
    for (auto & t : y) {
        y_buf.push_back(t);
    }

    auto circular_pchip_spline = pchip(std::move(x_buf), std::move(y_buf));

    for (Real t = x[0]; t <= x.back(); t += 0.25) {
        CHECK_ULP_CLOSE(t, circular_pchip_spline(t), 2);
        CHECK_ULP_CLOSE(Real(1), circular_pchip_spline.prime(t), 2);
    }

    circular_pchip_spline.push_back(x.back() + 1, y.back()+1);

    CHECK_ULP_CLOSE(Real(y.back() + 1), circular_pchip_spline(Real(x.back()+1)), 2);
    CHECK_ULP_CLOSE(Real(1), circular_pchip_spline.prime(Real(x.back()+1)), 2);

}

template<typename Real>
void test_interpolation_condition()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> x(n);
        std::vector<Real> y(n);
        std::default_random_engine rd;
        std::uniform_real_distribution<Real> dis(0,1);
        Real x0 = dis(rd);
        x[0] = x0;
        y[0] = dis(rd);
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = dis(rd);
        }

        auto x_copy = x;
        auto y_copy = y;
        auto s = pchip(std::move(x_copy), std::move(y_copy));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size(); ++i) {
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
        }

        x_copy = x;
        y_copy = y;
        // The interpolation condition is not affected by the endpoint derivatives, even though these derivatives might be super weird:
        s = pchip(std::move(x_copy), std::move(y_copy), Real(0), Real(0));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size(); ++i) {
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
        }

    }
}

template<typename Real>
void test_monotonicity()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> x(n);
        std::vector<Real> y(n);
        std::default_random_engine rd;
        std::uniform_real_distribution<Real> dis(0,1);
        Real x0 = dis(rd);
        x[0] = x0;
        y[0] = dis(rd);
        // Monotone increasing:
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = y[i-1] + dis(rd);
        }

        auto x_copy = x;
        auto y_copy = y;
        auto s = pchip(std::move(x_copy), std::move(y_copy));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size() - 1; ++i) {
            Real tmin = x[i];
            Real tmax = x[i+1];
            Real val = y[i];
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
            for (Real t = tmin; t < tmax; t += (tmax-tmin)/16) {
                Real greater_val = s(t);
                BOOST_ASSERT(val <= greater_val);
                val = greater_val;
            }
        }

        x[0] = dis(rd);
        y[0] = dis(rd);
        // Monotone decreasing:
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = y[i-1] - dis(rd);
        }

        x_copy = x;
        y_copy = y;
        s = pchip(std::move(x_copy), std::move(y_copy));
        //std::cout << "s = " << s << "\n";
        for (size_t i = 0; i < x.size() - 1; ++i) {
            Real tmin = x[i];
            Real tmax = x[i+1];
            Real val = y[i];
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
            for (Real t = tmin; t < tmax; t += (tmax-tmin)/16) {
                Real lesser_val = s(t);
                BOOST_ASSERT(val >= lesser_val);
                val = lesser_val;
            }
        }

    }
}

int main()
{
#if (__GNUC__ > 7) || defined(_MSC_VER) || defined(__clang__)
    test_constant<float>();
    test_linear<float>();
    test_interpolation_condition<float>();
    test_monotonicity<float>();

    test_constant<double>();
    test_linear<double>();
    test_interpolation_condition<double>();
    test_monotonicity<double>();

    test_constant<long double>();
    test_linear<long double>();
    test_interpolation_condition<long double>();
    test_monotonicity<long double>();

#ifdef BOOST_HAS_FLOAT128
    test_constant<float128>();
    test_linear<float128>();
#endif
#endif
    return boost::math::test::report_errors();
}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <sstream>

class fibonacci_dataset {
    public:
        using sample = int;
        enum { arity = 1 };

        struct iterator {
            public:
                iterator() : a{1}, b{1} { }

                int operator*() const { return b; }
                void operator++() {
                    a = a + b;
                    std::swap(a, b);
                }

            private:
                int a;
                int b;
        };

        fibonacci_dataset() { }

        boost::unit_test::data::size_t size() const {
            return boost::unit_test::data::BOOST_TEST_DS_INFINITE_SIZE;
        }

        iterator begin() const { return iterator(); }
};

namespace boost {
    namespace unit_test {
        namespace data {
            namespace monomorphic {
                template <>
                struct is_dataset<fibonacci_dataset> : boost::mpl::true_ { };
            }
        }
    }
}

BOOST_DATA_TEST_CASE(
    test1,
    fibonacci_dataset() ^ boost::unit_test::data::make(
                            { 1, 2, 3, 5, 8, 13, 21, 34, 55 }),
    fib_actual,
    expected)
{
    BOOST_TEST(fib_actual == expected);
}


#include "writer.hpp"
#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>

void apply_boundary_conditions(Eigen::ArrayXXd &u, int k) {
    auto N = u.rows();
    u(0, k) = u(1, k);
    u(N - 1, k) = u(N - 2, k);
}

//----------------upwindFDBegin----------------
/// Uses forward Euler and upwind finite differences to compute u from time 0 to
/// time T
///
/// @param[in] u0 the initial conditions in the physical domain, excluding
/// ghost-points.
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
upwindFD(const Eigen::VectorXd &u0,
         double dt,
         double T,
         const std::function<double(double)> &a,
         const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));

    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
    //// ANCSE_START_TEMPLATE
    u.col(0).segment(1, N) = u0;
    apply_boundary_conditions(u, 0);
    time[0] = 0.0;
    //// ANCSE_END_TEMPLATE

    /* Main loop */
    //// ANCSE_START_TEMPLATE
    for (int k = 0; k < nsteps; k++) {
        for (int j = 1; j < N + 1; j++) {

            double x = xL + (j - 1) * dx;

            double uL = u(j - 1, k);
            double uM = u(j, k);
            double uR = u(j + 1, k);
            double ax = a(x);
            double c = 0.5 * dt / dx;

            u(j, k + 1)
                = uM - c * (ax * (uR - uL) - fabs(ax) * (uR - 2 * uM + uL));
        }
        /* Outflow boundary conditions */
        apply_boundary_conditions(u, k + 1);
        time[k + 1] = (k + 1) * dt;
    }
    //// ANCSE_END_TEMPLATE

    return {std::move(u), std::move(time)};
}
//----------------upwindFDEnd----------------

//----------------centeredFDBegin----------------
/// Uses forward Euler and centered finite differences to compute u from time 0
/// to time T
///
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
centeredFD(const Eigen::VectorXd &u0,
           double dt,
           double T,
           const std::function<double(double)> &a,
           const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));
    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
    //// ANCSE_START_TEMPLATE
    u.col(0).segment(1, N) = u0;
    apply_boundary_conditions(u, 0);
    time[0] = 0.0;
    //// ANCSE_END_TEMPLATE

    /* Main loop */
    //// ANCSE_START_TEMPLATE
    for (int k = 0; k < nsteps; k++) {
        for (int j = 1; j < N + 1; j++) {
            double x = xL + (j - 1) * dx;
            u(j, k + 1)
                = u(j, k)
                  - dt / (2.0 * dx) * a(x) * (u(j + 1, k) - u(j - 1, k));
        }

        /* Outflow boundary conditions */
        apply_boundary_conditions(u, k + 1);
        time[k + 1] = (k + 1) * dt;
    }
    //// ANCSE_END_TEMPLATE

    return {std::move(u), std::move(time)};
}
//----------------centeredFDEnd----------------

/* Initial condition: rectangle */
double ic(double x) {
    if (x < 0.25 || x > 0.75)
        return 0.0;
    else
        return 2.0;
}

int main() {
    double T = 2.0;
    double dt = 0.002; // Change this for timestep comparison
    int N = 101;

    double xL = 0.0;
    double xR = 5.0;
    auto domain = std::pair<double, double>{xL, xR};

    auto a = [](double x) { return std::sin(2.0 * M_PI * x); };

    Eigen::VectorXd u0(N);
    double h = (xR - xL) / (N - 1.0);
    /* Initialize u0 */
    for (int i = 0; i < u0.size(); i++) {
        u0[i] = ic(xL + h * i);
    }

    const auto &[u_upwind, time_upwind] = upwindFD(u0, dt, T, a, domain);
    writeToFile("time_upwind.txt", time_upwind);
    writeMatrixToFile("u_upwind.txt", u_upwind);

    const auto &[u_centered, time_centered] = centeredFD(u0, dt, T, a, domain);
    writeToFile("time_centered.txt", time_centered);
    writeMatrixToFile("u_centered.txt", u_centered);
}


#ifndef MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP
#define MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    // N(x | 0, 1)
    double GetStandardNormalDist(const double x);

    // d/dx N(x | 0, 1)
    double GetStandardNormalDistDerivative(const double x);

    // integral_{- inf, x} N(x' | 0, 1) dx'
    double GetStandardNormalDistCdf(const double x);

    // N(x | mu, sigma^2)
    double GetNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx N(x | mu, sigma^2)
    double GetNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // LogNormal(x | mu, sigma^2)
    double GetLogNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx LogNormal(x | mu, sigma^2)
    double GetLogNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // log{ LogNormal(x | mu, sigma^2) }
    double GetLogOfLogNormalDist(const double x, const double mu, const double sigma_2);

    // d/dx log{ LogNormal(x | mu, sigma^2) }
    double GetLogOfLogNormalDistDerivative(const double x, const double mu, const double sigma_2);

    // N(x | mu, Sigma)
    double GetNormalDist(const Eigen::VectorXd& x,
                         const Eigen::VectorXd& mu,
                         const Eigen::MatrixXd& Sigma_inv,
                         const double           Sigma_det);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_PROBABILITY_DISTRIBUTIONS_HPP


#include <iostream>
#include <iterator>
#include <algorithm>
#include <math.h>
#include <cmath>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

class Fibonacci{
    cpp_int m_previousEven;
    cpp_int m_currentEven;
    cpp_int m_nextEven;

    cpp_int even_sum(int limit) {
        if(limit == 0){
            return 0;
        }
        cpp_int ef1 = 0, ef2 = 2;
        cpp_int sum = m_previousEven + m_currentEven;
        int count = 0;

        while (count <= limit-2) {
            count++;
            m_nextEven = 4*m_currentEven + m_previousEven;
            m_previousEven = m_currentEven;
            m_currentEven = m_nextEven;
            sum += m_currentEven;
        }

        return sum;
    }

public:
    Fibonacci(){
        m_previousEven = 0;
        m_currentEven = 2;
    }

    cpp_int get_even_sum(int limit){
        int m_totalEvenNumber;
        m_totalEvenNumber = limit / 3;
        return even_sum(m_totalEvenNumber);
    }

};

int main()
{
    Fibonacci obj;
    int m_input;
    std::cout<<"Enter nth number as Limit : ";
    std::cin>> m_input;
    std::cout<<"Sum of Even Number within limit: ";
    std::cout<<obj.get_even_sum(m_input);
    return 0;
}


/**
 * optimal_pow2_rational.cpp
 * This program finds a rational number of the form
 *     mult / 2^shift
 * which approximates a user-supplied fraction to within the roundoff
 * of multshiftround when multiplied by integers on a user-specified
 * range.
 *
 * Written in 2018 by Ben Tesch.
 * Originally distributed at https://github.com/slugrustle/numerical_routines
 *
 * To the extent possible under law, the author has dedicated all copyright
 * and related and neighboring rights to this software to the public domain
 * worldwide. This software is distributed without any warranty.
 * The text of the CC0 Public Domain Dedication should be reproduced at the
 * end of this file. If not, see http://creativecommons.org/publicdomain/zero/1.0/
 */

#include <cinttypes>
#include <iostream>
#include <limits>
#include <string>
#include <stdexcept>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/special_functions/round.hpp>

typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<80, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::et_off> cpp_bin_float_80;

int main(int argc, char *argv[])
{
  std::cout << "\n cmdline arguments: [range min] [range max] [fraction]\n" << std::endl;

  if (argc != 4)
  {
    std::cout << " ERROR: this program must be run with 3 arguments.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 range_min;
  try { range_min = cpp_bin_float_80(argv[1]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: range min argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((range_min < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest())) |
      (range_min > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: range min argument is outside the allowed range [-2^63,2^64-1]" << std::endl;
    std::cout << "   which is [" << std::numeric_limits<int64_t>::lowest() << ", " << std::numeric_limits<uint64_t>::max() << "]\n" << std::endl;
    return 0;
  }
  if (range_min != boost::math::round(range_min))
  {
    std::cout << " ERROR: range min argument must be an integer.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 range_max;
  try { range_max = cpp_bin_float_80(argv[2]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: range max argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((range_max < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest())) |
      (range_max > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: range max argument is outside the allowed range [-2^63,2^64-1]" << std::endl;
    std::cout << "   which is [" << std::numeric_limits<int64_t>::lowest() << ", " << std::numeric_limits<uint64_t>::max() << "]\n" << std::endl;
    return 0;
  }
  if (range_max != boost::math::round(range_max))
  {
    std::cout << " ERROR: range max argument must be an integer.\n" << std::endl;
    return 0;
  }

  if (range_max < range_min)
  {
    std::cout << " ERROR: range max must be greater than or equal to range min.\n" << std::endl;
    return 0;
  }

  cpp_bin_float_80 fraction;
  try { fraction = cpp_bin_float_80(argv[3]); }
  catch (const std::runtime_error &e)
  {
    std::cout << " ERROR: fraction argument could not be converted to an 80-bit mantissa float.\n" << std::endl;
    return 0;
  }
  if ((fraction < cpp_bin_float_80(0.0)) |
      (fraction > cpp_bin_float_80(std::numeric_limits<uint64_t>::max())))
  {
    std::cout << " ERROR: fraction argument is outside the allowed range [0.0,2.0^64-1.0]" << std::endl;
    std::cout << "   which is [0.0, " << std::numeric_limits<uint64_t>::max() << ".0]\n" << std::endl;
    return 0;
  }

  for (int8_t shift = 1; shift <= 63; shift++)
  {
    cpp_bin_float_80 two_exp     = cpp_bin_float_80(1ull << shift);
    cpp_bin_float_80 approx_mult = boost::math::round(fraction * two_exp);
    cpp_bin_float_80 min_prod    = range_min * approx_mult;
    cpp_bin_float_80 approx_min  = min_prod / two_exp;
    cpp_bin_float_80 max_prod    = range_max * approx_mult;
    cpp_bin_float_80 approx_max  = max_prod / two_exp;
    cpp_bin_float_80 ratio_min   = range_min * fraction;
    cpp_bin_float_80 ratio_max   = range_max * fraction;

    if (boost::multiprecision::fabs(approx_min-ratio_min) < cpp_bin_float_80(0.5) &&
        boost::multiprecision::fabs(approx_max-ratio_max) < cpp_bin_float_80(0.5))
    {
      std::cout << " The rational " << std::setprecision(24) << approx_mult << " / 2^" << static_cast<int16_t>(shift) << " = ";
      std::cout << std::setprecision(24) << approx_mult / two_exp << std::endl;
      std::cout << "   approximates fraction = " << std::setprecision(24) << fraction << std::endl;
      std::cout << "   to within roundoff when multiplied by numbers on the range" << std::endl;
      std::cout << "   [" << range_min << ", " << range_max << "].\n" << std::endl;

      std::cout << " The internal product" << std::endl;
      std::cout << "   " << range_min << " * " << approx_mult << " = " << std::setprecision(24) << min_prod << std::endl;
      if (min_prod == cpp_bin_float_80(0.0))
      {
        std::cout << "   will not underflow or overflow any integer type.\n" << std::endl;
      }
      else if (min_prod < cpp_bin_float_80(0.0))
      {
        if (min_prod < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest()))
        {
          std::cout << "   will underflow an int64_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int32_t>::lowest()))
        {
          std::cout << "   will underflow an int32_t but not an int64_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int16_t>::lowest()))
        {
          std::cout << "   will underflow an int16_t but not an int32_t.\n" << std::endl;
        }
        else if (min_prod < cpp_bin_float_80(std::numeric_limits<int8_t>::lowest()))
        {
          std::cout << "   will underflow an int8_t but not an int16_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not underflow an int8_t.\n" << std::endl;
        }
      }
      else if (min_prod > cpp_bin_float_80(0.0))
      {
        if (min_prod > cpp_bin_float_80(std::numeric_limits<uint64_t>::max()))
        {
          std::cout << "   will overflow a uint64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int64_t>::max()))
        {
          std::cout << "   will overflow an int64_t but not a uint64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint32_t>::max()))
        {
          std::cout << "   will overflow a uint32_t but not an int64_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int32_t>::max()))
        {
          std::cout << "   will overflow an int32_t but not a uint32_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint16_t>::max()))
        {
          std::cout << "   will overflow a uint16_t but not an int32_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int16_t>::max()))
        {
          std::cout << "   will overflow an int16_t but not a uint16_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<uint8_t>::max()))
        {
          std::cout << "   will overflow a uint8_t but not an int16_t.\n" << std::endl;
        }
        else if (min_prod > cpp_bin_float_80(std::numeric_limits<int8_t>::max()))
        {
          std::cout << "   will overflow an int8_t but not a uint8_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not overflow an int8_t.\n" << std::endl;
        }
      }

      std::cout << " The internal product" << std::endl;
      std::cout << "   " << range_max << " * " << approx_mult << " = " << std::setprecision(24) << max_prod << std::endl;
      if (max_prod == cpp_bin_float_80(0.0))
      {
        std::cout << "   will not underflow or overflow any integer type.\n" << std::endl;
      }
      else if (max_prod < cpp_bin_float_80(0.0))
      {
        if (max_prod < cpp_bin_float_80(std::numeric_limits<int64_t>::lowest()))
        {
          std::cout << "   will underflow an int64_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int32_t>::lowest()))
        {
          std::cout << "   will underflow an int32_t but not an int64_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int16_t>::lowest()))
        {
          std::cout << "   will underflow an int16_t but not an int32_t.\n" << std::endl;
        }
        else if (max_prod < cpp_bin_float_80(std::numeric_limits<int8_t>::lowest()))
        {
          std::cout << "   will underflow an int8_t but not an int16_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not underflow an int8_t.\n" << std::endl;
        }
      }
      else if (max_prod > cpp_bin_float_80(0.0))
      {
        if (max_prod > cpp_bin_float_80(std::numeric_limits<uint64_t>::max()))
        {
          std::cout << "   will overflow a uint64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int64_t>::max()))
        {
          std::cout << "   will overflow an int64_t but not a uint64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint32_t>::max()))
        {
          std::cout << "   will overflow a uint32_t but not an int64_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int32_t>::max()))
        {
          std::cout << "   will overflow an int32_t but not a uint32_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint16_t>::max()))
        {
          std::cout << "   will overflow a uint16_t but not an int32_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int16_t>::max()))
        {
          std::cout << "   will overflow an int16_t but not a uint16_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<uint8_t>::max()))
        {
          std::cout << "   will overflow a uint8_t but not an int16_t.\n" << std::endl;
        }
        else if (max_prod > cpp_bin_float_80(std::numeric_limits<int8_t>::max()))
        {
          std::cout << "   will overflow an int8_t but not a uint8_t.\n" << std::endl;
        }
        else
        {
          std::cout << "   will not overflow an int8_t.\n" << std::endl;
        }
      }
      return 0;
    }
  }

  std::cout << " No rational with base 2 denominator was found that" << std::endl;
  std::cout << "   approximates fraction = " << std::setprecision(24) << fraction << std::endl;
  std::cout << "   to within roundoff when multiplied by numbers on the range" << std::endl;
  std::cout << "   [" << range_min << ", " << range_max << "]" << std::endl;
  std::cout << "   for denominators ranging from 2 to 2^63 inclusive.\n" << std::endl;
  return 0;
}

/*
Creative Commons Legal Code

CC0 1.0 Universal

    CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE
    LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN
    ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS
    INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES
    REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS
    PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM
    THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED
    HEREUNDER.

Statement of Purpose

The laws of most jurisdictions throughout the world automatically confer
exclusive Copyright and Related Rights (defined below) upon the creator
and subsequent owner(s) (each and all, an "owner") of an original work of
authorship and/or a database (each, a "Work").

Certain owners wish to permanently relinquish those rights to a Work for
the purpose of contributing to a commons of creative, cultural and
scientific works ("Commons") that the public can reliably and without fear
of later claims of infringement build upon, modify, incorporate in other
works, reuse and redistribute as freely as possible in any form whatsoever
and for any purposes, including without limitation commercial purposes.
These owners may contribute to the Commons to promote the ideal of a free
culture and the further production of creative, cultural and scientific
works, or to gain reputation or greater distribution for their Work in
part through the use and efforts of others.

For these and/or other purposes and motivations, and without any
expectation of additional consideration or compensation, the person
associating CC0 with a Work (the "Affirmer"), to the extent that he or she
is an owner of Copyright and Related Rights in the Work, voluntarily
elects to apply CC0 to the Work and publicly distribute the Work under its
terms, with knowledge of his or her Copyright and Related Rights in the
Work and the meaning and intended legal effect of CC0 on those rights.

1. Copyright and Related Rights. A Work made available under CC0 may be
protected by copyright and related or neighboring rights ("Copyright and
Related Rights"). Copyright and Related Rights include, but are not
limited to, the following:

  i. the right to reproduce, adapt, distribute, perform, display,
     communicate, and translate a Work;
 ii. moral rights retained by the original author(s) and/or performer(s);
iii. publicity and privacy rights pertaining to a person's image or
     likeness depicted in a Work;
 iv. rights protecting against unfair competition in regards to a Work,
     subject to the limitations in paragraph 4(a), below;
  v. rights protecting the extraction, dissemination, use and reuse of data
     in a Work;
 vi. database rights (such as those arising under Directive 96/9/EC of the
     European Parliament and of the Council of 11 March 1996 on the legal
     protection of databases, and under any national implementation
     thereof, including any amended or successor version of such
     directive); and
vii. other similar, equivalent or corresponding rights throughout the
     world based on applicable law or treaty, and any national
     implementations thereof.

2. Waiver. To the greatest extent permitted by, but not in contravention
of, applicable law, Affirmer hereby overtly, fully, permanently,
irrevocably and unconditionally waives, abandons, and surrenders all of
Affirmer's Copyright and Related Rights and associated claims and causes
of action, whether now known or unknown (including existing as well as
future claims and causes of action), in the Work (i) in all territories
worldwide, (ii) for the maximum duration provided by applicable law or
treaty (including future time extensions), (iii) in any current or future
medium and for any number of copies, and (iv) for any purpose whatsoever,
including without limitation commercial, advertising or promotional
purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each
member of the public at large and to the detriment of Affirmer's heirs and
successors, fully intending that such Waiver shall not be subject to
revocation, rescission, cancellation, termination, or any other legal or
equitable action to disrupt the quiet enjoyment of the Work by the public
as contemplated by Affirmer's express Statement of Purpose.

3. Public License Fallback. Should any part of the Waiver for any reason
be judged legally invalid or ineffective under applicable law, then the
Waiver shall be preserved to the maximum extent permitted taking into
account Affirmer's express Statement of Purpose. In addition, to the
extent the Waiver is so judged Affirmer hereby grants to each affected
person a royalty-free, non transferable, non sublicensable, non exclusive,
irrevocable and unconditional license to exercise Affirmer's Copyright and
Related Rights in the Work (i) in all territories worldwide, (ii) for the
maximum duration provided by applicable law or treaty (including future
time extensions), (iii) in any current or future medium and for any number
of copies, and (iv) for any purpose whatsoever, including without
limitation commercial, advertising or promotional purposes (the
"License"). The License shall be deemed effective as of the date CC0 was
applied by Affirmer to the Work. Should any part of the License for any
reason be judged legally invalid or ineffective under applicable law, such
partial invalidity or ineffectiveness shall not invalidate the remainder
of the License, and in such case Affirmer hereby affirms that he or she
will not (i) exercise any of his or her remaining Copyright and Related
Rights in the Work or (ii) assert any associated claims and causes of
action with respect to the Work, in either case contrary to Affirmer's
express Statement of Purpose.

4. Limitations and Disclaimers.

 a. No trademark or patent rights held by Affirmer are waived, abandoned,
    surrendered, licensed or otherwise affected by this document.
 b. Affirmer offers the Work as-is and makes no representations or
    warranties of any kind concerning the Work, express, implied,
    statutory or otherwise, including without limitation warranties of
    title, merchantability, fitness for a particular purpose, non
    infringement, or the absence of latent or other defects, accuracy, or
    the present or absence of errors, whether or not discoverable, all to
    the greatest extent permissible under applicable law.
 c. Affirmer disclaims responsibility for clearing rights of other persons
    that may apply to the Work or any use thereof, including without
    limitation any person's Copyright and Related Rights in the Work.
    Further, Affirmer disclaims responsibility for obtaining any necessary
    consents, permissions or other rights required for any use of the
    Work.
 d. Affirmer understands and acknowledges that Creative Commons is not a
    party to this document and has no duty or obligation with respect to
    this CC0 or use of the Work.
*/


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Geometry>
#include <fstream>
#include <iostream>
#include <vtkDoubleArray.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkPolyDataWriter.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;
typedef Eigen::Quaterniond Quaterniond;
typedef Eigen::AngleAxisd AngleAxisd;

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

  // We expect four command line arguments:
  // 1. Number of particles in the simulation
  // 2. Name of data file from which to read rows
  // 3. Row number to plot -- 0 indexed row number
  // 4. Output VTK file name
  if (argc < 4)
  {
    std::cout << "Usage: ./plotRow <number_of_particles> <data_file> "
              << "<row_num> <output_vtk_file>" << std::endl;
    return 0;
  }

  // The number of particles in the simulation
  size_t N = std::stoi(std::string(argv[1]));

  // The row number to be written to vtk file
  size_t row = std::stoi(std::string(argv[3]));

  std::ifstream inputfile(argv[2]);
  if (!inputfile.is_open())
  {
    std::cout << "Unable to open file " << argv[2] << std::endl;
    return 0;
  }

  //**********************************************************************//
  // Now read the data file line by line
  //

  // The first line contains gamma and beta information
  std::string line, ignore;
  double_t gamma = 0, beta = 0;
  std::getline(inputfile, line);
  std::istringstream header(line);
  header >> ignore >> gamma >> ignore >> beta;

  // Skip to the required row
  auto rowCount = 0;
  while (rowCount < row)
  {
    std::getline(inputfile, line);
    rowCount++;
    continue;
  }

  std::getline(inputfile, line);
  std::cout << "Processing row " << rowCount << std::endl;
  std::istringstream rowStream(line);

  // We will extract 3*N doubles representing particle positions
  // from the stream
  std::string value;
  size_t valCount = 0;
  Matrix3Xd positions(3, N);
  while (valCount < 3 * N && rowStream.good())
  {
    std::getline(rowStream, value, ',');
    positions(valCount % 3, valCount / 3) = std::stod(value);
    valCount++;
  }

  // Read the rotation vectors
  Matrix3Xd rotVecs(3, N);
  valCount = 0;
  while (valCount < 3 * N && rowStream.good())
  {
    std::getline(rowStream, value, ',');
    rotVecs(valCount % 3, valCount / 3) = std::stod(value);
    valCount++;
  }

  // Calculate point normals using the rotation vectors
  Matrix3Xd normals(3, N);
  Quaterniond zaxis(0.0, 0.0, 0.0, 1.0);
  for (auto i = 0; i < N; ++i)
  {
    normals.col(i) = (Quaterniond(AngleAxisd(rotVecs.col(i).norm(),
                                             rotVecs.col(i).normalized())) *
                      zaxis *
                      (Quaterniond(AngleAxisd(rotVecs.col(i).norm(),
                                              rotVecs.col(i).normalized()))
                           .conjugate()))
                         .vec();
  }

  // Stereo graphic projection
  Matrix3Xd points(3, N);

  // Project points to unit sphere
  points = positions.colwise().normalized();

  // Reset the center of the sphere to origin by translating
  Vector3d center = points.rowwise().mean();
  points = points.colwise() - center;

  // Rotate all points so that the point in 0th column is along z-axis
  Vector3d c = points.col(0);
  double_t cos_t = c(2);
  double_t sin_t = std::sqrt(1 - cos_t * cos_t);
  Vector3d axis;
  axis << c(1), -c(0), 0.;
  Matrix3d rotMat, axis_cross, outer;
  axis_cross << 0., -axis(2), axis(1), axis(2), 0., -axis(0), -axis(1), axis(0),
      0.;

  outer.noalias() = axis * axis.transpose();

  rotMat =
      cos_t * Matrix3d::Identity() + sin_t * axis_cross + (1 - cos_t) * outer;
  Matrix3Xd rPts(3, N);
  rPts = rotMat * points; // The points on a sphere rotated

  // Calculate the stereographic projections
  Vector3d p0;
  Map3Xd l0(&(rPts(0, 1)), 3, N - 1);
  Matrix3Xd l(3, N - 1), proj(3, N - 1);
  p0 << 0, 0, -1;
  c = rPts.col(0);
  l = (l0.colwise() - c).colwise().normalized();
  for (auto j = 0; j < N - 1; ++j)
  {
    proj.col(j) = ((p0(2) - l0(2, j)) / l(2, j)) * l.col(j) + l0.col(j);
  }

  // Insert the projected points in a CGAL vertex_with_info vector
  std::vector<std::pair<Point, unsigned>> verts;
  for (auto j = 0; j < N - 1; ++j)
  {
    verts.push_back(std::make_pair(Point(proj(0, j), proj(1, j)), j + 1));
  }

  // Triangulate
  Delaunay dt;
  dt.insert(verts.begin(), verts.end());

  // Write the finite faces of the triangulation to a VTK file
  auto triangles = vtkSmartPointer<vtkCellArray>::New();
  for (auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
       ++ffi)
  {
    triangles->InsertNextCell(3);
    for (auto j = 2; j >= 0; --j)
      triangles->InsertCellPoint(ffi->vertex(j)->info());
  }

  // Iterate over infinite faces
  Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
  if (fc != 0)
  {
    do
    {
      triangles->InsertNextCell(3);
      for (auto j = 2; j >= 0; --j)
      {
        auto vh = fc->vertex(j);
        auto id = dt.is_infinite(vh) ? 0 : vh->info();
        triangles->InsertCellPoint(id);
      }
    } while (++fc != done);
  }

  // Write to vtk file
  auto ptsArr = vtkSmartPointer<vtkDoubleArray>::New();
  ptsArr->SetVoidArray((void *)positions.data(), 3 * N, 1);
  ptsArr->SetNumberOfComponents(3);
  auto pts = vtkSmartPointer<vtkPoints>::New();
  pts->SetData(ptsArr);
  auto poly = vtkSmartPointer<vtkPolyData>::New();
  poly->SetPoints(pts);
  poly->SetPolys(triangles);
  auto normArr = vtkSmartPointer<vtkDoubleArray>::New();
  normArr->SetVoidArray((void *)normals.data(), 3 * N, 1);
  normArr->SetNumberOfComponents(3);
  poly->GetPointData()->SetNormals(normArr);
  auto wr = vtkSmartPointer<vtkPolyDataWriter>::New();
  wr->SetFileName(argv[4]);
  wr->SetInputData(poly);
  wr->Write();

  inputfile.close();
  return 0;
}


#ifndef _FFTTOOLS_HPP_
#define _FFTTOOLS_HPP_

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

namespace fft_tools {

Eigen::MatrixXcf fft(const Eigen::MatrixXcf& timeMat);

Eigen::MatrixXcf fft(const Eigen::MatrixXf& timeMat);

Eigen::MatrixXcf ifft(const Eigen::MatrixXcf& freqMat);

Eigen::MatrixXcf fftshift(Eigen::MatrixXcf x, bool inverse_flag = false);

Eigen::MatrixXcf circshift(Eigen::MatrixXcf data, int shift_r, int shift_c);

/**
 * @brief
 *
 * @tparam Scalar
 * @tparam SizeX
 * @tparam
 * @tparam KSizeX
 * @tparam KSizeY
 * @param I
 * @param kernel
 * @param border_type 0: zero 1: border
 * @param conv_mode 0: full 1: valid
 * @return Eigen::Matrix< Scalar, SizeX, SizeY >
 */
Eigen::MatrixXcf  Convolution2(
    const Eigen::MatrixXcf &I,
    const Eigen::MatrixXcf &kernel,
    int border_type,
    int conv_mode);

Eigen::MatrixXcf  Convolution2(
    const Eigen::MatrixXcf &I,
    const Eigen::MatrixXcf &kernel,
    int conv_mode = 0);

std::complex<float> getMatValue(
    const Eigen::MatrixXcf& mat,
    int r, int c, const int& border_type);
}

#endif


#ifndef _polyvec_curve_bezier_h_
#define _polyvec_curve_bezier_h_

// Eigen
#include <Eigen/Core>

#include "curve.hpp"

namespace polyvec {

//The intrinsic parameters of a Bezier are the control points in the order
//  c1x, c1y, c2x, c2y, c3x, c3y, c4x, c4y
class BezierCurve : public GlobFitCurve{
public:
    BezierCurve() = default;

	GlobFitCurveType get_type() const { return GLOBFIT_CURVE_BEZIER; }
	int n_params() const { return 8; }

    Eigen::Vector2d pos ( const double t ) const override;
    Eigen::Vector2d dposdt ( const double t ) const override;
    Eigen::Vector2d dposdtdt ( const double t ) const override;
	Eigen::Vector2d dposdtdtdt ( const double t ) const override;

    Eigen::Matrix2Xd dposdparams ( const double t ) const override;
    Eigen::Matrix2Xd dposdtdparams ( const double t ) const override;
    Eigen::Matrix2Xd dposdtdtdparams ( const double t ) const override;
	Eigen::Matrix2Xd dposdtdtdtdparams ( const double t ) const override;

    double project ( const Eigen::Vector2d& point ) const override;

    Eigen::VectorXd dtprojectdparams ( const double t, const Eigen::Vector2d& point ) const override;
    Eigen::Matrix2Xd dposprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const override;
    Eigen::Matrix2Xd dposdtprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const override;

    void set_control_points ( const Eigen::Matrix2Xd& control_points_d0_in );
    const Eigen::Matrix2Xd& get_control_points() const;
    constexpr static int n_control_points() {
        return 4;
    }

	void set_params(const Eigen::VectorXd& params) override;
	Eigen::VectorXd get_params() const override;

    Eigen::Matrix2Xd get_tesselation2() const override;
    Eigen::VectorXd  get_tesselationt() const override;

    double length() const;
    Eigen::VectorXd dlengthdparams();

	GlobFitCurve* clone() const { return new BezierCurve(*this); }

	geom::aabb get_bounding_box() const;

	BezierCurve(const Eigen::Matrix2Xd& C);

	std::pair<GlobFitCurve*, GlobFitCurve*> split(double t) const;

private:
    constexpr static unsigned _n_tesselation = 50;

    Eigen::Matrix2Xd _control_points_d0;
    Eigen::Matrix2Xd _control_points_d1;
    Eigen::Matrix2Xd _control_points_d2;
	Eigen::Matrix2Xd _control_points_d3;
    Eigen::Matrix3Xd _tesselation;
};

}

#endif // _polyvec_curve_bezier_h_


#include <iostream>
using namespace std;

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

int main( int argc, char** argv )
{
    Eigen::Matrix<float, 2, 3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix<float,3,1> vd_3d;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Ones();
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout << matrix_23 << endl;

    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;

    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl << endl;
    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << 10*matrix_33 << endl;
    cout << matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl;

    cout <<"hello world"  << endl;

    return 0;
}