#include "fmatrix.h"
#include "sfm_utils.h"
#include "defines.h"

#include <iostream>
#include <Eigen/SVD>
#include <opencv2/calib3d.hpp>

namespace {

    void infoF(const cv::Matx33d &Fcv)
    {
        Eigen::MatrixXd F;
        copy(Fcv, F);

        Eigen::JacobiSVD<Eigen::MatrixXd> svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::MatrixXd U = svdf.matrixU();
        Eigen::VectorXd s = svdf.singularValues();
        Eigen::MatrixXd V = svdf.matrixV();

        std::cout << "F info:\nF:\n" << F << "\nU:\n" << U << "\ns:\n" << s << "\nV:\n" << V << std::endl;
    }

    cv::Matx33d estimateFMatrixDLT(const cv::Vec2d *m0, const cv::Vec2d *m1, int count)
    {
        int a_rows = count;
        int a_cols = 9;

        Eigen::MatrixXd A(a_rows, a_cols);

        for (int i_pair = 0; i_pair < count; ++i_pair) {

            double x0 = m0[i_pair][0];
            double y0 = m0[i_pair][1];

            double x1 = m1[i_pair][0];
            double y1 = m1[i_pair][1];

//            std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl;

            A.row(i_pair) << x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1.0;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd null_space = svda.matrixV().col(8);

        Eigen::MatrixXd F(3, 3);
        F.row(0) << null_space[0], null_space[1], null_space[2];
        F.row(1) << null_space[3], null_space[4], null_space[5];
        F.row(2) << null_space[6], null_space[7], null_space[8];

        Eigen::JacobiSVD<Eigen::MatrixXd> svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::MatrixXd U = svdf.matrixU();
        Eigen::VectorXd s = svdf.singularValues();
        Eigen::MatrixXd V = svdf.matrixV();

        Eigen::MatrixXd S = Eigen::MatrixXd(3, 3);
        S.setZero();
        S(0, 0) = s[0];
        S(1, 1) = s[1];
        S(2, 2) = 0.0;

        F = U * S * V.transpose();

        cv::Matx33d Fcv;
        copy(F, Fcv);

        return Fcv;
    }

    // get transform matrix so that after transform centroid of points will be zero and Root Mean Square distance from centroid will be sqrt(2)
    cv::Matx33d getNormalizeTransform(const std::vector<cv::Vec2d> &m, bool verbose=true)
    {
        cv::Vec2d centroid(0.0, 0.0);

        if (m.empty()) {
            throw std::runtime_error("Can't normalize transform");
        }

        for (int i = 0; i < (int) m.size(); ++i) {
            centroid += m[i];
        }

        centroid /= (double) m.size();

        double rms = 0;
        for (int i = 0; i < (int) m.size(); ++i) {
            cv::Vec2d d = m[i] - centroid;
            rms += d[0] * d[0] + d[1] * d[1];
        }
        rms = std::sqrt(rms / (2 * m.size()));

        if (rms == 0) {
            throw std::runtime_error("Can't normalize transform");
        }

        double s = std::sqrt(2) / rms;

        if (verbose) {
            std::cout << "NORMALIZE TRANSFORM: centroid = " << centroid << ", scale = " << s << std::endl;
        }

        cv::Matx33d T(s, 0.0, -s*centroid[0],
                      0.0, s, -s*centroid[1],
                      0.0, 0.0, 1.0);

        return T;
    }

    cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T)
    {
        cv::Vec3d tmp = T * cv::Vec3d(pt[0], pt[1], 1.0);

        if (tmp[2] == 0) {
            throw std::runtime_error("infinite point");
        }

        return cv::Vec2d(tmp[0] / tmp[2], tmp[1] / tmp[2]);
    }

    cv::Matx33d estimateFMatrixRANSAC(const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, double threshold_px, bool verbose=true)
    {
        if (m0.size() != m1.size()) {
            throw std::runtime_error("estimateFMatrixRANSAC: m0.size() != m1.size()");
        }

        const int n_matches = m0.size();

        cv::Matx33d TN0 = getNormalizeTransform(m0, verbose);
        cv::Matx33d TN1 = getNormalizeTransform(m1, verbose);

        std::vector<cv::Vec2d> m0_t(n_matches);
        std::vector<cv::Vec2d> m1_t(n_matches);
        for (int i = 0; i < n_matches; ++i) {
            m0_t[i] = transformPoint(m0[i], TN0);
            m1_t[i] = transformPoint(m1[i], TN1);
        }

        {
//             check log: centroid should become close to zero, scale close to 1
            getNormalizeTransform(m0_t, verbose);
            getNormalizeTransform(m1_t, verbose);
        }

        // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
        // будет отличаться от случая с гомографией
        const int n_trials = 10000;

        const int n_samples = 8;
        uint64_t seed = 1;

        int best_support = 0;
        cv::Matx33d best_F;

        std::vector<int> sample;
        for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
            phg::randomSample(sample, n_matches, n_samples, &seed);

            cv::Vec2d ms0[n_samples];
            cv::Vec2d ms1[n_samples];
            for (int i = 0; i < n_samples; ++i) {
                ms0[i] = m0_t[sample[i]];
                ms1[i] = m1_t[sample[i]];
            }

            cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples);

            // denormalize
            F = TN1.t() * F * TN0;

            int support = 0;
            for (int i = 0; i < n_matches; ++i) {
                //                    todo todo todo                                                         todo todo todo
                if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px))
                {
                    ++support;
                }
            }

            if (support > best_support) {
                best_support = support;
                best_F = F;

                if (verbose) {
                    std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl;
                    infoF(F);
                }

                if (best_support == n_matches) {
                    break;
                }
            }
        }

        if (verbose) {
            std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl;
        }

        if (best_support == 0) {
            throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix");
        }

        return best_F;
    }

}

cv::Matx33d phg::findFMatrix(const std::vector <cv::Vec2d> &m0, const std::vector <cv::Vec2d> &m1, double threshold_px, bool verbose) {
    return estimateFMatrixRANSAC(m0, m1, threshold_px, verbose);
}

cv::Matx33d phg::findFMatrixCV(const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, double threshold_px) {
    return cv::findFundamentalMat(m0, m1, cv::FM_RANSAC, threshold_px);
}

cv::Matx33d phg::composeFMatrix(const cv::Matx34d &P0, const cv::Matx34d &P1)
{
    // compute fundamental matrix from general cameras
    // Hartley & Zisserman (17.3 - p412)

    cv::Matx33d F;

#define det4(a, b, c, d) \
      ((a)(0) * (b)(1) - (a)(1) * (b)(0)) * ((c)(2) * (d)(3) - (c)(3) * (d)(2)) - \
      ((a)(0) * (b)(2) - (a)(2) * (b)(0)) * ((c)(1) * (d)(3) - (c)(3) * (d)(1)) + \
      ((a)(0) * (b)(3) - (a)(3) * (b)(0)) * ((c)(1) * (d)(2) - (c)(2) * (d)(1)) + \
      ((a)(1) * (b)(2) - (a)(2) * (b)(1)) * ((c)(0) * (d)(3) - (c)(3) * (d)(0)) - \
      ((a)(1) * (b)(3) - (a)(3) * (b)(1)) * ((c)(0) * (d)(2) - (c)(2) * (d)(0)) + \
      ((a)(2) * (b)(3) - (a)(3) * (b)(2)) * ((c)(0) * (d)(1) - (c)(1) * (d)(0))

    int i, j;
    for (j = 0; j < 3; j++)
        for (i = 0; i < 3; i++) {
            // here the sign is encoded in the order of lines ~ai
            const auto a1 = P0.row((i + 1) % 3);
            const auto a2 = P0.row((i + 2) % 3);
            const auto b1 = P1.row((j + 1) % 3);
            const auto b2 = P1.row((j + 2) % 3);

            F(j, i) = det4(a1, a2, b1, b2);
        }

#undef det4

    return F;
}


#include <iostream>

using namespace std;

#include <ctime>

#include <Eigen/Dense>

using namespace Eigen;

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

    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;

    // SVD
    clock_t time_svd = clock();
    cout << "The least-squares solution is:\n"
            << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_svd) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // QR
    clock_t time_qr = clock();
    cout << "\nThe solution using the QR decomposition is:\n"
        << A.colPivHouseholderQr().solve(b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_qr) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // Normal Equations
    clock_t time_normal = clock();
    cout << "\nThe solution using normal equations is:\n"
        << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
    cout << "time:"
       << 1000 * (clock() - time_normal) / (double) CLOCKS_PER_SEC << "ms" << endl;

    return 0;
}


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>
#include <vector>
#include <type_traits>

using namespace std;
using namespace Eigen;

//Codigo par abrir el archivo

MatrixXd openData(string fileToOpen)
{

    vector<double> matrixEntries;
    ifstream matrixDataFile(fileToOpen);
    string matrixRowString;
    string matrixEntry;

    int matrixRowNumber = 0;

    while (getline(matrixDataFile, matrixRowString))
    {
        stringstream matrixRowStringStream(matrixRowString);

        while (getline(matrixRowStringStream, matrixEntry, ','))
        {
            matrixEntries.push_back(stod(matrixEntry));
        }
        matrixRowNumber++;
    }

    return Map<Matrix<double, Dynamic, Dynamic, RowMajor>>(matrixEntries.data(), matrixRowNumber, matrixEntries.size() / matrixRowNumber);

}

//Codigo para guardar el archivo en cualquier formato

void saveData(string fileName, MatrixXd  matrix)
{

    const static IOFormat CSVFormat(FullPrecision, DontAlignCols, " ", "\n");

    ofstream file(fileName);
    if (file.is_open())
    {
        file << matrix.format(CSVFormat);
        file.close();
    }
}

//Inversa por Gauss Jordan

void Print(MatrixXd a, int n, int m)
{
    FILE *output_file;
    output_file=fopen("Inversa_Gauss_Jordan.txt", "w");

    for (int i = 0; i < n; i++) {
        for (int j = n; j < m; j++) {
           fprintf(output_file,"%.3f  ", a(i,j));
        }
        fprintf(output_file,"\n");
    }

     return;
}

void Gauss(MatrixXd m){

   MatrixXd I_ = MatrixXd::Identity(m.rows(),m.cols());

   MatrixXd C(m.rows(), m.cols()+I_.cols());

   C << m, I_;

   C.row(0).swap(C.row(m.rows() - 1));

   for (int i = 0; i < m.rows(); i++) {

        for (int j = 0; j < m.rows(); j++) {

            if (j != i) {

                float temp = C(j,i) / C(i,i);
                for (int k = 0; k < 2*m.rows(); k++) {

                    C(j,k) -= C(i,k)*temp;
                }
            }
        }
    }

    for (int i = 0; i < m.rows(); i++) {

        float temp_ = C(i,i);

        for (int j = 0; j < 2 * m.rows(); j++) {

            C(i,j) = C(i,j)/temp_;
        }

    }

    Print(C, m.cols(), 2*m.cols());
}

//Inversa por cofactores

void getCofactor(int** A , int** temp , int p, int q, int n)
{
    int i = 0, j = 0;

    for (int row = 0; row < n; row++)
    {
        for (int col = 0; col < n; col++)
        {

            if (row != p && col != q)
            {
                temp[i][j++] = A[row][col];

                if (j == n - 1)
                {
                    j = 0;
                    i++;
                }
            }
        }
    }
}

int determinant(int**A, int n)
{
    int D = 0;
    if (n == 1)
        return A[0][0];

    int** temp = new int* [n];
    for (int i = 0; i < n; i++) {
        temp[i] = new int[n];
    }

    int sign = 1;
    for (int f = 0; f < n; f++)
    {

        getCofactor(A, temp, 0, f, n);
        D += sign * A[0][f] * determinant(temp, n - 1);

        sign = -sign;
    }

    return D;
}

void adjoint(int** A, int** adj, int N)
{

    if (N == 1)
    {
        adj[0][0] = 1;
        return;
    }

    int sign = 1;
    int** temp = new int* [N];
    for (int i = 0; i < N; i++) {
        temp[i] = new int[N];
    }

    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {

            getCofactor(A, temp, i, j, N);

            sign = ((i + j) % 2 == 0) ? 1 : -1;

            adj[j][i] = (sign) * (determinant(temp, N - 1));
        }
    }
}

MatrixXd inverse(MatrixXd m, int N)
{

    int rows = m.rows();
    int cols = m.cols();
    int** matrix = new int* [rows];

    for (int i = 0; i < rows; i++)
    {
        matrix[i] = new int[cols];
    }

    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            *(*(matrix + i) + j) = m(i, j);

    float** aux = new float* [rows];

    for (int i = 0; i < rows; i++) {
        aux[i] = new float[cols];
    }

    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            *(*(aux + i) + j) = m(i, j);

    MatrixXd mx(N, N);

    int det = determinant(matrix, N);

    int** adj = new int* [N];
    for (int i = 0; i < N; i++)
        adj[i] = new int[N];
    adjoint(matrix, adj,N);
    for (int i = 0; i < N; i++)
        for (int j = 0; j < N; j++)
            aux[i][j] = adj[i][j] / float(det);

    for (int j = 0; j < N; j++) {
        for (int i = 0; i < N; i++) {
            mx(j, i) = aux[j][i];

        }
    }
    return mx;

}

//Otra forma de encontrar la inversa
//En vez de encontrar un vecotor dinamico lo que hacemos es trabajar directamente con MatrixXd

/*

int determ(MatrixXd m,int n)
  {

  int det=0, p, h, k, i, j;
  MatrixXd temp =  MatrixXd::Zero(m.rows(),m.cols());

  if(n==1) {
    return m(0,0);
  }

  else if(n==2){
    det=(m(0,0)*m(1,1)-m(0,1)*m(1,0));
    return det;
  }

  else {
    for(p=0;p<n;p++) {
      h = 0;
      k = 0;
      for(i=1;i<n;i++) {
        for( j=0;j<n;j++) {
          if(j==p) {
            continue;
          }

          temp(h,k) = m(i,j);
          k++;

          if(k==n-1) {
            h++;
            k = 0;
          }

        }
      }

      det=det+m(0,p)*pow(-1,p)*determ(temp,n-1);
    }

    return det;
  }

}

void cofact(MatrixXd m, int n)
{
    int i = 0, j = 0;

    MatrixXd temp = MatrixXd::Zero(m.rows(),m.cols());
    MatrixXd adj = MatrixXd::Zero(m.rows(),m.cols());

    for (int  p = 0; p<n; p++){

        for (int q = 0; q<n; q++){

            for (int row = 0; row < n; row++){

                for (int col = 0; col < n; col++){

                        if (row != q && col != p){

                            temp(i,j) = m(row,col);

                                if (j == n - 1)
                                {
                                    j = 0;
                                    i++;
                                }

                     }

                }

         }

        adj(q,p) = pow(-1,q+p)*determ(temp,n - 1);

      }

    }

}

Cabe destacar que el principal problema recido en que la adjunta da una matriz de ceros, por lo que no es posible encontrar la inversa

void Inverse(MatrixXd m, MatrixXd adj, int n){

    int i;
    int j;

    MatrixXd b = MatrixXd::Zero(m.rows(),m.cols());
    MatrixXd I_ = MatrixXd::Zero(m.rows(),m.cols());

    for (i=0;j<n;i++){

        for (j = 0; j<n; j++){

                b(i,j) = adj(j,i);

        }

    }

    //int d = determ(m,n);

    for (i=0;j<n;i++){

        for (j = 0; j<n; j++){

                I_(i,j) = b(i,j)/d;

        }

    }

   cout << I_ << endl;

}

*/

int main(){

    MatrixXd data_;
    data_ = openData("nums.txt");

    cout << "Matriz" << endl;
    cout << data_ << endl;

    Gauss(data_);

    MatrixXd cofact = inverse(data_, data_.rows());
    saveData("Inversa_por_cofactores.txt",cofact);

    MatrixXd Eigen_ = data_.inverse();
    saveData("Inversa_Eigen.txt",Eigen_);

    return 0;
}


#include <Eigen/Dense>
#include <iostream>
#include <complex>
#include <string>
#include <sstream>
typedef std::complex<double> Complex;
int main(int argc, char **argv) {
  using namespace std;
  string usage = string("Usage: ")
    + argv[0]
    + " [-?] [-v potential] [-k wvnumber]";

  if (argc>1 && argv[1]==string("-?")) {
    cout << usage << endl;
    exit (0);
  }

  // default values
   double v=0.5;
   double k=0.2;

  try {
    // overwritten by command line:
    for (int i=1; i<argc;i+=2) {
      istringstream stream(argv[i+1]);
      if (string(argv[i])=="-v") stream >> v;
      if (string(argv[i])=="-k") stream >> k;
    }
  }
  catch (exception &) {
    cout << usage << endl;
    exit (0);
  }

  Complex I(0,1.0);
  Complex nk=k*sqrt(Complex(1-v));

  Eigen::VectorXcd Y(4);
  Y(0)= -exp(-I*k);
  Y(1)= 0;
  Y(2)= -I*k*exp(-I*k);
  Y(3)= 0;

  Eigen::MatrixXcd A(4,4);
  // First row:
  A(0,0)= exp(I*k)      ;
  A(0,1)=-exp(-I*nk)    ;
  A(0,2)= -exp(I*nk)    ;
  A(0,3)=0              ;

  // Second row:
  A(1,0)= 0             ;
  A(1,1)= exp(I*nk)     ;
  A(1,2)= exp(-I*nk)    ;
  A(1,3)=-exp(I*k)      ;

  // Third row:
  A(2,0)= -I*k*exp(I*k)  ;
  A(2,1)=-I*nk*exp(-I*nk);
  A(2,2)= I*nk*exp(I*nk) ;
  A(2,3)=0                 ;

  // Fourth row:
  A(3,0)= 0                ;
  A(3,1)=I*nk*exp(I*nk)  ;
  A(3,2)=-I*nk*exp(-I*nk);
  A(3,3)=-I*k*exp(I*k)   ;

  Eigen::MatrixXcd AInv= A.inverse();
  Eigen::VectorXcd BCDF=AInv*Y;
  Complex B=BCDF(0), F=BCDF(3);
  cout << "Complex coefficients" << endl;
  cout << BCDF << endl;
  cout << endl;
  cout << "Reflection coefficient  : " << norm(B)         << endl;
  cout << "Transmission coefficient: " << norm(F)         << endl;
  cout << "Sum:                      " << norm(B)+norm(F) << endl;

}


//
// Created by Harold on 2021/5/1.
//

#include <Eigen/Dense>

#include <iostream>

// 2D Zero-Order Gaussian Regression Filter
// TODO: verify
Eigen::MatrixXd RGR0_2D(Eigen::MatrixXd const& data,
	                    double lambdax = 0.25,
	                    double lambday = 0.25,
	                    double dx = 0.01,
	                    double dy = 0.01,
	                    double tol = 1e-10,
	                    unsigned max_iteration_no = 9) {
	auto ny = data.rows();
	auto nx = data.cols();
	double constant = sqrt(log(2) / 2 / EIGEN_PI / EIGEN_PI);

	unsigned iterationNo = 0;
	Eigen::MatrixXd delta = Eigen::MatrixXd::Ones(ny, nx);
	double CBx = 1;
	double CB_old = 1;

	Eigen::MatrixXd w4(ny, nx);
	Eigen::MatrixXd r4(ny, nx);

	while (CBx > tol) {
		for (auto kx = 0; kx < nx; ++kx)
			for (auto ky = 0; ky < ny; ++ky) {
				auto px = Eigen::VectorXd::LinSpaced(nx, 0, nx);
				auto py = Eigen::VectorXd::LinSpaced(ny, 0, ny);
				Eigen::VectorXd v1 = exp(-0.5 * ((py.array() - ky) * dy / constant / lambday).array().pow(2));
				Eigen::RowVectorXd v2 = exp(-0.5 * ((px.array() - kx) * dx / constant / lambdax).array().pow(2)).transpose();
				Eigen::MatrixXd S = (1 / sqrt(2 * EIGEN_PI) / pow(constant, 2) / lambdax / lambday) * v1 * v2;
				Eigen::MatrixXd SMOD = S / S.sum();
				w4(ky, kx) = (SMOD * data * delta).sum() / (SMOD * delta).sum();
			}
		r4 = data - w4;
		double CB_next = 4.4 * r4.array().abs().sum() / r4.size();
		CBx = abs((CB_old - CB_next) / CB_old);
		for (auto i = 0; i < nx; i++)
			for (auto j = 0; j < ny; j++) {
				if (abs(r4(j, i) / CB_next) < 1)
					delta(j, i) = pow(1 - pow(r4(j, i) / CB_next, 2), 2);
				else
					delta(j, i) = 0;
			}
		CB_old = CB_next;
		++iterationNo;
		std::cout << "CBx: " << CBx << ", iteration no: " << iterationNo << std::endl;
		if (iterationNo > max_iteration_no)
			break;
	}
	return w4;
}

// 1D Second-Order Gaussian Regression Filter
// w(k,p)= Ax2 + Bx + C where x = (k-p)dx
Eigen::VectorXd RGR2_1D(Eigen::VectorXd const& data,
	                    double lambdac = 0.8,
	                    double dx = 0.01,
	                    double tol = 1e-10,
	                    unsigned max_iteration_no = 29) {
	auto n = data.size();
	double constant = sqrt(log(2) / 2 / EIGEN_PI / EIGEN_PI);
	Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(n, 0, n) * dx;

	unsigned iterationNo = 0;
	Eigen::VectorXd delta = Eigen::VectorXd::Ones(n);
	double CBx = 1;
	double CB_old = 1;

	Eigen::Matrix3d M;
	Eigen::Vector3d Q;
	Eigen::Vector3d P;
	Eigen::VectorXd w1(n);
	Eigen::VectorXd r1(n);

	while (CBx > tol) {
		for (auto k = 0; k < n; k++) {
			auto p = Eigen::VectorXd::LinSpaced(n, 0, n);
			Eigen::VectorXd S = (1/sqrt(2 * EIGEN_PI * EIGEN_PI)/constant/lambdac) * exp(-0.5 * ((k - p.array()) * dx / constant / lambdac).array().pow(2));
			S /= S.sum();
			x = (k - p.array()) * dx;
			M(0, 0) = (delta.array() * S.array() * (x.array().pow(0))).sum();  // A0
			M(0, 1) = (delta.array() * S.array() * (x.array().pow(1))).sum();  // A1
			M(0, 2) = (delta.array() * S.array() * (x.array().pow(2))).sum();  // A2
			M(1, 0) = M(0, 1);  // A1
			M(1, 1) = M(0, 2);  // A2
			M(1, 2) = (delta.array() * S.array() * (x.array().pow(3))).sum();  // A3
			M(2, 0) = M(0, 2);  // A2
			M(2, 1) = M(1, 2);  // A3
			M(2, 2) = (delta.array() * S.array() * (x.array().pow(4))).sum();  // A4

			Q(0) = (delta.array() * data.array() * S.array()).sum();  // F0
			Q(1) = (delta.array() * data.array() * S.array() * x.array()).sum();  // F1
			Q(2) = (delta.array() * data.array() * S.array() * (x.array().pow(2))).sum();  // F2

			P = M.inverse() * Q;
			w1(k) = P(0);
		}

		r1 = data - w1;
		double CB_next = 4.4 * r1.array().abs().sum() / r1.size();
		CBx = abs((CB_old - CB_next) / CB_old);
		for (auto j = 0; j < n; j++)
			if (abs(r1(j) / CB_next) < 1)
				delta(j) = pow(1 - pow(r1(j) / CB_next, 2), 2);
			else
				delta(j) = 0;
		CB_old = CB_next;
		++iterationNo;
		std::cout << "CBx: " << CBx << ", iteration no: " << iterationNo << std::endl;
		if (iterationNo > max_iteration_no)
			break;
	}
	return w1;
}

int main() {

	/*
	Eigen::MatrixXd data(15, 15);
	data.setRandom();
	std::cout << data << '\n' << std::endl;
	auto res = RGR0_2D(data);
	std::cout << res << std::endl;
	*/

	/*
	Eigen::VectorXd data1(100);
	data1.setRandom();
	std::cout << data1.transpose() <<'\n' << std::endl;
	auto res1 = RGR2_1D(data1);
	std::cout << res1.transpose() << std::endl;
	*/

	///*
	unsigned N = 20;
	double A = 2;
	Eigen::VectorXd data1(N);
	for (auto i = 0; i < N; ++i)
		data1(i) = A * sin((EIGEN_PI / N * i)) + 0.1 * rand() / (RAND_MAX / 2) - 1;

	std::cout << data1.transpose() << '\n' << std::endl;
	auto res1 = RGR2_1D(data1);
	std::cout << res1.transpose() << std::endl;
	//*/
	return 0;
}


#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include "matplotlibcpp.h"

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

template <class T>
T g(T x, T a, T b, T c)
{
  return ceres::exp(a * x * x + b * x + c);
}

struct CurveFittingCost {

  CurveFittingCost(double x, double y) : _x(x), _y(y) {}

  template <class T>
  bool operator() (const T* const abc, T *residuals) const
  {
    residuals[0] = T(_y) - g(T(_x), abc[0], abc[1], abc[2]);
    return true;
  }

  private:
  const double _x, _y;
};

int main(int argc, char **argv) {
  std::cout << "Gauss-Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = 5.5;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  // Add the residual blocks
  double abc[3] = {a, b, c}; // parameters
  ceres::Problem problem;
  for (int i = 0; i < N; ++i)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CurveFittingCost, 1, 3>(
        new CurveFittingCost(x_data[i], y_data[i])
        ),
      nullptr,
      abc
    );
  }
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  std::cout << summary.BriefReport() << std::endl;
  std::cout << "Estimated: " << abc[0] << " "  << abc[1] << " "  << abc[2] << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], abc[0], abc[1], abc[2]);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


/*
 MIT License

 Copyright (c) 2019 Yuki Koyama

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

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

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

#ifndef OPTIMIZATION_TEST_FUNCTIONS_HPP_
#define OPTIMIZATION_TEST_FUNCTIONS_HPP_

#include <cassert>
#include <iostream>
#if defined(OTF_WITH_EIGEN)
#include <Eigen/Core>
#endif

namespace otf
{
    enum class FunctionType
    {
        Beale,
        Rosenbrock,
        Sphere,
    };

    inline double GetValue(const double x[], const int n, const FunctionType type)
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                const double a = 1.500 - x[0] + x[0] * x[1];
                const double b = 2.250 - x[0] + x[0] * x[1] * x[1];
                const double c = 2.625 - x[0] + x[0] * x[1] * x[1] * x[1];
                return a * a + b * b + c * c;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                double value = 0.0;
                for (int i = 0; i < n - 1; ++ i)
                {
                    value += 100.0 * (x[i + 1] - x[i] * x[i]) * (x[i + 1] - x[i] * x[i]) + (1.0 - x[i]) * (1.0 - x[i]);
                }
                return value;
            }
            case FunctionType::Sphere:
            {
                double value = 0.0;
                for (int i = 0; i < n; ++ i)
                {
                    value += x[i] * x[i];
                }
                return value;
            }
        }
    }

    inline void GetGrad(const double x[],
                        const int n,
                        const FunctionType type,
                        double grad[])
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                const double a = 1.500 - x[0] + x[0] * x[1];
                const double b = 2.250 - x[0] + x[0] * x[1] * x[1];
                const double c = 2.625 - x[0] + x[0] * x[1] * x[1] * x[1];
                grad[0] = 2.0 * a * (- 1.0 + x[1]) + 2.0 * b * (- 1.0 + x[1] * x[1]) + 2.0 * c * (- 1.0 + x[1] * x[1] * x[1]);
                grad[1] = 2.0 * a * x[0] + 2.0 * b * 2.0 * x[0] * x[1] + 2.0 * c * 3.0 * x[0] * x[1] * x[1];
                return;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                grad[0] = - 400.0 * x[0] * (x[1] - x[0] * x[0]) + 2.0 * (x[0] - 1.0);
                for (int i = 1; i < n - 1; ++ i)
                {
                    grad[i] = 200.0 * (x[i] - x[i - 1] * x[i - 1]) - 400.0 * x[i] * (x[i + 1] - x[i] * x[i]) + 2.0 * (x[i] - 1.0);
                }
                grad[n - 1] = 200.0 * (x[n - 1] - x[n - 2] * x[n - 2]);
                return;
            }
            case FunctionType::Sphere:
            {
                for (int i = 0; i < n; ++ i)
                {
                    grad[i] = 2.0 * x[i];
                }
                return;
            }
        }
    }

    inline void GetSolution(const int n,
                            const FunctionType type,
                            double x[])
    {
        switch (type)
        {
            case FunctionType::Beale:
            {
                assert(n == 2);
                x[0] = 3.0;
                x[1] = 0.5;
                return;
            }
            case FunctionType::Rosenbrock:
            {
                assert(n >= 2);
                for (int i = 0; i < n; ++ i)
                {
                    x[i] = 1.0;
                }
                return;
            }
            case FunctionType::Sphere:
            {
                for (int i = 0; i < n; ++ i)
                {
                    x[i] = 0.0;
                }
                return;
            }
        }
    }

#if defined(OTF_WITH_EIGEN)
    inline double GetValue(const Eigen::VectorXd& x, const FunctionType type)
    {
        return GetValue(x.data(), x.size(), type);
    }

    inline Eigen::VectorXd GetGrad(const Eigen::VectorXd& x, const FunctionType type)
    {
        Eigen::VectorXd grad(x.size());
        GetGrad(x.data(), x.size(), type, grad.data());
        return grad;
    }

    inline Eigen::VectorXd GetSolution(const int n, const FunctionType type)
    {
        Eigen::VectorXd x(n);
        GetSolution(n, type, x.data());
        return x;
    }
#endif
}

#endif


//
// Created by Ferdinando Fioretto on 11/2/15.
//

#ifndef MISC_UTILS_LMATRIX_HPP
#define MISC_UTILS_LMATRIX_HPP

#include <cmath>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;

namespace misc_utils
{
    // Linear Algebra
    namespace lmatrix
    {

        /* Matrix inversion routine.
        Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
        template<class T>
        bool InvertMatrix(const matrix<T>& input, matrix<T>& inverse)
        {
            typedef permutation_matrix<std::size_t> pmatrix;

            // create a working copy of the input
            matrix<T> A(input);

            // create a permutation matrix for the LU-factorization
            pmatrix pm(A.size1());

            // perform LU-factorization
            int res = lu_factorize(A, pm);
            if (res != 0)
                return false;

            // create identity matrix of "inverse"
            inverse.assign(identity_matrix<T> (A.size1()));

            // backsubstitute to get the inverse
            lu_substitute(A, pm, inverse);

            return true;
        }

        template<class T>
        matrix<T> to_matrix(const std::vector<std::vector<T>>& other) {
            matrix<T> ret(other.size(), other[0].size());
            for (int i=0; i<other.size(); i++) {
                for (int j=0; j<other[i].size(); j++) {
                    ret.insert_element(i, j, other[i][j]);
                }
            }
            return ret;
        }

        template<class T>
        std::vector<std::vector<T>> to_matrix(matrix<T>& other) {
            std::vector<std::vector<T>> ret(other.size1());
            for (int i = 0; i < other.size1(); i++)
                ret[i].resize(other.size2());

            for (int i = 0; i < other.size1(); i++) {
                for (int j = 0; j < other.size2(); j++) {
                    ret[i][j] = other.at_element(i, j);
                }
            }
            return ret;
        }

        template <class T>
        std::vector<std::vector<T>> prod(const std::vector<std::vector<T>>& A, const std::vector<std::vector<T>>& B) {
            matrix<T> _A = to_matrix(A);
            matrix<T> _B = to_matrix(B);
            matrix<T> C = boost::numeric::ublas::prod(_A, _B);
            return to_matrix(C);
        }

        template <class T>
        std::vector<std::vector<T>> invert(const std::vector<std::vector<T>>& A) {
            matrix<T> _A = to_matrix(A);
            int order = A.size();
            matrix<T> _Z(order, order);
            InvertMatrix(_A, _Z);
           return to_matrix(_Z);
        }

        template <class T>
        std::vector<std::vector<T>> transpose(const std::vector<std::vector<T>>& A) {
            matrix<T> _A = to_matrix(A);
            matrix<T> _At = boost::numeric::ublas::trans(_A);
            return to_matrix(_At);
        }

        //template <class T>
        inline std::vector<std::vector<double>> alloc(int orderI, int orderJ) {
            std::vector<std::vector<double>> Z(orderI);
            for (int i = 0; i < orderI; i++)
                Z[i].resize(orderJ, 0.0);
            return Z;
        }

        template <class T>
        std::string to_string(std::vector<std::vector<T>> matrix) {
            std::string ret;
            for (int i=0; i<matrix.size(); i++) {
                for (int j=0; j<matrix[i].size(); j++) {
                    ret += std::to_string(matrix[i][j]) + " ";
                }
                ret += "\n";
            }
            return ret;
        }
    }
}

#endif


/**
 *  @file    FiniteElement.hpp
 *  @brief   Defines basis functions.
 *  @author  Francois Roy
 *  @date    01/06/2020
 */
#ifndef FINITEELEMENT_H
#define FINITEELEMENT_H

#include <vector>
#include <cmath>
#include <math.h>
#include <Eigen/Core>
#include "spdlog/spdlog.h"

namespace numerical {
namespace fem {
/**
*  A linear mapping from \f$X\f$ to \f$x\f$ can be written as:
*
*  \f[
*    x = \frac{1}{2}\left(x_L+x_R\right)+\frac{1}{2}\left(x_R-x_L\right)X
*  \f]
*
*  @param X The local coordinate in the reference cell.
*  @param omega_e The left and right global coordinates of the element.
*  @return The global coordinate.
*/
template <typename T>
T affine_mapping(T X, const T (&omega_e)[2]){
    T x_L = omega_e[0];
    T x_R = omega_e[1];
    return 0.5*(x_L + x_R) + 0.5*(x_R - x_L)*X;
}
/**
*  Returns the \f$i\f$-th Lagrange polynomial.
*
*  \f[
*      \phi_i(x) = \prod_{j=0, j\neq i}^N \frac{x-x_j}{x_i-x_j}
*  \f]
*
*  @param x The local coordinate in the reference cell.
*  @param i The Lagrange polynomial index.
*  @param nodes The interpolation nodes in the reference cell.
*  @return The \f$i\f$-th Lagrange polynomial.
*/
template <typename T>
T lagrange_polynomial(T x, int i,
    const Eigen::Matrix<T, Eigen::Dynamic, 1>& nodes){
    T p = 1.0;
    for(int j=0; j<nodes.size(); j++){
        if(j != i){
            p *= (x - nodes[j])/(nodes[i] - nodes[j]);
          }
    }
    return p;
};
/**
*  Returns the first derivative of the \f$i\f$-th Lagrange polynomial.
*
*  \f[
*    \begin{align}
*      \frac{\partial}{\partial x}\phi_i(x) &=
*      \frac{\partial}{\partial X}\phi_i(X)\frac{\partial X}{\partial x}\\
*      &= \frac{\partial}{\partial X}\phi_i(X)\frac{2}{h}
*    \end{align}
*  \f]
*
*  For details see Langtangen2019, p.188 Cellwise computation.
*
*  The first derivative of the Lagrange polynomial with respect to local
*  coordinates \f$X\f$ can be obtained using the logarithm of the Lagrange
*  polynamial, i.e.
*
*  \f[
*    \textrm{ln}\left(\prod_{j=0, j\neq i}^N \frac{x-x_j}{x_i-x_j}\right) =
*      \sum_{j=0, j\neq i}^N \textrm{ln}\left(\frac{x-x_j}{x_i-x_j}\right)
*  \f]
*
*  The derivative of the logarithm is
*
*  \f[
*    \frac{\partial}{\partial X}\left(\textrm{ln}(f(x))\right) = \frac{1}{f(X)}
*      \frac{\partial}{\partial X}f(X)
*  \f]
*
*  with \f$f(x)=\frac{x-x_j}{x_i-x_j}\f$, we get
*
*  \f[
*    \frac{\partial}{\partial X}\phi_i(X) = \phi_i(X)\sum_{j=0, j\neq i}^N
*      \frac{1}{x-x_j}
*  \f]
*
*  Note that this result is only valid for \f$(x-x_j)\neq 0\f$.
*
*  @param x The local coordinate.
*  @param i The polynomial index.
*  @param nodes The interpolation nodes in the reference cell.
*  @param h The element size.
*  @param d The polynomial order, default=1.
*  @return The \f$i\f$-th Lagrange polynomial.
*/
template <typename T>
T lagrange_polynomial_first_derivative(T x, int i,
    const Eigen::Matrix<T, Eigen::Dynamic, 1>& nodes, T h, int d=1){
    T p = 0.0;
    T denom = 1.0;
    T lp = lagrange_polynomial(x, i, nodes);
    for(int j=0; j<nodes.size(); j++){
        if(j != i){ // d > 1
            denom *= (nodes[i] - nodes[j]);
            if(d == 1){
                p += 1.0;
            } else if(d ==2){
                p += (x - nodes[j]);
            }
          }
    }
    p *= 2.0 / h / denom;
    return p;
};
/**
*  Returns all local basis function \f$\phi\f$ and their derivatives,
*  in physical coordinates, as functions of the local point \f$X\f$ in a
*  reference cell with \f$d+1\f$ nodes.
*
*  Example:
*  ~~~~~~~~~~~~~~~{.cpp}
*  #include "numerical/fem/FiniteElement.hpp"
*
*  using namespace numerical;
*
*  int main()
*  {
*    double f0 = fem::phi(0, 0, 0.0);  // basis func 0 at X=0
*    double fp0 = fem::phi(0, 0, 0.0);  // 1st x-derivative at X=0
*    spdlog::info("f0: {}, fp0: {}", f0, fp0);
*    return 0;
*  }
*  ~~~~~~~~~~~~~~~
*  Output:
*  ~~~~~~~~~~~~~~~{.sh}
*  $ f0: 0.5, fp0: -2.0
*  ~~~~~~~~~~~~~~~
*
*  @param i 0 if function, 1 if first derivative.
*  @param j The index of the basis function.
*  @param x The local coordinate.
*  @param h The element size.
*  @param d The polynomial order.
*  @return The value of the basis function/derivative at the local
*    coordinate.
*/
template <typename T>
T phi(int i, int j, T x, T h=2.0, int d=1){
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
    Vec nodes = Vec::LinSpaced(d+1, -1.0, 1.0);  // local nodes
    if(j>d){
        spdlog::error("Invalid value, index j must be in the range [0,d].");
        return 0.0;
    }
    if(std::abs(x)>1){
        spdlog::error("Invalid value, x must be in the range [-1,1].");
        return 0.0;
    }
    if(i==0){  // function
        return lagrange_polynomial(x, j, nodes);
    } else if(i==1){
        return lagrange_polynomial_first_derivative(x, j, nodes, h, d);
    } else{
        spdlog::error("Invalid value, 'i' is either 0 or 1.");
        return 0.0;
    }
}
/**
*  Return points and weights for Gauss-Legendre rules on [-1,1]. The number of
*  points implemented are 1-4.
*
*  Numerical integration rules can be expressed in a common form:
*
*  \f[
*    \int_{-1}^{1} g(X)dX\approx \sum_{j=0}^M w_jg(\bar{X}_j)
*  \f]
*
*  where \f$\bar{X}_j\f$ are integration points, and \f$w_j\f$ are integration
*  weights, for \f$j=0,\ldots ,M\f$.
*
*  More accurate rules, for a given \f$M\f$, arise if the location of the
*  integration points are optimized for polynomial integrands. The
*  Gauss-Legendre quadrature constitute one such class of integration methods.
*  Two widely applied Gauss-Legendre rules in this family have the choice:
*
*  for \f$M=1\f$:
*
*  \f[
*    \bar{X}_0 = -\frac{1}{\sqrt{3}},\quad \bar{X}_1 = \frac{1}{\sqrt{3}},
*      \quad w_0=w_1=1
*  \f]
*
*  For \f$M=2\f$:
*
*  \f[
*    \bar{X}_0 = -\sqrt{\frac{3}{5}},\quad \bar{X}_1 = 0,
*      \quad \bar{X}_2 = \sqrt{\frac{3}{5}}, \quad w_0=w_2=\frac{5}{9},
*      \qaud w_1 = \frac{8}{9}
*  \f]
*
*  These rules integrate 3rd and 5th degree polynomials exactly. In general,
*  an \f$M\f$-point Gauss-Legendre rule integrates a polynomial of degree
*  \f$2M + 1\f$ exactly.
*
*  @param n The number of integration points.
*  @return The points and weights.
*/
template <typename T>
std::vector<std::vector<T>> GaussLegendre(int n){
    std::vector<T> points;
    std::vector<T> weights;
    if(n == 1){
        points = {0.0};
        weights = {2.0};
    } else if(n == 2){
        points = {-0.5773502691896257645091488,
                   0.5773502691896257645091488};
        weights = {1.0, 1.0};
    } else if(n == 3){
        points = {-0.7745966692414833770358531,
                  0.0,
                  0.7745966692414833770358531};
        weights = {0.5555555555555555555555556,
                   0.8888888888888888888888889,
                   0.5555555555555555555555556};
    } else if(n == 4){
        points = {-0.8611363115940525752239465,
                  -0.3399810435848562648026658,
                  0.3399810435848562648026658,
                  0.8611363115940525752239465};
        weights = {0.3478548451374538573730639,
                   0.6521451548625461426269361,
                   0.6521451548625461426269361,
                   0.3478548451374538573730639};
    } else {
        spdlog::error("Gauss-Legendre: The number of points is not supported");
        return {{}};
    }
    return {points, weights};
}

}  // namespace fem
} // namespace numerical

#endif  // FINITEELEMENT_H


/*  example for MatlabPlotter: Natural cubic splines (core problem)

	An in-class exercise using equations for the cubic splines as
	shown in the lecture/manuscript for Numerical Methods for CSE
	by Prof. R. Hiptmair, ETH Zürich

	Include the Eigen3 library as shown in documentation for Eigen3.

	use piping to store the .m file. Example call:
	natcsi >example.m

	given nodes (t_i,y_i) for i = 0,...,n

	and using eq 3.5.5, the calculation of y = s(x) boils down to
	- finding the correct interval [t_j-1,t_j] for x
	- apply s|[t_j-1,t_j](x) = s_j(x) =
		y_j-1 * (1 - 3 * tau^2 + 2 * tau^3) +
		y_j   * (3 * tau^2 - 2 * tau^3) +
		m_j-1 * (tau - 2 * tau^2 + tau^3) +
		m_j   * (-tau^2 + tau^3)

		with
		m1_j = m_j-1 = h_j * c_j-1,
		m2_j = m_j   = h_j * c_j

		c_j   = s'(t_j)

		h_j   = t_j - t_j-1

		tau   = (t - t_j-1) / h_j

	current runtime:
	- initialization: O(n), Thomas algorithm for solving the tridiagonal system matrix
	- evaluation:
		* worst case:        O(log n)
		* best/typical case: O(1)

	space requirements: O(n) all the time

	output: is given again as a Matlab script that
	allows plotting all data in Matlab.

	v1.1.1 2015-11-05 / 2015-11-29 Pirmin Schmid
*/

#include <cassert>
#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "matlab_plotter.h"

using namespace std;
using namespace Eigen;

// cubic spline interpolant with natural boundaries
class NatCSI {
public:

	// build cubic spline interpolant with natural boundaries
	// input:  t  nodes of the grid for pairs (t_i, y_i), sorted!
	//         y  values y_i at t_i for pairs (t_i, y_i), sorted!
	// input in std::vector<double> type
	NatCSI(const vector<double>& t, const vector<double>& y) {
		// map std::vector<double> to a VectorXd
		VectorXd tmp_t = VectorXd::Map(t.data(), t.size());
		VectorXd tmp_y = VectorXd::Map(y.data(), y.size());

		// store a copy of the data
		this->t = tmp_t;
		this->y = tmp_y;

		compute_coefficients();
	}

	// build cubic spline interpolant with natural boundaries
	// input:  t  nodes of the grid for pairs (t_i, y_i), sorted!
	//         y  values y_i at t_i for pairs (t_i, y_i), sorted!
	// input in Eigen::VectorXd type
	NatCSI(const VectorXd& t, const VectorXd& y) {
		// store a copy of the input vectors
		this->t = t;
		this->y = y;

		compute_coefficients();
	}

	// interpolant evaluation at x
	// input:   x value where to evaluate the spline
	// return:  y = s(x) if x in defined range of s(x)
	//          or 0.0 otherwise
	double operator() (double x) {
		// out of defined interval
		if(x < min || max < x) {
			return 0.0;
		}

		// get proper j, using a heuristic
		// 1) check whether x is in last range, O(1)
		// 2) check whether x is in next range, O(1)
		// 3) use binary search tree, O(log n)
		if(!in_range(x, last_j)) {
			if(last_j < n && in_range(x, last_j+1)) {
				// note: the second test is only applied if last_j actually allows it
				last_j++;
			}
			else {
				find_index(x);
			}
		}

		// apply function in interval j
		int j = last_j; // just for convenience

		double tau  = (x - t[j-1]) / h[j];
		double tau2 = tau * tau;
		double tau3 = tau2 * tau;

		double result = y[j-1] * (1.0 - 3.0 * tau2 + 2.0 * tau3);
		result += y[j] * (3.0 * tau2 - 2.0 * tau3);
		result += m1[j] * (tau - 2.0 * tau2 + tau3);
		result += m2[j] * (-tau2 + tau3);
		return result;
	}

private:
	// we need to store the vectors t and y
	// and a vector for m and h as described above
	// m is calculated after having solved a LSE
	// A * c = d as described below (and in the manuscript)

	VectorXd t, y, h, m1, m2;

	// number of segments of the spline function
	// note: n is a valid index for the vectors. Their size is (n+1).
	int n = 0;

	// valid range for x
	double min = 0.0;
	double max = 0.0;

	// since most requests for x calculations are in vicinity
	// of the prior one (or even sorted), remembering
	// the last requested section will help to bring
	// evaluation time to O(1) compared to O(log n) if only
	// a binary search tree is used.
	// the binary search tree is currently used as a fallback
	// in case the current partition/segment or the next one
	// has not been successful.
	int last_j = 1;

	// returns true if x is in interval j [t_j-1, t_j]
	bool in_range(const double x, const int j) const {
		assert(1 <= j && j <= n);
		return t[j-1] <= x && x <= t[j];
	}

	// sets last_j to  index of matching interval j [t_j-1, t_j] if x in range
	// note: x had to be tested before whether it is actually within the range of the spline function
	// note: optimizations (first test whether last interval was already ok or test the next
	// interval have already been done when one calls this function)
	// finds next j in O(log n) time
	void find_index(const double x) {
		assert(min <= x && x <= max);

		int j = last_j;
		int a = 1;
		int b = n;
		bool found = false;
		while(!found) {
			if(x < t[j-1]) {
				b = j;
				j = (a+b)/2;
			}
			else if(t[j] < x) {
				if(a == b-1) {
					// special case needed due to integer division rounding down
					j = b;
					found = true;
				}
				else {
					a = j;
					j = (a+b)/2;
				}
			}
			else {
				// match
				found = true;
			}
		}

		last_j = j;
	}

	void compute_coefficients() {
		if(t.size() != y.size()) {
			// only minimalistic error management
			cout << "Error: size of t and y must be identical." << endl;
			exit(1);
		}

		// n
		n = (int)t.size() - 1;

		// min/max
		min = t[0];
		max = t[n];

		// h_j = t_j - t_j-i
		h.resize(n + 1);
		h.segment(1, n) = t.tail(n) - t.head(n);

		// build a virtual (n+1)x(n+1) system matrix A; however, only the diagonal vectors will actually be built
		// A = diag(a) + diag(b,1) + diag(b,-1), with size(a) = n+1 and size(b) = n

		// b_i = 1/h_i+1 for i=0 to n-1
		VectorXd b = h.tail(n).cwiseInverse();

		// a_i = 2/h_i + 2/h_i+1 for i=1 to n-1
		// a_0 = 2/h_1 (see natural cubic spline)
		// a_n = 2/h_n (see natural cubic spline)
		VectorXd a(n + 1);
		a.segment(1, n-1) = 2.0 * (h.segment(1, n-1).cwiseInverse() + h.tail(n-1).cwiseInverse());
		a[0] = 2.0 / h[1];
		a[n] = 2.0 / h[n];

		// right hand side d for A*c = d
		// for i=1 to n-1
		// d_i = 3.0 * ( (y_i - y_i-1) / h2_i  +  (y_i+1 - y_i) / h2_i+1 )
		// d_0 = 3.0 * (y_1 - y_0) / h2_1
		// d_n = 3.0 * (y_n - y_n-1) / h2_n

		VectorXd h2 = h.cwiseProduct(h); // h^2

		// note: y_i - y_i-1 is stored in delta_y[i-1]
		VectorXd delta_y = y.tail(n) - y.head(n);

		// quotient is either left or right side inside the brackets
		VectorXd quotient = delta_y.cwiseQuotient(h2.tail(n));

		// build d
		VectorXd d(n + 1);
		d.segment(1, n-1) = quotient.head(n-1) + quotient.tail(n-1);
		d[0] = delta_y[0] / h2[1];
		d[n] = delta_y[n-1] / h2[n];
		d = 3.0 * d;

		// Solution of this LSE with n+1 equations and n+1 unknowns c0 to cn
		// using the Thomas algorithm for tridiagonal matrices, needs O(n)
		// typical description with vectors b1..bn, a2..an, c1..cn-1
		// 'b' is a here
		// 'a' and 'c' are b here
		// 'x' is our c
		// note: different indices, too; also we have size n+1 instead of n

		// forward preparation
		VectorXd bprime(n);
		VectorXd dprime(n+1);

		// cprime -> bprime
		bprime[0] = b[0] / a[0];
		for(int i = 1; i < (n-1); ++i) {
			bprime[i] = b[i] / (a[i] - bprime[i-1] * b[i]);
		}

		// dprime
		dprime[0] = d[0] / a[0];
		for(int i = 1; i < n; ++i) {
			dprime[i] = (d[i] - dprime[i-1] * b[i]) / (a[i] - bprime[i-1] * b[i]);
		}

		// back substitution to get x, or c in our case
		VectorXd c(n + 1);
		c[n] = dprime[n];
		for(int i = n-1; i >= 0; --i) {
			c[i] = dprime[i] - bprime[i] * c[i+1];
		}

		// we do not actually need c but
		// m1_j = m_j-1 = h_j * c_j-1,
		// m2_j = m_j   = h_j * c_j
		m1.resize(n+1);
		m2.resize(n+1);
		m1.segment(1, n) = h.tail(n).cwiseProduct( c.head(n) );
		m2.segment(1, n) = h.tail(n).cwiseProduct( c.tail(n) );
	}

};

// just a quick implementation of the function
double f(const double x) {
	return exp(sin(2.0 * M_PI * x));
}

#define MIN -3
#define MAX 3
#define N_T 61
#define N_X 600

int main() {
	// initialization
	VectorXd t = VectorXd::LinSpaced(N_T, MIN, MAX);
	VectorXd y(N_T);
	for(int i = 0; i < N_T; ++i) {
		y[i] = f(t[i]);
	}

	NatCSI ncsi(t, y);

	// test 1
	VectorXd xx = VectorXd::LinSpaced(N_X, MIN, MAX);
	VectorXd yy(N_X);
	for(int i = 0; i < N_X; ++i) {
		yy[i] = ncsi(xx[i]);
	}

	// test 2. test the binary search in find_index()
	VectorXd y2(N_T);
	for(int i = N_T-1; i >= 0; --i) {
		y2[i] = ncsi(t[i]);
	}

	// test 3. test border case (last node)
	// make sure that the 1st AND the last node are highlighted
	// in the plot (last node: see special case handling due to
	// integer division in find_index() )
	VectorXd x3(2);
	VectorXd y3(2);
	x3 << t[0], t[N_T-1];
	for(int i = 0; i < x3.size(); ++i) {
		y3[i] = ncsi(x3[i]);
	}

	// test 4. test std::vector<double> (no legend item)
	vector<double> x4 = {MIN, MAX};
	vector<double> y4 = {1.0, 1.0};

	// plot results
	MatlabPlotter p;
	p.comment("Example: natural cubic splines");
	p.comment("Code generated by natcsi.cpp");
	p.figure("Natural CSI");
	p.plot_fx("exp(sin(2.*pi.*x))", MIN, MAX, N_X, "b-");
	p.hold();
	p.plot(t, y, "b*");
	p.plot(xx, yy, "r--");
	p.plot(t, y2, "ro");
	p.plot(x3, y3, "g*");
	p.plot(x4, y4, "k:");
	p.xylabels("x", "y");
	p.legend("exp(sin(2*pi*x))", "t0 to tn", "Natural CSI", "check findindex()", "check corner cases");
	p.hold(false);

	// for testing / illustration purpose: semilog scale for y axis
	p.figure("Natural CSI. semilog scale", MatlabPlotter::SEMILOGY);
	p.plot_fx("exp(sin(2.*pi.*x))", MIN, MAX, N_X, "b-");
	p.hold();
	p.plot(t, y, "b*");
	p.plot(xx, yy, "r--");
	p.plot(t, y2, "ro");
	p.plot(x3, y3, "g*");
	p.plot(x4, y4, "k:");
	p.xylabels("x", "y (log scale)");
	p.legend("exp(sin(2*pi*x))", "t0 to tn", "Natural CSI", "check findindex()", "check corner cases");
	p.hold(false);
}


#include <Eigen/SVD>

using namespace Eigen;

namespace Geometry
{

	/// Compute the pseudo-inverse of a matrix
	Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
		double  tolerance=1e-6;
		int u=(int)A.rows(), v=(int)A.cols(), n=std::min<int>(u,v);
		MatrixXd sigma(v,u);
		sigma.setZero();
		sigma.topLeftCorner(n,n)=svd.singularValues().asDiagonal();
		for (int i=0;i<n;i++)
			if (sigma(i,i)<tolerance) sigma(i,i)=0;
			else sigma(i,i)=1.0/sigma(i,i);
		MatrixXd pinv=svd.matrixV()*sigma*svd.matrixU().transpose();
		return pinv;
	}

	/// Compute the pseudo-inverse and null-space of a matrix
	void pseudoInverseAndNullspace(const Eigen::Matrix<double,3,4>& P, Eigen::Matrix<double,4,3>& Pinv, Eigen::Vector4d& C)
	{
		JacobiSVD<MatrixXd> svd(P, ComputeFullU | ComputeFullV);
		double  tolerance=1e-6;
		int u=(int)P.rows(), v=(int)P.cols(), n=std::min<int>(u,v);
		MatrixXd sigma(v,u);
		sigma.setZero();
		sigma.topLeftCorner(n,n)=svd.singularValues().asDiagonal();
		for (int i=0;i<n;i++)
			if (sigma(i,i)<tolerance) sigma(i,i)=0;
			else sigma(i,i)=1.0/sigma(i,i);
		Pinv=svd.matrixV()*sigma*svd.matrixU().transpose();
		auto V=svd.matrixV();
		C=V.col(V.cols()-1);
	}

	/// Compute right null-space of A
	Eigen::VectorXd nullspace(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
		auto V=svd.matrixV();
		return V.col(V.cols()-1);
	}

	// Enforce rank deficiency
	Eigen::MatrixXd makeRankDeficient(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
		VectorXd sigma=svd.singularValues();
		sigma[sigma.size()-1]=0;
		return svd.matrixU()*sigma.asDiagonal()*svd.matrixV().transpose();
	}

	// Compute left null-space of A
	Eigen::VectorXd nullspace_left(const Eigen::MatrixXd& A)
	{
		return nullspace(A.transpose());
	}

} // namespace Geometry


#include "hypothesis.h"
#include <math.h>
#include <boost/math/quadrature/gauss.hpp>
#include <boost/math/distributions/chi_squared.hpp>
using namespace std;

Hypothesis::Hypothesis() = default;

// Hypothesis test for the avarage, we use the library Boost to integrate the function
// that corresponds for the Normal Cumulative Distribution Function, we use gaussian
// quadrature method to integrate passing the lambda function that corresponds to the
// Normal. We followed "Curso de Planejamento Experimental - FEMEC - UFU" available
// on Moodle to every step to do.
double Hypothesis::testAverage(double sampleAvg, double sampleStdDev, unsigned long sampleNumElements,
                               double confidencelevel, double avg, H1Comparition comp) {
    using namespace boost::math::quadrature;

    double stdError = sampleStdDev / sqrt(sampleNumElements);

    double z_calc = (sampleAvg - avg) / stdError;

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double e = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(e);
        return constant * sqrtE;
    };

    double Q = gauss<double, 10000>::integrate(f, -INFINITY, z_calc);

    double alpha = confidencelevel / 100;

    testHypothesisNull(comp, alpha, Q);
    return Q;

}

// Really similar to the previous method, we used the same things and followed
// the document on Moodle.
double Hypothesis::testProportion(double sampleProp, unsigned long sampleNumElements, double confidencelevel,
                                  double prop, H1Comparition comp) {
    using namespace boost::math::quadrature;

    double z_cal = ((sampleProp - prop) / sqrt((prop * (1 - prop)) / sampleNumElements));

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double e = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(e);
        return constant * sqrtE;
    };

    double Q = gauss<double, 10000>::integrate(f, -INFINITY, z_cal);

    double alpha = confidencelevel / 100;

    testHypothesisNull(comp, alpha, Q);
    return Q;
}

// This one is different from the rest, following "Curso de estatistica.pdf" on moodle,
// we need to use chi square method to calculate p-value, so after some research we used
// an implementation available online.
double Hypothesis::testVariance(double sampleVar, unsigned long sampleNumElements, double confidencelevel,
                                double var, H1Comparition comp) {
    using namespace boost::math::quadrature;

    int df = (int)sampleNumElements - 1;
    double chi_cal = (df * sampleVar) / var;

    double chi = ChiSquare(chi_cal, df);

    double alpha = confidencelevel / 100;

    double value_calc = 1 - chi;

    testHypothesisNull(comp, alpha, value_calc);
    return chi;
}

// Hypothesis test acording to p-value and confidence level and the type of comparation.
void Hypothesis::testHypothesisNull(H1Comparition comp, double alpha, double value_calc){

    double value_critical = 1 - alpha;

    if (comp == H1Comparition::DIFFERENT) {

        double value_inf = alpha / 2;
        double value_sup = value_critical + value_inf;

        if (value_calc > value_sup or value_calc < value_inf) {
            printf("\n   # REJECT H0 => NOT EQUAL WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => EQUAL WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    } else if (comp == H1Comparition::LESS_THAN) {

        if (value_calc < alpha) {
            printf("\n   # REJECT H0 => LESS THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => NOT LESS THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    } else {

        if (value_calc > value_critical) {
            printf("\n   # REJECT H0 => GREATER THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => NOT GREATER THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    }
}

// Reference: https://jamesmccaffrey.wordpress.com/2010/11/01/programmatically-computing-chi-square-critical-values
double Hypothesis::ChiSquare(double x, int df) {
    // x = a computed chi-square value
    // df = degrees of freedom
    using namespace boost::math::quadrature;

    if (x <= 0.0 || df < 1) {
        printf("parameter x must be positive and parameter df must be 1 or greater");
    };

    double a = 0; // 299 variable names
    double y = 0;
    double s = 0;
    double z = 0;
    double e = 0;
    double c;

    bool even; // is df even?

    a = 0.5 * x;
    if (df % 2 == 0) even = true; else even = false;

    if (df > 1) y = Exp(-a); // ACM update remark (4)

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double ex = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(ex);
        return constant * sqrtE;
    };

    if (even) s = y; else s = 2.0 * gauss<double, 10000>::integrate(f, -INFINITY, -sqrt(x));

    if (df > 2)
    {
        x = 0.5 * (df - 1.0);
        if (even) z = 1.0; else z = 0.5;
        if (a > 40.0) // ACM remark (5)
        {
            if (even) e = 0.0; else e = 0.5723649429247000870717135; // log(sqrt(pi))
            c = log(a); // log base e
            while (z <= x)
            {
                e = log(z) + e;
                s = s + Exp(c * z - a - e); // ACM update remark (6)
                z = z + 1.0;
            }
            return s;
        } // a > 40.0
        else
        {
            if (even) e = 1.0; else e = 0.5641895835477562869480795 / sqrt(a); // (1 / sqrt(pi))
            c = 0.0;
            while (z <= x)
            {
                e = e * (a / z); // ACM update remark (7)
                c = c + e;
                z = z + 1.0;
            }
            return c * y + s;
        }
    }
    else
    {
        return s;
    }
}

// Reference: https://jamesmccaffrey.wordpress.com/2010/11/01/programmatically-computing-chi-square-critical-values/
double Hypothesis::Exp(double x) { // ACM update remark (3)
    if (x < -40.0) // 40.0 is a magic number. ACM update remark (8)
        return 0.0;
    else
        return exp(x);
}


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXf m = MatrixXf::Random(3,2);
cout << "Here is the matrix m:" << endl << m << endl;
JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
Vector3f rhs(1, 0, 0);
cout << "Now consider this rhs vector:" << endl << rhs << endl;
cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;

  return 0;
}


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>

template<typename T>
T fourth_power(T x)
{
    x *= x;
    return x *= x;
}

int main()
{
    using namespace boost::math::differentiation;

    constexpr int Order=5; // The highest order derivative to be calculated.
    const autodiff_fvar<double,Order> x = make_fvar<double,Order>(2.0); // Find derivatives at x=2.
    const autodiff_fvar<double,Order> y = fourth_power(x);
    for (int i=0 ; i<=Order ; ++i)
        std::cout << "y.derivative("<<i<<") = " << y.derivative(i) << std::endl;
    return 0;
}
/*
Output:
y.derivative(0) = 16
y.derivative(1) = 32
y.derivative(2) = 48
y.derivative(3) = 48
y.derivative(4) = 24
y.derivative(5) = 0
**/


#include <vector>
#include <cassert>

#include <iostream>

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

class NatCSI {
public:
    NatCSI(const std::vector<double> & t, const std::vector<double> & y)
        : t(t), y(y), h(t.size()-1), c(t.size()) {
        // Size check
        assert( ( t.size() == y.size() ) && "Error: mismatched size of t and y!");

        // m is the number of conditions (t goes from t_0 to t_n)
        // WARNING: m = n+1
        m = t.size();

        // Vector containing increments (from the right)
        for(int i = 0; i < (m - 1); ++i) {
            h(i) = t[i+1] - t[i];
            // Check that t is sorted
            assert( ( h(i) > 0 ) && "Error: array t must be sorted!");
        }

        // System matrix and rhs as in 3.5.9, we remove first and last row (known by natural contition)
        Eigen::SparseMatrix<double> A(m,m);
        Eigen::VectorXd b(m);

        // WARNING: sparse reserve space
        A.reserve(3);

        // Fill in natural conditions 3.5.10 for matrix
        A.coeffRef(0,0) = 2 / h(0);
        A.coeffRef(0,1) = 1 / h(0);
        A.coeffRef(m-1,m-2) = 1 / h(m-2);
        A.coeffRef(m-1,m-1) = 2 / h(m-2);

        // Reuse computation for rhs
        double bold = (y[1] - y[0]) / (h(0)*h(0));
        b(0) = 3*bold; // Fill in natural conditions 3.5.10
        // Fill matrix A and rhs b
        for(int i = 1; i < m-1; ++i) {
            // PRecompute b_i
            double hinv = 1./h(i);

            // Fill in a
            A.coeffRef(i,i-1) = hinv;
            A.coeffRef(i,i) = 2./h(i) + 2./h(i-1);
            A.coeffRef(i,i+1) = hinv;

            // Reuse computation for rhs b
            double bnew = (y[i+1] - y[i]) / (h(i)*h(i));
            b(i) = 3. * (bnew + bold);
            bold = bnew;
        }
        b(m-1) = 3*bold; // Fill in natural conditions 3.5.10
        // Compress the matrix
        A.makeCompressed();
        std::cout << A;

        // Factorize A and solve system A*c(1:end) = b
        Eigen::SparseLU<Eigen::SparseMatrix<double>> lu;
        lu.compute(A);
        c = lu.solve(b);

    }

    double operator() (double x) const {
        // Assert that x \in (t_0, t_n)
        assert( ( t[0] <= x && x <= t[m-1] ) && "Error: x must be in (t_0, t_n)");

        // Find j s.t. x \in (t_j,t_j+1)
        // as with the previous sheet (PS 6), we perform an efficient binary search on the data (log. complexity)
        auto j = (std::lower_bound(t.begin(), t.end(), x) - t.begin()) - 1;
        if( j == -1 ) j++;

        // Precompute tau and evaluate spline (3.5.5)
        double tau = (x - t[j]) / h(j);
        double tau2 = tau*tau;
        double tau3 = tau2*tau;
        return y[j] *         ( 1. - 3. * tau2 + 2 * tau3 ) +
               y[j+1] *           ( 3 * tau2 - 2 * tau3 ) +
               h(j) * c(j) *  ( tau - 2 * tau2 + tau3 ) +
               h(j) * c(j+1) *    ( - tau2 + tau3 );

    }

private:
    // Provided nodes and values to compute spline, same size Eigen vectors
    std::vector<double> t, y;
    // Difference t(i)-t(i-1) and coefficients of spline (s'(t_j))
    Eigen::VectorXd h, c;
    // Size of t, y and c  = m. h has size m-1
    int m;
};

int main() {

    int n = 8, m = 100;
    std::vector<double> t;
    std::vector<double> y;
    t.resize(n);
    y.resize(n);
    for(int i = 0; i < n; ++i) {
        t[i] = (double) i / (n-1);
        y[i] = cos(t[i]);
    }

    NatCSI N(t,y);

    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(m, 0, 1);
    for(int i = 0; i < x.size(); ++i) {
        x(i) = N(x(i));
    }
    std::cout << x << std::endl;
}

Nummerical Methods for CSE/PS8/solutions_ps8/natcsi.cpp


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

void gramSchmidtOrthogonalization(Eigen::MatrixXd &matrix,Eigen::MatrixXd &orthonormalMatrix)
{
/*
    In this method you make every column perpendicular to it's previous columns,
    here if a and b are representation vector of two columns, c=b-((b.a)/|a|).a
        ^
       /
    b /
     /
    /
    ---------->
        a
        ^
       /|
    b / |
     /  | c
    /   |
    ---------->
        a
    you just have to normilze every vector after make it perpendicular to previous columns
    so:
    q1=a.normalized();
    q2=b-(b.q1).q1
    q2=q2.normalized();
    q3=c-(c.q1).q1 - (c.q2).q2
    q3=q3.normalized();
    Now we have Q, but we want A=QR so we just multiply both side by Q.transpose(), since Q is orthonormal, Q*Q.transpose() is I
    A=QR;
    Q.transpose()*A=R;
*/
    Eigen::VectorXd col;
    for(int i=0;i<matrix.cols();i++)
    {
        col=matrix.col(i);
        col=col.normalized();
        for(int j=0;j<i-1;j++)
        {
            //orthonormalMatrix.col(i)
        }

        orthonormalMatrix.col(i)=col;
    }
    Eigen::MatrixXd A(4,3);

    A<<1,2,3,-1,1,1,1,1,1,1,1,1;
    Eigen::Vector4d a=A.col(0);
    Eigen::Vector4d b=A.col(1);
    Eigen::Vector4d c=A.col(2);

    Eigen::Vector4d q1=  a.normalized();
    Eigen::Vector4d q2=b-(b.dot(q1))*q1;
    q2=q2.normalized();

    Eigen::Vector4d q3=c-(c.dot(q1))*q1 - (c.dot(q2))*q2;
    q3=q3.normalized();

    std::cout<< "q1:"<<std::endl;
    std::cout<< q1<<std::endl;
    std::cout<< "q2"<<std::endl;
    std::cout<< q2<<std::endl;
    std::cout<< "q3:"<<std::endl;
    std::cout<< q3<<std::endl;

    Eigen::MatrixXd Q(4,3);
    Q.col(0)=q1;
    Q.col(1)=q2;
    Q.col(2)=q3;

    Eigen::MatrixXd R(3,3);
    R=Q.transpose()*(A);

    std::cout<<"Q"<<std::endl;
    std::cout<< Q<<std::endl;

    std::cout<<"R"<<std::endl;
    std::cout<< R.unaryExpr(std::ptr_fun(exp))<<std::endl;

    //MatrixXd A(4,3), thinQ(4,3), Q(4,4);

    Eigen::MatrixXd thinQ(4,3), q(4,4);

    //A.setRandom();
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    q = qr.householderQ();
    thinQ.setIdentity();
    thinQ = qr.householderQ() * thinQ;
    std::cout << "Q computed by Eigen" << "\n\n" << thinQ << "\n\n";
    std::cout << q << "\n\n" << thinQ << "\n\n";

}

void gramSchmidtOrthogonalizationExample()
{
    Eigen::MatrixXd matrix(3,4),orthonormalMatrix(3,4) ;
    matrix=Eigen::MatrixXd::Random(3,4);////A.setRandom();

    gramSchmidtOrthogonalization(matrix,orthonormalMatrix);
}

int main()
{

}


//
// Created by jhwangbo on 23.09.16.
// This function is to solve linear equation Ax=b

#ifndef RAI_CONJUGATEGRADIENT_HPP
#define RAI_CONJUGATEGRADIENT_HPP
#include <functional>
#include <Eigen/Core>
#include <iostream>

namespace rai{
//namespace Math{

template <typename Derived>
Derived conjugateGradient(std::function< void(Eigen::Matrix<Derived, -1, 1>&,
                                           Eigen::Matrix<Derived, -1, 1>&)> eval,
                       Eigen::Matrix<Derived, -1, 1> b,
                       int iterN, Derived tol, Eigen::Matrix<Derived, -1, 1>& sol)
{
  int dim = b.rows();
  Eigen::Matrix<Derived, -1, 1> x, p, r, Ap, Ar, pnew, Apnew;
  x.setZero(dim); Ap.setZero(dim); Ar.setZero(dim); pnew.setZero(dim);
  r = b;
  p = b;

  Derived rdotr = r.squaredNorm();
  Derived newrdotr=0;
  int i;
  for(i = 0; i < iterN && i < dim; i++){
    eval(p, Ap);
    Derived a = rdotr / p.dot(Ap);
    x += a * p;
    r -= a * Ap;
    newrdotr = r.dot(r);
    if (newrdotr < tol)
      break;
    Derived b = newrdotr/rdotr;
    p = r + b * p;
    rdotr = newrdotr;
  }
  sol = x;
  return newrdotr;
};

}
//}

#endif //RAI_CONJUGATEGRADIENT_HPP


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

using namespace std;
using namespace Eigen;

int main()
{
  VectorXf v(2);
  MatrixXf m(2,2), n(2,2);

  v << -1,
       2;

  m << 1,-2,
       -3,4;

  cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
  cout << "v.norm() = " << v.norm() << endl;
  cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
  cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;

  cout << endl;
  cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
  cout << "m.norm() = " << m.norm() << endl;
  cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
  cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
}


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief One step of autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z). Steps of size h.
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] z0 initial data z(0)
//! \param[in] h size of the step
//! \param[in] gamma parameter
//! \return next step z1
template <class StateType>
StateType sdirkStep(const StateType & z0, double h, double gamma) {
    // Matrix A for evaluation of f
    Eigen::Matrix2d A;
    A << 0.,1.,-1.,-1.;

    // Reuse factorization
    auto A_lu = (Eigen::Matrix2d::Identity()-h*gamma*A).partialPivLu();
    Eigen::Vector2d az = A*z0;

    // Increments
    Eigen::Vector2d k1 = A_lu.solve(az);
    Eigen::Vector2d k2 = A_lu.solve(az + h*(1-2*gamma)*A*k1);

    // Next step
    return z0 + h*0.5*(k1 + k2);
}

//! \brief Solve autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z), with N equidistant steps
//! \tparam StateType type of solution space z = [y,y']! and initial data z0 = [y(0), y'(0)]
//! \param[in] z0 initial data z(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time of simulation
//! \param[in] gamma parameter
//! \return vector containing each step of z_k (y and y')
template <class StateType>
std::vector<StateType> sdirkSolve(const StateType & z0, unsigned int N, double T, double gamma) {
    // Solution vector
    std::vector<StateType> res(N+1);
    // Equidistant step size
    const double h = T/N;

    // Push initial data
    res.at(0) = z0;

    // Main loop
    for(unsigned int i = 1; i <= N; ++i) {
        res.at(i) = sdirkStep(res.at(i-1), h, gamma);
    }

    return res;
}

int main() {
    // Initial data z0 = [y(0), y'(0)]
    Eigen::Vector2d z0;
    z0 << 1,0;
    // Final time
    const double T = 10;
    // Parameter
    const double gamma = (3.+std::sqrt(3.)) / 6.;
    // Mesh sizes
    std::vector<int> N = {20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240};
    // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
    auto yex = [&z0] (double t) {
        return 1./3.*std::exp(-t/2.) * ( 3.*z0(0) * std::cos( std::sqrt(3.)*t/2. ) +
        std::sqrt(3.)*z0(0) * std::sin( std::sqrt(3.)*t/2. ) +
        2.*std::sqrt(3.)*z0(1) * std::sin( std::sqrt(3.)*t/2. ) );
    };

    // Store old error for rate computation
    double errold = 0;
    std::cout << std::setw(15) << "n" << std::setw(15) << "maxerr" << std::setw(15) << "rate" << std::endl;
    // Loop over all meshes
    for(unsigned int i = 0; i < N.size(); ++i) {
        int n = N.at(i);
        // Get solution
        auto sol =  sdirkSolve(z0, n, T, gamma);
        // Compute error
        double err = std::abs(sol.back()(0) - yex(T));

        // I/O
        std::cout << std::setw(15) << n << std::setw(15) << err;
        if(i > 0) std::cout << std::setw(15) << std::log2(errold /err);
        std::cout << std::endl;

        // Store old error
        errold = err;
    }
}

Nummerical Methods for CSE/PS14/solutions_ps14/sdirk.cpp


//
// Created by Harold on 2021/4/11.
//

#ifndef M_MATH_M_FFT_EIGEN_HPP
#define M_MATH_M_FFT_EIGEN_HPP

#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/FFT>
#include <complex>

#ifdef _OPENMP
#include <omp.h>
#endif

#include "m_circshift_eigen.hpp"

namespace M_MATH {
    // 2D fft, requires fft shift
    template<typename Derived>
    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic>
    ForwardFFT(Eigen::MatrixBase<Derived> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<typename Derived::Scalar> fft;
        Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
        for (auto i = 0; i < rows; ++i) {
            Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, 1> tmp(cols);
            fft.fwd(tmp, mat.row(i).eval());
            res.row(i) = tmp;
        }
        for (auto i = 0; i < cols; ++i) {
            Eigen::Matrix<std::complex<typename Derived::Scalar>, 1, Eigen::Dynamic> tmp(rows);
            fft.fwd(tmp, res.col(i).eval());
            res.col(i) = tmp;
        }
        return res;
    }

    // 2D ifft, requires fft shift
    template<typename Scalar>
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
    BackwardFFT(Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<Scalar> fft;
        Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> tmpMat(rows, cols);
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
        for (auto i = 0; i < rows; ++i) {
            Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> tmp(cols);
            fft.inv(tmp, mat.row(i).eval());
            tmpMat.row(i) = tmp;
        }
        for (auto i = 0; i < cols; ++i) {
            Eigen::Matrix<std::complex<Scalar>, 1, Eigen::Dynamic> tmp(rows);
            fft.inv(tmp, tmpMat.col(i).eval());
            res.col(i) = tmp.real();
        }
        return res;
    }

    // OMP one is slower according to benchmark result (see bench/fft_bench.cpp)
    template<typename Derived>
    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic>
    ForwardFFT_OMP(Eigen::MatrixBase<Derived> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<typename Derived::Scalar> fft;
        Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
#pragma omp parallel sections
        {
#pragma omp section
            {
#pragma omp parallel for if (rows > 10) schedule(static)
                for (auto i = 0; i < rows; ++i) {
                    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, 1> tmp(cols);
                    fft.fwd(tmp, mat.row(i).eval());
                    res.row(i) = tmp;
                }
#pragma omp parallel for if (cols > 10) schedule(static)
                for (auto i = 0; i < cols; ++i) {
                    Eigen::Matrix<std::complex<typename Derived::Scalar>, 1, Eigen::Dynamic> tmp(rows);
                    fft.fwd(tmp, res.col(i).eval());
                    res.col(i) = tmp;
                }
            }
        }
        return res;
    }

    template <typename XprType, typename RowShift, typename ColShift>
    Eigen::CircShiftedView<XprType, RowShift, ColShift> circShift(Eigen::DenseBase<XprType>& x, RowShift r, ColShift c)
    {
        return Eigen::CircShiftedView<XprType, RowShift, ColShift>(x.derived(), r, c);
    }

    template <typename XprType>
    Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index> fftshift(Eigen::DenseBase<XprType>& x)
    {
        Eigen::Index rs = x.rows() / 2;
        Eigen::Index cs = x.cols() / 2;
        return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
    }

    template <typename XprType>
    Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index> ifftshift(Eigen::DenseBase<XprType>& x)
    {
        Eigen::Index rs = (x.rows() + 1) / 2;
        Eigen::Index cs = (x.cols() + 1) / 2;
        return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
    }
}

#endif //M_MATH_M_FFT_EIGEN_HPP


#pragma once
#include <Eigen/Core>

namespace mtao::linear_algebra {
    // the gram schmidt algorithm with a threshold for when 0-eigenvalues are found
    template <typename Derived>
        void gram_schmidt_in_place(Eigen::PlainObjectBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
            for(int j = 0; j < M.cols(); ++j) {
                auto u = M.col(j);
                Scalar n = u.squaredNorm();
                for(int k = 0; k < j; ++k) {
                    auto v = M.col(k);
                    u -= v.dot(u) * v;
                }
                if(u.squaredNorm() < eps * n) {
                    u.setZero();
                } else {
                    u.normalize();
                }
            }
        }
    template <typename Derived>
        auto gram_schmidt(const Eigen::MatrixBase<Derived>& M) {
            typename Derived::PlainMatrix O = M;
            gram_schmidt_in_place(O);
            return O;
        }
}


#pragma once

#include <iostream>		// For console i/o
#include <vector>		// For vectors, not including linear algebra
#include <random>		// For random number generation
#include <algorithm>	// For min/max functions

// Additional libraries that you will have to download.
// First is Eigen, which we use for linear algebra: http://eigen.tuxfamily.org/index.php?title=Main_Page
#include <Eigen/Dense>
// Second is Boost, which we use for ibeta_inv: https://www.boost.org/
#include <boost/math/special_functions/beta.hpp>

// Typically these shouldn't be in a .hpp file.
using namespace std;
using namespace Eigen;
using namespace boost::math;

// This function returns the inverse of Student's t CDF using the degrees of
// freedom in nu for the corresponding probabilities in p. That is, it is
// a C++ implementation of Matlab's tinv function: https://www.mathworks.com/help/stats/tinv.html
// To see how this was created, see the "quantile" block here: https://www.boost.org/doc/libs/1_58_0/libs/math/doc/html/math_toolkit/dist_ref/dists/students_t_dist.html
double tinv(double p, unsigned int nu) {
	double x = ibeta_inv((double)nu / 2.0, 0.5, 2.0 * min(p, 1.0 - p));
	return sign(p - 0.5) * sqrt((double)nu * (1.0 - x) / x);
}

// Get the sample standard deviation of the vector v (an Eigen::VectorXd)
double stddev(const VectorXd& v) {
	double mu = v.mean(), result = 0;			// Get the sample mean
	for (unsigned int i = 0; i < v.size(); i++)
		result += (v[i] - mu) * (v[i] - mu);	// Compute the sum of the squared differences between samples and the sample mean
	result = sqrt(result / (v.size() - 1.0));	// Get the sample variance by dividing by the number of samples minus one, and then the sample standard deviation by taking the square root.
	return result;								// Return the value that we've computed.
}

// Assuming v holds i.i.d. samples of a random variable, compute
// a (1-delta)-confidence upper bound on the expected value of the random
// variable using Student's t-test. That is:
// sampleMean + sampleStandardDeviation/sqrt(n) * tinv(1-delta, n-1),
// where n is the number of points in v.
//
// If numPoints is provided, then ttestUpperBound predicts what its output would be if it were to
// be run using a new vector, v, containing numPoints values sampled from the same distribution as
// the points in v.
double ttestUpperBound(const VectorXd& v, const double& delta, const int numPoints = -1) {
	unsigned int n = (numPoints == -1 ? (unsigned int)v.size() : (unsigned int)numPoints);	// Set n to be numPoints if it was provided, and the number of points in v otherwise.
	return v.mean() + (numPoints != -1 ? 2.0 : 1.0) * stddev(v) / sqrt((double)n) * tinv(1.0 - delta, n - 1u);
}

double ttestLowerBound(const VectorXd& v, const double& delta, const int numPoints = -1) {
  unsigned int n = (numPoints == -1 ? (unsigned int)v.size() : (unsigned int)numPoints);	// Set n to be numPoints if it was provided, and the number of points in v otherwise.
  return v.mean() - (numPoints != -1 ? 2.0 : 1.0) * stddev(v) / sqrt((double)n) * tinv(1.0 - delta, n - 1u);
}

/*
This function implements CMA-ES (http://en.wikipedia.org/wiki/CMA-ES). Return
value is the minimizer / maximizer. This code is written for brevity, not clarity.
See the link above for a description of what this code is doing.
*/
VectorXd CMAES(
	const VectorXd& initialMean,											// Starting point of the search
	const double& initialSigma,												// Initial standard deviation of the search around initialMean
	const unsigned int& numIterations,										// Number of iterations to run before stopping
	// f, below, is the function to be optimized. Its first argument is the solution, the middle arguments are variables required by f (listed below), and the last is a random number generator.
	double(*f)(const VectorXd& theta, const void* params[], mt19937_64& generator),
	const void* params[],													// Parrameters of f other than theta
	const bool& minimize,													// If true, we will try to minimize f. Otherwise we will try to maximize f
	mt19937_64& generator)													// The random number generator to use
{
	// Define all of the terms that we will use in the iterations
	unsigned int N = (unsigned int)initialMean.size(), lambda = 4 + (unsigned int)floor(3.0 * log(N)), hsig;
	double sigma = initialSigma, mu = lambda / 2.0, eigeneval = 0, chiN = pow(N, 0.5) * (1.0 - 1.0 / (4.0 * N) + 1.0 / (21.0 * N * N));
	VectorXd xmean = initialMean, weights((unsigned int)mu);
	for (unsigned int i = 0; i < (unsigned int)mu; i++)
		weights[i] = i + 1;
	weights = log(mu + 1.0 / 2.0) - weights.array().log();
	mu = floor(mu);
	weights = weights / weights.sum();
	double mueff = weights.sum() * weights.sum() / weights.dot(weights), cc = (4.0 + mueff / N) / (N + 4.0 + 2.0 * mueff / N), cs = (mueff + 2.0) / (N + mueff + 5.0), c1 = 2.0 / ((N + 1.3) * (N + 1.3) + mueff), cmu = min(1.0 - c1, 2.0 * (mueff - 2.0 + 1.0 / mueff) / ((N + 2.0) * (N + 2.0) + mueff)), damps = 1.0 + 2.0 * max(0.0, sqrt((mueff - 1.0) / (N + 1.0)) - 1.0) + cs;
	VectorXd pc = VectorXd::Zero(N), ps = VectorXd::Zero(N), D = VectorXd::Ones(N), DSquared = D, DInv = 1.0 / D.array(), xold, oneOverD;
	for (unsigned int i = 0; i < DSquared.size(); i++)
		DSquared[i] *= DSquared[i];
	MatrixXd B = MatrixXd::Identity(N, N), C = B * DSquared.asDiagonal() * B.transpose(), invsqrtC = B * DInv.asDiagonal() * B.transpose(), arx(N, (int)lambda), repmat(xmean.size(), (int)(mu + .1)), artmp, arxSubMatrix(N, (int)(mu + .1));
	vector<double> arfitness(lambda);
	vector<unsigned int> arindex(lambda);
	// Perform the iterations
	for (unsigned int counteval = 0; counteval < numIterations;) {
	        cout << "Sampling Population" << endl;
		// Sample the population
		for (unsigned int k = 0; k < lambda; k++) {
			normal_distribution<double> distribution(0, 1);
			VectorXd randomVector(N);
			for (unsigned int i = 0; i < N; i++)
				randomVector[i] = D[i] * distribution(generator);
			arx.col(k) = xmean + sigma * B * randomVector;
 		}
		// Evaluate the population
		vector<VectorXd> fInputs(lambda);
		for (unsigned int i = 0; i < lambda; i++) {
		        cout << "Evaluating Candidate " << counteval+i << "/" << numIterations << endl;
			fInputs[i] = arx.col(i);
                        cout << "Theta: " << fInputs[i].transpose() << endl;
			arfitness[i] = (minimize ? 1 : -1) * f(fInputs[i], params, generator);
		}
		// Update the population distribution
		counteval += lambda;
		xold = xmean;
		for (unsigned int i = 0; i < lambda; ++i)
			arindex[i] = i;
		std::sort(arindex.begin(), arindex.end(), [&arfitness](unsigned int i1, unsigned int i2) {return arfitness[i1] < arfitness[i2]; });
		for (unsigned int col = 0; col < (unsigned int)mu; col++)
			arxSubMatrix.col(col) = arx.col(arindex[col]);
		xmean = arxSubMatrix * weights;
		ps = (1.0 - cs) * ps + sqrt(cs * (2.0 - cs) * mueff) * invsqrtC * (xmean - xold) / sigma;
		hsig = (ps.norm() / sqrt(1.0 - pow(1.0 - cs, 2.0 * counteval / lambda)) / (double)chiN < 1.4 + 2.0 / (N + 1.0) ? 1 : 0);
		pc = (1 - cc) * pc + hsig * sqrt(cc * (2 - cc) * mueff) * (xmean - xold) / sigma;
		for (unsigned int i = 0; i < repmat.cols(); i++)
			repmat.col(i) = xold;
		artmp = (1.0 / sigma) * (arxSubMatrix - repmat);
		C = (1 - c1 - cmu) * C + c1 * (pc * pc.transpose() + (1u - hsig) * cc * (2 - cc) * C) + cmu * artmp * weights.asDiagonal() * artmp.transpose();
		sigma = sigma * exp((cs / damps) * (ps.norm() / (double)chiN - 1.0));
		if ((double)counteval - eigeneval > (double)lambda / (c1 + cmu) / (double)N / 10.0) {
			eigeneval = counteval;
			for (unsigned int r = 0; r < C.rows(); r++)
				for (unsigned int c = r + 1; c < C.cols(); c++)
					C(r, c) = C(c, r);
			EigenSolver<MatrixXd> es(C);
			D = C.eigenvalues().real();
			B = es.eigenvectors().real();
			D = D.array().sqrt();
			for (unsigned int i = 0; i < B.cols(); i++)
				B.col(i) = B.col(i).normalized();
			oneOverD = 1.0 / D.array();
			invsqrtC = B * oneOverD.asDiagonal() * B.transpose();
		}
	} // End loop over iterations
	return arx.col(arindex[0]);
}


/* This sample is based on the Chapter 1 from book
 * "Building Machine Learning Systems with Python" by Willi Richert
 */

// third party includes
#include <csv.h>
#include <plot.h>
#include <Eigen/Dense>

// stl includes
#include <algorithm>
#include <experimental/filesystem>
#include <iostream>
#include <random>
#include <string>

// application includes
#include "../ioutils.h"
#include "../utils.h"

// Namespace and type aliases
namespace fs = std::experimental::filesystem;
typedef double DType;
using Matrix = Eigen::Matrix<DType, Eigen::Dynamic, Eigen::Dynamic>;

auto standardize(const Matrix& v) {
  assert(v.cols() == 1);
  auto m = v.colwise().mean();
  auto n = v.rows();
  DType sd = std::sqrt((v.rowwise() - m).array().pow(2).sum() /
                       static_cast<DType>(n - 1));
  Matrix sv = (v.rowwise() - m) / sd;
  return std::make_tuple(sv, m(0, 0), sd);
}

auto generate_polynomial(const Matrix& x, size_t degree) {
  assert(x.cols() == 1);
  auto rows = x.rows();

  Matrix poly_x = Matrix::Zero(rows, degree);
  // fill additional column for simpler vectorization
  {
    auto xv = poly_x.block(0, 0, rows, 1);
    xv.setOnes();
  }
  // copy initial data
  {
    auto xv = poly_x.block(0, 1, rows, 1);
    xv = x;
  }
  // generate additional terms
  for (size_t i = 2; i < degree; ++i) {
    auto xv = poly_x.block(0, i, rows, 1);
    xv = x.array().pow(static_cast<DType>(i));
  }
  return poly_x;
}

auto bgd(const Matrix& x, const Matrix& y) {
  size_t batch_size = 8;
  size_t n_epochs = 1000;
  DType lr = 0.0015;

  auto rows = x.rows();
  auto cols = x.cols();

  size_t batches = rows / batch_size;  // some samples will be skipped
  Matrix b = Matrix::Zero(cols, 1);

  DType prev_cost = std::numeric_limits<DType>::max();
  for (size_t i = 0; i < n_epochs; ++i) {
    for (size_t bi = 0; bi < batches; ++bi) {
      auto s = bi * batch_size;
      auto batch_x = x.block(s, 0, batch_size, cols);
      auto batch_y = y.block(s, 0, batch_size, 1);

      auto yhat = batch_x * b;
      auto error = yhat - batch_y;

      auto grad =
          (batch_x.transpose() * error) / static_cast<DType>(batch_size);

      b = b - lr * grad;
    }

    DType cost = (y - x * b).array().pow(2.f).sum() / static_cast<DType>(rows);

    std::cout << "BGD iteration : " << i << " Cost = " << cost << std::endl;
    if (cost <= prev_cost)
      prev_cost = cost;
    else
      break;  // early stopping
  }
  return b;
}

int main() {
  // Download the data
  const std::string data_path{"web_traffic.tsv"};
  if (!fs::exists(data_path)) {
    const std::string data_url{
        R"(https://raw.githubusercontent.com/luispedro/BuildingMachineLearningSystemsWithPython/master/ch01/data/web_traffic.tsv)"};
    if (!utils::DownloadFile(data_url, data_path)) {
      std::cerr << "Unable to download the file " << data_url << std::endl;
      return 1;
    }
  }

  // Read the data
  io::CSVReader<2, io::trim_chars<' '>, io::no_quote_escape<'\t'>> data_tsv(
      data_path);

  std::vector<DType> raw_data_x;
  std::vector<DType> raw_data_y;

  bool done = false;
  do {
    try {
      DType x = 0, y = 0;
      done = !data_tsv.read_row(x, y);
      if (!done) {
        raw_data_x.push_back(x);
        raw_data_y.push_back(y);
      }
    } catch (const io::error::no_digit& err) {
      // ignore bad formated samples
      std::cout << err.what() << std::endl;
    }
  } while (!done);

  // shuffle data
  size_t seed = 3465467546;
  std::shuffle(raw_data_x.begin(), raw_data_x.end(),
               std::default_random_engine(seed));
  std::shuffle(raw_data_y.begin(), raw_data_y.end(),
               std::default_random_engine(seed));

  // map data to the tensor
  size_t rows = raw_data_x.size();
  const auto data_x = Eigen::Map<Matrix>(raw_data_x.data(), rows, 1);
  std::cout << "X shape " << data_x.rows() << ":" << data_x.cols() << std::endl;
  Matrix x;
  std::tie(x, std::ignore, std::ignore) = standardize(data_x);

  const auto data_y = Eigen::Map<Matrix>(raw_data_y.data(), rows, 1);
  std::cout << "Y shape " << data_y.rows() << ":" << data_y.cols() << std::endl;
  // I'm not  using structured binding, because hard to debug such values
  Matrix y;
  DType ym{0};
  DType ysd{0};
  std::tie(y, ym, ysd) = standardize(data_y);

  // Scale data to prevent float overflow in BGD
  size_t p_degree = 64;
  const DType scale = 0.6;
  x *= scale;
  y *= scale;

  // solve normal equation
  Matrix poly_x = generate_polynomial(x, p_degree);
  Matrix b_eq =
      (poly_x.transpose() * poly_x).ldlt().solve(poly_x.transpose() * y);
  auto cost =
      (y - poly_x * b_eq).array().pow(2.f).sum() / static_cast<DType>(rows);
  std::cout << "cost for normal equation solution : " << cost << std::endl;

  // optimize with BGD
  Matrix b = bgd(poly_x, y);

  // generate new data
  const size_t new_x_size = 500;
  std::vector<DType> x_coord(new_x_size);
  auto new_x = Eigen::Map<Matrix>(x_coord.data(), new_x_size, 1);
  new_x = Eigen::Matrix<DType, Eigen::Dynamic, 1>::LinSpaced(
      new_x_size, data_x.minCoeff(), data_x.maxCoeff());
  Matrix new_x_std;
  std::tie(new_x_std, std::ignore, std::ignore) = standardize(new_x);
  new_x_std *= scale;
  Matrix new_poly_x = generate_polynomial(new_x_std, p_degree);

  // make predictions
  std::vector<DType> polyline_eq(new_x_size);
  auto new_y_eq = Eigen::Map<Matrix>(polyline_eq.data(), new_x_size, 1);
  new_y_eq = new_poly_x * b_eq;

  std::vector<DType> polyline(new_x_size);
  auto new_y = Eigen::Map<Matrix>(polyline.data(), new_x_size, 1);
  new_y = new_poly_x * b;

  // restore scalenew_y
  new_y_eq /= scale;
  new_y_eq = (new_y_eq * ysd).array() + ym;

  new_y /= scale;
  new_y = (new_y * ysd).array() + ym;

  // plot the data we read and approximate
  plotcpp::Plot plt(true);
  plt.SetTerminal("qt");
  plt.SetTitle("Web traffic over the last month");
  plt.SetXLabel("Time");
  plt.SetYLabel("Hits/hour");
  plt.SetAutoscale();
  plt.GnuplotCommand("set grid");

  auto time_range = data_x.maxCoeff() - data_x.minCoeff();
  auto tic_size = 7 * 24;
  auto time_tics = time_range / tic_size;
  plt.SetXRange(-tic_size / 2, data_x.maxCoeff() + tic_size / 2);

  plotcpp::Plot::Tics xtics;
  for (size_t t = 0; t < time_tics; ++t) {
    xtics.push_back({"week " + std::to_string(t), t * tic_size});
  }
  plt.SetXTics(xtics);

  plt.Draw2D(plotcpp::Points(raw_data_x.begin(), raw_data_x.end(),
                             raw_data_y.begin(), "data", "lc rgb 'black' pt 1"),
             plotcpp::Lines(x_coord.begin(), x_coord.end(), polyline_eq.begin(),
                            "neq approx", "lc rgb 'red' lw 2"),
             plotcpp::Lines(x_coord.begin(), x_coord.end(), polyline.begin(),
                            "bgd approx", "lc rgb 'cyan' lw 2"));
  plt.Flush();

  return 0;
}


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

// computes the center of a circle from 3 points on its boundary
Eigen::Vector2d circleCenter(const Eigen::Vector2d &P1, const Eigen::Vector2d &P2, const Eigen::Vector2d &P3)
{
	Eigen::Matrix<double,3,2> mat;
	mat.block<1,2>(0,0) = (P2 - P1).transpose();
	mat.block<1,2>(1,0) = (P3 - P2).transpose();
	mat.block<1,2>(2,0) = (P1 - P3).transpose();

	Eigen::Vector3d vec;
	vec.x() = 0.5*(P2 - P1).squaredNorm() + (P2 - P1).dot(P1);
	vec.y() = 0.5*(P3 - P2).squaredNorm() + (P3 - P2).dot(P2);
	vec.z() = 0.5*(P1 - P3).squaredNorm() + (P1 - P3).dot(P3);

	return mat.colPivHouseholderQr().solve(vec);
}

int main(int argc, char** argv)
{
	double t = 0.21111111111111111111111111111111111111111111111111111111111;
	Eigen::Vector2d P1( 0.0, 0.0),
					P2( 1.0, 0.0),
					P3( 1.0, 1.0),
					P4( 0.0, 1.0);
	Eigen::Matrix2d mat;
	mat <<  1.0, 0.0,
		    0.0, 1.0;
	Eigen::Vector2d tr(0.8,0.6);
	P1 = t*mat*P1 + tr;
	P2 = t*mat*P2 + tr;
	P3 = t*mat*P3 + tr;
	P4 = t*mat*P4 + tr;

	Eigen::Vector2d C = circleCenter(P1,P2,P3);

	double d1 = (C - P1).norm(),
		   d2 = (C - P2).norm(),
		   d3 = (C - P3).norm(),
		   d4 = (C - P4).norm();

	std::cout.precision(100);
	std::cout << "d1 " << d1 << std::endl;
	std::cout << "d2 " << d2 << std::endl;
	std::cout << "d3 " << d3 << std::endl;
	std::cout << "d4 " << d4 << std::endl;

	std::cout << std::endl;
	std::cout << "Test d4 == d1" << std::endl;
	if(d4 == d1)
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= 6.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= 6.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= 7.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= 7.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test (float)d4 == (float)d1" << std::endl;
	if((float)d4 == (float)d1)
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::abs(d1+1.0) << " " << abs(d1+1.0) << std::endl;

	return 0;
}


#include <iostream>

#include <Eigen/Dense>

/* @brief
 * @param[in] z An $n$-dimensional vector containing one side of input data
 * @param[in] c An $n$-dimensional vector containing the other side of input data
 * @param[out] x The vector of parameters $(\alpha,\beta)$, intercept and slope of the line fitted
 */
Eigen::Vector2d lsqEst(const Eigen::VectorXd &z, const Eigen::VectorXd &c) {
	Eigen::Vector2d x;
	int n = z.size();

	Eigen::MatrixXd A(n, 2);
	A.col(0) = z;
	A(0, 1) = z(1);
	A(n - 1, 1) = z(n - 2);

	for(int i = 1; i < n - 1; i++) {
		A(i, 1) = z(i - 1) + z(i + 1);
	}

	x = (A.transpose() * A).llt().solve(A.transpose() * c);

	return x;
}

int main() {
    int n = 10;
    Eigen::VectorXd z(n), c(n);
    for(int i = 0; i < n; ++i) {
		z(i) = i + 1;
		c(i) = n - i;
	}

	Eigen::Vector2d x = lsqEst(z, c);

	std::cout << "alpha = " << x(0) << std::endl;
	std::cout << "beta = "  << x(1) << std::endl;

	return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CBRT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing cbrt capabilities

    Compute the cubic root: \f$\sqrt[3]{x}\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = cbrt(x);
    @endcode

    is similar to:

    @code
    T r = pow(x, T(1/3.0));
    @endcode

    but not equivalent because pow cannot raise a negative base to a fractional exponent.
    we have for all non Nan floating values cbrt(-x) = -cbrt(x).

    @par Decorators

    std_ for floating entries

    @see pow, sqrt

  **/
  Value cbrt(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/cbrt.hpp>
#include <boost/simd/function/simd/cbrt.hpp>

#endif


/////////////
///
///    Constants.h
///         Constant miscellanea!!
///
////////

#ifndef ____CONSTANTS__
#define ____CONSTANTS__

#include <iostream>
#include <iomanip>
#include <math.h>
#include <string>
#include <map>
#include <fstream>
#include <vector>
#include <array>
#include <complex>
#include <chrono>
#include <ctime>
#define ARMA_DONT_USE_WRAPPER
// Take care with this macro: Use wisely!!!
#define ARMA_NO_DEBUG
#include <armadillo>

typedef std::complex<double> dcomplex;
/* typedef std::vector<std::vector<double>> matrix; */
/* typedef std::vector<std::vector<dcomplex>> cmatrix; */

using namespace std;
//using namespace arma;
//cout.precision(15);

/// Complex numbers
const dcomplex Im {0.0,1.0};
const dcomplex One {1.0,0.0};
const dcomplex Zero {0.0,0.0};

/// pi
const double pi = 4.0*atan(1.0);
const double twopi = 2.0 * pi;
const double one_over_pi = 1.0 / pi;

/// Square root two
const double sqrt_two = sqrt(2.0);
const double one_over_sqrt_two = 1.0 / sqrt_two;

/// Speed of light, in a.u. units
const double speed_light = 137.0;
const double one_over_speed_light = 1.0 / speed_light;
// Speed of light in meters over seconds
const double speed_light_is = 2.99792458e8;

/// electron charge
const double charge_electron_au = -1.;
/// Proton mass
const double mass_proton_au = 1836.;
/// Bohr radius (IS)
const double bohr_radius  = 5.29177211e-11;
const double aulength_nm  = 5.29177211e-2;
const double hatree_to_eV = 27.211;
// au time
const double autime_fs = 2.418884326505e-2;
const double autime_s  = 2.418884326505e-17;
// au intensity
const double auintensity = 3.5e16;

#endif  // ____constants__ //