// Solution: https://www.techiedelight.com/probability-alive-after-taking-n-steps-island/

#include <iostream>
#include <unordered_map>
#include <unordered_set>
#include <map>
#include <set>
#include <string>
//#include <boost/functional/hash.hpp>

using namespace std;

#define N 3

// Find the probability that person is alive after he walks n steps
// from location (x, y) on the island
double aliveProbability( int x, int y, int steps_left, unordered_map<string, double>& dp )
{
    // base case
    if ( steps_left == 0 )
        return 1.0;

    // calculate unique map key from current coordinates(x, y) of person
    // and number of steps(n) left
    string key = to_string( x ) + "|" + to_string( y ) + "|" + to_string( steps_left );
    //"0|0|4" -> [0;1]

    // if sub-problem is seen for the first time
    if ( dp.find( key ) == dp.end() )
    {
        double p = 0.0;

        // move one step up
        if ( y > 0 )
            p += 0.25 * aliveProbability( x, y - 1, steps_left - 1, dp );

        // move one step down
        if ( y < N - 1 )
            p += 0.25 * aliveProbability( x, y + 1, steps_left - 1, dp );

        // move one step left
        if ( x > 0 )
            p += 0.25 * aliveProbability( x - 1, y, steps_left - 1, dp );

        // move one step right
        if ( x < N - 1 )
            p += 0.25 * aliveProbability( x + 1, y, steps_left - 1, dp );

        dp[ key ] = p;
    }

    return dp[ key ];
}

struct Cell
{
    int x, y, n;
};

bool operator==( const Cell& lhs, const Cell& rhs )
{
    return lhs.x == rhs.x && lhs.y == rhs.y && lhs.n == rhs.n;
}

struct MyHash
{
    std::size_t operator()( const Cell& cell ) const noexcept
    {
        std::cout << "Using MyHash" << std::endl;
        std::hash<int> XHash;
        std::size_t h1 = XHash(cell.x);
        std::size_t h2 = std::hash<int>{}(cell.y);
        std::size_t h3 = std::hash<int>{}(cell.n);
        return h1 ^ (h2 << 1) ^ (h3 << 2); // or use boost::hash_combine
    }
};

// custom specialization of std::hash can be injected in namespace std
namespace std
{
    template<>
    struct hash<Cell>
    {
        std::size_t operator()( const Cell& cell) const noexcept
        {
            std::size_t h1 = std::hash<int>{}(cell.x);
            std::size_t h2 = std::hash<int>{}(cell.y);
            std::size_t h3 = std::hash<int>{}(cell.n);
            return h1 ^ (h2 << 1) ^ (h3 << 2); // or use boost::hash_combine

            //size_t seed = 0;
            //hash_combine( seed, cell.x );
            //hash_combine( seed, cell.y );
            //hash_combine( seed, cell.steps_left );
            //return seed;
        }
    };
}

struct Cell1
{
    int x, y, n;

    bool operator==( const Cell& rhs )
    {
        return x == rhs.x && y == rhs.y && n == rhs.n;
    }

    struct Hash
    {
        size_t operator()( const Cell& cell ) const noexcept
        {
            std::hash<int> XHash;
            std::size_t h1 = XHash( cell.x );
            std::size_t h2 = std::hash<int> {}( cell.y );
            std::size_t h3 = std::hash<int> {}( cell.n );
            return h1 ^ ( h2 << 1 ) ^ ( h3 << 2 ); // or use boost::hash_combine
        }
    };
};

double aliveProbability( int x, int y, int n, unordered_map<Cell1, double, Cell1::Hash>& dp )
{
    // base case
    if ( n == 0 )
        return 1.0;

    // calculate unique map key from current coordinates(x, y) of person
    // and number of steps(n) left
    Cell1 key = { x, y, n };

    // if sub-problem is seen for the first time
    if ( dp.find( key ) == dp.end() )
    {
        double p = 0.0;

        // move one step up
        if ( y > 0 )
            p += 0.25 * aliveProbability( x, y - 1, n - 1, dp );

        // move one step down
        if ( y < N - 1 )
            p += 0.25 * aliveProbability( x, y + 1, n - 1, dp );

        // move one step left
        if ( x > 0 )
            p += 0.25 * aliveProbability( x - 1, y, n - 1, dp );

        // move one step right
        if ( x < N - 1 )
            p += 0.25 * aliveProbability( x + 1, y, n - 1, dp );

        dp[ key ] = p;
    }

    return dp[ key ];
}

int main()
{
    int n = 4;        // number of steps to be taken
    int x = 0, y = 0; // starting coordinates

    // map to store solution to already computed sub-problems
    //unordered_map<string, double> dp; // with key 'string'
    unordered_map<Cell1, double, Cell1::Hash> dp; // with key 'Cell1' and hash function Cell1::Hash
    //unordered_map<Cell, double> dp; // with key 'Cell' and hash function that is injected in namespace std

    // calculate alive Probability
    cout << "Alive probability is " << aliveProbability(x, y, n, dp);

    return 0;
}


/**
 * @file finitevolumesineconslaw.cc
 * @brief NPDE homework "FiniteVolumeSineConsLaw" code
 * @author Oliver Rietmann
 * @date 25.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "finitevolumesineconslaw.h"

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

namespace FiniteVolumeSineConsLaw {

/* SAM_LISTING_BEGIN_1 */
constexpr double PI = 3.14159265358979323846;

double f(double x) { return std::sin(PI * x); }

double sineGodFlux(double v, double w) {
  double result;
  // Rankine-Hugoniot speed. No safeguards against cancellation are taken. For v
  // close to w the result will severely be affected by amplified round-off
  // error. However, this does not do any harm, because we are interested in the
  // the sign alone.
  double s = (v != w) ? (f(w) - f(v)) / (w - v) : PI * std::cos(PI * v);
  // Treat all different cases separately
  if (((v < w) && (s > 0.0)) || ((v >= w) && (std::cos(PI * v) > 0.0))) {
    result = f(v);
  } else if (((v < w) && (s < 0.0)) || ((v >= w) && (std::cos(PI * w) < 0.0))) {
    result = f(w);
  } else
    result = f(0.5);

  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd sineClawRhs(const Eigen::VectorXd &mu) {
  int N = mu.size();
  double h = 12.0 / N;
  Eigen::VectorXd result(N);

  double F_minus;
  double F_plus = sineGodFlux(0.0, mu(0));
  for (int j = 0; j < N - 1; ++j) {
    F_minus = F_plus;
    F_plus = sineGodFlux(mu(j), mu(j + 1));
    result(j) = -1.0 / h * (F_plus - F_minus);
  }
  F_minus = F_plus;
  F_plus = sineGodFlux(mu(N - 1), 0.0);
  result(N - 1) = -1.0 / h * (F_plus - F_minus);

  return result;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
bool blowup(const Eigen::VectorXd &mu) {
  return mu.minCoeff() < 0.0 || mu.maxCoeff() > 2.0;
}

unsigned int findTimesteps() {
  const unsigned int N = 600;
  unsigned int ML = 100;
  unsigned int MR = 200;

  for (int i = 0; i < 100; ++i) {
    if (ML == MR) {
      return MR;
    }
    unsigned int M = (unsigned int)(0.5 * (ML + MR));
    if (blowup(solveSineConsLaw(&sineClawRhs, N, M))) {
      ML = M + 1;
    } else {
      MR = M;
    }
  }

  return MR;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd sineClawReactionRhs(const Eigen::VectorXd &mu, double c) {
  Eigen::VectorXd rhs(mu.size());
  rhs = sineClawRhs(mu) - c * mu;
  return rhs;
}
/* SAM_LISTING_END_4 */

}  // namespace FiniteVolumeSineConsLaw


#include <iostream>
#include <fstream>
#include <random>

#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

float normal_pdf(float x, float m, float s) {
    static const float inv_sqrt_2pi = 0.3989422804014327;
    float a = (x - m) / s;

    return (inv_sqrt_2pi / s) * std::exp(-0.5f * a * a);
}

void solve_1D_wave_equation(double mu, double sigma, string output_filepath) {
    IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
    ofstream outfile(output_filepath);

    /* Problem parameters */
    double c = 1.0;  // Propagation speed of the wave.
    double L = 1.0;  // Length of the domain.
    int N = 100;     // Number of grid points.

    double dx = L/N;
    double dt = 0.01;
    double t_end = 1.0;

    double alpha = c*c * dt*dt / (2*dx*dx);

    /* We'll discretize the wave equation using the Crank-Nicolson method and
     * solve the resulting linear system A*u = b at every time step.
     */
    MatrixXf A(N,N);
    VectorXf b(N);
    VectorXf u_n(N);
    VectorXf u_nm1(N);

    // Initialize matrix of coefficients.
    A(0,0)     = 1 + 2*alpha;
    A(N-1,N-1) = 1 + 2*alpha;
    A(0,1)     = -alpha;
    A(N-1,N-2) = -alpha;

    for (int i = 1; i < N-1; i++) {
        A(i,i) = 1 + 2*alpha;
        A(i,i+1) = -alpha;
        A(i,i-1) = -alpha;
    }

    // Set initial conditions.
    u_nm1(0)   = 0;
    u_nm1(N-1) = 0;
    for (int i = 1; i < N-1; i++)
        u_nm1(i) = normal_pdf(i*dx, mu, sigma);

    // Output first row corresponding to initial condition (t = 0)
    outfile << u_nm1.format(CommaInitFmt) << '\n';

    // Take the first time step (requires special scheme).
    u_n(0)   = 0;
    u_n(N-1) = 0;

    for (int i = 1; i < N-1; i++)
        u_n(i) = u_nm1(i) + (c*c/2) * (u_nm1(i+1) - 2*u_nm1(i) + u_nm1(i-1));

    // Output second row corresponding to first time step.
    outfile << u_n.format(CommaInitFmt) << '\n';

    double t = dt; // We already took one step so t = dt now.

    while (t < t_end) {
        t += dt;

        // Set up right-hand side vector b.
        b(0)   = 2*(1-alpha)*u_n(0) - u_nm1(0) + alpha*u_n(1);
        b(N-1) = 2*(1-alpha)*u_n(N-1) - u_nm1(N-1) + alpha*u_n(N-1);
        for (int i = 1; i < N-1; i++)
            b(i) = 2*(1-alpha)*u_n(i) - u_nm1(i) + alpha*(u_n(i+1) + u_n(i-1));

        u_nm1 = u_n;

        VectorXf u_np1 = A.colPivHouseholderQr().solve(b);

        u_np1(0)   = 0;
        u_np1(N-1) = 0;

        outfile << u_np1.format(CommaInitFmt) << '\n';

        u_n = u_np1;
    }

    outfile.close();
}

void solve_1D_wave_equations(int M, float *mu, float *sigma) {
    # pragma acc kernels
    {
    for (int i = 0; i < M; i++) {
        string file_suffix = to_string(i);
        file_suffix.insert(file_suffix.begin(), 3 - file_suffix.length(), '0');
        string filename = "cpu_wave_" + file_suffix + ".dat";

        cout << "Solving wave equation problem " << i << "..." << " (mu=" << mu[i] << ", sigma="
             << sigma[i] << ")\n";

        solve_1D_wave_equation(mu[i], sigma[i], filename);
    }
    }
}

int main() {
    int M = 25;  // Number of 1D wave equation problems to solve.

    std::random_device rd;  // Obtain a random number generator from hardware.
    std::mt19937 mt(rd()); // Seed the Mersenne Twister generator.

    /* We will impose a Gaussian wave initial condition for the 1D wave equation, with randomly
     * generated means (0.2 < mu < 0.8) and standard deviations (0.01 < sigma < 0.5).
     */
    std::uniform_real_distribution<float> uniform_mu(0.2, 0.8);
    std::uniform_real_distribution<float> uniform_sigma(0.01, 0.5);

    float *mu = new float[M];
    float *sigma = new float[M];

    for(int i = 0; i < M; i++) {
        mu[i] = uniform_mu(mt);
        sigma[i] = uniform_sigma(mt);
    }

    // Solve the M problems one-by-one on the CPU.
    solve_1D_wave_equations(M, mu, sigma);

    delete [] mu;
    delete [] sigma;
}


//  Solves linear equations for simple tridiagonal matrix using the iterative Jacobi method
//  This is armadillo version that calls the function solve.
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <string>
#include <armadillo>
// use namespace for output and input
using namespace std;
using namespace arma;

// object for output files
ofstream ofile;
// Functions used
double f(double x){return 100.0*exp(-10.0*x);
}
double exact(double x) {return 1.0-(1-exp(-10))*x-exp(-10*x);}

// Begin main program
int main(int argc, char *argv[]){
  int exponent;
    string filename;
    // We read also the basic name for the output file and the highest power of 10^n we want
    if( argc <= 1 ){
          cout << "Bad Usage: " << argv[0] <<
              " read also file name on same line and max power 10^n" << endl;
          exit(1);
    }
        else{
        filename = argv[1]; // first command line argument after name of program
        exponent = atoi(argv[2]);
    }
    // Loop over powers of 10
    for (int i = 1; i <= exponent; i++){
      int  n = (int) pow(10.0,i);
      // Declare new file name
      string fileout = filename;
      // Convert the power 10^i to a string
      string argument = to_string(i);
      // Final filename as filename-i-
      fileout.append(argument);
      double h = 1.0/(n);
      double hh = h*h;
      n = n-1;  //  shift so that only points between endpoints are studied
      mat A = zeros<mat>(n,n);
      // Set up arrays for the simple case
      vec b(n);  vec x(n);
      A(0,1) = -1;  x(0) = h;  b(0) =  hh*f(x(0));
      x(n-1) = x(0)+(n-1)*h; b(n-1) = hh*f(x(n-1));
      for (int i = 1; i < n-1; i++){
        x(i) = x(i-1)+h;
	b(i) = hh*f(x(i));
        A(i,i-1)  = -1.0;
        A(i,i+1)  = -1.0;
      }
      A(n-2,n-1) = -1.0; A(n-1,n-2) = -1.0;
  // solve Ax = b by iteration with a random starting vector
     int maxiter = 100; double diff = 1.0;
     double epsilon = 1.0e-10;  int iter = 0;
      vec SolutionOld  = randu<vec>(n);
      vec SolutionNew  = zeros<vec>(n);
      //  Start of Jacobi solver, note the division by from A(i,i) =2
      while (iter <= maxiter || diff > epsilon){
	SolutionNew = (b -A*SolutionOld)*0.5;
        iter++; diff = fabs(sum(SolutionNew-SolutionOld)/n);
        SolutionOld = SolutionNew;
      }
      vec solution = SolutionOld;
      ofile.open(fileout);
      ofile << setiosflags(ios::showpoint | ios::uppercase);
      for (int i = 0; i < n ;i++) {
	double RelativeError = fabs((exact(x(i))-solution(i))/exact(x(i)));
	ofile << setw(15) << setprecision(8) << x(i);
	ofile << setw(15) << setprecision(8) << solution(i);
	ofile << setw(15) << setprecision(8) << exact(x(i));
        ofile << setw(15) << setprecision(8) << log10(RelativeError) << endl;
      }
      ofile.close();
    }
    return 0;
}


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

using namespace std;
using namespace Eigen;

/*
Test Example: f(v)=x^2+(x+y-10)^2+(2*y-z)^2+2*z;
Approximate Hessian Matrix using SR1.
*/

double f(Vector3d v)
{
	double x = v(0);
	double y = v(1);
	double z = v(2);
	double res;

	res = x * x + (x + y - 10)*(x + y - 10) + (2 * y - z)*(2 * y - z) + 2 * z;

	return res;
}

Vector3d J(Vector3d v)
{
	Vector3d J;
	double x = v(0);
	double y = v(1);
	double z = v(2);

	double dx = 2 * x + 2 * (x + y - 10);
	double dy = 2 * (x + y - 10) + 2 * (2 * y - z) * 2;
	double dz = 2 * (z - 2 * y) + 2;

	J(0) = dx; J(1) = dy; J(2) = dz;
	return J;
}

bool PositiveDefinite(Matrix3d G)
{
	Vector3cd E = G.eigenvalues();

	bool res = E(0).real() > 0 && E(1).real() > 0 && E(2).real() > 0;

	return res;
}

int main()
{
	Vector3d v(100, 100, 100);
	double u = 0.25;
	Matrix3d G; G.setIdentity();
	Matrix3d I; I.setIdentity();

	while (1)
	{
		Vector3d g = J(v);
		if (g.norm() < 1e-6) break;
		while (!PositiveDefinite(G + u * I))
		{
			u *= 4;
		}
		Matrix3d K = G + u * I;
		Vector3d s = -K.inverse()*g;

		double fn = f(v + s);
		double qk = f(v) + g.transpose()*s + 0.5*s.transpose()*G*s;
		double rk = (f(v) - fn) / (f(v)-qk);

		if (rk < 0.25) u *= 4.0;
		else if (rk > 0.75) u /= 2.0;

		if (rk > 0)
		{
			v = v + s;
			Vector3d sk = -s;
			Vector3d yk = g-J(v);
			double den = (sk - G * yk).transpose()*yk;
			G = G + (sk - G * yk)*(sk - G * yk).transpose() / den;
		}
	}

	cout << v.transpose() << endl;
	cout << f(v) << endl;

}


#include "ode45.hpp"

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>
#include <Eigen/QR>

//! \file stabrk.cpp Solution for Problem 1, PS13, involving ode45 and matrix ODEs

//! \brief Solve matrix IVP Y' = -(Y-Y')*Y using ode45 up to time T
//! \param[in] Y0 Initial data Y(0) (as matrix)
//! \param[in] T final time of simulation
//! \return Matrix of solution of IVP at t = T
Eigen::MatrixXd matode(const Eigen::MatrixXd & Y0, double T) {
	auto f=[](const Eigen::MatrixXd & Y){return (-(Y-Y.transpose())*Y);};
	ode45<Eigen::MatrixXd> O(f);
    O.options.rtol = 10e-8;
    O.options.atol = 10e-10;
    return O.solve(Y0,T).back().first;
    // TODO: evolve Y0 up to T using ode45 (read ode45.hpp)
}

//! \brief Find if invariant is preserved after evolution with matode
//! \param[in] Y0 Initial data Y(0) (as matrix)
//! \param[in] T final time of simulation
//! \return true if invariant was preserved (up to round-off), i.e. if norm was less than 10*eps
bool checkinvariant(const Eigen::MatrixXd & M, double T) {
	return (matode(M,0.1).norm()==matode(M,T).norm());
		// TODO: check if invariant is preserved applying matode
}

//! \brief Implement ONE step of explicit Euler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd expeulstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    return Y0+h*A*Y0;
    // TODO: ose step of EE
}

//! \brief Implement ONE step of implicit Euler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd impeulstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    size_t n = Y0.cols();
    Eigen::MatrixXd Y1(n,n);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n,n);
    return (I-h*A).partialPivLu().inverse()*Y0;
    // TODO: ose step of IE
}

//! \brief Implement ONE step of implicit midpoint ruler applied to Y0, of ODE Y' = A*Y
//! \param[in] A matrix A of the ODE
//! \param[in] Y0 Initial state
//! \param[in] h step size
//! \return next step
Eigen::MatrixXd impstep(const Eigen::MatrixXd & A, const Eigen::MatrixXd & Y0, double h) {
    size_t n = Y0.cols();
    Eigen::MatrixXd Y1(n,n);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n,n);
    return (I-h*0.5*A).partialPivLu().inverse()*(I+h*0.5*A)*Y0;
    // TODO: ose step of IMP
}

int main() {

    double T = 1;
    unsigned int n = 3;

    Eigen::MatrixXd M(n,n);
    M << 8,1,6,3,5,7,4,9,2;

    std::cout << "SUBTASK 1. c)" << std::endl;
    // Test preservation of orthogonality

    // Build Q
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(M.rows(), M.cols());
    qr.compute(M);
    Eigen::MatrixXd Q = qr.householderQ();

    // Build A
    Eigen::MatrixXd A(n,n);
    A << 0, 1, 1, -1, 0, 1, -1, -1, 0;
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n,n);

    // TODO: compute norm of Y'Y-I for 20 steps and print table
    Eigen::MatrixXd EE(n,n);
    EE=Q;
    Eigen::MatrixXd IE(n,n);
    IE=Q;
    Eigen::MatrixXd IM(n,n);
	IM=Q;
	double EE_norm,IE_norm,IM_norm;
    std::cout << "\tStep" << std::setw(15) << "expl. Euler" << std::setw(15) << "impl. Euler" << std::setw(15) << "Impl.  Midpoint" << std::endl;
    for (int i=0; i< 21; i++){
		double h=0.01;
		EE=expeulstep(A, EE, h);
		IE=impeulstep(A, IE, h);
		IM=impstep(A, IM, h);
		EE_norm= (EE.transpose()*EE-I).norm();
		IE_norm= (IE.transpose()*IE-I).norm();
		IM_norm= (IM.transpose()*IM-I).norm();
		std::cout << "\t"<< i << std::setw(15) << EE_norm << std::setw(15) << IE_norm << std::setw(15) << IM_norm << std::endl;
}

    std::cout << "SUBTASK 1. d)" << std::endl;
    // Test implementation of ode45
    Eigen::MatrixXd mat_ode =matode(M,T);
    std::cout << mat_ode <<std::endl;
    std::cout << "test matode norm : "  << (mat_ode.transpose()*mat_ode-I).norm() << std::endl;
    // TODO: TEST matode

    std::cout << "SUBTASK 1. g)" << std::endl;
    // Test whether invariant was preserved or not

    if (checkinvariant(M,T)){std::cout << " Invariant preserved" << std::endl;}
    else {std::cout << " Invariant not preserved" << std::endl;}
    // TODO: TEST if matode preserves invariant using checkinvariant

    return 0;
}

Nummerical Methods for CSE/PS13/matrix_ode.cpp


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

void matrixCreation()
{
    Eigen::Matrix4d m; // 4x4 double

    Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex

    //a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients,
    Eigen::Matrix3f a;

    //b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all.
    Eigen::MatrixXf b;

    //A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients.
    Eigen::MatrixXf A(10, 15);
}

void arrayCreation()
{
    // ArrayXf
    Eigen::Array<float, Eigen::Dynamic, 1> a1;
    // Array3f
    Eigen::Array<float, 3, 1> a2;
    // ArrayXXd
    Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> a3;
    // Array33d
    Eigen::Array<double, 3, 3> a4;
    Eigen::Matrix3d matrix_from_array = a4.matrix();
}

void vectorCreation()
{
    // Vector3f is a fixed column vector of 3 floats:
    Eigen::Vector3f objVector3f;

    // RowVector2i is a fixed row vector of 3 integer:
    Eigen::RowVector2i objRowVector2i;

    // VectorXf is a column vector of size 10 floats:
    Eigen::VectorXf objv(10);

    //V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients.
    Eigen::VectorXf V(30);
}

void buildMatrixFromVector()
{
    Eigen::Matrix2d mat;
    mat<<1,2,3,4;

    std::cout<<"matrix is:\n"  <<mat <<std::endl;

    Eigen::RowVector2d firstRow=mat.row(0);
    Eigen::Vector2d firstCol=mat.col(0);

    std::cout<<"First column of the matrix is:\n "<<firstCol <<std::endl;
    std::cout<<"First column dims are: "  <<firstCol.rows()<<","<<firstCol.cols() <<std::endl;

    std::cout<<"First row of the matrix is: \n" <<firstRow <<std::endl;
    std::cout<<"First row dims are: " <<firstRow.rows()<<"," <<firstRow.cols() <<std::endl;

    firstRow = Eigen::RowVector2d::Random();
    firstCol = Eigen::Vector2d::Random();

    mat.row(0) =firstRow;
    mat.col(0) = firstCol;

    std::cout<<"the new matrix is:\n"  <<mat <<std::endl;

}

void initialization()
{
    std::cout <<"///////////////////Initialization//////////////////"<< std::endl;

    Eigen::Matrix2d rndMatrix;
    rndMatrix.setRandom();

    Eigen::Matrix2d constantMatrix;
    constantMatrix.setRandom();
    constantMatrix.setConstant(4.3);

    Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6);

    Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3);

    Eigen::ArrayXXf table(10, 4);
    table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90);

}

void elementAccess()
{
    std::cout <<"//////////////////Elements Access////////////////////"<< std::endl;

    Eigen::MatrixXf matrix(4, 4);
    matrix << 1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16;

    std::cout<<"matrix is:\n"<<matrix <<std::endl;

    std::cout<<"All Eigen matrices default to column-major storage order. That means, matrix(2) is  matix(2,0):" <<std::endl;

    std::cout<<"matrix(2): "<<matrix(2) <<std::endl;
    std::cout<<"matrix(2,0): "<<matrix(2,0) <<std::endl;

    std::cout <<"//////////////////Pointer to data ////////////////////"<< std::endl;

    for (int i = 0; i < matrix.size(); i++)
    {
          std::cout << *(matrix.data() + i) << "  ";
    }
    std::cout <<std::endl;

    std::cout <<"//////////////////Row major Matrix////////////////////"<< std::endl;
    Eigen::Matrix<double, 4,4,Eigen::RowMajor> matrixRowMajor(4, 4);
    matrixRowMajor << 1, 2, 3, 4,
        5, 6, 7, 8,
        9, 10, 11, 12,
        13, 14, 15, 16;

    for (int i = 0; i < matrixRowMajor.size(); i++)
    {
          std::cout << *(matrixRowMajor.data() + i) << "  ";
    }
    std::cout <<std::endl;

    std::cout <<"//////////////////Block Elements Access////////////////////"<< std::endl;

    std::cout << "Block elements in the middle" << std::endl;

    int starting_row,starting_column,number_rows_in_block,number_cols_in_block;

    starting_row=1;
    starting_column=1;
    number_rows_in_block=2;
    number_cols_in_block=2;

    std::cout << matrix.block(starting_row,starting_column,number_rows_in_block,number_cols_in_block) << std::endl;

    for (int i = 1; i <= 3; ++i)
    {
        std::cout << "Block of size " << i << "x" << i << std::endl;
        std::cout << matrix.block(0, 0, i, i) << std::endl;
    }
}

void matrixReshaping()
{
    //https://eigen.tuxfamily.org/dox/group__TutorialReshapeSlicing.html
    /*
    Eigen::MatrixXd m1(12,1);
    m1<<0,1,2,3,4,5,6,7,8,9,10,11;
    std::cout<<m1<<std::endl;

    //Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> m2(m1);
    //Eigen::Map<Eigen::MatrixXd> m3(m2.data(),3,4);
    Eigen::Map<Eigen::MatrixXd> m2(m1.data(),4,3);
    std::cout<<m2.transpose()<<std::endl;
    //solution*/
    //https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
}

//https://eigen.tuxfamily.org/dox/group__TutorialReshapeSlicing.html
void matrixSlicing()
{

}
void matrixResizing()
{
    std::cout <<"//////////////////Matrix Resizing////////////////////"<< std::endl;
    int rows, cols;
    rows=3;
    cols=4;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> dynamicMatrix;

    dynamicMatrix.resize(rows,cols);
    dynamicMatrix=Eigen::MatrixXd::Random(rows,cols);

    std::cout<<"Matrix size is: " << dynamicMatrix.size()<<std::endl;

    std::cout<<"Matrix is:\n" << dynamicMatrix<<std::endl;

    dynamicMatrix.resize(2,6);
    std::cout<<"New Matrix size is: " << dynamicMatrix.size()<<std::endl;
    std::cout<<"Matrix is:\n" << dynamicMatrix<<std::endl;

    std::cout <<"//////////////////Matrix conservativeResize////////////////////"<< std::endl;

    dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1);
    dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector2d(1, 4);

    std::cout<< dynamicMatrix<<std::endl;
}

void convertingMatrixtoArray()
{
    Eigen::Matrix<double, 4,4> mat1=Eigen::MatrixXd::Random(4,4);
    Eigen::Matrix<double, 4,4> mat2=Eigen::MatrixXd::Random(4,4);

    Eigen::Array<double, 4,4> array1=mat1.array();
    Eigen::Array<double, 4,4> array2=mat2.array();

    std::cout<<"Matrix multipication:\n" << mat1*mat2 <<std::endl;
    std::cout<<"Array multipication(coefficientsweise) :\n"<<array1*array2 <<std::endl;
    std::cout<<"Matrix coefficientsweise multipication :\n"<<mat1.cwiseProduct(mat2) <<std::endl;

}

void coefficientWiseOperations()
{
    std::cout <<"/////////////Matrix Coefficient Wise Operations///////////////////"<< std::endl;
    Eigen::Matrix<double, 2, 3> my_matrix;
    my_matrix << 1, 2, 3, 4, 5, 6;
    int i,j;
    std::cout <<"The matrix is: \n" << my_matrix << std::endl;

    std::cout <<"The matrix transpose is: \n" << my_matrix.transpose() << std::endl;

    std::cout<<"The minimum element is: " <<my_matrix.minCoeff(&i, &j)<<" and its indices are:" << i<<","<< j <<std::endl;

    std::cout<<"The maximum element is: " <<my_matrix.maxCoeff(&i, &j)<<" and its indices are:" << i<<","<< j <<std::endl;

    std::cout<<"The multipication of all elements: " <<my_matrix.prod()<<std::endl;
    std::cout<<"The sum of all elements: " <<my_matrix.sum()<<std::endl;
    std::cout<<"The mean of all element: " <<my_matrix.mean()<<std::endl;
    std::cout<<"The trace of the matrix is: " <<my_matrix.trace()<<std::endl;
    std::cout<<"The means of columns: " <<my_matrix.colwise().mean()<<std::endl;
    std::cout<<"The max of each columns: "  <<my_matrix.rowwise().maxCoeff()<<std::endl;
    std::cout<<"Norm 2 of the matrix is: " <<my_matrix.lpNorm<2>()<<std::endl;
    std::cout<<"Norm infinty of the matrix is: " <<my_matrix.lpNorm<Eigen::Infinity>()<<std::endl;
    std::cout<<"If all elemnts are positive: "  << (my_matrix.array()>0).all()<<std::endl;
    std::cout<<"If any element is greater than 2: "<<(my_matrix.array()>2).any()<<std::endl;
    std::cout<<"Counting the number of elements greater than 1"<<(my_matrix.array()>1).count()<<std::endl;
    std::cout <<"subtracting 2 from all elements:\n" << my_matrix.array() - 2 << std::endl;
    std::cout <<"abs of the matrix: \n" << my_matrix.array().abs() << std::endl;
    std::cout << "square of the matrix: \n" <<my_matrix.array().square() << std::endl;

    std::cout <<"exp of the matrix: \n" <<my_matrix.array().exp() << std::endl;
    std::cout << "log of the matrix: \n" <<my_matrix.array().log() << std::endl;
    std::cout << "square root of the matrix: \n" <<my_matrix.array().sqrt() << std::endl;

}

void maskingArray()
{
    std::cout <<"//////////////////Maskin Matrix/Array ////////////////////"<< std::endl;

    //Eigen::MatrixXf P, Q, R; // 3x3 float matrix.
    // (R.array() < s).select(P,Q ); // (R < s ? P : Q)
    // R = (Q.array()==0).select(P,R); // R(Q==0) = P(Q==0)
    int cols, rows;
    cols=2; rows=3;
    Eigen::MatrixXf R=Eigen::MatrixXf::Random(rows, cols);

    Eigen::MatrixXf Q=Eigen::MatrixXf::Zero(rows, cols);
    Eigen::MatrixXf P=Eigen::MatrixXf::Constant(rows, cols,1.0);

    double s=0.5;
    Eigen::MatrixXf masked=(R.array() < s).select(P,Q ); // (R < s ? P : Q)

    std::cout<<"R\n" <<R <<std::endl;
    std::cout<<"masked\n" <<masked <<std::endl;
    std::cout<<"P\n"<< P <<std::endl;
    std::cout<<"Q\n" << Q<<std::endl;
}

void AdditionSubtractionOfMatrices()
{

}

void transpositionConjugation()
{

}

void scalarMultiplicationDivision()
{

}

void multiplicationDotCrossProduct()
{

}

int main()
{
    //buildMatrixFromVector();
    //vectorCreation();
    //elementAccess();
    //matrixResizing();
    //coefficientWiseOperations();
    //convertingMatrixtoArray();
    //maskingArray();

}


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example least-squares.cpp
*
*   This tutorial shows how least Squares problems for matrices from ViennaCL or Boost.uBLAS can be solved solved.
*
*   We start with including the respective header files:
**/

// activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// include necessary system headers
//
#include <iostream>

// Boost headers
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// ViennaCL headers
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/linalg/qr.hpp"
#include "viennacl/linalg/lu.hpp"
#include "viennacl/linalg/direct_solve.hpp"

/**
*  The minimization problem of finding x such that \f$ \Vert Ax - b \Vert \f$ is solved as follows:
*   - Compute the QR-factorization of A = QR.
*   - Compute \f$ b' = Q^{\mathrm{T}} b \f$ for the equivalent minimization problem \f$ \Vert Rx - Q^{\mathrm{T}} b \f$.
*   - Solve the triangular system \f$ \tilde{R} x = b' \f$, where \f$ \tilde{R} \f$ is the upper square matrix of R.
*
**/
int main (int, const char **)
{
  typedef float               ScalarType;     //feel free to change this to 'double' if supported by your hardware

  typedef boost::numeric::ublas::matrix<ScalarType>              MatrixType;
  typedef boost::numeric::ublas::vector<ScalarType>              VectorType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>   VCLMatrixType;
  typedef viennacl::vector<ScalarType>                           VCLVectorType;

  /**
  *  Create vectors and matrices with data:
  **/
  VectorType ublas_b(4);
  ublas_b(0) = -4;
  ublas_b(1) =  2;
  ublas_b(2) =  5;
  ublas_b(3) = -1;

  MatrixType ublas_A(4, 3);

  ublas_A(0, 0) =  2; ublas_A(0, 1) = -1; ublas_A(0, 2) =  1;
  ublas_A(1, 0) =  1; ublas_A(1, 1) = -5; ublas_A(1, 2) =  2;
  ublas_A(2, 0) = -3; ublas_A(2, 1) =  1; ublas_A(2, 2) = -4;
  ublas_A(3, 0) =  1; ublas_A(3, 1) = -1; ublas_A(3, 2) =  1;

  /**
  * Setup the matrix and vector with ViennaCL objects and copy the data from the uBLAS objects:
  **/
  VCLVectorType vcl_b(ublas_b.size());
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_b, vcl_b);
  viennacl::copy(ublas_A, vcl_A);

  /**
  *
Option 1: Using Boost.uBLAS

  *
  * The implementation in ViennaCL accepts both uBLAS and ViennaCL types.
  * We start with a single-threaded implementation using Boost.uBLAS.
  **/

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  /**
  * The first (and computationally most expensive) step is to compute the QR factorization of A.
  * Since we do not need A later, we directly overwrite A with the householder reflectors and the upper triangular matrix R.
  * The returned vector holds the scalar coefficients (betas) for the Householder reflections \f$ I - \beta v v^{\mathrm{T}} \f$
  **/
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);

  /**
  * Compute the modified RHS of the minimization problem from the QR factorization, but do not form \f$ Q^{\mathrm{T}} \f$ explicitly:
  * b' := Q^T b
  **/
  viennacl::linalg::inplace_qr_apply_trans_Q(ublas_A, ublas_betas, ublas_b);

  /**
  * Final step: triangular solve: Rx = b'', where b'' are the first three entries in b'
  * We only need the upper left square part of A, which defines the upper triangular matrix R
  **/
  boost::numeric::ublas::range ublas_range(0, 3);
  boost::numeric::ublas::matrix_range<MatrixType> ublas_R(ublas_A, ublas_range, ublas_range);
  boost::numeric::ublas::vector_range<VectorType> ublas_b2(ublas_b, ublas_range);
  boost::numeric::ublas::inplace_solve(ublas_R, ublas_b2, boost::numeric::ublas::upper_tag());

  std::cout << "Result: " << ublas_b2 << std::endl;

  /**
  *
Option 2: Use ViennaCL types

  *
  *  ViennaCL is used for the computationally intensive BLAS 3 computations.
  *  Boost.uBLAS is used for the panel factorization on the host (CPU).
  */

  std::cout << "--- ViennaCL (hybrid implementation)  ---" << std::endl;
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  /**
  * compute modified RHS of the minimization problem: \f$ b' := Q^T b \f$
  **/
  viennacl::linalg::inplace_qr_apply_trans_Q(vcl_A, hybrid_betas, vcl_b);

  /**
  * Final step: triangular solve: Rx = b'.
  * We only need the upper part of A such that R is a square matrix
  **/
  viennacl::range vcl_range(0, 3);
  viennacl::matrix_range<VCLMatrixType> vcl_R(vcl_A, vcl_range, vcl_range);
  viennacl::vector_range<VCLVectorType> vcl_b2(vcl_b, vcl_range);
  viennacl::linalg::inplace_solve(vcl_R, vcl_b2, viennacl::linalg::upper_tag());

  std::cout << "Result: " << vcl_b2 << std::endl;

  /**
  *  That's it.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#include <armadillo>
#include <iostream>
#include <stdio.h>

using namespace std;
using namespace arma;

mat computeCost(const mat& X, const mat& y, const mat& theta)
{
	mat J;
	int m;
	m = y.n_rows;
	J = arma::sum((pow(((X*theta)-y), 2))/(2*m)) ;
	return J;
}

void gradientDescent(const mat&    X,
                     const mat&    y,
                           double  alpha,
                           int     num_iters,
                           mat&    theta)
{
	mat delta;
	int iter;
	int m ;
	m = y.n_rows;
	//vec J_history = arma::zeros<vec>(num_iters) ;
	for (iter = 0; iter < num_iters; iter++)
	{
		delta = arma::trans(X)*(X*theta-y)/m ;
		theta = theta-alpha*delta ;
	}
}

int main()
{
	mat data;
	data.load("ex1data1.txt");
	mat X = data.col(0);
	mat y = data.col(1);

	int m = X.n_elem;
	cout << "m = " << m << endl;

	vec X_One(m);
	X_One.ones();
	X.insert_cols(0, X_One);

	mat theta = arma::zeros<vec>(2);
	int iterations = 1500 ;
	double alpha = 0.01 ;

	mat J = computeCost(X, y, theta);
	J.print("J:");

	gradientDescent(X, y, alpha, iterations, theta) ;
	printf("Theta found by gradient descent: \n") ;
	printf("%f %f \n", theta(0), theta(1)) ;

	return 0;
}


#define _USE_MATH_DEFINES

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

using namespace Eigen;

Eigen::Quaterniond euler2Quaternion(double roll, double pitch, double yaw ){
	Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
	Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitX());

	Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
    return q;
}

int main(){
    MatrixXd rotationA(3,3),rotationB(3,3);

    double rollA = M_PI/4, rollB = -M_PI/3;
    double pitchA = 0, pitchB = 0;
    double yawA = M_PI/3, yawB = 0;

    Eigen::Quaterniond qA = euler2Quaternion(rollA,pitchA,yawA);
    Eigen::Quaterniond qB = euler2Quaternion(rollB,pitchB,yawB);
    //Eigen::Quaterniond qA1 = euler2Quaternion1(rollA,pitchA,yawA);
    rotationA = qA.toRotationMatrix();
    rotationB = qB.toRotationMatrix();

    //Eigen::Matrix3d rmx1 = qA1.matrix();
    // Q2
    std::cout<<"\nQuestion 2\n";
    std::cout<<"Rotation matrix A is\n";
    std::cout << rotationA <<"\n";
    //std::cout <<"\n\n"<< rmx1 <<"\n\n";

    std::cout<<"Rotation matrix B is\n";
    std::cout << rotationB <<"\n";

    std::cout<<"matrixA*matrixB is\n";
    std::cout << rotationA*rotationB <<"\n";

    std::cout<<"matrixB*matrixA is\n";
    std::cout << rotationB*rotationA <<"\n";
    // Q3
    std::cout<<"\nQuestion 3\n";
    std::cout<<"Quarternion A:"<<qA.w()<<" "<<qA.x()<<" "<<qA.y()<<" "<<qA.z()<<std::endl;
    std::cout<<"Quarternion B:"<<qB.w()<<" "<<qB.x()<<" "<<qB.y()<<" "<<qB.z()<<std::endl;

    // Q4
    std::cout<<"\nQuestion 4\n";
    Quaterniond qC = qA*qB;
    Quaterniond qD = qB*qA;
    std::cout<<"Quarternion C:"<<qC.w()<<" "<<qC.x()<<" "<<qC.y()<<" "<<qC.z()<<std::endl;
    std::cout<<"Quarternion D:"<<qD.w()<<" "<<qD.x()<<" "<<qD.y()<<" "<<qD.z()<<std::endl;
    Quaterniond qE = qA * qB.inverse();
    Quaterniond qF = qE*qB;
    std::cout<<"Quarternion E:"<<qE.w()<<" "<<qE.x()<<" "<<qE.y()<<" "<<qE.z()<<std::endl;
    std::cout<<"Quarternion F:"<<qF.w()<<" "<<qF.x()<<" "<<qF.y()<<" "<<qF.z()<<std::endl;
}


#include "iostream"
using namespace std;

#include <ctime>
// include Core and Dense compuation parts from Eigen
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 20

/**************************************************
 * Exercise 6
 * Different ways to solve linear equation: Ax = b
 * For more info, can refer to: https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
 * https://eigen.tuxfamily.org/dox/group__LeastSquares.html
 * 1. (If invertible): Take A^-1 and left multiply to b
 * 2. SVD decomposition
 * 3. QR decomposition
 * 4. Use Normal equations: solve A^T Ax = A^T b
 * 5. LU decomposition
 * 6. LLT decomposition: Need A to be positive definite
 * 7. LDLT decomposition (Cholesky algorithm): Need A to be positive/negative semi-definite
 * *************************************************/

int main(int argc, char** argv) {
    Matrix<double, Dynamic, Dynamic> A = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    // since A*A^T is always positive definite, use it as an example
    A = A * A.transpose();

    Matrix<double, Dynamic, Dynamic> b = MatrixXd::Random(MATRIX_SIZE, 1);

    // 1. take the inverse
    // count the time
    clock_t start_time = clock();
    Matrix<double, MATRIX_SIZE, 1> x1 = A.inverse() * b;
    cout << "time of normal inverse is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "result is: " << x1.transpose() << endl;

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

    // 3. QR decomposition
    /**
     * The solve() method in QR decomposition classes also computes the least squares solution.
     * There are three QR decomposition classes: HouseholderQR (no pivoting, fast but unstable if your matrix is not rull rank),
     * ColPivHouseholderQR (column pivoting, thus a bit slower but more stable)
     * and FullPivHouseholderQR (full pivoting, so slowest and slightly more stable than ColPivHouseholderQR).
     */
    start_time = clock();
    cout << "The least-square QR decomposition solution is: \n" << A.colPivHouseholderQr().solve(b) << endl;
    cout << "time of QR decomposition is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // 4. Normal Equation
    /**
     * This method is usually the fastest, especially when A is "tall and skinny".
     * However, if the matrix A is even mildly ill-conditioned, this is not a good method,
     * because the condition number of ATA is the square of the condition number of A.
     * This means that you lose roughly twice as many digits of accuracy using the normal equation, compared to the more stable methods mentioned above.
     */
    start_time = clock();
    cout << "The Normal equation solution is: \n" << (A.transpose() * A).ldlt().solve(A.transpose() * b) << endl;
    cout << "time of Normal Equation is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // 5. LU decomposition
    start_time = clock();
    cout << "The LU decomposition solution is: \n" << A.lu().solve(b) << endl;
    cout << "time of LU decomposition is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // 6. LLT decomposition
    start_time = clock();
    cout << "The LLT decomposition solution is: \n" << A.llt().solve(b) << endl;
    cout << "time of LLT decomposition is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;

    // 6. LDLT decomposition
    start_time = clock();
    cout << "The LDLT decomposition solution is: \n" << A.ldlt().solve(b) << endl;
    cout << "time of LDLT decomposition is " << 1000 * (clock() - start_time) / (double) CLOCKS_PER_SEC << "ms" << endl;

}


/* Point-based registration implemented via the method of "Least-Squares Fitting of Two 3-D Point Sets", Arun et al, 1987 */

#include <iostream>

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

#include <PointMatching.hpp>
#include <Exceptions.hpp>
#include <Util.hpp>

Eigen::Matrix3d find_rotation(const Eigen::MatrixXd& H) {
    // Estimate rotation matrix given H, the matrix product of residuals in both pointsets.

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

    Eigen::Matrix3d rotation;

    auto proposed_rotation = svd.matrixV()*(svd.matrixU()).transpose();
    if(isApproxEqual(proposed_rotation.determinant(), 1)) {
        rotation = proposed_rotation;
    } else if(isApproxEqual(proposed_rotation.determinant(), -1)) {
        // Determinant of -1 can mean we've calculated a reflection (and so can compute a rotation) or we have insurmountable noise problems.
        auto lambda = svd.singularValues();
        if(isApproxEqual(lambda(2), 0) && !isApproxEqual(lambda(1), 0) && !isApproxEqual(lambda(0), 0)) { // This is a reflection.
            auto V_new = svd.matrixV();
            V_new.block(0,2,V_new.rows(),1) = -1 * V_new.block(0,2,V_new.rows(),1);
            rotation = V_new*(svd.matrixU()).transpose();
        } else {
            std::cerr << "Could not find a rotation. Colinear point cloud seems likely, or perhaps very noisy data?" << std::endl;
            throw(PointMatchingEx);
        }
    } else {
        std::cerr << "Could not find a rotation from SVD. Very noisy or otherwise invalid data?" << std::endl;
        throw(PointMatchingEx);
    }

    return rotation;
}

Eigen::Matrix4d estimate_rigid_transform(const Eigen::MatrixXd& pointset, const Eigen::MatrixXd& pointset_dash) {
    // Find a rigid transform that maps pointset to pointset_dash, with least error.

    if(pointset.cols() < 4 || pointset_dash.cols() < 4) {
        std::cerr << "Not enough points provided -- there should be at least four points in the point cloud." << std::endl;
        throw(PointMatchingEx);
    }

    if(pointset.rows() != 3 || pointset_dash.rows() != 3) {
        std::cerr << "Points must be 3D." << std::endl;
        throw(PointMatchingEx);
    }

    if(pointset.cols() != pointset_dash.cols()) {
        std::cerr << "Pointsets must have the same number of points." << std::endl;
        throw(PointMatchingEx);
    }

    auto p_average = find_pointset_average(pointset);
    auto p_dash_average = find_pointset_average(pointset_dash);

    auto q = residuals_from_point(pointset, p_average);
    auto q_dash = residuals_from_point(pointset_dash, p_dash_average);

    auto H = q * q_dash.transpose();

    auto rotation = find_rotation(H);

    auto translation = p_dash_average - rotation*p_average;

    auto final_transform = compose_final_transform(rotation, translation);

    return final_transform;
}

double fiducial_registration_error(const Eigen::MatrixXd& pointset, const Eigen::MatrixXd& pointset_dash, const Eigen::Matrix4d& transform) {
    auto transformed = apply_transform(pointset, transform);

    auto error_per_vector = distances_between_pointsets(transformed, pointset_dash);
    auto fre = root_mean_square(error_per_vector);

    return fre;
}


/*
https://stackoverflow.com/questions/34662940/how-to-compute-basis-of-nullspace-with-eigen-library
*/
#include <Eigen/Dense>
#include <iostream>

void fullPivLU()
{
//https://stackoverflow.com/questions/31041921/how-to-get-rank-of-a-matrix-in-eigen-library

    Eigen::Matrix3d mat;
    mat<<2, 1, -1,
         -3, -1, 2,
         -2, 1, 2;

    Eigen::FullPivLU<Eigen::Matrix3d> lu_decomp(mat);
    //Eigen::FullPivHouseholderQR
    auto rank = lu_decomp.rank();
    std::cout<<"Rank:" <<rank <<std::endl;

    std::cout<<"Kernel:\n" <<lu_decomp.kernel()<<std::endl;

    std::cout<<"MatrixLU:\n" <<lu_decomp.matrixLU()<<std::endl;

    std::cout<<"Determinant:" <<lu_decomp.determinant()<<std::endl;

//    lu_decomp.permutationP();
//    std::cout<<lu_decomp.kernel()<<std::endl;

//    lu_decomp.permutationQ();
//    std::cout<<lu_decomp.kernel()<<std::endl;

}

void completeOrthogonalDecompositionNullSpace()
{
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat37(3,7);
    mat37 = Eigen::MatrixXd::Random(3, 7);

    Eigen::CompleteOrthogonalDecomposition<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > cod;
    cod.compute(mat37);
    std::cout << "rank : " << cod.rank() << "\n";
    // Find URV^T
    Eigen::MatrixXd V = cod.matrixZ().transpose();
    Eigen::MatrixXd Null_space = V.block(0, cod.rank(),V.rows(), V.cols() - cod.rank());
    Eigen::MatrixXd P = cod.colsPermutation();
    Null_space = P * Null_space; // Unpermute the columns
    // The Null space:
    std::cout << "The null space: \n" << Null_space << "\n" ;
    // Check that it is the null-space:
    std::cout << "mat37 * Null_space = \n" << mat37 * Null_space  << '\n';
}

int main()
{
    completeOrthogonalDecompositionNullSpace();
}


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

#ifndef LGM_SE3_PUBLIC_HPP
#define LGM_SE3_PUBLIC_HPP

#include <Eigen/Core>

/////////////////////////////////////////////////////////////////////////////////////////////
/// Lie Group Math - Special Euclidean Group
/////////////////////////////////////////////////////////////////////////////////////////////
namespace lgmath {
namespace se3 {

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

} // se3
} // lgmath

#endif // LGM_SE3_PUBLIC_HPP


//
// Created by light on 01.03.21.
//

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

using namespace Eigen;
using namespace std;

bool COMPARE (Eigen::MatrixXd& A, Eigen::MatrixXd& B){

    if (A.norm() - B.norm() < 1e-4){

        return true;
    }

}

int main(){

/*  A = [ 1 2 3 4 ; 5 6 7 8 ; 9 10 11 12; 13 14 15 16]

 For simplicity reasons we would be considering this matrix. b = [ 1  1 1 1], in case we have to Ax=b

  */

// creating a Matrix

Eigen::Matrix <double, 4,4, Eigen::RowMajor> Matrix1;
Matrix1 << 1, 2, 3, 4,  5, 6, 7, 8 , 9, 10, 11, 12, 13, 14, 15, 16;

// creating a Matrix 2nd way using a temporary matrix
Eigen::Matrix<double, 4,4, Eigen::RowMajor> Matrix2 = (Eigen::Matrix<double,4,4,RowMajor> () << 1, 2, 3, 4,  5, 6, 7, 8 , 9, 10, 11, 12, 13, 14, 15, 16 ).finished();

std::cout << "The first matrix is  \n " << Matrix1  << " \n"
          << " Second matrix is    \n " << Matrix2 << std::endl;

/* create a row vector or column vector

 a. Vector2f, Vector3f, Vector4f This should give nx1 matrix (n rows and 1 column) something like [1 2 3 4]'
 b. RowVector2f, RowVector3f, RowVector4f Gives out  1xn (1 row and n columns), something like [1 2 3 4]
 c. VectorXf or VectorXd -> Dynamic Columns of floats or doubles.
 In general d implies double and f suffix implies it's float

*/

Vector2f V2 = (Vector2f() << 1,2).finished();
Vector3d V3;
V3 << 1,2,3 ;

RowVector2f  RV2 = (RowVector2f()<< 1,2 ).finished();

/*std::cout << "Default is a colum vector \n" <<
             " V2 is = \n" << V2 << "\n" <<
             " v3 is = \n " << V3 << std::endl;
*/
//std::cout << "RowVector2f is given by \n" << RV2 << std::endl;
// Other nicer ways to initialize a matrix are
Eigen::Matrix < double, 4,4, RowMajor > A_zero = Eigen::MatrixXd::Zero(4,4);
Matrix4d A_identity = MatrixXd::Identity(4,4);
Matrix4d A_setone = MatrixXd::Ones(4,4); // the r value is basically eye(4,4)

//reinitialize an existing Matrix
Matrix1.setZero();
//std::cout << "A_zero is \n" << A_zero << "\n" << "Matrix1 is reset to zero \n" << Matrix1 << std::endl;
Matrix1.setOnes();
Matrix1.setRandom();
//std::cout << "Matrix1 is reset to Random \n" << Matrix1 << std::endl;
Matrix1.setIdentity(); // Matrix1 = eye(N);

// Lengths of different tensors.

Matrix1.size(); // total size 4x4 = 16
Matrix1.rows(); //number of rows
Matrix1.cols(); // number of cols

// Element wise operations

//to access specific element
Matrix1(1,2);  // This should give out element at 2nd row and third cols, Remember the indices are i-1 and j-1 if you think in terms of matlab
// for vectors it's simply V2(1) or V2(2) etc.,

// to access specific row
Matrix2.row(0); // This gives out first row
std::cout << Matrix2.row(0) << std::endl;
Matrix2.col(0); // This gives out 1 col
std::cout << Matrix2.col(0) << std::endl;

// to change specific row or cols

Matrix2.col(0) << 1,2,3,4;
std::cout << Matrix2.col(0) << std::endl;
std::cout << Matrix2 << std::endl;

// to access a specific block;

Matrix2.block<2,2>(1, 1)  << 1.2, 1.3, 1.4, 1.5 ; // This is an interesting operation, it extracts the specific block and assigns value to it
// for example here we would like to extract a block of 2 rows and 2 columns as descrbed inside <> . And the numbers inside braces indicate the starting col and starting row
// here it's 1,1 which means 2,2 as the value is taken 1 less than actual value of rol or col
//std::cout << Matrix2 << std::endl;

// Resizing an existing matrix, matrix1 is 4x4. This can resized to 2x8, 8x2, 1x16 and 16x1
//Matrix1.resize(2,8); // This normally fails due to assertations, i wouldn't wanna tamper with assertations at this stage
//std::cout << Matrix1  << std::endl;

// Stacking Vectors or Matrices
Matrix1.setRandom();
MatrixXd M( Matrix1.rows() + Matrix1.rows()+ Matrix1.rows(), Matrix1.cols()); // I could have written Matrix1.rows()*3, in case there were m << A,B,C; then we need to write em individually
M << Matrix1, Matrix1, Matrix1;
std::cout << M << std::endl;

// Filling all the elements with some constant value

Matrix2.fill(1.0);
std::cout << Matrix2 << std::endl;

VectorXd VX = VectorXd::LinSpaced(4,1,5) ;       // linspace(low,high,size)'
VX.setLinSpaced(4,1,5);               // v = linspace(low,high,size)'
std::cout << VX << std::endl;

// Matrix slicing
// blocks -> always go for templated version, it has better speed up
// there are two types of blocks one with vectors and second one with Matrix

// for vectors, you simply pass on a single value
int const n = 2;
VX.head<n>() << 1, 2; // for head initial 2 values
std::cout << VX << std::endl;
VX.tail<n>() << 3,4; // bottom two values or N-n till N

// for matrix there is a block actually, you essentially pass on initial inidices of starting with size of the matrix as shown in line 101
// Other try outs
/*
    P.col(j)                           // P(:, j+1)
    P.leftCols<cols>()                 // P(:, 1:cols)
    P.leftCols(cols)                   // P(:, 1:cols)
    P.middleCols<cols>(j)              // P(:, j+1:j+cols)
    P.middleCols(j, cols)              // P(:, j+1:j+cols)
    P.rightCols<cols>()                // P(:, end-cols+1:end)
    P.rightCols(cols)                  // P(:, end-cols+1:end)
    P.topRows<rows>()                  // P(1:rows, :)
    P.topRows(rows)                    // P(1:rows, :)
    P.middleRows<rows>(i)              // P(i+1:i+rows, :)
    P.middleRows(i, rows)              // P(i+1:i+rows, :)
    P.bottomRows<rows>()               // P(end-rows+1:end, :)
    P.bottomRows(rows)                 // P(end-rows+1:end, :)
    P.topLeftCorner(rows, cols)        // P(1:rows, 1:cols)
    P.topRightCorner(rows, cols)       // P(1:rows, end-cols+1:end)
    P.bottomLeftCorner(rows, cols)     // P(end-rows+1:end, 1:cols)
    P.bottomRightCorner(rows, cols)    // P(end-rows+1:end, end-cols+1:end)
    P.topLeftCorner<rows,cols>()       // P(1:rows, 1:cols)
    P.topRightCorner<rows,cols>()      // P(1:rows, end-cols+1:end)
    P.bottomLeftCorner<rows,cols>()    // P(end-rows+1:end, 1:cols)
    P.bottomRightCorner<rows,cols>()   // P(end-rows+1:end, end-cols+1:end)
*/

// Views, transpose, etc;
// Eigen                           // Matlab
 /*    R.adjoint()                        // R'
    R.transpose()                      // R.' or conj(R')       // Read-write
    R.diagonal()                       // diag(R)               // Read-write
    x.asDiagonal()                     // diag(x)
    R.transpose().colwise().reverse()  // rot90(R)              // Read-write
    R.rowwise().reverse()              // fliplr(R)
    R.colwise().reverse()              // flipud(R)
    R.replicate(i,j)                   // repmat(P,i,j)
*/

    // All the same as Matlab, but matlab doesn't have *= style operators.
// Matrix-vector.  Matrix-matrix.   Matrix-scalar.
  /*
    y  = M*x;          R  = P*Q;        R  = P*s;
    a  = b*M;          R  = P - Q;      R  = s*P;
    a *= M;            R  = P + Q;      R  = P/s;
    R *= Q;          R  = s*P;
    R += Q;          R *= s;
    R -= Q;          R /= s;

   // Vectorized operations on each element independently
// Eigen                       // Matlab
R = P.cwiseProduct(Q);         // R = P .* Q
R = P.array() * s.array();     // R = P .* s
R = P.cwiseQuotient(Q);        // R = P ./ Q
R = P.array() / Q.array();     // R = P ./ Q
R = P.array() + s.array();     // R = P + s
R = P.array() - s.array();     // R = P - s
R.array() += s;                // R = R + s
R.array() -= s;                // R = R - s
R.array() < Q.array();         // R < Q
R.array() <= Q.array();        // R <= Q
R.cwiseInverse();              // 1 ./ P
R.array().inverse();           // 1 ./ P
R.array().sin()                // sin(P)
R.array().cos()                // cos(P)
R.array().pow(s)               // P .^ s
R.array().square()             // P .^ 2
R.array().cube()               // P .^ 3
R.cwiseSqrt()                  // sqrt(P)
R.array().sqrt()               // sqrt(P)
R.array().exp()                // exp(P)
R.array().log()                // log(P)
R.cwiseMax(P)                  // max(R, P)
R.array().max(P.array())       // max(R, P)
R.cwiseMin(P)                  // min(R, P)
R.array().min(P.array())       // min(R, P)
R.cwiseAbs()                   // abs(P)
R.array().abs()                // abs(P)
R.cwiseAbs2()                  // abs(P.^2)
R.array().abs2()               // abs(P.^2)
(R.array() < s).select(P,Q );  // (R < s ? P : Q)
R = (Q.array()==0).select(P,A) // R(Q==0) = P(Q==0)
R = P.unaryExpr(ptr_fun(func)) // R = arrayfun(func, P)   // with: scalar func(const scalar &x

//// Type conversion
// Eigen                  // Matlab
A.cast<double>();         // double(A)
A.cast<float>();          // single(A)
A.cast<int>();            // int32(A)
A.real();                 // real(A)
A.imag();                 // imag(A)
// if the original type equals destination type, no work is done

// Note that for most operations Eigen requires all operands to have the same type:
MatrixXf F = MatrixXf::Zero(3,3);
A += F;                // illegal in Eigen. In Matlab A = A+F is allowed
A += F.cast<double>(); // F converted to double and then added (generally, conversion happens on-the-fly)

// Eigen can map existing memory into Eigen matrices.
float array[3];
Vector3f::Map(array).fill(10);            // create a temporary Map over array and sets entries to 10
int data[4] = {1, 2, 3, 4};
Matrix2i mat2x2(data);                    // copies data into mat2x2
Matrix2i::Map(data) = 2*mat2x2;           // overwrite elements of data with 2*mat2x2
MatrixXi::Map(data, 2, 2) += mat2x2;      // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time)

// Solve Ax = b. Result stored in x. Matlab: x = A \ b.
x = A.ldlt().solve(b));  // A sym. p.s.d.    #include <Eigen/Cholesky>
x = A.llt() .solve(b));  // A sym. p.d.      #include <Eigen/Cholesky>
x = A.lu()  .solve(b));  // Stable and fast. #include <Eigen/LU>
x = A.qr()  .solve(b));  // No pivoting.     #include <Eigen/QR>
x = A.svd() .solve(b));  // Stable, slowest. #include <Eigen/SVD>
// .ldlt() -> .matrixL() and .matrixD()
// .llt()  -> .matrixL()
// .lu()   -> .matrixL() and .matrixU()
// .qr()   -> .matrixQ() and .matrixR()
// .svd()  -> .matrixU(), .singularValues(), and .matrixV()

// Eigenvalue problems
// Eigen                          // Matlab
A.eigenvalues();                  // eig(A);
EigenSolver<Matrix3d> eig(A);     // [vec val] = eig(A)
eig.eigenvalues();                // diag(val)
eig.eigenvectors();               // vec
// For self-adjoint matrices use SelfAdjointEigenSolver<>

This was copied from

*/

}


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

using namespace std;
using namespace Eigen;

// TAREA 2

// El codigo para la manipulacion de archivos fue modificado de la fuente dada por el curso
// -> https://aleksandarhaber.com/eigen-matrix-library-c-tutorial-saving-and-loading-data-in-from-a-csv-file/

//Funcion para guardar archivos
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();
	}
}

//Funcion para abrir archivos
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);

}

//------------------------------------------------------------------------------

//Funcion para obtener matriz identidad
MatrixXd identidad(int filas, int cols) {
	MatrixXd N;
	N.resize(filas, cols);
	for (int i = 0; i < filas; i++) {
		for (int j = 0; j < cols; j++) {
			N(i,j) = 0.0;
		}
		N(i,i) = 1.0;
	}
	return N;
}

//Funcion para cambiar filas

MatrixXd cambiarFilas(MatrixXd  N, int r1, int r2) {
	float aux;

	for (int i = 0; i < N.cols(); i++) {
		aux = N(r1,i);
		N(r1,i) = N(r2,i);
		N(r2,i) = aux;
	}

	return N;
}

//Funcion Multiplicar fila por constante

MatrixXd FilaXCte(MatrixXd M, int r, float c) {
	for (int i = 0; i < M.cols(); i++) {
		M(r,i)*= c;
	}

	return M;
}

//Funcion Sumar filas

MatrixXd sumarFilas(MatrixXd M, int r1, int r2, float c) {
	for (int i = 0; i < M.cols(); i++) {
		M(r1,i) += M(r2,i) * c;
	}

	return M;
}

//Gauss Jordan

MatrixXd gaussjordann(MatrixXd  M) {

	MatrixXd Inv = identidad(M.rows(), M.cols());
	if (M.rows() == M.cols()) {

		for (int i = 0; i < M.rows(); i++) {
			cout << Inv << "\n";

			if (M(i,i) == 0.0) {

				int r = 0;
				while (r < M.cols() && (M(i,r) == 0 || M(r,i) == 0)) {
					r++;
				}

				M = cambiarFilas(M,i,r);
			}
			system("pause");
			double c = 1.0 / M(i,i);
			M = FilaXCte(M, i, c);
			Inv = FilaXCte(Inv, i, c);
			for (int j = 0; j < M.rows(); j++) {
				if (i != j) {
					c = -M(j,i);
					M = sumarFilas(M, j, i, c);
					Inv = sumarFilas(Inv, j, i, c);
				}
			}
		}
	}else {
		printf("Error La matriz no cumple la regla de ser cuadrada \n");

	}
	return Inv;
}

//Cofactores

MatrixXd Cofactores(MatrixXd  M) {

	MatrixXd Inv = identidad(M.rows(), M.cols());
	if (M.rows() == M.cols()) {

		for (int i = 0; i < M.rows(); i++) {
			cout << Inv << "\n";
			system("pause");
			if (M(i,i) == 0) {
				int r = 0;
				while (r < M.cols() && (M(i,r) == 0 || M(r,i) == 0)) {
					r++;
				}

				M = cambiarFilas(M,i,r);
			}
			double c = 1.0 / M(i,i);
			M = FilaXCte(M, i, c);
			Inv = FilaXCte(Inv, i, c);
			for (int j = 0; j < M.rows(); j++) {
				if (i != j) {
					c = -M(j,i);
					M = sumarFilas(M, j, i, c);
					Inv = sumarFilas(Inv, j, i, c);
				}
			}
		}
	}else {
		printf("Error La matriz no cumple la regla de ser cuadrada \n");

	}
	return Inv;
}

//==========================================================================================
//============================================MAIN==========================================
//==========================================================================================
int main()
{
	//OBTENER MATRIZ DE ARCHIVO DADO -------------------------------------------------------

	string Documento;
	cout << "Introduzca el nombre del documento CSV:  \n";
	//cin >> Documento;
	Documento = "mxnC.csv";

	MatrixXd matrizDada;
	matrizDada = openData(Documento);

	cout << "la matriz m es de tamano " << matrizDada.rows() << "x" << matrizDada.cols()  << " y es la siguiente: \n";
	cout << matrizDada << "\n";
	//a) Gauss Jordan

	cout << "la matriz m invertida es: \n" << gaussjordann(matrizDada) << endl << "\n";

	//b) Cofactores

	cout << "la matriz m invertida es: \n" << Cofactores(matrizDada) << endl << "\n";

	//CREAR ARCHIVOS CON RESULTADOS ---------------------------------------------------------

	saveData("matrizInvertidaGauss.csv", gaussjordann(matrizDada));
	saveData("matrizInvertidaEigen.csv", matrizDada.inverse());
	saveData("matrizInvertidaCofactores.csv", Cofactores(matrizDada));

}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Operations.hpp
/// \brief Header file for the SO3 Lie Group math functions.
/// \details These namespace functions provide implementations of the special orthogonal (SO)
///          Lie group functions that we commonly use in robotics.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_SO3_PUBLIC_HPP
#define LGM_SO3_PUBLIC_HPP

#include <Eigen/Core>

/////////////////////////////////////////////////////////////////////////////////////////////
/// Lie Group Math - Special Orthogonal Group
/////////////////////////////////////////////////////////////////////////////////////////////
namespace lgmath {
namespace so3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 skew symmetric matrix
///
/// The hat (^) operator, builds the 3x3 skew symmetric matrix from the 3x1 vector:
///
/// v^ = [0.0  -v3   v2]
///      [ v3  0.0  -v1]
///      [-v2   v1  0.0]
///
/// See eq. 5 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d hat(const Eigen::Vector3d& vector);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds a rotation matrix using the exponential map
///
/// This function builds a rotation matrix, C_ab, using the exponential map (from an axis-
/// angle parameterization).
///
///   C_ab = exp(aaxis_ba^),
///
/// where aaxis_ba is a 3x1 axis-angle vector, the magnitude of the angle of rotation
/// can be recovered by finding the norm of the vector, and the axis of rotation is the unit-
/// length vector that arises from normalization. Note that the angle around the axis,
/// aaxis_ba, is a right-hand-rule (counter-clockwise positive) angle from 'a' to 'b'.
///
/// Alternatively, we that note that
///
///   C_ba = exp(-aaxis_ba^) = exp(aaxis_ab^).
///
/// Typical robotics convention has some oddity when it comes using this exponential map in
/// practice. For example, if we wish to integrate the kinematics:
///
///   d/dt C = omega^ * C,
///
/// where omega is the 3x1 angular velocity, we employ the convention:
///
///   C_20 = exp(deltaTime*-omega^) * C_10,
///
/// Noting that omega is negative (left-hand-rule).
/// For more information see eq. 97 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2rot(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms = 0);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds and returns both the rotation matrix and SO(3) Jacobian
///
/// Similar to the function 'vec2rot', this function builds a rotation matrix, C_ab, using an
/// equivalent expression to the exponential map, but allows us to simultaneously extract
/// the Jacobian of SO(3), which is also required in some cases.
///
///   J_ab = jac(aaxis_ba)
///   C_ab = exp(aaxis_ba^) = identity + aaxis_ba^ * J_ab
///
/// For more information see eq. 97 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
void vec2rot(const Eigen::Vector3d& aaxis_ba, Eigen::Matrix3d* out_C_ab,
             Eigen::Matrix3d* out_J_ab);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute the matrix log of a rotation matrix
///
/// Compute the inverse of the exponential map (the logarithmic map). This lets us go from
/// a 3x3 rotation matrix back to a 3x1 axis angle parameterization. In some cases, when the
/// rotation matrix is 'numerically off', this involves some 'projection' back to SO(3).
///
///   aaxis_ba = ln(C_ab)
///
/// where aaxis_ba is a 3x1 axis angle, where the axis is normalized and the magnitude of
/// the rotation can be recovered by finding the norm of the axis angle. Note that the
/// angle around the axis, aaxis_ba, is a right-hand-rule (counter-clockwise positive)
/// angle from 'a' to 'b'.
///
/// Alternatively, we that note that
///
///   aaxis_ab = -aaxis_ba = ln(C_ba) = ln(C_ab^T)
///
/// See Barfoot-TRO-2014 Appendix B2 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d rot2vec(const Eigen::Matrix3d& C_ab);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 Jacobian matrix of SO(3)
///
/// Build the 3x3 left Jacobian of SO(3).
///
/// For the sake of a notation, we assign subscripts consistence with the rotation,
///
///   J_ab = J(aaxis_ba),
///
/// although we note to the SO(3) novice that this Jacobian is not a rotation matrix, and
/// should be used with care.
///
/// For more information see eq. 98 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2jac(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms = 0);

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 inverse Jacobian matrix of SO(3)
///
/// Build the 3x3 inverse left Jacobian of SO(3).
///
/// For the sake of a notation, we assign subscripts consistence with the rotation,
///
///   J_ab_inverse = J(aaxis_ba)^{-1},
///
/// although we note to the SO(3) novice that this Jacobian is not a rotation matrix, and
/// should be used with care. Also, please note that J_ab_inverse is not equivalent to J_ba:
///
///   J(aaxis_ba)^{-1} != J(-aaxis_ba)
///
/// For more information see eq. 99 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2jacinv(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms = 0);

} // so3
} // lgmath

#endif // LGM_SO3_PUBLIC_HPP


// students_t_example1.cpp

// Copyright Paul A. Bristow 2006, 2007.

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

// Example 1 of using Student's t

// http://en.wikipedia.org/wiki/Student's_t-test  says:
// The t statistic was invented by William Sealy Gosset
// for cheaply monitoring the quality of beer brews.
// "Student" was his pen name.
// WS Gosset was statistician for Guinness brewery in Dublin, Ireland,
// hired due to Claude Guinness's innovative policy of recruiting the
// best graduates from Oxford and Cambridge for applying biochemistry
// and statistics to Guinness's industrial processes.
// Gosset published the t test in Biometrika in 1908,
// but was forced to use a pen name by his employer who regarded the fact
// that they were using statistics as a trade secret.
// In fact, Gosset's identity was unknown not only to fellow statisticians
// but to his employer - the company insisted on the pseudonym
// so that it could turn a blind eye to the breach of its rules.

// Data for this example from:
// P.K.Hou, O. W. Lau & M.C. Wong, Analyst (1983) vol. 108, p 64.
// from Statistics for Analytical Chemistry, 3rd ed. (1994), pp 54-55
// J. C. Miller and J. N. Miller, Ellis Horwood ISBN 0 13 0309907

// Determination of mercury by cold-vapour atomic absorption,
// the following values were obtained fusing a trusted
// Standard Reference Material containing 38.9% mercury,
// which we assume is correct or 'true'.
double standard = 38.9;

const int values = 3;
double value[values] = {38.9, 37.4, 37.1};

// Is there any evidence for systematic error?

// The Students't distribution function is described at
// http://en.wikipedia.org/wiki/Student%27s_t_distribution
#include <boost/math/distributions/students_t.hpp>
   using boost::math::students_t;  // Probability of students_t(df, t).

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

int main()
{
  cout << "Example 1 using Student's t function. " << endl;

  // Example/test using tabulated value
  // (deliberately coded as naively as possible).

  // Null hypothesis is that there is no difference (greater or less)
  // between measured and standard.

  double degrees_of_freedom = values-1; // 3-1 = 2
  cout << "Measurement 1 = " << value[0] << ", measurement 2 = " << value[1] << ", measurement 3 = " << value[2] << endl;
  double mean = (value[0] + value[1] + value[2]) / static_cast<double>(values);
  cout << "Standard = " << standard << ", mean = " << mean << ", (mean - standard) = " << mean - standard  << endl;
  double sd = sqrt(((value[0] - mean) * (value[0] - mean) + (value[1] - mean) * (value[1] - mean) + (value[2] - mean) * (value[2] - mean))/ static_cast<double>(values-1));
  cout << "Standard deviation = " << sd << endl;
  if (sd == 0.)
  {
      cout << "Measured mean is identical to SRM value," << endl;
      cout << "so probability of no difference between measured and standard (the 'null hypothesis') is unity." << endl;
      return 0;
  }

  double t = (mean - standard) * std::sqrt(static_cast<double>(values)) / sd;
  cout << "Student's t = " << t << endl;
  cout.precision(2); // Useful accuracy is only a few decimal digits.
  cout << "Probability of Student's t is " << cdf(students_t(degrees_of_freedom), std::abs(t)) << endl;
  //  0.91, is 1 tailed.
  // So there is insufficient evidence of a difference to meet a 95% (1 in 20) criterion.

  return 0;
}  // int main()

/*

Output is:

Example 1 using Student's t function.
Measurement 1 = 38.9, measurement 2 = 37.4, measurement 3 = 37.1
Standard = 38.9, mean = 37.8, (mean - standard) = -1.1
Standard deviation = 0.964365
Student's t = -1.97566
Probability of Student's t is 0.91

*/


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

//! \brief Given a matrix A of linearly independent columns, returns Gram-Schmidt orthonormalization
//! Ustable GS algorithm. Output is prone to cancellation issues.
//! \param[in] A Matrix of linearly independent columns
//! \return Matrix with ONB of $span(a_1, \cdots, a_n)$
template <class Matrix>
Matrix gramschmidt( const Matrix & A ) {

    Matrix Q = A;
    // First vector just gets normalized
    Q.col(0).normalize();

    for(unsigned int j = 1; j < A.cols(); ++j) {
        // Replace inner loop over each previous vector in Q with fast matrix-vector multiplication
        Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j));

        // Normalize vector if possible (othw. means colums of A almsost lin. dep.
        if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) {
            std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl;
            break;
        } else {
            Q.col(j).normalize();
        }
    }

    return Q;
}

int main(void) {
    // Ortho test
    unsigned int n = 9;
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n,n);
    Eigen::MatrixXd Q = gramschmidt( A );

    // Output should be idenity matrix
    std::cout << Q*Q.transpose() << std::endl;
}

Nummerical Methods for CSE/solutions/solution_0/gramschmidt.cpp


// Arguments: Ints, Doubles, Doubles
#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/binomial.hpp>

using stan::math::var;
using std::numeric_limits;
using std::vector;

class AgradCdfNegBinomial : public AgradCdfTest {
 public:
  void valid_values(vector<vector<double> >& parameters, vector<double>& cdf) {
    vector<double> param(3);

    param[0] = 15;  // Failures/Counts
    param[1] = 50;  // Successes/Shape
    param[2] = 3;   // logit(p)/Inverse Scale
    parameters.push_back(param);
    cdf.push_back(0.4240861277740262114122);  // expected cdf

    param[0] = 0;   // Failures/Counts
    param[1] = 15;  // Successes/Shape
    param[2] = 3;   // logit(p)/Inverse Scale
    parameters.push_back(param);
    cdf.push_back(0.013363461010158063716);  // expected cdf
  }

  void invalid_values(vector<size_t>& index, vector<double>& value) {
    // Successes/Shape
    index.push_back(1U);
    value.push_back(-1);

    // logit(p)/Inverse Scale
    index.push_back(2U);
    value.push_back(-1);
  }

  bool has_lower_bound() { return false; }

  bool has_upper_bound() { return false; }

  template <typename T_n, typename T_shape, typename T_inv_scale, typename T3,
            typename T4, typename T5>
  typename stan::return_type<T_shape, T_inv_scale>::type cdf(
      const T_n& n, const T_shape& alpha, const T_inv_scale& beta, const T3&,
      const T4&, const T5&) {
    return stan::math::neg_binomial_cdf(n, alpha, beta);
  }

  template <typename T_n, typename T_shape, typename T_inv_scale, typename T3,
            typename T4, typename T5>
  typename stan::return_type<T_shape, T_inv_scale>::type cdf_function(
      const T_n& n, const T_shape& alpha, const T_inv_scale& beta, const T3&,
      const T4&, const T5&) {
    using stan::math::binomial_coefficient_log;
    using std::exp;
    using std::log;

    typename stan::return_type<T_shape, T_inv_scale>::type cdf(0);

    for (int i = 0; i <= n; i++) {
      typename stan::return_type<T_shape, T_inv_scale>::type temp;
      temp = binomial_coefficient_log(i + alpha - 1, i);

      cdf += exp(temp + alpha * log(beta / (1 + beta))
                 + i * log(1 / (1 + beta)));
    }

    return cdf;
  }
};


#include <iostream>
#include <iomanip>
#include <string>
#include <map>
#include <unordered_map>
#include <iterator>
#include <random>
#include <tr1/cmath>  // to pick up beta()
#include <eigen3/Eigen/Core>
#include <boost/math/special_functions/binomial.hpp>

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

//double binomial_coefficient_nCk(int const n, int const k) {
//    if (n == 0 || n == k) return 1;
//    return 1/ ((n+1) * std::tr1::beta( n-k+1, k+1 ));
//}

double binomial_probability_mass_function(int const k_successes, int const n_trials, double const probability_of_success) {
//    auto bc =           binomial_coefficient_nCk(n_trials, k_successes);
    auto bc =           boost::math::binomial_coefficient<int>( n_trials, k_successes);
    auto p_k =          std::pow(probability_of_success, k_successes);
    auto p_complement = std::pow(1 - probability_of_success, n_trials - k_successes);
    return bc * p_k * p_complement;
}

void histogram_binomial_map(int const trials, double const p_success ) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::binomial_distribution<> dist(trials, p_success);
    std::map<int, int> histogram;
    cout << "Probability Mass/Density Function Graph - Binomial Distribution with trials, likelyhood: " << trials << ", " << p_success << std::endl;

    for (int n = 0; n < 1'000'000; ++n) {
        ++histogram[dist(gen)];
    }
    for (auto p : histogram) {
        cout << std::setw(4) << p.first << ' '
                  << std::setw(10) << p.second << ' '
                  << '\n';
    }
}

void histogram_binomial_vector(int const trials, double const p_success ) {
    std::random_device rd;
    std::mt19937 gen( rd() );
    std::binomial_distribution<> dist( trials, p_success );
    std::vector<int> histogram( static_cast<size_t>( trials + 1 ) , 0);
    cout << "Probability Mass/Density Function Graph - Binomial Distribution with trials, likelyhood: " << trials << ", " << p_success << std::endl;

    double probes = 1'000'000;
    for (long n = 0; n < probes; ++n) {
        ++histogram[ static_cast<size_t>( dist(gen) )];
    }
    int i = 0;
    for (auto p : histogram) {
        cout << std::setw(4) << i++ << ' '
                  << std::setw(10) << p/probes << ' '
                  << '\n';
    }
//    std::copy(histogram.begin(), histogram.end(), std::ostream_iterator<int>( cout, " \n" ) );
}

void histogram_binomial_array(int const trials, double const p_success ) {
    constexpr int MAX_TRIALS = 1000;
    assert(1 <= trials && trials <= MAX_TRIALS);
    assert( !(0.0 > p_success || p_success > 1.0) );
    std::random_device rd;
    std::mt19937 gen( rd() );
    std::binomial_distribution<> dist( trials, p_success );
    std::array<int, MAX_TRIALS> histogram;
    cout << "Probability Mass/Density Function Graph - Binomial Distribution with trials, likelyhood: " << trials << ", " << p_success << std::endl;

    double probes = 1'000'000;
    for (long n = 0; n < probes; ++n) {
        ++histogram[ static_cast<size_t>( dist(gen) )];
    }
    for (int i = 0; i < trials+1; ++i ) {
        cout << std::setw(4) << i << ' '
                  << std::setw(10) << histogram[i]/probes << ' '
                  << '\n';
    }
}

void distribution_values_binomial(double const mean, int const num_values = 100 ) { // creates a vector loaded with values from the distribution during its initialization.
    std::random_device rd;
    std::mt19937 gen(rd());

    std::poisson_distribution<int> dist( mean /*4.1*/);       // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=720
    auto poisson = [&] (int) {return dist(gen);};
    Eigen::RowVectorXi v = Eigen::RowVectorXi::NullaryExpr(num_values, poisson );
    std::cout << "Eigen::RowVextorXi:Poisson( mean ): (" << mean << "), "  << v << "\n";
}

void distribution_values_binomial(int const trials, double const p_success, int const num_values = 100 ) { // creates a vector loaded with values from the distribution during its initialization.
    std::random_device rd;
    std::mt19937 gen(rd());

    std::binomial_distribution<int> dist( trials, p_success );       // http://eigen.tuxfamily.org/bz/show_bug.cgi?id=720
    auto binomial = [&] (int) {return dist(gen);};
    Eigen::RowVectorXi v = Eigen::RowVectorXi::NullaryExpr(num_values, binomial );
    std::cout << "Eigen::RowVextorXi:Binomial( trials, p_success ): (" << trials <<", "<< p_success <<"), "  << v << "\n";
}

int main()
{
    cout << binomial_coefficient_nCk(0,0) << ", " << binomial_coefficient_nCk(5,6) << ", "<<binomial_coefficient_nCk(-5,0)<<
    int trials = 10;
    histogram_binomial_array( trials, 0.5 );
    histogram_binomial_vector( trials, 0.7 );
    histogram_binomial_vector( trials, 1.0 );
    distribution_values_binomial( 4.1, 100 );
    distribution_values_binomial( trials, 0.5, 100 );

    double mu_probability_in_bin        = 0.9;
    int n_samples_drawn                 = 10;
    double nu_probability_of_sample        = 0.1;
    int k_successes = static_cast<int>( nu_probability_of_sample * n_samples_drawn );
    auto r1 = binomial_probability_mass_function(k_successes, n_samples_drawn, mu_probability_in_bin);
    cout << "binomial_probability_mass_function(int k_successes, int n_trials, double p): " << k_successes << ", " << n_samples_drawn << ", " << mu_probability_in_bin << endl;
    cout << r1 << endl;

    mu_probability_in_bin               = 0.9;
    n_samples_drawn                     = 10;
    nu_probability_of_sample            = 0;
    k_successes = nu_probability_of_sample * n_samples_drawn;
    auto r2 = binomial_probability_mass_function(k_successes, n_samples_drawn, mu_probability_in_bin);
    cout << "binomial_probability_mass_function(int k_successes, int n_trials, double p): " << k_successes << ", " << n_samples_drawn << ", " << mu_probability_in_bin << endl;
    cout << r2 << endl;
    cout << r1+r2 << endl;

    mu_probability_in_bin               = 0.9;
    n_samples_drawn                     = 10;
    nu_probability_of_sample            = 0.4;
    k_successes = nu_probability_of_sample * n_samples_drawn;
    mu_probability_in_bin               = 0.9;
    r2 = binomial_probability_mass_function(k_successes, n_samples_drawn, mu_probability_in_bin);
    cout << "binomial_probability_mass_function(int k_successes, int n_trials, double p): " << k_successes << ", " << n_samples_drawn << ", " << mu_probability_in_bin << endl;
    cout << r2 << endl;

    mu_probability_in_bin               = 0.6;
    n_samples_drawn                     = 10;
    nu_probability_of_sample            = 0;
    k_successes = nu_probability_of_sample * n_samples_drawn;
    r2 = binomial_probability_mass_function(k_successes, n_samples_drawn, mu_probability_in_bin);
    cout << "binomial_probability_mass_function(int k_successes, int n_trials, double p): " << k_successes << ", " << n_samples_drawn << ", " << mu_probability_in_bin << endl;
    cout << r2 << endl;

    std::cout << "###" << std::endl;
    return 0;
}

/* std::cout << "Pascal's triangle:\n";
for(int n = 1; n < 10; ++n) {
    std::cout << std::string(20-n*2, ' ');
    for(int k = 1; k < n; ++k)
        std::cout << std::setw(3) << binomial_coefficient_nCk(n,k) << ' ';
    std::cout << '\n';
} */