#ifndef _EIGEN_QUADSOLVE_HPP_
#define _EIGEN_QUADSOLVE_HPP_

/*
 FILE eiquadprog.hh

 NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
       uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
       Luca Di Gaspero, working with ublas data structures.

 The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
 for the solution of a (convex) Quadratic Programming problem
by means of a dual method.

The problem is in the form:

min 0.5 * x G x + g0 x
s.t.
    CE^T x + ce0 = 0
    CI^T x + ci0 >= 0

 The matrix and vectors dimensions are as follows:
     G: n * n
		g0: n

		CE: n * p
	 ce0: p

	  CI: n * m
   ci0: m

     x: n

 The function will return the cost of the solution written in the x vector or
 std::numeric_limits::infinity() if the problem is infeasible. In the latter case
 the value of the x vector is not correct.

 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
             strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.

 Notes:
  1. pay attention in setting up the vectors ce0 and ci0.
	   If the constraints of your problem are specified in the form
	   A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
  2. The matrix G is modified within the function since it is used to compute
     the G = L^T L cholesky factorization for further computations inside the function.
     If you need the original matrix G you should make a copy of it and pass the copy
     to the function.

 The author will be grateful if the researchers using this software will
 acknowledge the contribution of this modified function and of Di Gaspero's
 original version in their research papers.

LICENSE

Copyright (2011) Benjamin Stephens
Copyright (2010) Gael Guennebaud
Copyright (2008) Angelo Furfaro
Copyright (2006) Luca Di Gaspero

This file is a porting of QuadProg++ routine, originally developed
by Luca Di Gaspero, exploiting uBlas data structures for vectors and
matrices instead of native C++ array.

uquadprog is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

uquadprog is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with uquadprog; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

*/

#include <Eigen/Core>
#include <Eigen/Cholesky>

namespace Eigen {

// namespace internal {

template<typename Scalar>
inline Scalar distance(Scalar a, Scalar b)
{
	Scalar a1, b1, t;
	a1 = std::abs(a);
	b1 = std::abs(b);
	if (a1 > b1)
	{
		t = (b1 / a1);
		return a1 * std::sqrt(1.0 + t * t);
	}
	else
		if (b1 > a1)
		{
			t = (a1 / b1);
			return b1 * std::sqrt(1.0 + t * t);
		}
	return a1 * std::sqrt(2.0);
}

// }

inline void compute_d(VectorXd &d, const MatrixXd& J, const VectorXd& np)
{
  d = J.adjoint() * np;
}

inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,  int iq)
{
  z = J.rightCols(z.size()-iq) * d.tail(d.size()-iq);
}

inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
{
  r.head(iq)= R.topLeftCorner(iq,iq).triangularView<Upper>().solve(d.head(iq));
}

bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u,  int p, int& iq, int l);

/* solve_quadprog2 is used when the Cholesky decomposition of the G matrix is precomputed */
double solve_quadprog2(LLT<MatrixXd,Lower> &chol,  double c1, VectorXd & g0,
                      const MatrixXd & CE, const VectorXd & ce0,
                      const MatrixXd & CI, const VectorXd & ci0,
                      VectorXd& x);

/* solve_quadprog is used for on-demand QP solving */
inline double solve_quadprog(MatrixXd & G,  VectorXd & g0,
                      const MatrixXd & CE, const VectorXd & ce0,
                      const MatrixXd & CI, const VectorXd & ci0,
                      VectorXd& x){

  LLT<MatrixXd,Lower> chol(G.cols());
  double c1;

  /* compute the trace of the original matrix G */
  c1 = G.trace();

  /* decompose the matrix G in the form LL^T */
  chol.compute(G);

  return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x);

}

/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed */
inline double solve_quadprog2(LLT<MatrixXd,Lower> &chol,  double c1, VectorXd & g0,
                      const MatrixXd & CE, const VectorXd & ce0,
                      const MatrixXd & CI, const VectorXd & ci0,
                      VectorXd& x)
{
  int i, j, k, l; /* indices */
  int ip, me, mi;
  int n=g0.size();
  int p=CE.cols();
  int m=CI.cols();
  MatrixXd R(g0.size(),g0.size()), J(g0.size(),g0.size());

  VectorXd s(m+p), z(n), r(m + p), d(n),  np(n), u(m + p);
  VectorXd x_old(n), u_old(m + p);
  double f_value, psi, c2, sum, ss, R_norm;
  const double inf = std::numeric_limits<double>::infinity();
  double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
    * and the full step length t2 */
  VectorXi A(m + p), A_old(m + p), iai(m + p), iaexcl(m+p);
  int q;
  int iq, iter = 0;

  me = p; /* number of equality constraints */
  mi = m; /* number of inequality constraints */
  q = 0;  /* size of the active set A (containing the indices of the active constraints) */

  /*
   * Preprocessing phase
   */

  /* initialize the matrix R */
  d.setZero();
  R.setZero();
	R_norm = 1.0; /* this variable will hold the norm of the matrix R */

	/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
  // J = L^-T
  J.setIdentity();
  J = chol.matrixU().solve(J);
	c2 = J.trace();
#ifdef TRACE_SOLVER
 print_matrix("J", J, n);
#endif

	/* c1 * c2 is an estimate for cond(G) */

	/*
   * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
   * this is a feasible point in the dual space
	 * x = G^-1 * g0
   */
  x = chol.solve(g0);
  x = -x;
	/* and compute the current solution value */
	f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
  std::cerr << "Unconstrained solution: " << f_value << std::endl;
  print_vector("x", x, n);
#endif

	/* Add equality constraints to the working set A */
  iq = 0;
	for (i = 0; i < me; i++)
	{
    np = CE.col(i);
    compute_d(d, J, np);
		update_z(z, J, d,  iq);
		update_r(R, r, d,  iq);
#ifdef TRACE_SOLVER
		print_matrix("R", R, iq);
		print_vector("z", z, n);
		print_vector("r", r, iq);
		print_vector("d", d, n);
#endif

    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
      becomes feasible */
    t2 = 0.0;
	if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
      t2 = (-np.dot(x) - ce0(i)) / z.dot(np);

    x += t2 * z;

    /* set u = u+ */
    u(iq) = t2;
    u.head(iq) -= t2 * r.head(iq);

    /* compute the new solution value */
    f_value += 0.5 * (t2 * t2) * z.dot(np);
    A(i) = -i - 1;

    if (!add_constraint(R, J, d, iq, R_norm))
    {
      // FIXME: it should raise an error
      // Equality constraints are linearly dependent
      return f_value;
    }
  }

	/* set iai = K \ A */
	for (i = 0; i < mi; i++)
		iai(i) = i;

l1:	iter++;
#ifdef TRACE_SOLVER
  print_vector("x", x, n);
#endif
  /* step 1: choose a violated constraint */
	for (i = me; i < iq; i++)
	{
	  ip = A(i);
		iai(ip) = -1;
	}

	/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
	ss = 0.0;
	psi = 0.0; /* this value will contain the sum of all infeasibilities */
	ip = 0; /* ip will be the index of the chosen violated constraint */
	for (i = 0; i < mi; i++)
	{
		iaexcl(i) = 1;
		sum = CI.col(i).dot(x) + ci0(i);
		s(i) = sum;
		psi += std::min((double)0.0, sum);
	}
#ifdef TRACE_SOLVER
  print_vector("s", s, mi);
#endif

	if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
	{
    /* numerically there are not infeasibilities anymore */
    q = iq;
		return f_value;
  }

  /* save old values for u, x and A */
   u_old.head(iq) = u.head(iq);
   A_old.head(iq) = A.head(iq);
   x_old = x;

l2: /* Step 2: check for feasibility and determine a new S-pair */
	for (i = 0; i < mi; i++)
	{
		if (s(i) < ss && iai(i) != -1 && iaexcl(i))
		{
			ss = s(i);
			ip = i;
		}
	}
  if (ss >= 0.0)
  {
    q = iq;
    return f_value;
  }

  /* set np = n(ip) */
  np = CI.col(ip);
  /* set u = (u 0)^T */
  u(iq) = 0.0;
  /* add ip to the active set A */
  A(iq) = ip;

#ifdef TRACE_SOLVER
	std::cerr << "Trying with constraint " << ip << std::endl;
	print_vector("np", np, n);
#endif

l2a:/* Step 2a: determine step direction */
  /* compute z = H np: the step direction in the primal space (through J, see the paper) */
  compute_d(d, J, np);
  update_z(z, J, d, iq);
  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
  update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
  std::cerr << "Step direction z" << std::endl;
		print_vector("z", z, n);
		print_vector("r", r, iq + 1);
    print_vector("u", u, iq + 1);
    print_vector("d", d, n);
    print_ivector("A", A, iq + 1);
#endif

  /* Step 2b: compute step length */
  l = 0;
  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
  t1 = inf; /* +inf */
  /* find the index l s.t. it reaches the minimum of u+(x) / r */
  for (k = me; k < iq; k++)
  {
    double tmp;
    if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
    {
      t1 = tmp;
      l = A(k);
    }
  }
  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
  if (std::abs(z.dot(z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
    t2 = -s(ip) / z.dot(np);
  else
    t2 = inf; /* +inf */

  /* the step is chosen as the minimum of t1 and t2 */
  t = std::min(t1, t2);
#ifdef TRACE_SOLVER
  std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif

  /* Step 2c: determine new S-pair and take step: */

  /* case (i): no step in primal or dual space */
  if (t >= inf)
  {
    /* QPP is infeasible */
    // FIXME: unbounded to raise
    q = iq;
    return inf;
  }
  /* case (ii): step in dual space */
  if (t2 >= inf)
  {
    /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
    u.head(iq) -= t * r.head(iq);
    u(iq) += t;
    iai(l) = l;
    delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
    std::cerr << " in dual space: "
      << f_value << std::endl;
    print_vector("x", x, n);
    print_vector("z", z, n);
		print_ivector("A", A, iq + 1);
#endif
    goto l2a;
  }

  /* case (iii): step in primal and dual space */

  x += t * z;
  /* update the solution value */
  f_value += t * z.dot(np) * (0.5 * t + u(iq));

  u.head(iq) -= t * r.head(iq);
  u(iq) += t;
#ifdef TRACE_SOLVER
  std::cerr << " in both spaces: "
    << f_value << std::endl;
	print_vector("x", x, n);
	print_vector("u", u, iq + 1);
	print_vector("r", r, iq + 1);
	print_ivector("A", A, iq + 1);
#endif

  if (t == t2)
  {
#ifdef TRACE_SOLVER
    std::cerr << "Full step has taken " << t << std::endl;
    print_vector("x", x, n);
#endif
    /* full step has taken */
    /* add constraint ip to the active set*/
		if (!add_constraint(R, J, d, iq, R_norm))
		{
			iaexcl(ip) = 0;
			delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
      print_matrix("R", R, n);
      print_ivector("A", A, iq);
#endif
			for (i = 0; i < m; i++)
				iai(i) = i;
			for (i = 0; i < iq; i++)
			{
				A(i) = A_old(i);
				iai(A(i)) = -1;
				u(i) = u_old(i);
			}
			x = x_old;
      goto l2; /* go to step 2 */
		}
    else
      iai(ip) = -1;
#ifdef TRACE_SOLVER
    print_matrix("R", R, n);
    print_ivector("A", A, iq);
#endif
    goto l1;
  }

  /* a patial step has taken */
#ifdef TRACE_SOLVER
  std::cerr << "Partial step has taken " << t << std::endl;
  print_vector("x", x, n);
#endif
  /* drop constraint l */
	iai(l) = l;
	delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
  print_matrix("R", R, n);
  print_ivector("A", A, iq);
#endif

  s(ip) = CI.col(ip).dot(x) + ci0(ip);

#ifdef TRACE_SOLVER
  print_vector("s", s, mi);
#endif
  goto l2a;
}

inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm)
{
 int n=J.rows();
#ifdef TRACE_SOLVER
  std::cerr << "Add constraint " << iq << '/';
#endif
	int i, j, k;
	double cc, ss, h, t1, t2, xny;

  /* we have to find the Givens rotation which will reduce the element
		d(j) to zero.
		if it is already zero we don't have to do anything, except of
		decreasing j */
	for (j = n - 1; j >= iq + 1; j--)
	{
    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
			 If cc is one, then element (j) of d is zero compared with element
			 (j - 1). Hence we don't have to do anything.
			 If cc is zero, then we just have to switch column (j) and column (j - 1)
			 of J. Since we only switch columns in J, we have to be careful how we
			 update d depending on the sign of gs.
			 Otherwise we have to apply the Givens rotation to these columns.
			 The i - 1 element of d has to be updated to h. */
		cc = d(j - 1);
		ss = d(j);
		h = distance(cc, ss);
		if (h == 0.0)
			continue;
		d(j) = 0.0;
		ss = ss / h;
		cc = cc / h;
		if (cc < 0.0)
		{
			cc = -cc;
			ss = -ss;
			d(j - 1) = -h;
		}
		else
			d(j - 1) = h;
		xny = ss / (1.0 + cc);
		for (k = 0; k < n; k++)
		{
			t1 = J(k,j - 1);
			t2 = J(k,j);
			J(k,j - 1) = t1 * cc + t2 * ss;
			J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
		}
	}
  /* update the number of constraints added*/
	iq++;
  /* To update R we have to put the iq components of the d vector
    into column iq - 1 of R
    */
  R.col(iq-1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
  std::cerr << iq << std::endl;
#endif

	if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
		// problem degenerate
		return false;
	R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
	return true;
}

inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u,  int p, int& iq, int l)
{

  int n = R.rows();
#ifdef TRACE_SOLVER
  std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
	int i, j, k, qq;
	double cc, ss, h, xny, t1, t2;

	/* Find the index qq for active constraint l to be removed */
  for (i = p; i < iq; i++)
  if (A(i) == l)
  {
    qq = i;
    break;
  }

  /* remove the constraint from the active set and the duals */
  for (i = qq; i < iq - 1; i++)
  {
    A(i) = A(i + 1);
    u(i) = u(i + 1);
    R.col(i) = R.col(i+1);
  }

  A(iq - 1) = A(iq);
  u(iq - 1) = u(iq);
  A(iq) = 0;
  u(iq) = 0.0;
  for (j = 0; j < iq; j++)
    R(j,iq - 1) = 0.0;
  /* constraint has been fully removed */
  iq--;
#ifdef TRACE_SOLVER
  std::cerr << '/' << iq << std::endl;
#endif

  if (iq == 0)
    return;

  for (j = qq; j < iq; j++)
  {
    cc = R(j,j);
    ss = R(j + 1,j);
    h = distance(cc, ss);
    if (h == 0.0)
      continue;
    cc = cc / h;
    ss = ss / h;
    R(j + 1,j) = 0.0;
    if (cc < 0.0)
    {
      R(j,j) = -h;
      cc = -cc;
      ss = -ss;
    }
    else
      R(j,j) = h;

    xny = ss / (1.0 + cc);
    for (k = j + 1; k < iq; k++)
    {
      t1 = R(j,k);
      t2 = R(j + 1,k);
      R(j,k) = t1 * cc + t2 * ss;
      R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
    }
    for (k = 0; k < n; k++)
    {
      t1 = J(k,j);
      t2 = J(k,j + 1);
      J(k,j) = t1 * cc + t2 * ss;
      J(k,j + 1) = xny * (J(k,j) + t1) - t2;
    }
  }
}

}

#endif


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

using namespace Eigen;
using namespace std;

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

  MatrixXf result(2,2);

  //initialize matrices
  m << 1,2,
       3,4;

  n << 5,6,
       7,8;

  // mix of array and matrix operations
  //   first coefficient-wise addition
  //   then the result is used with matrix multiplication
  result = (m.array() + 4).matrix() * m;

  cout << "-- Combination 1: --" << endl
    << result << endl << endl;

  // mix of array and matrix operations
  //   first coefficient-wise multiplication
  //   then the result is used with matrix multiplication
  result = (m.array() * n.array()).matrix() * m;

  cout << "-- Combination 2: --" << endl
    << result << endl << endl;

}


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

using namespace Eigen;
using namespace std;

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

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


#include <geogram/basic/geometry_nd.h>
#include <Eigen/Geometry>
#include <cassert>
#undef IGL_STATIC_LIBRARY
#include <igl/point_simplex_squared_distance.h>

#define sqr(x) (x) * (x)

double inner_point_box_squared_distance(const Eigen::Vector3d& p, const Eigen::AlignedBox3d& B)
{
    assert(B.contains(p));
    double result = sqr(p[0] - B.min()[0]);
    result = std::min(result, sqr(p[0] - B.max()[0]));
    for (int c = 1; c < 3; ++c) {
        result = std::min(result, sqr(p[c] - B.min()[c]));
        result = std::min(result, sqr(p[c] - B.max()[c]));
    }
    return result;
}

double point_box_signed_squared_distance(const Eigen::Vector3d& p, const Eigen::AlignedBox3d& B)
{
    bool inside = true;
    double result = 0.0;
    for (int c = 0; c < 3; c++) {
        if (p[c] < B.min()[c]) {
            inside = false;
            result += sqr(p[c] - B.min()[c]);
        }
        else if (p[c] > B.max()[c]) {
            inside = false;
            result += sqr(p[c] - B.max()[c]);
        }
    }
    if (inside) {
        result = -inner_point_box_squared_distance(p, B);
    }
    return result;
}

double point_box_center_squared_distance(const Eigen::Vector3d& p, const Eigen::AlignedBox3d& B)
{
    double result = 0.0;
    for (int c = 0; c < 3; ++c) {
        double d = p[c] - 0.5 * (B.min()[c] + B.max()[c]);
        result += sqr(d);
    }
    return result;
}

void get_point_facet_nearest_point(const Eigen::MatrixXd& V,
                                   const Eigen::MatrixXi& F,
                                   const Eigen::Vector3d& p,
                                   int f,
                                   Eigen::Vector3d& nearest_p,
                                   double& squared_dist)
{
    assert(F.cols() == 3);
#if 0
    igl::point_simplex_squared_distance<3>(p, V, F, f, squared_dist, nearest_p);
#else
    GEO::vec3 query(p.data());
    GEO::vec3 pts[3];
    for (int lv = 0; lv < 3; ++lv) {
        int i = F(f, lv);
        pts[lv] = GEO::vec3(V(i, 0), V(i, 1), V(i, 2));
    }
    double lambda1, lambda2, lambda3;  // barycentric coords, not used.
    GEO::vec3 x;
    squared_dist = GEO::Geom::point_triangle_squared_distance(query, pts[0], pts[1], pts[2], x,
                                                              lambda1, lambda2, lambda3);
    nearest_p << x[0], x[1], x[2];
#endif
}


#include <iostream>

#include <Eigen/Dense>
#include <Eigen/LU>

#include "AprilTags/GrayModel.h"

namespace AprilTags {

GrayModel::GrayModel() : A(), v(), b(), nobs(0), dirty(false) {
  A.setZero();
  v.setZero();
  b.setZero();
}

void GrayModel::addObservation(float x, float y, float gray) {
  float xy = x*y;

  // update only upper-right elements. A'A is symmetric,
  // we'll fill the other elements in later.
  A(0,0) += x*x;
  A(0,1) += x*y;
  A(0,2) += x*xy;
  A(0,3) += x;
  A(1,1) += y*y;
  A(1,2) += y*xy;
  A(1,3) += y;
  A(2,2) += xy*xy;
  A(2,3) += xy;
  A(3,3) += 1;

  b[0] += x*gray;
  b[1] += y*gray;
  b[2] += xy*gray;
  b[3] += gray;

  nobs++;
  dirty = true;
}

float GrayModel::interpolate(float x, float y) {
  if (dirty) compute();
  return v[0]*x + v[1]*y + v[2]*x*y + v[3];
}

void GrayModel::compute() {
  // we really only need 4 linearly independent observations to fit our answer, but we'll be very
  // sensitive to noise if we don't have an over-determined system. Thus, require at least 6
  // observations (or we'll use a constant model below).

  dirty = false;
  if (nobs >= 6) {
    // make symmetric
    Eigen::Matrix4d Ainv;
    for (int i = 0; i < 4; i++)
      for (int j = i+1; j < 4; j++)
        A(j,i) = A(i,j);

    //    try {
    //      Ainv = A.inverse();
    bool invertible;
    double det_unused;
    A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible);
    if (invertible) {
      v = Ainv * b;
      return;
    }
    std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
    //    }
    //    catch (std::underflow_error&) {
    //      std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
    //    }
  }

  // If we get here, either nobs < 6 or the matrix inverse generated
  // an underflow, so use a constant model.
  v.setZero();   // need the cast to avoid operator= ambiguity wrt. const-ness
  v[3] = b[3] / nobs;
}

} // namespace


/**
 * @file systemode_main.cc
 * @brief NPDE homework SystemODE
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <iomanip>
#include <iostream>

#include "../../../lecturecodes/helperfiles/polyfit.h"
#include "systemode.h"

/* SAM_LISTING_BEGIN_0 */
int main() {
  // PARAMETERS
  double T = 1;
  int n = 5;

  // INITIAL VALUE
  Eigen::VectorXd y0(2 * n);
  for (int i = 0; i < n; ++i) {
    y0(i) = (i + 1.) / n;
    y0(i + n) = -1;
  }

  // SETUP
  double conv_rate = 0;
  std::cout << std::setw(8) << "M" << std::setw(20) << "Error" << std::endl;

  // IMPLEMENTATION OF RIGHT-HAND SIDE f
  // Build tridiagonal C matrix
  Eigen::SparseMatrix<double> C(n, n);
  C.reserve(Eigen::VectorXi::Constant(n, 3));
  C.insert(0, 0) = 2;
  for (int i = 1; i < n; ++i) {
    C.insert(i, i) = 2;
    C.insert(i, i - 1) = -1;
    C.insert(i - 1, i) = -1;
  }
  C.makeCompressed();
  // Compute the right-hand side f
  // The system of ODEs is y' = f(y) with y = [u;v],
  // and f(y) = f([u;v]) = [v;C^{-1}r(u)]
  auto f = [n, C](Eigen::VectorXd y) {
    Eigen::VectorXd fy(2 * n);
    fy.head(n) = y.tail(n);
    Eigen::VectorXd r(n);
    r(0) = y(0) * (y(1) + y(0));
    r(n - 1) = y(n - 1) * (y(n - 1) + y(n - 2));
    for (int i = 1; i < n - 1; ++i) {
      r(i) = y(i) * (y(i - 1) + y(i + 1));
    }
    Eigen::SparseLU<Eigen::SparseMatrix<double>> Csolver;
    Csolver.compute(C);
    fy.tail(n) = Csolver.solve(r);
    return fy;
  };

  // COMPUTE AN "EXACT" SOLUTION
  // Use N=2^12 steps to calculate an approximate "exact" solution.
  int N_exact = std::pow(2, 12);  // number of steps
  double h = T / N_exact;         // step size
  Eigen::VectorXd yT_exact = y0;  // initial value
  Eigen::VectorXd y_next;
  for (int step = 0; step < N_exact; step++) {
    y_next = SystemODE::rk4step(f, h, yT_exact);
    yT_exact = y_next;
  }

  // CONVERGENCE ANALYSIS
  // Calculate solution using N=2,...,2^kmax steps.
  int kmax = 10;
  Eigen::VectorXd Error(kmax);
  for (int k = 0; k < kmax; k++) {
    int M = std::pow(2, k + 1);  // number of steps
    double h = T / M;            // step size
    Eigen::VectorXd yT = y0;     // initial value
    // Take N RK4 steps:
    for (int step = 0; step < M; step++) {
      // yT is the solution at time t=h*step
      y_next = SystemODE::rk4step(f, h, yT);
      yT = y_next;
    }
    Error(k) = (yT - yT_exact).norm();
    std::cout << std::setw(8) << M << std::setw(20) << Error(k) << std::endl;
  }

  // Estimate convergence rate
  // Get natural logarithm of M by log(M) = log(2)*log2(N).
  Eigen::VectorXd logM =
      std::log(2) * Eigen::VectorXd::LinSpaced(kmax, 1, kmax);
  Eigen::VectorXd coeffs = polyfit(logM, Error.array().log(), 1);
  conv_rate = coeffs(0);

  std::cout << "Convergence rate: " << std::round(std::abs(conv_rate))
            << std::endl;

  return 0;
}
/* SAM_LISTING_END_0 */


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

#pragma once

// This header is private, do not include it directly, include pnp.hpp instead.

// An implementation of the EPnP algorithm (for >=6 points) based on the following publication:
// V. Lepetit, F. Moreno-Noguer, P. Fua, EPnP: An Accurate O(n) Solution to the PnP Problem,
// International Journal Of Computer Vision (IJCV), 2009
// See manuscript at http://infoscience.epfl.ch/record/160138/files/top.pdf
// TODO: extend to 4,5 points

#include <Eigen/Dense>

#include "packages/pnp/gems/generic/utils.hpp"
#include "packages/pnp/gems/pnp.hpp"  // using status codes from the public interface

namespace isaac {
namespace pnp {
namespace epnp {

// Output pose and internal results from ComputeCameraPose()
// Output pose as cam_point = rotation * world_point + translation
// where world_point is a 3D point in world coords and cam_point in camera coords.
// Note that cam_position = -rotation.transpose() * translation,
// given that cam_point = rotation * (world_point - cam_position)
struct Result {
  Matrix3d rotation;           // camera orientation matrix
  Vector3d translation;        // translation vector
  int input_dims;              // dimensionality of the input point cloud
  Matrix3Xd ctl_points_world;  // control points in world coordinates (chosen 3D basis)
  Matrix3Xd ctl_points_cam;    // control points in camera coordinates
  MatrixXd bary_coeffs;        // barycentric coordinates of the input 3D points
  MatrixXd proj_coeffs;        // coefficient matrix in the projection equations (M in Eq.7)
  MatrixXd solution_basis;     // 4 basis vectors of the solution space of ctl_points_cam
  VectorXd singular_values;    // all singular values of proj_coeffs in increasing order
  VectorXd rss_repr_errors;    // RSS reprojection error for each solution space dimension
  int solution_dims;           // optimal #dimensions of the solution space (based on repr. error)
};

// High-level function that implements the full EPnP pipeline by using the other functions below.
// The other functions are exposed to enable detailed testing of individual parts.
// Requires the 4 standard camera intrinsic parameters (assumes pre-calibrated camera) and
// at least 6 2D-3D point correspondences without gross outliers for the result to be meaningful.
// Outputs the 3D pose of the camera and some internal results in Result given:
//  focal_u,v      relative focal lengths in horizontal / vertical pixel sizes from calibration
//  principal_u,v  principal point coordinates in pixels from calibration
//  points3        input 3D points as 3xN matrix (N>=6)
//  points2        input 2D points as 2xN matrix (in corresponding order to 3D points)
pnp::Status ComputeCameraPose(double focal_u, double focal_v, double principal_u,
                              double principal_v, const Matrix3Xd& points3,
                              const Matrix2Xd& points2, epnp::Result* result);

// Compute 3D basis aligned with a 3D point cloud: origin in the centroid and axes aligned and
// scaled with the principal components.
// Returns the basis as D+1 control points in columns of a 3x(D+1) matrix: the centroid, followed
// by axis end-points along non-vanishing principal directions in decreasing order of length.
// Possible cases:
//   D=0: all input points coincide within tolerance or no valid input (unusable for EPnP)
//   D=1: all input points are along a line up to tolerance (unusable for EPnP)
//   D=2: all input points are in a plane up to tolerance (planar case, works with EPnP)
//   D=3: input points are non-planar (works with EPnP)
// In case of failure a 3x0 matrix is returned.
Matrix3Xd ChooseBasis(const Matrix3Xd& points3, double tol = 1e-3);

// Compute barycentric coordinates of N 3D points with respect to a basis defined by
// 3 or 4 control points, subject to hom(points3) = hom(ctl_points) * bary_coords,
// where hom() denotes addition of an extra row of all 1's.
//   points3       Input 3D points as a 3xN matrix.
//   ctl_points    3xC matrix containing C = 3 or 4 control points, see ChooseBasis() for details.
// Returns a CxN matrix of barycentric coefficients with column sums equal to 1.
// These barycentric coordinates can be negative or positive with no particular bound in general
// because control points from ChooseBasis() are based on variances (PCA) and individual points
// can be arbitrarily far from the population.
MatrixXd ComputeBaryCoords(const Matrix3Xd& points3, const Matrix3Xd& ctl_points);

// Find the solution space of the projection equations (Eqs. 4-7 in the paper above).
// Projection can be written in the homogeneous linear form
//                          M*x = 0    (Eq.7 in the paper),
// where x is a vector of 9 or 12 unknown 3D camera coordinates of 3 (planar case) or 4 control
// points and M is a 2Nx9 (planar case) or 2Nx12 matrix (non-planar case).
// The least 1,2,3 or 4 singular values of M may all be close to 0 and any vector x lying in
// a 1-,2-,3- or 4-D solution space will satisfy the constraints M*x ~ 0.
// Instead of arbitrarily thresholding the singular values, EPnP considers
// the first 1,2,3 or 4 columns of the 9x4 (planar case) or 12x4 output matrix
// sol_basis as the basis of the solution space.
// See SolveControlPoints() for the solution in each case (in 1,2,3,4 dimensional solution space).
//  focal_u,v      relative focal lengths in horizontal / vertical pixel sizes
//  principal_u,v  principal point coordinates in pixels
//  bary_coords    3xN (planar case) or 4xN matrix, the barycentric coordinates of N>=6 3-D points
//  points2        2D projection coordinates of the 3D points
//  proj_coeffs    Coefficient matrix of the homogenenous form of the projections (see above)
//  sol_basis      9x4 (planar case) or 12x4 matrix, 4 basis vectors considered for solution space.
//  sing_values    9 or 12 singular values of M sorted in increasing order.
//                 The first 4 singular values correspond to columns of sol_basis.
// Returns true in case of success and outputs undefined in case of failure.
bool SolveProjConstraints(double focal_u, double focal_v, double principal_u, double principal_v,
                          const MatrixXd& bary_coords, const Matrix2Xd points2,
                          MatrixXd* proj_coeffs, MatrixXd* sol_basis, VectorXd* sing_values);

// Place elements of a vector of length 3*N into a 3xN matrix column-wise.
// In EPnP, this is applied to the 3*N solution vector of 3D camera coordinates of N control points.
// If the length of the input vector is not a multiple of 3, an empty 3x0 matrix is returned.
Matrix3Xd ReshapeToMatrix3xN(const VectorXd& vec);

// Given the solution space for the camera coordinates of the control points and given all pairwise
// distances between control points to preserve, compute weights of the linear combination
// of the basis vectors (Eq.8 in the EPnP paper) such that the control points can be obtained as
//                       ReshapeToMatrix3xN(sol_basis * weights).
// The solution space can be 1,2,3 or 4 dimensional in theory.
//  sol_basis      Solution space basis vectors output by SolveProjConstraints().
//  sol_dims       Number of basis vectors in sol_basis to consider for the solution.
//                 Supported values: 1,2 for planar and 1,2,3 for non-planar case.
//  distances      All pairwise distances between control points as returned by ComputeDistances().
//                 3 distances for 3 points (planar case) and 6 distances for 4 points (non-planar).
// Returns 4 weights. All weights are zero for unsupported sol_dims values and
// only the first sol_dims weights are non-zero in supported cases (see above).
Vector4d SolveControlPoints(const MatrixXd& sol_basis, int sol_dims, const VectorXd& distances);

// Compute distances between all possible pairs (i,j) of N input points.
// Input points are in matrix columns, and i,j are column indices.
// Every j = i+1,i+2,...,N-1 are listed first for each of i = 0,1,...,N-2
// EPnP only applies this for the 3 or 4 control points.
// Order for 4 points: (0,1)(0,2)(0,3)(1,2)(1,3)(2,3)
// Order for 3 points: (0,1)(0,2)(1,2)
VectorXd ComputeDistances(const MatrixXd& points);

// Project a set of 3D points directly given in camera coordinates into the image.
// Camera intrinsics are provided in the upper triangular 3x3 camera calibration matrix.
// Returns the 2D projections in a 2xN matrix of pixel coordinates for a 3xN input point matrix.
// This function serves to validate solutions to the projection equations and,
// therefore, intentionally does not make a distinction of points behind the camera.
Matrix2Xd ProjectPoints(const Matrix3d& calib_matrix, const Matrix3Xd& points3);

}  // namespace epnp
}  // namespace pnp
}  // namespace isaac


#include <iostream>
#include <vector>

#include <Eigen/Dense>

using Eigen::MatrixXd;

typedef Eigen::Matrix<float, 3, 3> Matrix33f;
typedef Eigen::Matrix<float, 3, 1> Vector3f;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> DMatrix;  // dynamic matrix

void initial() {
    Matrix33f a;
    Vector3f v;
    DMatrix m(10, 5);

    a = Matrix33f::Identity();
    a << 1, 2, 3, 4, 5, 6, 7, 8, 9;  // comma-initializer syntax

    // change an element of matrix directly
    a(0, 0) = 4;

    v = Vector3f::Random();

    std::cout << a << std::endl;
    std::cout << v << std::endl;
}

void initialByMap() {
    int data[] = {1,2,3,4};
    Eigen::Map<Eigen::RowVectorXi> v(data,4);
    std::vector<float> data1 = {1,2,3,4,5,6,7,8,9};
    Eigen::Map<Matrix33f> a(data1.data());

    std::cout << v << std::endl;
    std::cout << a << std::endl;
}

void calc() {
    Matrix33f a, b;
    a = Matrix33f::Random();
    b = Matrix33f::Random();

    std::cout << a+b << std::endl;  // eigen overload operators, like +/-/*
    std::cout << a.array() * b.array() << std::endl;  // element-wise multiplication
    std::cout << a*b << std::endl;   // matrix multiplication

}

int main()
{
    std::cout << "intial: " << std::endl;
    initial();

    std::cout << "intial by Eigen::Map: " << std::endl;
    initialByMap();

    std::cout << "calculate use Eigen: " << std::endl;
    calc();

    return 0;
}


/***********************************************************************
* Short Title: linear algebra functions on R3
*
* Comments: declaration of necessary linear algebra functions for
*     blitz::TinyVector  and  blitz::TinyMatrix
*
* <license text>
***********************************************************************/

#ifndef R3LINALG_HPP_INCLUDED
#define R3LINALG_HPP_INCLUDED

#include <blitz/array.h>

#include "Counter.hpp"

namespace R3 {

////////////////////////////////////////////////////////////////////////
// Declarations
////////////////////////////////////////////////////////////////////////

// constants

const int Ndim = 3;
using blitz::all;

// types

typedef blitz::TinyMatrix<double,Ndim,Ndim> Matrix;
typedef blitz::TinyVector<double,Ndim> Vector;

// functions

double determinant(const Matrix& A);
Matrix inverse(const Matrix& A);
Matrix transpose(const Matrix& A);
const Matrix& product(const Matrix&, const Matrix&);

template <class V> double norm(const V&);
template <class V> double distance(const V& u, const V& v);
template <class V> double dot(const V& u, const V& v);
template <class V> Vector cross(const V& u, const V& v);
const Vector& product(const Vector&, const Matrix&);

template <class M>
    bool MatricesAlmostEqual(const M& A, const M& B, double precision=0.0);

template <class V>
    bool VectorsAlmostEqual(const V& A, const V& B, double precision=0.0);

////////////////////////////////////////////////////////////////////////
// Definitions
////////////////////////////////////////////////////////////////////////

template <class V>
inline double norm(const V& u)
{
    static Counter* R3_norm_calls = Counter::getCounter("R3_norm_calls");
    R3_norm_calls->count();
    return sqrt(R3::dot(u, u));
}

template <class V>
inline double distance(const V& u, const V& v)
{
    static Counter* R3_distance_calls =
        Counter::getCounter("R3_distance_calls");
    R3_distance_calls->count();
    static R3::Vector duv;
    duv[0] = u[0] - v[0];
    duv[1] = u[1] - v[1];
    duv[2] = u[2] - v[2];
    return R3::norm(duv);
}

template <class V>
inline double dot(const V& u, const V& v)
{
    return (u[0]*v[0] + u[1]*v[1] + u[2]*v[2]);
}

template <class V>
inline Vector cross(const V& u, const V& v)
{
    Vector res;
    res[0] = u[1]*v[2] - u[2]*v[1];
    res[1] = u[2]*v[0] - u[0]*v[2];
    res[2] = u[0]*v[1] - u[1]*v[0];
    return res;
}

inline const Vector& product(const Vector& u, const Matrix& M)
{
    static Vector res;
    res[0] = u[0]*M(0,0) + u[1]*M(1,0)+ u[2]*M(2,0);
    res[1] = u[0]*M(0,1) + u[1]*M(1,1)+ u[2]*M(2,1);
    res[2] = u[0]*M(0,2) + u[1]*M(1,2)+ u[2]*M(2,2);
    return res;
}

template <class M>
bool MatricesAlmostEqual(const M& A, const M& B, double precision)
{
    for (int i = 0; i < Ndim; ++i)
    {
        for (int j = 0; j < Ndim; ++j)
        {
            if (fabs(A(i,j) - B(i,j)) > precision)  return false;
        }
    }
    return true;
}

template <class V>
bool VectorsAlmostEqual(const V& u, const V& v, double precision)
{
    for (int i = 0; i < Ndim; ++i)
    {
        if (fabs(u[i] - v[i]) > precision)  return false;
    }
    return true;
}

}       // End of namespace R3

#endif  // R3LINALG_HPP_INCLUDED


#include <iostream>

#include <Eigen/Dense>

using namespace Eigen;

int main()
{
    MatrixXd m(2, 2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    std::cout << "Here is the matrix m:\n"
              << m << std::endl;
    VectorXd v(2);
    v(0) = 4;
    v(1) = v(0) - 1;
    std::cout << "Here is the vector v:\n"
              << v << std::endl;

    MatrixXf A = MatrixXf::Random(3, 2);
    VectorXf b = VectorXf::Random(3);
    std::cout << "The least squares solution to Ax=b is\n"
              << A.fullPivHouseholderQr().solve(b) << '\n';

    return 0;
}


#include "ThinPlateSpline.hpp"
#include <Eigen/QR>

ThinPlateSpline::ThinPlateSpline(const PointList &src, const PointList &dst)
    : mSrcPoints(src), mDstPoints(dst) {}

void ThinPlateSpline::solve() {

  if (mSrcPoints.size() != mDstPoints.size())
    return;

  const int num(int(mSrcPoints.size()));
  const int rows(num + 3 + 1);

  // Create L Matrix
  mL = Eigen::MatrixXd::Zero(rows, rows);

  for (int i(0); i < num; ++i) {

    int j(i + 1);

    for (; j < num; ++j)
      mL(i, j) = mL(j, i) = radialBasis(
          (mSrcPoints[std::size_t(i)] - mSrcPoints[std::size_t(j)]).norm());

    mL(j, i) = mL(i, j) = 1.0;
    ++j;

    for (int posElm(0); j < rows; ++posElm, ++j)
      mL(j, i) = mL(i, j) = mSrcPoints[std::size_t(i)][posElm];
  }

  // Create Y Matrix
  Eigen::MatrixXd Y = Eigen::MatrixXd::Zero(rows, 3);

  for (int i(0); i < num; ++i)
    Y.row(i) = mDstPoints[std::size_t(i)];

  // Solve L W^T = Y as W^T = L^-1 Y
  mW = mL.colPivHouseholderQr().solve(Y);
}

Eigen::Vector3d ThinPlateSpline::interpolate(const Eigen::Vector3d &p) const {

  Eigen::Vector3d res = Eigen::Vector3d::Zero();
  int i(0);

  for (; i < mW.rows() - (3 + 1); ++i) {
    double rb = radialBasis((mSrcPoints[std::size_t(i)] - p).norm());
    res += mW.row(i) * rb;
  }

  res += mW.row(i);
  i++;

  for (int j(0); j < 3; ++j, ++i)
    res += mW.row(i) * p[j];

  return res;
}


#include <iostream>
#include <Eigen\Core>
#include "pca.h"

using namespace std;
using Eigen::ArrayXXf;
using Eigen::MatrixXf;
using Eigen::VectorXf;

using namespace eos::pca;

int main(int argc, char** argv) {
	VectorXf eigenvalues;
	MatrixXf eigenvectors;

	MatrixXf meanfree_data = MatrixXf::Random(10, 50); // rows, cols
	cout << "meanfree_data: \n" << meanfree_data << endl;

	Eigen::RowVectorXf mean_data = meanfree_data.colwise().mean();
	cout << "colwise().mean(): \n" << mean_data << endl;

	meanfree_data.rowwise() -= mean_data;
	cout << "meanfree_data: \n" << meanfree_data << endl;

	Covariance covariance_type = Covariance::AtA;

	std::tie(eigenvectors, eigenvalues) = pca(meanfree_data, covariance_type);
	cout << "Eigenvectors: \n" << eigenvectors << endl;
	cout << "Eigenvalues: \n" << eigenvalues << endl;

	system("pause");
	return EXIT_SUCCESS;
}


#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <Eigen/Dense>
#include <Eigen/LU>

using namespace Eigen;

// ガウス分布の数
#define K 2

// 次元数
#define D 2

// データの数
#define N 10

#define PI 4*atan(1.0)

// TODO: D,1 にする
typedef Eigen::Matrix<float, 1, D> m_d;

// 多次元 (多変量) ガウス分布
float gaussian(m_d& x, m_d& mu, Matrix<float,D,D>& sigma) {
  return exp( -0.5 * (x - mu).dot( sigma.inverse() * (x - mu).transpose() ) )
    / pow(sqrt(2 * PI), D) * sqrt(sigma.determinant());
}

float likelihood(
  std::vector<float>& pi,
  std::vector<m_d>& x,
  std::vector<m_d>& mu,
  std::vector<Matrix<float,D,D> >& sigma
) {
  float s = 0.0;
  for(int n=0; n<N; n++) {
    float t = 0.0;
    for(int k=0; k<K; k++) {
      t += pi[k] * gaussian(x[n], mu[k], sigma[k]);
    }
    s += log(t);
  }
  return s;
}

int main() {
  // TODO: matrix<float, N, 1> にする
  // TODO: 実際に使えるデータを用意する
  // N 行 D 列
  std::vector<m_d> x(N, m_d::Random());

  // 平均 mu, 分散 sigma, 混合係数 pi を初期化する
  // 平均
  /*
      [
        [mu_x, mu_y], // 次元数 (D) 個
        [mu_x, mu_y],
        ... K 個
      ]
      ガウス分布 K に対する, X軸の平均，Y軸の平均 ...
   */
  std::vector<m_d> mu(K, m_d::Zero());

  std::random_device rd;
  std::mt19937 mt(rd());
  std::uniform_real_distribution<double> distribution(0.0, 1.0);

  // 分散 (分散共分散行列)
  // D x D 行列
  Matrix<float, 1, D> v;
  for(int i=0;i<D;i++) {
    v(i) = distribution(mt);
  }
  std::vector<Matrix<float,D,D> > sigma(K, Matrix<float,D,D>(v.asDiagonal()));

  // TODO: 制約 Sigma(k) pi_k = 1 を満たすように初期化する
  std::vector<float> pi(K);
  for(int i=0; i<K; i++) {
    pi[i] = distribution(mt);
  }

  // gamma, N, K
  std::vector<std::vector<float> > gamma(N, std::vector<float>(K));

  // 尤度
  float like = likelihood(pi, x, mu, sigma);

  while(true) {
    // E-step: パラメータ (mu, sigma, pi) を使って負担率 gamma を計算する
    for(int n = 0; n < N; n++) {
      float t = 0.0;
      for(int k = 0; k < K; k++) {
        t += pi[k] * gaussian(x[n], mu[k], sigma[k]);
      }

      for(int k = 0; k < K; k++)
        gamma[n][k] = pi[k] * gaussian(x[n], mu[k], sigma[k]) / t;
    }

    // M-step: 負担率を使ってパラメータを更新する
    for(int k = 0; k < K; k++) {
      // N_k
      float Nk = 0.0;
      for(int n = 0; n < N; n++) {
        Nk += gamma[n][k];
      }

      // 平均
      m_d _mu = m_d::Zero();
      for(int n = 0; n < N; n++) {
        _mu += gamma[n][k] * x[n];
      }
      mu[k] = _mu / Nk;

      // 分散
      Matrix<float, D, D> _sigma = Matrix<float,D,D>::Zero();
      for(int n = 0; n < N; n++) {
        _sigma += gamma[n][k] * (x[n] - mu[k]).transpose() * (x[n] - mu[k]);
      }
      sigma[k] = _sigma / Nk;

      // 混合係数
      pi[k] = Nk / N;

    }

    // TODO: x と mu の値が同じになる問題, x の初期値に Random を使っているせい ?
    std::cout << x[0] << "\n" << mu[0] << "\n" << sigma[0] << std::endl;

    // 収束性の確認
    // 対数尤度関数 Sigma(n) { log ( Sigma(k) pi * N )}
    float _like = likelihood(pi, x, mu, sigma);
    std::cout << _like << std::endl;
    if(_like - like < 0.01 || _like == nan)
      break;
    like = _like;

  }
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant
    Generates a value of the chosen type which represents the conjugate Golden Ratio.

    The conjugate Golden Ratio (\f$\bar\phi\f$) is defined as \f$\frac{1-\sqrt5}{2}\f$.

    @par Semantic:

    @code
    T r = Cgold<T>();
    @endcode

    is equivalent to:

    @code
    T r = (1-simd::sqrt(T(5)))/2;
    @endcode

    @return A value of type @c T containing the conjugate Golden Ratio.

    @see functional::cgold
  **/
  template<typename T> T Cgold();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generates a value of the chosen type which represents the conjugate Golden Ratio.

      The conjugate Golden Ratio (\f$\bar\phi\f$) is defined as \f$\frac{1-\sqrt5}{2}\f$.

      @par Semantic:

      For any value @c x of type @c T:
      @code
      T r = simd::functional::cgold( boost::simd::as(x));
      @endcode

      is equivalent to:

      @code
      T r = simd::Cgold<T>();
      @endcode

      @return A value of type @c T containing the conjugate Golden Ratio.

      @see Cgold
    **/
    Value Cgold();
  }
} }
#endif

#include <boost/simd/constant/definition/cgold.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/** @file 25.cpp Problem 25: 1000-digit Fibonacci number
 *
 * The Fibonacci sequence is defined by the recurrence relation:
 *
 *      F[n] = F[n-1] + F[n-2], where F[1] = 1 and F[2] = 1.
 *
 * Hence the first 12 terms will be:
 *
 *      F[1] = 1
 *      F[2] = 1
 *      F[3] = 2
 *      F[4] = 3
 *      F[5] = 5
 *      F[6] = 8
 *      F[7] = 13
 *      F[8] = 21
 *      F[9] = 34
 *      F[10] = 55
 *      F[11] = 89
 *      F[12] = 144
 *
 * The 12th term, F[12], is the first term to contain three digits.
 *
 * What is the first term in the Fibonacci sequence to contain 1000 digits?
 */

#include "int_util.hpp"                     // pow

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

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

using boost::multiprecision::cpp_int;
using int_util::pow;
using std::swap;

int first(int n)
{
    cpp_int b = 10;                 // base of digits
    cpp_int m = pow(b, n - 1);      // lowest number having n digits
    int r = 1;
    for (cpp_int x = 1, y = 1; x < m; x += y, swap(x, y))
        ++r;
    return r;
}

int main()
{
    assert(first(3) == 12);
    std::cout << first(1000) << std::endl;
}


#include <vector>
#include <iostream>
#include <math.h>
#include <Eigen/IterativeLinearSolvers>
#include <iomanip>
#include "dataImport.h"

// parallel computing library: OpenOMP library
// #include <omp.h>

// generate increaseing sequence
#include <numeric>

#include <deque>

using namespace std;
using namespace Eigen;

//Sigmoid function
double sigmoid(double x){
	return 1 / (1 + exp(x));
}

//Posterior probability
double eta(vector<double>& A, vector<double>& B){
	double temp = 0;
	for (int i = 0; i < A.size(); i++){
		temp =temp - A[i] * B[i];
	}
	return sigmoid(temp);
}

//Objective function
double obj(vector<vector<double> >& X, vector<double>& theta, vector<double>& y, double lambda){
	double sum = 0;
	for (int i = 0; i < y.size(); i++){
		double temp = 0;
		for (int j = 0; j < theta.size(); j++){
			temp += theta[j] * X[i][j];
		}
		sum += (1 - y[i])*temp + log(1 + exp(-temp));
	}
	double reg = 0;
	for (int k = 0; k < theta.size(); k++){
		reg += theta[k] * theta[k];
	}
	sum = sum + lambda*reg;
	return sum;
}

//Gradient
void grad(vector<vector<double> >& X, vector<double>& theta, vector<double>& sum,
	vector<double>& y, double lambda){

	for (int i = 0; i < y.size(); i++){
		double temp = 0;
		for (int j = 0; j < theta.size(); j++){
			temp += theta[j] * X[i][j];
		}
		for (int k = 0; k < theta.size(); k++){
			sum[k] += X[i][k] * (1 - y[i] - sigmoid(temp));
		}
	}

	for (int i = 0; i < sum.size(); i++){
		sum[i] += 2 * lambda*theta[i];
	}
}

//Hessian
void hess(vector<vector<double> >& X, vector<vector<double> >& sum, vector<double>& theta,
	vector<double>& y, double lambda, float startTime){
	sum.clear();
	vector<double> oneRow(theta.size(), 0);

	for (int len = 0; len < theta.size(); len++){
		sum.push_back(oneRow);
	}

	deque<int> y_ids(y.size());

	iota(y_ids.begin(), y_ids.end(), 0);

	int th_id;
	int one_y;

	int theta_size = theta.size();
    {
    	// init mapper's local sum pool
    	vector<vector<double> > local_sum;
		vector<double> emptyRow(theta_size, 0);

		for (int len = 0; len < theta_size; len++)
		{
			local_sum.push_back(emptyRow);
		}

		while(!y_ids.empty())
    	{
    		vector<double> X_row_i;

    		one_y = -1;
            // any access to shared memory should be critical
            // #pragma omp critical
            {
                if(!y_ids.empty())
                {
                    one_y = y_ids.front();
                    y_ids.pop_front();
                    X_row_i = X[one_y];
                }
            }

    		double temp = 0;
    		if(one_y != -1)
    		{
    			for (int j = 0; j < theta_size; j++)
				{
					temp += theta[j] * X_row_i[j];
				}

				for (int k = 0; k < theta_size; k++)
				{
					for (int l = 0; l < theta_size; l++)
					{
						local_sum[l][k] += X_row_i[l] * X_row_i[k] * sigmoid(temp)*sigmoid(temp)*exp(temp);
					}
				}
    		}

    	}

		for (int k = 0; k < theta_size; k++)
		{
			for (int l = 0; l < theta_size; l++)
			{
				// #pragma omp critical
				{
					sum[l][k] += local_sum[l][k];
				}
			}
		}

		// }
    }

    // #pragma omp barrier

	for (int k = 0; k < theta.size(); k++){
		for (int l = 0; l < theta.size(); l++){
			if (k==l){
				sum[k][l] += 2 * lambda;
			}
		}
	}
}

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

	// prepare for parallel computing for Map reduce

	int th_id, nthreads;

	// start timing
    float currentTime;
    float startTime;
    // shared variables;
    int n, d;

    vector<vector<double> > X;
    vector<double> y;

    //Initialization
	double lambda = 5;

    {
		// #pragma omp master
		{
			cout << "start running with " << nthreads << " cores." << endl;
		}
	}
	// Read in data
	string dataSetFileName = argv[1]; //"covtype_blank.data";
	RawDataSet rawDataSet;

	// only read in the first 30000 instance

	rawDataSet.readDataFromFile(dataSetFileName, 30000);
	cout << "Read file done" << endl;
	rawDataSet.printDataMatrixSize();

	for (int len = 0; len < rawDataSet.rawDataTable.size(); len++){
		y.push_back(rawDataSet.rawDataTable[len].back());

		/////////////////////
		//// NOTICE: code modified to fit the specific test case input: covertype
		////

		// choose covertype 2 as the flag variable
		if (y[len] == 2){
			y[len] = 1;
		}
		else
		{
			y[len] = 0;
		}
		///////////////////////

		rawDataSet.rawDataTable[len].pop_back();
		X.push_back(rawDataSet.rawDataTable[len]);
	}

	n = y.size();
	d = X[1].size();

	for (int i = 0; i < n; i++){
		X[i].push_back(1);
	}

	//Tolerance
	double epsilon = 1.0e-5;
	int iteration = 1000;

	//Gradient descent
	vector<double> theta(d + 1, 0);
	vector<double> theta2(d + 1, 0);

	for (int itr = 0; itr < iteration; itr++)
	{
		vector<double> G(d + 1, 0);
		grad(X, theta, G, y, lambda);

    	cout << "Grad of " <<  itr << " finished with " << currentTime << " seconds elapsed." << endl;

		vector<vector<double> > H;
		hess(X, H, theta, y, lambda, startTime);

    	cout << "Hess of " <<  itr << " finished with " << currentTime << " seconds elapsed." << endl;

		VectorXd x(d + 1), b(d + 1);
		MatrixXd A(d + 1, d + 1);

		for (int i = 0; i < d+1; ++i) {
			A.row(i) = VectorXd::Map(&H[i][0], H[i].size());
		}

		b = VectorXd::Map(&G[0], G.size());

		ConjugateGradient<MatrixXd> cg;
		cg.compute(A);
		x = cg.solve(b);

		//vector<vector<double> > H_inv;
		//MatrixInversion(H, d + 1, H_inv);

		for (int j = 0; j < d + 1; j++){
			double temp = x(j);
			theta2[j] = theta[j];
			theta[j] = theta[j] - temp;
		}

		double obj1 = obj(X, theta, y, lambda);
		double obj2 = obj(X, theta2, y, lambda);
		double delta = abs(obj1 - obj2);
		if (delta < epsilon) break;
	}

    cout << "The program finished with "<< currentTime << " seconds elapsed." << endl;

	return 0;
}


#include <armadillo>
#include <cmath>
#include <vector>

typedef std::pair<arma::vec, arma::vec> trainingData;

class Network {
public:
  explicit Network(std::vector<int> sizes) : sizes(sizes) {
    layerNumber = sizes.size();

    for (int i = 1; i < sizes.size(); ++i) {
      biases.push_back(arma::vec(sizes[i], arma::fill::randn));
      biasesShape.push_back(arma::vec(sizes[i], arma::fill::zeros));
    }

    for (int i = 0; i < sizes.size() - 1; ++i) {
      weights.push_back(arma::mat(sizes[i], sizes[i + 1], arma::fill::randn));
      weightsShape.push_back(
          arma::mat(sizes[i], sizes[i + 1], arma::fill::zeros));
    }
  }

  static arma::vec sigmoid(arma::vec z) {
    z.transform([](double val) { return 1 / (1 + exp(-val)); });
    return z;
  }

  static arma::vec sigmoidPrime(arma::vec z) {
    arma::vec sigmoidZ = sigmoid(z);
    arma::vec rhs = sigmoidZ;
    rhs.transform([](double val) { return 1 - val; });
    return sigmoidZ * arma::diagmat(rhs);
  }

  arma::vec feedforward(const arma::vec &input);
  void sgd(std::vector<trainingData> &trainingSet, const int epochs,
           const int miniBatchSize, const double eta);
  void updateMiniBatch(std::vector<trainingData *> &miniBatch,
                       const double eta);
  void backprop(arma::vec &in, arma::vec &out, std::vector<arma::vec> &partialB,
                std::vector<arma::mat> &partialW);
  arma::vec costDerivative(arma::vec &out, arma::vec &y);

private:
  Network(){};

  int layerNumber;
  std::vector<int> sizes;
  // biases[i] represents bias at (i+1)th layer
  std::vector<arma::vec> biases, biasesShape;
  // weights[i] represents weight from ith layer to (i+1)th layer
  std::vector<arma::mat> weights, weightsShape;
};


/**
 * @file advectionfv2d.cc
 * @brief NPDE homework AdvectionFV2D code
 * @author Philipp Egg
 * @date 21.06.2020
 * @copyright Developed at ETH Zurich
 */

#include "advectionfv2d.h"

#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>
#include <array>
#include <memory>
#include <stdexcept>
#include <vector>

namespace AdvectionFV2D {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 2, 3> gradbarycoordinates(
    const Eigen::Matrix<double, 2, 3> &triangle) {
  Eigen::Matrix3d X;

  // solve for the coefficients of the barycentric coordinate functions
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = triangle.transpose();
  return X.inverse().block<2, 3>(1, 0);
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<Eigen::Matrix<double, 2, Eigen::Dynamic>>>
computeCellNormals(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Appears only in mastersolution

  // Initialize datastruture for the result
  lf::mesh::utils::CodimMeshDataSet<Eigen::Matrix<double, 2, Eigen::Dynamic>>
      result(mesh_p, 0);

  // Compute normal vectors
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    const lf::geometry::Geometry *geo_p = cell->Geometry();
    const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    if (corners.cols() == 3) {
      Eigen::Matrix<double, 2, Eigen::Dynamic> normal_vectors(2, 3);

      // Compute normal vectors
      Eigen::Matrix<double, 2, 3> bary_coord = gradbarycoordinates(corners);

      // Reorder computed normal vectors
      // normal_vectors[0] -> normal_vector of edge[0]
      normal_vectors.col(0) = -(bary_coord.col(2)).normalized();
      normal_vectors.col(1) = -(bary_coord.col(0)).normalized();
      normal_vectors.col(2) = -(bary_coord.col(1)).normalized();

      // Save the matrix in our datastructure
      result(*cell) = normal_vectors;
    } else {  // corners.cols() == 4
      Eigen::Matrix<double, 2, Eigen::Dynamic> normal_vectors(2, 4);

      // Split the quadrilateral into two triangles (1,2,3) and (1,3,4)
      Eigen::Matrix<double, 2, 3> tria_1;
      Eigen::Matrix<double, 2, 3> tria_2;
      tria_1 << corners.col(0), corners.col(1), corners.col(2);
      tria_2 << corners.col(0), corners.col(2), corners.col(3);

      // Compute normal vectors
      Eigen::Matrix<double, 2, 3> bary_coord_1 = gradbarycoordinates(tria_1);
      Eigen::Matrix<double, 2, 3> bary_coord_2 = gradbarycoordinates(tria_2);

      // Reorder computed normal vectors
      // normal_vectors[0] -> normal_vector of edge[0]
      normal_vectors.col(0) = -(bary_coord_1.col(2)).normalized();
      normal_vectors.col(1) = -(bary_coord_1.col(0)).normalized();
      normal_vectors.col(2) = -(bary_coord_2.col(0)).normalized();
      normal_vectors.col(3) = -(bary_coord_2.col(1)).normalized();

      // Save the matrix in our datastructure
      result(*cell) = normal_vectors;
    }
  }
  return std::make_shared<lf::mesh::utils::CodimMeshDataSet<
      Eigen::Matrix<double, 2, Eigen::Dynamic>>>(result);

}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 4>>>
getAdjacentCellPointers(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Initialize auxilary object
  lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 2>>
      aux_obj(mesh_p, 1, {nullptr, nullptr});

  // Iterate over every cell
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    auto cell_edges = cell->SubEntities(1);

    // Iterate over every edge of the cell
    for (const lf::mesh::Entity *edge : cell_edges) {
      // If aux_obj at index 0 was not set, save cell there
      // otherwise save at the second position
      // (The first position has to be set; the second might be set)
      if (aux_obj(*edge)[0] == nullptr) {
        aux_obj(*edge)[0] = cell;
      } else if (aux_obj(*edge)[1] == nullptr) {
        aux_obj(*edge)[1] = cell;
      } else {
        throw std::runtime_error("Error in aux_obj");
      }
    }
  }

  // Initialize datastructure for result
  lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 4>>
      result(mesh_p, 0, {nullptr, nullptr, nullptr, nullptr});

  // Collect the objects of the auxilary object
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    auto cell_edges = cell->SubEntities(1);
    int counter = 0;
    for (const lf::mesh::Entity *edge : cell_edges) {
      if (aux_obj(*edge)[0] != cell) {
        result(*cell)[counter] = aux_obj(*edge)[0];
      } else {
        result(*cell)[counter] = aux_obj(*edge)[1];
      }
      counter++;
    }
  }

  return std::make_shared<lf::mesh::utils::CodimMeshDataSet<
      std::array<const lf::mesh::Entity *, 4>>>(result);
}
/* SAM_LISTING_END_3 */

// Function returning the barycenter of TRIA or QUAD
/* SAM_LISTING_BEGIN_4 */
Eigen::Vector2d barycenter(const Eigen::MatrixXd corners) {
  Eigen::Vector2d midpoint;
  if (corners.cols() == 3) {
    midpoint = (corners.col(0) + corners.col(1) + corners.col(2)) / 3.0;
  } else if (corners.cols() == 4) {
    midpoint =
        (corners.col(0) + corners.col(1) + corners.col(2) + corners.col(3)) /
        4.0;
  } else {
    throw std::runtime_error("Wrong geometrie in barycenter()");
  }
  return midpoint;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
double computeHmin(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  // Vector to store the distances between cells
  std::vector<double> min_h;

  // Get Adjectent Cells
  std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
      std::array<const lf::mesh::Entity *, 4>>>
      adjacentCells = AdvectionFV2D::getAdjacentCellPointers(mesh_p);

  // Iterate over all cells
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    const lf::geometry::Geometry *geo_p = cell->Geometry();
    const Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    // Compute the barycenter of the cell
    Eigen::Vector2d cur_midpoint = barycenter(corners);

    // Iterate over all adjecent cells
    for (const lf::mesh::Entity *neighbour_cell : (*adjacentCells)(*cell)) {
      // Check that the neighbor exists
      if (neighbour_cell != nullptr) {
        const lf::geometry::Geometry *geo_p_neighbour =
            neighbour_cell->Geometry();
        const Eigen::MatrixXd neighbour_corners =
            lf::geometry::Corners(*geo_p_neighbour);

        // Compute barycenter of neighbour cell
        Eigen::Vector2d neighbour_midpoint = barycenter(neighbour_corners);

        // Compute distances between cells
        double distance = (cur_midpoint - neighbour_midpoint).norm();

        // Store value in vector
        min_h.push_back(distance);
      }
    }
  }

  // Find the minimum value in the vector and return it
  return *std::min_element(std::begin(min_h), std::end(min_h));
}
/* SAM_LISTING_END_5 */

}  // namespace AdvectionFV2D


<%
cfg['compiler_args'] = ['-std=c++11']
cfg['include_dirs'] = ['../notebooks/eigen3']
setup_pybind11(cfg)
%>

#include <cmath>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/LU>

namespace py = pybind11;
using namespace Eigen;

VectorXd logistic(VectorXd xs) {
    return 1 - 1 / (1 + exp(xs.array()));
}

MatrixXd gd(MatrixXd X, VectorXd y, VectorXd beta, double alpha, int niter) {

    int i = 0, n, p;
    MatrixXd Xt;
    VectorXd y_pred, epsilon, grad;

    n = X.rows();
    p = X.cols();
    Xt = X.transpose();
    y_pred = VectorXd::Zero(n);
    epsilon = VectorXd::Zero(n);
    grad = VectorXd::Zero(p);

    for(i = 0; i < niter; i++) {
        y_pred = logistic(X * beta);
        epsilon = y - y_pred;
        grad = Xt * epsilon / n;
        beta += alpha * grad;
    }

    return beta;
}

PYBIND11_MODULE(logistic_gd, m) {
    m.doc() = "auto-compiled c++ extension";
    m.def("logistic", &logistic);
    m.def("gd", &gd);
}


//#####################################################################
//  Copyright (c) 2011-2013 Nathan Mitchell, Eftychios Sifakis.
//  This file is covered by the FreeBSD license. Please refer to the
//  license.txt file for more information.
//#####################################################################

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

using namespace Eigen;

namespace{
template<class T_MATRIX>
void Print_Formatted(const T_MATRIX& A,std::ostream& output)
{
    for(int i=0;i<A.rows();i++){
        for(int j=0;j<A.cols();j++){
            output<<std::setw(12)<<A(i,j);
            if(j<A.cols()-1) output<<" ";}
        output<<std::endl;}
}
}

template<class T>
void Singular_Value_Decomposition_Reference(const T A[9], T U[9], T Sigma[3], T V[9])
{
    Map<const Matrix<T,3,3>> mA=Map<const Matrix<T,3,3>>(A);

    Map<Matrix<T,3,3>> mU=Map<Matrix<T,3,3>>(U);
    Map<Matrix<T,3,1>> mSigma=Map<Matrix<T,3,1>>(Sigma);
    Map<Matrix<T,3,3>> mV=Map<Matrix<T,3,3>>(V);

    JacobiSVD<Matrix<T,3,3>> svd(mA, ComputeFullU|ComputeFullV);
    mU=svd.matrixU();
    mSigma=svd.singularValues();
    mV=svd.matrixV();

    if(mU.determinant() < 0.) {
        mU.col(2) *= -1.;
        mSigma(2) *= -1.;
    }

    if(mV.determinant() < 0.) {
        mV.col(2) *= -1.;
        mSigma(2) *= -1.;
    }
}

template<class T>
bool Singular_Value_Decomposition_Compare(const T U[9], const T Sigma[3], const T V[9],
                                          const T U_reference[9], const T Sigma_reference[3], const T V_reference[9])
{

    Map<const Matrix<T,3,3>> mU=Map<const Matrix<T,3,3>>(U);
    Map<const Matrix<T,3,1>> mSigma=Map<const Matrix<T,3,1>>(Sigma);
    Map<const Matrix<T,3,3>> mV=Map<const Matrix<T,3,3>>(V);

    Map<const Matrix<T,3,3>> mU_reference=Map<const Matrix<T,3,3>>(U_reference);
    Map<const Matrix<T,3,1>> mSigma_reference=Map<const Matrix<T,3,1>>(Sigma_reference);
    Map<const Matrix<T,3,3>> mV_reference=Map<const Matrix<T,3,3>>(V_reference);

    Matrix<T,3,3> mU_adjusted = mU_reference;
    Matrix<T,3,1> mSigma_adjusted = mSigma_reference;
    Matrix<T,3,3> mV_adjusted = mV_reference;

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

      if (mU.col(i).dot(mU_adjusted.col(i)) < 0.) {
          mU_adjusted.col(i) *= -1.;
          mSigma_adjusted(i) *= -1.;
          mU_adjusted.col(2) *= -1.;
          mSigma_adjusted(2) *= -1.;
      }

      if (mV.col(i).dot(mV_adjusted.col(i)) < 0.) {
          mV_adjusted.col(i) *= -1.;
          mSigma_adjusted(i) *= -1.;
          mV_adjusted.col(2) *= -1.;
          mSigma_adjusted(2) *= -1.;
      }

    }

    std::cout<<"Computed matrix U :"<<std::endl;Print_Formatted(mU,std::cout);
    std::cout<<"Reference matrix U :"<<std::endl;Print_Formatted(mU_reference,std::cout);
    std::cout<<"Adjusted matrix U :"<<std::endl;Print_Formatted(mU_adjusted,std::cout);
    std::cout<<"Difference = "<< (mU-mU_adjusted).norm()  <<std::endl;

    std::cout<<std::endl;
    std::cout<<"Computed matrix Sigma :"<<std::endl;Print_Formatted(mSigma,std::cout);
    std::cout<<"Reference matrix Sigma :"<<std::endl;Print_Formatted(mSigma_reference,std::cout);
    std::cout<<"Adjusted matrix Sigma :"<<std::endl;Print_Formatted(mSigma_adjusted,std::cout);
    std::cout<<"Difference = "<<  (mSigma-mSigma_adjusted).norm()  <<std::endl;

    std::cout<<std::endl;
    std::cout<<"Computed matrix V :"<<std::endl;Print_Formatted(mV,std::cout);
    std::cout<<"Reference matrix V :"<<std::endl;Print_Formatted(mV_reference,std::cout);
    std::cout<<"Adjusted matrix V :"<<std::endl;Print_Formatted(mV_adjusted,std::cout);
    std::cout<<"Difference = "<< (mV-mV_adjusted).norm()  <<std::endl;

    Matrix<T,3,3> mA=mU*mSigma.asDiagonal()*mV.transpose();
    Matrix<T,3,3> mA_reference=mU_reference*mSigma_reference.asDiagonal()*mV_reference.transpose();

    std::cout<<std::endl;
    std::cout<<"Computed matrix A=U*Sigma*V^T :"<<std::endl;Print_Formatted(mA,std::cout);
    std::cout<<"Reference matrix A=U*Sigma*V^T :"<<std::endl;Print_Formatted(mA_reference,std::cout);
    std::cout<<"Difference = "<< (mA-mA_reference).norm()  <<std::endl;

    if(!((mU-mU_adjusted).norm() < 0.00001) )
        return false;

    if(!((mSigma-mSigma_adjusted).norm() < 0.00001) )
        return false;

    if(!((mV-mV_adjusted).norm() < 0.00001) )
        return false;

    return true;
}

template void Singular_Value_Decomposition_Reference(const float A[9], float U[9], float Sigma[3], float V[9]);
template bool Singular_Value_Decomposition_Compare(const float U[9], const float Sigma[3], const float V[9],
                                                   const float U_reference[9], const float Sigma_reference[3], const float V_reference[9]);


#include "tile/math/bignum.h"

#include <boost/math/common_factor_rt.hpp>

namespace vertexai {
namespace tile {
namespace math {

Integer Floor(const Rational& x) {
  if (x < 0) {
    return (numerator(x) - denominator(x) + 1) / denominator(x);
  } else {
    return numerator(x) / denominator(x);
  }
}

Integer Ceil(const Rational& x) { return Floor(Rational(numerator(x) - 1, denominator(x))) + 1; }

Rational FracPart(const Rational& x) { return x - Floor(x); }

Integer Abs(const Integer& x) {
  if (x < 0) {
    return -x;
  }
  return x;
}

Rational Abs(const Rational& x) {
  if (x < 0) {
    return -x;
  }
  return x;
}

Rational Reduce(const Rational& v, const Rational& m) { return v - Floor(v / m) * m; }

Integer XGCD(const Integer& a, const Integer& b, Integer& x, Integer& y) {  // NOLINT(runtime/references)
  if (b == 0) {
    x = 1;
    y = 0;
    return a;
  }

  Integer x1;
  Integer gcd = XGCD(b, a % b, x1, x);
  y = x1 - (a / b) * x;

  if (gcd < 0) {
    gcd *= -1;
    x *= -1;
    y *= -1;
  }

  return gcd;
}

Rational XGCD(const Rational& a, const Rational& b, Integer& x, Integer& y) {  // NOLINT(runtime/references)
  Integer m = boost::math::lcm(denominator(a), denominator(b));
  Rational o;
  o = Rational(XGCD(numerator(a * m), numerator(b * m), x, y), m);

  if (o < 0) {
    o *= -1;
    x *= -1;
    y *= -1;
  }
  return o;
}

Rational GCD(const Rational& a, const Rational& b) {
  Integer m = boost::math::lcm(denominator(a), denominator(b));
  Integer g = boost::math::gcd(numerator(a * m), numerator(b * m));
  return Rational(g, m);
}

Integer GCD(const Integer& a, const Integer& b) { return boost::math::gcd(a, b); }

Integer LCM(const Integer& a, const Integer& b) { return boost::math::lcm(a, b); }

Integer Min(const Integer& a, const Integer& b) {
  if (a < b)
    return a;
  else
    return b;
}

Rational Min(const Rational& a, const Rational& b) {
  if (a < b)
    return a;
  else
    return b;
}

Integer Max(const Integer& a, const Integer& b) {
  if (a < b)
    return b;
  else
    return a;
}

Rational Max(const Rational& a, const Rational& b) {
  if (a < b)
    return b;
  else
    return a;
}

Integer RatDiv(const Rational& a, const Rational& b, Rational& r) {  // NOLINT(runtime/references)
  Integer q = Floor(a / b);
  r = a - q * b;
  return q;
}

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#ifndef Interpolators__2D_BilinearInterpolator_hpp
#define Interpolators__2D_BilinearInterpolator_hpp

/** @file BilinearInterpolator.hpp
  * @brief
  * @author C.D. Clark III
  * @date 12/27/16
  */

#include "InterpolatorBase.hpp"
#include <boost/range/algorithm/lower_bound.hpp>
#include <boost/range/adaptor/strided.hpp>

/** @class
  * @brief Linear interpolation for 2D functions.
  * @author C.D. Clark III, Aaron Hoffman
  */

namespace _2D {

template<class Real>
class BilinearInterpolator : public InterpolatorBase<BilinearInterpolator<Real>>
{
  public:
    using BASE = InterpolatorBase<BilinearInterpolator<Real>>;
    using VectorType = typename BASE::VectorType;
    using MapType = typename BASE::MapType;

    // types used to view data as 2D coordinates
    using MatrixType    = typename BASE::MatrixType;
    using _2DVectorView = typename BASE::_2DVectorView;
    using _2DMatrixView = typename BASE::_2DMatrixView;

    // types used for 2x2 matrix algebra
    using Matrix22 = Eigen::Matrix<Real,2,2 >;
    using Matrix22Array = Eigen::Array< Matrix22, Eigen::Dynamic, Eigen::Dynamic >;
    using ColVector2 = Eigen::Matrix<Real,2,1 >;
    using RowVector2 = Eigen::Matrix<Real,1,2 >;

  protected:

    using BASE::xView;
    using BASE::yView;
    using BASE::zView;
    using BASE::X;
    using BASE::Y;
    using BASE::Z;

    Matrix22Array Q; // naming convention used by wikipedia article (see Wikipedia https://en.wikipedia.org/wiki/Bilinear_interpolation)

  public:

    template<typename I>
    BilinearInterpolator( I n, Real *x, Real *y, Real *z ) {this->setData(n,x,y,z);}

    template<typename X, typename Y, typename Z>
    BilinearInterpolator( X &x, Y &y, Z &z ) {this->setData(x,y,z);}

    BilinearInterpolator():BASE(){}
    BilinearInterpolator(const BilinearInterpolator& rhs)
    :BASE(rhs)
    ,Q(rhs.Q)
    {}

    // copy-swap idiom
    friend void swap( BilinearInterpolator& lhs, BilinearInterpolator& rhs)
    {
      lhs.Q.swap(rhs.Q);
      swap( static_cast<BASE&>(lhs), static_cast<BASE&>(rhs) );
    }

    BilinearInterpolator& operator=(BilinearInterpolator rhs)
    {
      swap(*this,rhs);
      return *this;
    }

    Real operator()( Real x, Real y ) const;

  private:

    void setupInterpolator();
    friend BASE;

};

template<class Real>
void
BilinearInterpolator<Real>::setupInterpolator()
{

  // Interpolation will be done by multiplying the coordinates by coefficients.

  Q = Matrix22Array( X->size()-1, Y->size()-1 );

  // We are going to pre-compute the interpolation coefficients so
  // that we can interpolate quickly
  for(int i = 0; i < X->size() - 1; i++)
  {
    for( int j = 0; j < Y->size() - 1; j++)
    {
      Real tmp = ( ((*X)(i+1) - (*X)(i) )*( (*Y)(j+1) - (*Y)(j) ) );
      Q(i,j) = Z->block(i,j,2,2)/tmp;
    }
  }
}

template<class Real>
Real
BilinearInterpolator<Real>::operator()( Real x, Real y ) const
{
  BASE::checkData();

  // no extrapolation...
  if( x < (*X)(0)
   || x > (*X)(X->size()-1)
   || y < (*Y)(0)
   || y > (*Y)(Y->size()-1) )
  {
    return 0;
  }
  // find the x index that is just to the LEFT of x
  //int i  = Utils::index__last_lt( x, *X, X->size() );
  // NOTE: X data is strided.
  auto xrng = std::make_pair( X->data(), X->data()+X->size()*X->innerStride() ) | boost::adaptors::strided(X->innerStride());
  int i = boost::lower_bound( xrng, x) - boost::begin(xrng) - 1;
  if(i < 0)
    i = 0;

  // find the y index that is just BELOW y
  //int j  = Utils::index__last_lt( y, *Y, Y->size() );
  // NOTE: Y data is NOT strided
  auto yrng = std::make_pair( Y->data(), Y->data()+Y->size() );
  int j = boost::lower_bound( yrng, y) - boost::begin(yrng) - 1;
  if(j < 0)
    j = 0;

  // now, create the coordinate vectors (see Wikipedia https://en.wikipedia.org/wiki/Bilinear_interpolation)
  RowVector2 vx;
  ColVector2 vy;
  vx << ((*X)(i+1) - x), (x - (*X)(i));
  vy << ((*Y)(j+1) - y), (y - (*Y)(j));

  // interpolation is just x*Q*y

  return vx*Q(i,j)*vy;
}

}

#endif // include protector


#include <Eigen/Core>
#include <Eigen/QR>

#include "tools.hpp"

using namespace std;

// For converting back and forth between radians and degrees.
constexpr double pi() {
  return M_PI;
}

double deg2rad(double x) {
  return x * pi() / 180;
}

double rad2deg(double x) {
  return x * 180 / pi();
}

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.rfind("}]");
  if (found_null != string::npos) {
    return "";
  } else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 2);
  }
  return "";
}

// Evaluate a polynomial.
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); j++) {
    for (int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

// 2D affine transformation for a list of points
void Tranform2d(std::vector<double>       &dst_x,
                std::vector<double>       &dst_y,
                const std::vector<double> &src_x,
                const std::vector<double> &src_y,
                double translation_x, double translation_y, double psi) {

  // make sure destination vectors have same length as source vectors
  dst_x.resize(src_x.size());
  dst_y.resize(src_y.size());

  for (int idx = 0; idx < (int) src_x.size(); idx++) {
    double dx = src_x[idx] - translation_x;
    double dy = src_y[idx] - translation_y;
    dst_x[idx] = dx*cos(psi) - dy*sin(psi);
    dst_y[idx] = dx*sin(psi) + dy*cos(psi);
  }
}


/*****************************************************************************
 * array.cpp        Blitz++ Array stencilling example
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    int N = 64;

    // Create three-dimensional arrays of float
    Array<float,3> A(N,N,N), B(N,N,N);

    // Set up initial conditions: +30 C over an interior block,
    // and +22 C elsewhere
    A = 22.0;

    Range interior(N/4,3*N/4);
    A(interior,interior,interior) = 30.0;

    int numIters = 301;

    Range I(1,N-2), J(1,N-2), K(1,N-2);

#ifdef BZ_HAVE_STD
#ifdef BZ_ARRAY_SPACE_FILLING_TRAVERSAL
    generateFastTraversalOrder(TinyVector<int,2>(N-2,N-2));
#endif
#endif

    for (int i=0; i < numIters; ++i)
    {
        double c = 1/6.5;

        B(I,J,K) = c * (.5 * A(I,J,K) + A(I+1,J,K) + A(I-1,J,K)
            + A(I,J+1,K) + A(I,J-1,K) + A(I,J,K+1) + A(I,J,K-1));

        A(I,J,K) = c * (.5 * B(I,J,K) + B(I+1,J,K) + B(I-1,J,K)
            + B(I,J+1,K) + B(I,J-1,K) + B(I,J,K+1) + B(I,J,K-1));

        // Output the result along a line through the centre
        for (int j=0; j < 8; ++j)
            cout << setprecision(2) << A(N/2,N/2,j*N/8) << " ";

        cout << endl;
        cout.flush();
    }

    return 0;
}


#ifndef CTCD_DISTANCE_HPP
#define CTCD_DISTANCE_HPP 1

#include <Eigen/Geometry>
#include <algorithm>

// Source: Etienne Vouga
// https://github.com/evouga/collisiondetection

namespace mcl {
namespace ctcd
{

  // Efficiently calculates whether or not a point p is closer to than eta distance to the plane spanned by vertices q0, q1, and q2.
  // This method does not require any floating point divisions and so is significantly faster than computing the distance itself.
  static inline bool vertexPlaneDistanceLessThan(const Eigen::Vector3d &p,
					    const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2, double eta)
  {
    Eigen::Vector3d c = (q1-q0).cross(q2-q0);
    return c.dot(p-q0)*c.dot(p-q0) < eta*eta*c.dot(c);
  }

  // Efficiently calculates whether or not the line spanned by vertices (p0, p1) is closer than eta distance to the line spanned by vertices (q0, q1).
  // This method does not require any floating point divisions and so is significantly faster than computing the distance itself.
  static inline bool lineLineDistanceLessThan(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1,
					  const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, double eta)
  {
	Eigen::Vector3d c = (p1-p0).cross(q1-q0);
	return c.dot(q0-p0)*c.dot(q0-p0) < eta*eta*c.dot(c);
  }

  // Computes the vector between a point p and the closest point to p on the triangle (q0, q1, q2). Also returns the barycentric coordinates of this closest point on the triangle;
  // q0bary is the barycentric coordinate of q0, etc. (The distance from p to the triangle is the norm of this vector.)
  static inline Eigen::Vector3d vertexFaceDistance(const Eigen::Vector3d &p,
					    const Eigen::Vector3d &q0, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2,
					    double &q0bary, double &q1bary, double &q2bary, bool clamp_barys = true)
  {
  Eigen::Vector3d ab = q1-q0;
  Eigen::Vector3d ac = q2-q0;
  Eigen::Vector3d ap = p-q0;

  double d1 = ab.dot(ap);
  double d2 = ac.dot(ap);

  // corner and edge cases

  if(d1 <= 0 && d2 <= 0)
    {
      q0bary = 1.0;
      q1bary = 0.0;
      q2bary = 0.0;
      return q0-p;
    }

  Eigen::Vector3d bp = p-q1;
  double d3 = ab.dot(bp);
  double d4 = ac.dot(bp);
  if(d3 >= 0 && d4 <= d3)
    {
      q0bary = 0.0;
      q1bary = 1.0;
      q2bary = 0.0;
      return q1-p;
    }

  double vc = d1*d4 - d3*d2;
  if((vc <= 0) && (d1 >= 0) && (d3 <= 0))
    {
      double v = d1 / (d1-d3);
      q0bary = 1.0 - v;
      q1bary = v;
      q2bary = 0;
      return (q0 + v*ab)-p;
    }

  Eigen::Vector3d cp = p-q2;
  double d5 = ab.dot(cp);
  double d6 = ac.dot(cp);
  if(d6 >= 0 && d5 <= d6)
    {
      q0bary = 0;
      q1bary = 0;
      q2bary = 1.0;
      return q2-p;
    }

  double vb = d5*d2 - d1*d6;
  if((vb <= 0) && (d2 >= 0) && (d6 <= 0))
    {
      double w = d2/(d2-d6);
      q0bary = 1-w;
      q1bary = 0;
      q2bary = w;
      return (q0 + w*ac)-p;
    }

  double va = d3*d6 - d5*d4;
  if((va <= 0) && (d4-d3 >= 0) && (d5-d6 >= 0))
    {
      double w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
      q0bary = 0;
      q1bary = 1.0 - w;
      q2bary = w;

      return (q1 + w*(q2-q1))-p;
    }

  // face case
  double denom = 1.0 / (va + vb + vc);
  double v = vb * denom;
  double w = vc * denom;
  double u = 1.0 - v - w;
  q0bary = u;
  q1bary = v;
  q2bary = w;

  if (clamp_barys)
  {
    q0bary = std::clamp(q0bary, 0.0, 1.0);
    q1bary = std::clamp(q1bary, 0.0, 1.0);
    q2bary = std::clamp(q2bary, 0.0, 1.0);
  }

  return (u*q0 + v*q1 + w*q2)-p;
  }

  // Computes the shotest vector between a segment (p0, p1) and segment (q0, q1). Also returns the barycentric coordinates of the closest points on both segments; p0bary is the barycentric
  // coordinate of p0, etc. (The distance between the segments is the norm of this vector).
  static inline Eigen::Vector3d edgeEdgeDistance(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1,
					  const Eigen::Vector3d &q0, const Eigen::Vector3d &q1,
					  double &p0bary, double &p1bary,
					  double &q0bary, double &q1bary,
						bool clamp_barys = true )
  {
  Eigen::Vector3d d1 = p1-p0;
  Eigen::Vector3d d2 = q1-q0;
  Eigen::Vector3d r = p0-q0;
  double a = d1.squaredNorm();
  double e = d2.squaredNorm();
  double f = d2.dot(r);

  auto myclamp = [&clamp_barys](double x)
  {
    if (!clamp_barys) { return x; }
    return std::clamp(x, 0.0, 1.0);
  };

  double s,t;

  double c = d1.dot(r);
  double b = d1.dot(d2);
  double denom = a*e-b*b;
  if(denom != 0.0)
    {
      s = myclamp( (b*f-c*e)/denom );
    }
  else
    {
      //parallel edges and/or degenerate edges; values of s doesn't matter
      s = 0;
    }
  double tnom = b*s + f;
  if(tnom < 0 || e == 0)
    {
      t = 0;
      if(a == 0)
	s = 0;
      else
	s = myclamp(-c/a);
    }
  else if(tnom > e)
    {
      t = 1.0;
      if(a == 0)
	s = 0;
      else
	s = myclamp( (b-c)/a );
    }
  else
    t = tnom/e;

  Eigen::Vector3d c1 = p0 + s*d1;
  Eigen::Vector3d c2 = q0 + t*d2;

  p0bary = 1.0-s;
  p1bary = s;
  q0bary = 1.0-t;
  q1bary = t;

  if (clamp_barys)
  {
    p0bary = std::clamp(p0bary, 0.0, 1.0);
    p1bary = std::clamp(p1bary, 0.0, 1.0);
    q0bary = std::clamp(q0bary, 0.0, 1.0);
    q1bary = std::clamp(q1bary, 0.0, 1.0);
  }

  return c2-c1;
  }

} // ns ctcd
} // ns mcl

#endif


#include <iostream>
using namespace std;

#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;

#define MATRIX_SIZE 50

int main(int argc, char **argv)
{
    Matrix<float, 2, 3> matrix_23;
    Vector3d v_3d;
    Matrix<float, 3, 1> vd_3d;
    Matrix3d matrix_33 = Matrix3d::Zero();
    Matrix<double, Dynamic, Dynamic> matrix_dynamic;

    MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout<< "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

    // visit element via ()
    cout << "print matrix 2x3: " << endl;
    for(int i = 0; i < 2; ++i) {
        for(int j = 0; j < 3; ++j) cout << matrix_23(i, j) << "\t";
        cout << endl;
    }

    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;
    Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << "[1,2,3;4,5,6]*[3,2,1]" << result.transpose() << endl;

    return 0;
}


#include "eigen_ext.hpp"
#include "integrate_func.hpp"
#include "parameters.hpp"
#include <cassert>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace pear {
double triangle_area(Vec &xp, Vec &yp) {
  double area;
  area = xp(1) * yp(2) + xp(0) * yp(1) + xp(2) * yp(0) - xp(1) * yp(0) -
         xp(0) * yp(2) - xp(2) * yp(1);
  return area;
}

Mat int_func_block(Vec &xp, Vec &yp) {
  double area = triangle_area(xp, yp);
  Mat int_func_block(3, 3);

  double b11 = 6 * xp(0) + 2 * xp(1) + 2 * xp(2);
  double b12 = 2 * xp(0) + 2 * xp(1) + xp(2);
  double b13 = 2 * xp(0) + xp(1) + 2 * xp(2);

  double b21 = 2 * xp(0) + 2 * xp(1) + xp(2);
  double b22 = 2 * xp(0) + 6 * xp(1) + 2 * xp(2);
  double b23 = xp(0) + 2 * xp(1) + 2 * xp(2);

  double b31 = 2 * xp(0) + xp(1) + 2 * xp(2);
  double b32 = xp(0) + 2 * xp(1) + 2 * xp(2);
  double b33 = 2 * xp(0) + 2 * xp(1) + 6 * xp(2);

  int_func_block << b11, b12, b13, b21, b22, b23, b31, b32, b33;
  int_func_block = int_func_block * area / 60;

  return int_func_block;
}

Mat int_func(Vec &xp, Vec &yp, MatI &t) {

  int np = xp.rows();
  int nt = t.rows();

  Mat int_F(np, np);
  Mat int_F_block(3, 3);

  VecI t_loc(3);
  Vec xp_loc(3);
  Vec yp_loc(3);

  for (int idxm = 0; idxm < nt; idxm++) {
    t_loc = t.row(idxm);
    xp_loc = pear::extract<Vec>(xp, t_loc);
    yp_loc = pear::extract<Vec>(yp, t_loc);

    int_F_block = int_func_block(xp_loc, yp_loc);
    for (int idx1 = 0; idx1 < 3; idx1++) {
      for (int idx2 = 0; idx2 < 3; idx2++) {
        int_F(t_loc(idx1), t_loc(idx2)) += int_F_block(idx1, idx2);
      }
    }
  }
  return int_F;
}

} // namespace pear


#include <Eigen/Core>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <mathtoolbox/numerical-optimization.hpp>
#include <optimization-test-functions.hpp>

int main()
{
    constexpr otf::FunctionType type       = otf::FunctionType::Rosenbrock;
    constexpr int               dimensions = 100;

    std::srand(static_cast<unsigned>(std::time(nullptr)));

    mathtoolbox::optimization::Setting setting;
    setting.algorithm          = mathtoolbox::optimization::Algorithm::Bfgs;
    setting.x_init             = Eigen::VectorXd::Random(dimensions);
    setting.f                  = [](const Eigen::VectorXd& x) { return otf::GetValue(x, type); };
    setting.g                  = [](const Eigen::VectorXd& x) { return otf::GetGrad(x, type); };
    setting.type               = mathtoolbox::optimization::Type::Min;
    setting.max_num_iterations = 1000;

    const mathtoolbox::optimization::Result result = mathtoolbox::optimization::RunOptimization(setting);

    const Eigen::VectorXd expected_solution = otf::GetSolution(dimensions, type);
    const double          expected_value    = otf::GetValue(expected_solution, type);
    const double          initial_value     = otf::GetValue(setting.x_init, type);
    const double          found_value       = otf::GetValue(result.x_star, type);

    std::cout << "#iterations: " << result.num_iterations << std::endl;
    std::cout << "Initial solution: " << setting.x_init.transpose() << " (" << initial_value << ")" << std::endl;
    std::cout << "Found solution: " << result.x_star.transpose() << " (" << found_value << ")" << std::endl;
    std::cout << "Expected solution: " << expected_solution.transpose() << " (" << expected_value << ")" << std::endl;

    return 0;
}


/*
 * find_crossing.cpp
 *
 * Finds the energy threshold crossing for a damped oscillator.
 * The algorithm uses a dense out stepper with find_if to first find an
 * interval containing the threshold crossing and the utilizes the dense out
 * functionality with a bisection to further refine the interval until some
 * desired precision is reached.
 *
 * Copyright 2015 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <utility>
#include <algorithm>
#include <array>

#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>
#include <boost/numeric/odeint/iterator/adaptive_iterator.hpp>

namespace odeint = boost::numeric::odeint;

typedef std::array<double, 2> state_type;

const double gam = 1.0;  // damping strength

void damped_osc(const state_type &x, state_type &dxdt, const double /*t*/)
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam * x[1];
}

struct energy_condition {

    // defines the threshold crossing in terms of a boolean functor

    double m_min_energy;

    energy_condition(const double min_energy)
        : m_min_energy(min_energy) { }

    double energy(const state_type &x) {
        return 0.5 * x[1] * x[1] + 0.5 * x[0] * x[0];
    }

    bool operator()(const state_type &x) {
        // becomes true if the energy becomes smaller than the threshold
        return energy(x) <= m_min_energy;
    }
};

template<class System, class Condition>
std::pair<double, state_type>
find_condition(state_type &x0, System sys, Condition cond,
               const double t_start, const double t_end, const double dt,
               const double precision = 1E-6) {

    // integrates an ODE until some threshold is crossed
    // returns time and state at the point of the threshold crossing
    // if no threshold crossing is found, some time > t_end is returned

    auto stepper = odeint::make_dense_output(1.0e-6, 1.0e-6,
                                             odeint::runge_kutta_dopri5<state_type>());

    auto ode_range = odeint::make_adaptive_range(std::ref(stepper), sys, x0,
                                                 t_start, t_end, dt);

    // find the step where the condition changes
    auto found_iter = std::find_if(ode_range.first, ode_range.second, cond);

    if(found_iter == ode_range.second)
    {
        // no threshold crossing -> return time after t_end and ic
        return std::make_pair(t_end + dt, x0);
    }

    // the dense out stepper now covers the interval where the condition changes
    // improve the solution by bisection
    double t0 = stepper.previous_time();
    double t1 = stepper.current_time();
    double t_m;
    state_type x_m;
    // use odeint's resizing functionality to allocate memory for x_m
    odeint::adjust_size_by_resizeability(x_m, x0,
                                         typename odeint::is_resizeable<state_type>::type());
    while(std::abs(t1 - t0) > precision) {
        t_m = 0.5 * (t0 + t1);  // get the mid point time
        stepper.calc_state(t_m, x_m); // obtain the corresponding state
        if (cond(x_m))
            t1 = t_m;  // condition changer lies before midpoint
        else
            t0 = t_m;  // condition changer lies after midpoint
    }
    // we found the interval of size eps, take it's midpoint as final guess
    t_m = 0.5 * (t0 + t1);
    stepper.calc_state(t_m, x_m);
    return std::make_pair(t_m, x_m);
}

int main(int argc, char **argv)
{
    state_type x0 = {{10.0, 0.0}};
    const double t_start = 0.0;
    const double t_end = 10.0;
    const double dt = 0.1;
    const double threshold = 0.1;

    energy_condition cond(threshold);
    state_type x_cond;
    double t_cond;
    std::tie(t_cond, x_cond) = find_condition(x0, damped_osc, cond,
                                              t_start, t_end, dt, 1E-6);
    if(t_cond > t_end)
    {
        // time after t_end -> no threshold crossing within [t_start, t_end]
        std::cout << "No threshold crossing found." << std::endl;
    } else
    {
        std::cout.precision(16);
        std::cout << "Time of energy threshold crossing: " << t_cond << std::endl;
        std::cout << "State: [" << x_cond[0] << " , " << x_cond[1] << "]" << std::endl;
        std::cout << "Energy: " << cond.energy(x_cond) << std::endl;
    }
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)
/*
    boost::math::fft example 06

    Fast Polynomial Multiplication with quadmath
*/

#include <boost/math/fft/bsl_backend.hpp>
#include <boost/multiprecision/complex128.hpp>
#include <iostream>
#include <vector>
#include <exception>

template<class Real, class Complex>
std::vector<Real> multiply_complex(
    const std::vector<Real>& A,
    const std::vector<Real>& B)
{
  const std::size_t N = A.size();
  std::vector< Complex > TA(N),TB(N);
  boost::math::fft::bsl_rdft<Real> P(N);
  P.real_to_complex(A.begin(),A.end(),TA.begin());
  P.real_to_complex(B.begin(),B.end(),TB.begin());

  std::vector<Real> C(N);

  for(unsigned int i=0;i<N;++i)
  {
    TA[i]*=TB[i];
  }

  P.complex_to_real(TA.begin(),TA.end(),C.begin());
  std::transform(C.begin(), C.end(), C.begin(),
                 [N](Real x) { return x / N; });

  return C;
}

template<class Real>
Real difference(const std::vector<Real>& A, const std::vector<Real>& B)
{
  using std::abs;
  Real diff{};
  if(A.size()!=B.size()) return -1;
  for(unsigned int i=0;i<A.size();++i)
  {
    diff += abs(A[i]-B[i]);
  }
  return diff;
}

template<typename Real, typename Complex>
void multiply() {
  using std::abs;
  std::vector<Real> A{1.,4.,-5.,1.,0.,0.,0.,0.};
  std::vector<Real> B{-1.,1.,2.,3.,0.,0.,0.,0.};
  std::vector<Real> C{-1,-3,11,5,3,-13,3,0};

  std::vector<Real> result;
  Real diff;

  result = multiply_complex<Real,Complex>(A,B);
  diff = difference(result,C);
  if(abs(diff)>1e-3)
    throw std::runtime_error("wrong result");
}

int main()
{
  multiply<
    ::boost::multiprecision::float128,
    ::boost::multiprecision::complex128>();
  return 0;
}


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

Eigen::MatrixXd sigmoid(Eigen::MatrixXd mat){
    int r = mat.rows() ; int c = mat.cols();
    for (int i=0; i<r; i++){
        for (int j=0; j<c; j++){
            mat(i,j) = 1/(1+exp(-mat(i,j)));
        }
    }
    return mat;
};

class NeuralNetwork{
    public:
    std::vector <Eigen::MatrixXd> biases;
    std::vector <Eigen::MatrixXd> weights;
    int num_layers;
    std::vector <int> sizes;

    NeuralNetwork(std::vector <int> Sizes){
        num_layers = Sizes.size();
        sizes = Sizes;

        // Initializing random biases
        // std::cout<< "Initializing bases"<< std::endl;
        for (int i=1; i<sizes.size(); i++){
            int y = sizes[i];
            auto m = Eigen::MatrixXd::Random(y,1);
            biases.push_back(m);
            // std::cout<< "Shape : "<<y << " "<< 1 << std::endl;
        }

        // Initializing random weights
        // std::cout<< "Initializing weights"<< std::endl;
        for (int i=0; i<sizes.size()-1; i++){
            int x = sizes[i] ; int y = sizes[i+1];
            auto m = Eigen::MatrixXd::Random(y,x);
            weights.push_back(m);
            // std::cout<< "Shape : "<<y<< " "<<x<< std::endl;
        }
    };

    Eigen::MatrixXd predict(Eigen::MatrixXd input){
        for (int i=0; i<biases.size(); i++){
            input = sigmoid((weights[i]*input) + biases[i]);
        }

        return input;
    };

};