#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;
}


//
// Helper data structures and types for Tree-iLQR.
//

#pragma once

#include <Eigen/Dense>

#include <functional>
#include <ostream>

namespace lqr
{

// Linearized dynamics parameters in terms of the extended-state [x, 1].
struct Dynamics
{
    // Extended linear dynamics matrix. [dim(x) + 1] x [dim(x) + 1]
    // (extension is last row is [\vec{0}, 1])
    Eigen::MatrixXd A;

    // Extended controls matrix. [dim(x) + 1] x [dim(u)]
    // (extension is last row is [\vec{0}])
    Eigen::MatrixXd B;
};

// Quadratic cost parameters in terms of the extended-state [x, 1].
struct Cost
{
    // Extended quadratic state-cost matrix. [dim(x) + 1] x [dim(x) + 1]
    Eigen::MatrixXd Q;

    // Quadratic control-cost matrix. [dim(u)] x [dim(u)]
    Eigen::MatrixXd R;
};

// Each plan node represents a timestep.
class PlanNode
{
public:
    PlanNode(int state_dim,
             int control_dim,
             const Eigen::MatrixXd A,
             const Eigen::MatrixXd B,
             const Eigen::MatrixXd Q,
             const Eigen::MatrixXd R,
             const double probablity);

    // Throws exception if a size of an item (dynamics, cost, etc.)
    // doesn't match expected sizes. Used for debugging.
    void check_sizes();

    // Linearized dynamics.
    Dynamics dynamics_;

    // Quadratic approximation of the cost.
    Cost cost_;

    // Feedback gain matrix on the extended-state, [dim(u)] x [dim(x) + 1]
    Eigen::MatrixXd K_;

    // Value matrix in x^T V x. [dim(x) + 1] x [dim(x) + 1]
    Eigen::MatrixXd V_;

    double probability_;

    // Uses the x_ and u_ to update the linearization of the dynamics.
    void update_dynamics();

    // Uses the x_ and u_ to update the quadraticization of the cost.
    void update_cost();

    //
    // Get and set the forward iLQR pass x and u.
    //

    // Set the state with a [dim(x)]  (note, not extended) vector.
    void set_x(const Eigen::VectorXd &x);
    void set_u(const Eigen::VectorXd &u);
    // Returns the extended-state [x, 1] for the iLQR forward pass.
    const Eigen::VectorXd& x() const { return x_; }
    // Returns the control [u] for the iLQR forward pass.
    const Eigen::VectorXd& u() const { return u_; };

private:
    int state_dim_;
    int control_dim_;

    // State from this iLQR forward pass. [dim(x)] x [1]
    Eigen::VectorXd x_;
    // Control from this iLQR forward pass. [dim(u)] x [1]
    Eigen::VectorXd u_;
    // Combined vector [x, u], used to call numerical differentiators (e.g. Jacobian).
    Eigen::VectorXd xu_;

};

// Allows the PlanNode to be printed.
std::ostream& operator<<(std::ostream& os, const lqr::PlanNode& node);

} // namespace ilqr


#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


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_bfgs.h>
#include <OpenTissue/core/math/big/big_generate_random.h>
#include <OpenTissue/core/math/big/big_generate_PD.h>
#include <OpenTissue/core/math/big/io/big_matlab_write.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef double real_type;
typedef ublas::compressed_matrix<real_type> matrix_type;
typedef ublas::vector<real_type>            vector_type;
typedef vector_type::size_type              size_type;

class F
{
public:
  matrix_type const & m_A;
  vector_type const & m_b;

  F(matrix_type const & A, vector_type const & b)
    : m_A(A)
    , m_b(b)
  {}

  real_type operator()( vector_type const & x ) const
  {
    return ublas::inner_prod(x, ublas::prod(m_A,x)) - inner_prod(m_b, x);
  }
};

class nabla_F
{
public:
  matrix_type const & m_A;
  vector_type const & m_b;

  nabla_F(matrix_type const & A, vector_type const & b)
    : m_A(A)
    , m_b(b)
  {}

  vector_type operator()( vector_type const & x ) const
  {
    return  vector_type( 2*ublas::prod(m_A,x) - m_b );
  }
};

template<typename func_functor, typename grad_functor>
void do_unconstrained_minimizer_test(func_functor & f, grad_functor & nabla_f, vector_type & x, matrix_type & H, vector_type const & solution )
{
  using namespace OpenTissue::math::big;

  size_type max_iterations       = 100;
  real_type absolute_tolerance   = boost::numeric_cast<real_type>(1e-6);
  real_type relative_tolerance   = boost::numeric_cast<real_type>(0.000000001);
  real_type stagnation_tolerance = boost::numeric_cast<real_type>(0.000000001);
  size_t status = 0;
  size_type iteration = 0;
  real_type accuracy = boost::numeric_cast<real_type>(0.0);
  real_type alpha = boost::numeric_cast<real_type>(0.0001);
  real_type beta = boost::numeric_cast<real_type>(0.5);

  OpenTissue::math::optimization::bfgs(
    f
    , nabla_f
    , H
    , x
    , max_iterations
    , absolute_tolerance
    , relative_tolerance
    , stagnation_tolerance
    , status
    , iteration
    , accuracy
    , alpha
    , beta
    );

  std::cout << "status     = "
    << OpenTissue::math::optimization::get_error_message(status)
    << std::endl;
  std::cout << "absolute   = "
    << accuracy
    << std::endl;
  std::cout << "iterations = "
    << iteration
    << std::endl;
  std::cout << "x          = "
    << x
    << std::endl;

  if(status==OpenTissue::math::optimization::ABSOLUTE_CONVERGENCE)
  {
    BOOST_CHECK( accuracy < absolute_tolerance );
    BOOST_CHECK( iteration <= max_iterations );
  }

  double tol = 0.001;
  BOOST_CHECK_CLOSE( x(0), solution(0), tol);
  BOOST_CHECK_CLOSE( x(1), solution(1), tol);
}

class F_rosenbrock
{
public:

  F_rosenbrock(){}

  real_type operator()( vector_type const & x ) const
  {
    real_type x_1 = x(0);
    real_type x_2 = x(1);
    return (10.0*(x_2-x_1*x_1)*(x_2-x_1*x_1) + (1- x_1)*(1- x_1));
  }
};

class nabla_F_rosenbrock
{
public:

  nabla_F_rosenbrock(){}

  vector_type operator()( vector_type const & x ) const
  {
    real_type x_1 = x(0);
    real_type x_2 = x(1);
    vector_type retur(2);
    retur(0)=-40.0*(x_1*x_2 -x_1*x_1*x_1) - 2*(1-x_1);
    retur(1)=20.0*(x_2-x_1*x_1);
    return  retur;
  }
};

BOOST_AUTO_TEST_SUITE(opentissue_math_big_bfgs);

BOOST_AUTO_TEST_CASE(simple_test_case)
{
  using namespace OpenTissue::math::big;

  // We are solving the problem
  //
  //   min_x Q(x) = x^T A x - b^T x
  //
  // where the gradient is given by
  //
  //   nabla Q(x) = 2 A x - b = 0
  //
  // and the exact Hessian is
  //
  //   H = nabla^2 Q(x) = 2 A
  //
  // The stationary points are given by
  //
  //  | 4 0| |x_1| + | -1| = 0
  //  | 0 4| |x_2|   | -2|
  //
  // and has the unique solution x = [-0.25, -0.5]^T
  //
  size_type N = 2;

  matrix_type A;
  A.resize(N,N,false);

  vector_type b;
  b.resize(N,false);

  A(0,0) = 2.0;  A(0,1) = 0.0;
  A(1,0) = 0.0;  A(1,1) = 2.0;

  b(0) = -1.0;
  b(1) = -2.0;

  vector_type solution;
  solution.resize(N,false);
  solution(0) = -0.25;
  solution(1) = -0.5;

  F f(A,b);
  nabla_F nabla_f(A,b);

  vector_type x;
  x.resize(N,false);
  matrix_type H;
  H.resize(N,N,false);

  // use H = I/4, and x = 0
  x.clear();
  H(0,0) = 0.25;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 0.25;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I, and x = 0
  x.clear();
  H(0,0) = 1.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 1.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = 4*I, and x = 0
  x.clear();
  H(0,0) = 4.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 4.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I/4, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  H(0,0) = 0.25;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 0.25;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  H(0,0) = 1.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 1.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = 4*I, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  H(0,0) = 4.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 4.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = random PD, and x = 0
  x.clear();
  OpenTissue::math::big::generate_PD(2, H);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = random PD, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  OpenTissue::math::big::generate_PD(2, H);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // H = g g^T, x = 0
  x.clear();
  vector_type g;
  g.resize(N,false);
  g = nabla_f(x);
  H = ublas::outer_prod(g,g);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // H = g g^T, x = random
  OpenTissue::math::big::generate_random( 2, x);
  g = nabla_f(x);
  H = ublas::outer_prod(g,g);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // H = exact Hessian, x = solution!
  x(0) = -0.25;
  x(1) = -0.5;
  H(0,0) = 4.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 4.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

}

BOOST_AUTO_TEST_CASE(rosenbrock_test_case)
{
  using namespace OpenTissue::math::big;

  size_type N = 2;

  vector_type solution;
  solution.resize(N,false);
  solution(0) =  1.0;
  solution(1) =  1.0;

  F_rosenbrock f;
  nabla_F_rosenbrock nabla_f;

  vector_type x;
  x.resize(N,false);
  matrix_type H;
  H.resize(N,N,false);

  // use H = I/4, and x = 0
  x.clear();
  x(0)=2.0;
  x(1)=2.0;
  H(0,0) = 0.25;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 0.25;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I, and x = 0
  std::cout << std::endl;

  x.clear();
  x(0)=2.0;
  x(1)=2.0;
  std::cout << "using H = I   x = " << x <<  std::endl;
  H(0,0) = 1.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 1.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = 4*I, and x = 0
  std::cout << std::endl;

  x.clear();
  std::cout << "using H = 4*I   x = "<< x << std::endl;
  H(0,0) = 4.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 4.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I/4, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  x *= 3.0;
  H(0,0) = 0.25;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 0.25;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = I, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  x *= 3.0;
  H(0,0) = 1.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 1.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = 4*I, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  x *= 3.0;
  H(0,0) = 4.0;   H(0,1) = 0.0;
  H(1,0) = 0.0;    H(1,1) = 4.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = random PD, and x = 0
  x.clear();
  OpenTissue::math::big::generate_PD(2, H);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // use H = random PD, and x = random
  OpenTissue::math::big::generate_random( 2, x);
  x *= 3.0;
  OpenTissue::math::big::generate_PD(2, H);
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

  // H = exact Hessian, x = solution!
  x(0) = 1.0;
  x(1) = 1.0;
  H(0,0) = 82.0;   H(0,1) = -40.0;
  H(1,0) = -40.0;    H(1,1) = 20.0;
  do_unconstrained_minimizer_test(f,nabla_f,x,H,solution);

}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @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;
}


/*
	kstatboost
	Ver. k09.00

	Written by Koji Yamamoto
	Copyright (C) 2020 Koji Yamamoto
	In using this, please read the document which states terms of use.

	Statistical Computations using Boost

*/

/* ********** Preprocessor Directives ********** */

#ifndef kstatboost_cpp_include_guard
#define kstatboost_cpp_include_guard

#include <vector>

#include <boost/math/tools/bivariate_statistics.hpp>

/* ********** Using Directives ********** */

//using namespace std;

/* ********** Type Declarations: enum, class, etc. ********** */

/* ********** Function Declarations ********** */

double corrBoost( const std::vector <double> &, const std::vector <double> &);

/* ********** Type Definitions: enum, class, etc. ********** */

/* ********** Global Variables ********** */

/* ********** Definitions of Static Member Variables ********** */

/* ********** Function Definitions ********** */

double corrBoost( const std::vector <double> &xvec, const std::vector <double> &yvec)
{

	double ret = boost::math::tools::correlation_coefficient( xvec, yvec);
	return ret;

}

/* ********** Definitions of Member Functions ********** */

#endif /* kstatboost_cpp_include_guard */


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <cmath>

template <typename T_N, typename T_n>
void test_binom_coefficient(const T_N& N, const T_n& n) {
  using stan::math::binomial_coefficient_log;
  EXPECT_FLOAT_EQ(lgamma(N + 1) - lgamma(n + 1) - lgamma(N - n + 1),
                  binomial_coefficient_log(N, n));
}

TEST(MathFunctions, binomial_coefficient_log) {
  using stan::math::binomial_coefficient_log;
  EXPECT_FLOAT_EQ(1.0, exp(binomial_coefficient_log(2.0, 2.0)));
  EXPECT_FLOAT_EQ(2.0, exp(binomial_coefficient_log(2.0, 1.0)));
  EXPECT_FLOAT_EQ(3.0, exp(binomial_coefficient_log(3.0, 1.0)));
  EXPECT_NEAR(3.0, exp(binomial_coefficient_log(3.0, 2.0)), 0.0001);

  EXPECT_FLOAT_EQ(29979.16, binomial_coefficient_log(100000, 91116));

  for (int n = 0; n < 1010; ++n) {
    test_binom_coefficient(1010, n);
    test_binom_coefficient(1010.0, n);
    test_binom_coefficient(1010, static_cast<double>(n));
    test_binom_coefficient(1010.0, static_cast<double>(n));
  }

  test_binom_coefficient(1e9, 1e5);
  test_binom_coefficient(1e50, 1e45);
  test_binom_coefficient(1e20, 1e15);
}

TEST(MathFunctions, binomial_coefficient_log_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::binomial_coefficient_log(2.0, nan));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::binomial_coefficient_log(nan, 2.0));
  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::binomial_coefficient_log(nan, nan));
}


/***********************************************************************
* 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>
#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


#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;
}


/*
 * SpectralClustering.cpp
 *
 *  Created on: 04-Mar-2009
 *      Author: sbutler
 */
#define EIGEN2_SUPPORT

#include "SpectralClustering.h"
#include "ClusterRotate.h"
#include "Kmeans.h"

#include <Eigen/QR>

 /**
 * Performs eigenvector decomposition of an affinity matrix
 *
 * @param data 		the affinity matrix
 * @param numDims	the number of dimensions to consider when clustering
 */
SpectralClustering::SpectralClustering(int numDims) :
	mNumDims(numDims),
	mNumClusters(0)
{

}

SpectralClustering::~SpectralClustering() {
}

Eigen::MatrixXd SpectralClustering::CalcEigenVectors(Eigen::MatrixXd& affinityMatrix)
{
	Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(affinityMatrix.rows(), affinityMatrix.cols());

	// calc normalised laplacian
	for (int i = 0; i < affinityMatrix.cols(); i++)
	{
		Deg(i, i) = 1 / (sqrt((affinityMatrix.row(i).sum())));
	}
	Eigen::MatrixXd Lapla = Deg * affinityMatrix * Deg;

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
	Eigen::VectorXd val = s.eigenvalues();
	Eigen::MatrixXd vec = s.eigenvectors();

	//sort eigenvalues/vectors
	int n = affinityMatrix.cols();
	for (int i = 0; i < n - 1; ++i)
	{
		int k;
		val.segment(i, n - i).maxCoeff(&k);
		if (k > 0)
		{
			std::swap(val[i], val[k + i]);
			vec.col(i).swap(vec.col(k + i));
		}
	}

	//choose the number of eigenvectors to consider
	if (mNumDims < vec.cols())
	{
		mEigenVectors = vec.block(0, 0, vec.rows(), mNumDims);
	}
	else
	{
		mEigenVectors = vec;
	}

	return mEigenVectors;
}

/**
 * Cluster by rotating the eigenvectors and evaluating the quality
 */
std::vector<std::vector<int> > SpectralClustering::clusterRotate(Eigen::MatrixXd& eigenVectors)
{

	ClusterRotate* clusterRotate = new ClusterRotate();
	std::vector<std::vector<int> > clusters = clusterRotate->cluster(eigenVectors);

	mNumClusters = clusters.size();

	return clusters;
}

/**
 * Cluster by kmeans
 *
 * @param numClusters	the number of clusters to assign
 */
std::vector<std::vector<int> > SpectralClustering::clusterKmeans(Eigen::MatrixXd& eigenVectors,int numClusters)
{
	mNumClusters = numClusters;
	return Kmeans::cluster(mEigenVectors, numClusters);
}


#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;

  }
}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of self_adjoint_kernel_solver class.
 */
#include "num_collect/interp/kernel/impl/self_adjoint_kernel_solver.h"

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "is_finite.h"
#include "num_collect/interp/kernel/calc_kernel_mat.h"
#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"
#include "num_collect/interp/kernel/rbf_kernel.h"

TEST_CASE("num_collect::interp::kernel::self_adjoint_kernel_solver") {
    using num_collect::interp::kernel::calc_kernel_mat;
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::rbf_kernel;
    using num_collect::interp::kernel::impl::self_adjoint_kernel_solver;

    const auto vars = std::vector<double>{0.0, 0.1, 0.2, 0.4, 0.6, 1.0};
    const auto data = Eigen::VectorXd{{0.0, 0.2, 0.4, 0.7, 1.0, 2.0}};

    auto kernel =
        rbf_kernel<euclidean_distance<double>, gaussian_rbf<double>>();
    constexpr double len_param = 0.1;
    kernel.len_param(len_param);

    const Eigen::MatrixXd kernel_mat = calc_kernel_mat(kernel, vars);

    SECTION("compute") {
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);

        REQUIRE(solver.eigenvalues().size() == data.size());
        for (num_collect::index_type i = 0; i < data.size(); ++i) {
            INFO("i = " << i);
            REQUIRE(solver.eigenvalues()(i) > 0.0);
        }
    }

    SECTION("solve without regularization") {
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);
        Eigen::VectorXd coeff;
        solver.solve(0.0, coeff);

        const Eigen::VectorXd retrieved_data = kernel_mat * coeff;
        REQUIRE_THAT(retrieved_data, eigen_approx(data));
    }

    SECTION("solve with regularization") {
        constexpr double reg_param = 1.234;
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);
        Eigen::VectorXd coeff;
        solver.solve(reg_param, coeff);

        const Eigen::VectorXd retrieved_data =
            (kernel_mat +
                Eigen::MatrixXd::Identity(data.size(), data.size()) *
                    reg_param) *
            coeff;
        REQUIRE_THAT(retrieved_data, eigen_approx(data));
    }

    SECTION("calculate MLE objective function") {
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);

        const double mle_zero = solver.calc_mle_objective(0.0);
        REQUIRE_THAT(mle_zero, is_finite());

        constexpr double large_param = 1e+3;
        const double mle_large = solver.calc_mle_objective(large_param);
        REQUIRE_THAT(mle_large, is_finite());

        REQUIRE(mle_large > mle_zero);
    }

    SECTION("calculate the coefficient of the kernel common in variables") {
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);

        REQUIRE(solver.calc_common_coeff(0.0) > 0.0);
        constexpr double reg_param = 1e-3;
        REQUIRE(solver.calc_common_coeff(reg_param) > 0.0);
    }

    SECTION("calculate the regularization term for a vector") {
        auto solver =
            self_adjoint_kernel_solver<Eigen::MatrixXd, Eigen::VectorXd>();
        solver.compute(kernel_mat, data);

        constexpr double reg_param = 1e-3;
        const auto vec = Eigen::VectorXd{{0.1, 0.2, 0.4, 0.5, 1.0, 0.7}};
        Eigen::LLT<Eigen::MatrixXd> llt;
        llt.compute(kernel_mat +
            reg_param * Eigen::MatrixXd::Identity(data.size(), data.size()));
        const Eigen::VectorXd inv_kernel_vec = llt.solve(vec);
        const auto expected = inv_kernel_vec.dot(vec);
        REQUIRE_THAT(solver.calc_reg_term(reg_param, vec),
            Catch::Matchers::WithinRel(expected));
    }
}


//==================================================================================================
/*!
  @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
    \brief Simple plot of 1D data of values with uncertainty.
    \details An example to demonstrate simple 1D plot using two vectors,
     including showing values with uncertainty information as
     "plus minus" and degrees of freedom estimates.
*/

// demo_1d_uncertainty.cpp

// Copyright Paul A Bristow 2009, 2012, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_1d_uncertainty_1

/*`First we need a few includes to use Boost.Plot:
*/
#include <boost/quan/unc.hpp>  // Handles information uncertainty.
// using boost::quan::unc; // Holds value and uncertainty information.
#include <boost/svg_plot/detail/functors.hpp>
//  using boost::svg::detail::unc_1d_convert;
#include <boost/svg_plot/svg_1d_plot.hpp>
//using namespace boost::svg;
// If list of settings required:
#include <boost/svg_plot/show_1d_settings.hpp>
//void boost::svg::show_1d_plot_settings(svg_1d_plot&);

#include <iostream>
// using std::cout;
// using std::endl;
#include <vector>
//  using std::vector;
#include <iterator>
//  using std::ostream_iterator;
#include <algorithm>
//  using std::copy;
//] [/demo_1d_uncertainty_1]

int main()
{
  using namespace boost::svg;  // Convenient to avoid specifying each SVG color name separately.

  using boost::quan::unc;
  using boost::quan::uncun; // Uncertain Uncorrelated type (the normal case).

//[demo_1d_uncertainty_2

    /*`A STL @c std::vector is used as the container for our three data-series,
    and values are inserted using `std::push_back`. (Since this is a 1-D plot
    the order of data values is not important).
    */
    setUncDefaults(std::cout);
    constexpr float NaN = std::numeric_limits<float>::quiet_NaN();  // NotANumber used for unknown values and unknown standard deviation.

    std::vector<uncun> A_times;
    A_times.push_back(unc<false>(3.1, 0.02F, 8));  // Not using a typedef uncun.
    A_times.push_back(uncun(4.2, 0.01F, 14, 0U));  // Using a typedef uncun - usually easier to read.

    std::vector<uncun > B_times;
    short unsigned int t = UNC_KNOWN | UNC_EXPLICIT| DEG_FREE_EXACT | DEG_FREE_KNOWN; // An uncertain type. (But use of uncertain type t is not yet implemented.)

    B_times.push_back(uncun(2.1, 0.001F, 30, t)); // Value (2.1), uncertainty (0.001F), degrees of freedom (30) and uncertain type known.
    B_times.push_back(unc<>(5.1, 0.025F, 20, 0U)); // Value, uncertainty, and degrees of freedom known - the usual case.
    B_times.push_back(uncun(7.8, 0.0025F, 1, 0U)); // Value and uncertainty known, but not degrees of freedom.
    B_times.push_back(uncun(3.4, 0.03F, 1, 0U)); // Value and uncertainty known, but not degrees of freedom.
    //B_times.push_back(uncun(6.9, 0.0F, 0, 0U)); // Only value known - no information available about uncertainty but treated as exact.
    B_times.push_back(uncun(5.9, NaN, 1, 0U)); // Only value known - uncertainty explicit, NaN meaning no information available about uncertainty.
    // So in both cases show all possibly significant digits (usually 15).
    // This is ugly on a graph, so best to be explicit about uncertainty.

    std::vector<unc<false> > C_times; //
    C_times.push_back(uncun(2.6, 0.1F, 5, 0U));
    C_times.push_back(uncun(5.4, 0.2F, 11, 0U));

    /*`Echo the values (and their uncertainty information) input: */

    std::cout << plusminus << addlimits << adddegfree << std::endl;
    std::cout << "A_times: " << std::endl;
    std::copy(A_times.begin(), A_times.end(), std::ostream_iterator<uncun>(std::cout, "\t ")); // 3.10 4.200
    std::cout << std::endl;
    std::cout << "B_times: " << std::endl;
    std::copy(B_times.begin(), B_times.end(), std::ostream_iterator<uncun>(std::cout, "\t ")); // 2.1000 5.10 7.800 3.40 5.900
    std::cout << std::endl;

    std::cout << plusminus << addlimits << adddegfree << "A_times[0] = " << A_times[0] << std::endl;
    // B_times[0] = 2.1000 +/-0.0010 <2.10, 2.10> (30)

    /*`The constructor initializes a new 1D plot, called `my_plot`,
    and also sets all the very many defaults for axes, width, colors, etc.
    */
    svg_1d_plot my_plot;
    /*`A few (member) functions that are set (using concatenation or chaining) should be fairly self-explanatory:
``
    .title() provides a title at the top for the whole plot,
    .legend_on(true) will mean that titles of data-series and their markers will display in the legend box.
    .x_range(-1, 11) sets the axis limits from -1 to +11 (instead of the default -10 to +10).
    .background_border_color(blue) sets just one of the very many other options.
``
    Also some autoscaling settings:
    */
    my_plot.autoscale_check_limits(false); // Default is true.
    my_plot.autoscale_plusminus(1); //
    //  //! Set how many std_dev or standard-deviation to allow for ellipse when autoscaling.
    //! Default is 3 for 99% confidence.
    // my_plot.confidence(0.01);  // Change alpha from default 0.05 == 95% to 0.01 == 99%.
    //  my_plot.plusminus_sds(2.); // Show uncertainty as plusminus as two times standard deviation.
    // my_plot.xy_values(false);

    my_plot
    .image_x_size(600)
    .image_y_size(300)
    .plot_window_on(true)
    .background_border_color(blue)
    .plot_border_color(yellow)
    .plot_border_width(1)
    //.x_ticks_on_window_or_axis(0) // now the default.
    .legend_on(false) // or true to show a legend box.
    .title("A, B and C Times")
    .x_range(0, 10) // but will be over-ridden by `.x_autoscale(B_times)` below:
    .x_autoscale(B_times) // Use the data-point values in series B_times to autoscale the X-axis.
        // Note that this might not be ideal scaling for A_times and/or C_times.
    .x_label("times (sec)")
    .x_values_on(true)
//   .x_values_precision(0) // Automatic number of digits of precision.
    .x_values_precision(2) // User-chosen std::ios precision decimal digits, for example "1.23".
    //.x_values_rotation(steepup) // steeper - but need more image and plot window vertical height for all data-point info.
    .x_values_rotation(slopeup) // value description at x = 7.8 overflows both plot window and image!
      // so might need to change `.x_range` or slope to avoid this.
    .x_plusminus_on(true)
    .x_plusminus_color(blue)
    .x_addlimits_on(true) // Show plus/minus +/- confidence limits/interval for data-point value-labels.
    .x_addlimits_color(purple)  // Show +/- in darkgreen, for example: "+/- 0.03".
    .x_df_on(true) // Show degrees of freedom (usually observations -1) for data-points, for example: "11".
    .x_df_color(green)  // Show degrees of freedom (usually observations -1) in green.
    ;
    /*`
    Then we add our three data-series, and add optional data-series titles, "A_times" "B_times" ...
    (very helpful if we want them to show on the legend).

    All the data-points are also labeled with their value,
    and uncertainty (+/-) and degrees of freedom if known.
    The A_times mark data-points with a red-border circle with a green fill,
    The B_times use a blue vertical line.
    */

    my_plot.plot(A_times, "A").shape(circlet).size(10).stroke_color(red).fill_color(green);
    my_plot.plot(B_times, "B").shape(vertical_line).stroke_color(blue);

    /*`C_times use ellipses whose width (X radius) is from the uncertainty,
    1st standard deviation shows as ellipse in magenta, and 2nd as yellow, and 3rd as.

    Or one can explicitly set some (brighter) colors for the uncertainty ellipses, or none to omit.
    */
    my_plot
      .one_sd_color(pink) // Color of ellipse for one standard deviation (~68% probability).
      .two_sd_color(magenta) // Color of ellipse for two standard deviation (~95%).
      .three_sd_color(yellow); // Color of ellipse for two standard deviation (~99%).

    my_plot.plot(C_times, "C").shape(unc_ellipse).fill_color(black).stroke_color(black);

    /*`
    Finally, we can write the SVG to a file of our choice.
    */
    std::string svg_file =  (my_plot.legend_on() == true) ?
                            "./demo_1d_uncertainty_legend.svg" : "./demo_1d_uncertainty.svg";

    my_plot.write(svg_file);

    std::cout <<"Plot written to file " << svg_file << std::endl;

    /*`Optionally we can list all the very many setting in use:*/
    using boost::svg::show_1d_plot_settings;
   // show_1d_plot_settings(my_plot);

//] [/demo_1d_uncertainty_2]

    return 0;
} // int main()

/*

Output:
//[demo_1d_uncertainty_output
demo_1d_uncertainty.cpp
Linking...
Embedding manifest...
Autorun j:\Cpp\SVG\debug\demo_1d_uncertainty.exe
3.1?0.02 (8)  4.2?0.01 (14)
2.1?0.001 (30)  7.8?0.0025 (21)  3.4?0.03 6.9
Build Time 0:03
//] [/demo_1d_uncertainty_output]
*/

/*
Data for another 1D example, also use Boost.Math?

The data is taken from J. C. Banford el al, Analyst (1983) 107, 195
and compares the concentration of thiol of the lysate of the blood of two groups
of volunteers as control and a second suffering from rhematoid arthritis.

  map<pair<unc, unc> > control;
  control.push_back(unc(1.921, 0.076F, 7));
  map<pair<unc, unc> > rheumatoid;
  control.push_back(unc(3.456, 0.44F, 6));

*/

/* Output Netbeans 4.7.2

3.10 4.200
2.1000 5.10 7.800 3.40 5.900
Plot written to file ./demo_1d_uncertainty.svg

axes_on false
background_border_width 1
background_border_color RGB(0,0,255)
background_color RGB(255,255,255)
...

 */


/** @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 "algebra/Matrix.hpp"
#include "algebra/Vector.hpp"
#include "utils/GreaterThanUtils.hpp"
#include <NTL/ZZX.h>
#include <cassert>
void test_Matrix()
{
#ifdef USE_EIGEN
    MDL::Matrix<double> mat(3, 3);

    mat[0][0] = 10;
    mat[1][1] = 3;
    mat[2][2] = 4;

    auto inv = mat.inverse();
    auto I   = inv.dot(mat);
    assert(I[0][0] == I[1][1] == I[2][2] == 1.0);
    assert(I[0][1] == I[1][2] == I[0][2] == 0.0);
    {
        MDL::Matrix<long> mat(3, 3);
        mat[0][0] = 10;
        mat[1][1] = 3;
        mat[2][2] = 4;
        auto prod = mat.dot(mat);
        assert(prod[0][0] == 100);
        assert(prod[1][1] == 9);
        assert(prod[2][2] == 16);
        auto submat = mat.submatrix(0, 1);
        auto matT = submat.transpose();
        std::cout << submat << std::endl;
        std::cout << matT << std::endl;
        prod = matT.dot(submat);
        std::cout << prod << std::endl;
    }
#endif
}

void test_Vector()
{
    MDL::Vector<NTL::ZZX> vec(3);

    vec[0] = NTL::to_ZZX(1);
    vec[1] = NTL::to_ZZX(4);
    vec[2] = NTL::to_ZZX(3);
    assert(std::abs(vec.L2() - std::sqrt(26.0)) < 1e-9);
}

void test_random_permutation()
{
    auto vecs  = permutated_range(10, 6);
    auto noise = random_noise(10, 6, 10);
}

int main() {
    test_Matrix();
    test_Vector();
    test_random_permutation();
    printf("Passed all test!\n");
    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;
};


#include <gtest/gtest.h>
#include <boost/math/constants/constants.hpp>
#include <functional>
#include <vector>
#include <iostream>
#include <iomanip>

#include "quadrature/qhermite.hpp"
#include "quadrature/qhermitew.hpp"
#include "quadrature/qmaxwell.hpp"
#include "quadrature/qmaxwellw.hpp"
#include "spectral/hermiten.hpp"
#include "spectral/hermitenw.hpp"

using namespace std;
using namespace boltzmann;

TEST(quadrature, maxwell)
{
  int digits = 128;

  static const double PI = boost::math::constants::pi<double>();
  std::vector<int> Ns = {1, 5, 10, 21, 40, 60, 80};

  for (auto N : Ns) {
    QMaxwell qmaxwell(1, N, digits);

    // test integration \int \exp{-r^2} r \dd r = pi
    double sum = 0;
    for (int i = 0; i < N; ++i) {
      sum += qmaxwell.wts(i);
    }
    sum *= 2 * PI;
    EXPECT_NEAR(sum, PI, 1e-12) << boost::lexical_cast<string>(N)
                                << ": integrate: e^{r^2} r, on [0,\\infty)";

    // test integration \int r \exp{-r^2} r \dd r = 1/2 pi^3/2
    sum = 0;
    for (int i = 0; i < N; ++i) {
      sum += qmaxwell.wts(i) * qmaxwell.pts(i);
    }
    sum *= 2 * PI;
    EXPECT_NEAR(sum, 0.5 * std::pow(PI, 1.5), 1e-12)
        << boost::lexical_cast<string>(N) << "integrate: e^{r^2} r^2, on [0,\\infty)";
  }
}

TEST(quadrature, maxwellw)
{
  int digits = 128;

  static const double PI = boost::math::constants::pi<double>();
  std::vector<int> Ns = {1, 5, 10, 21, 40, 60, 80};

  for (auto N : Ns) {
    QMaxwellW qmaxwell(1, N, digits);

    // test integration \int \exp{-r^2} r \dd r = pi
    double sum = 0;
    for (int i = 0; i < N; ++i) {
      double x = qmaxwell.pts(i);
      sum += qmaxwell.wts(i) * std::exp(-x*x);
    }
    sum *= 2 * PI;
    EXPECT_NEAR(sum, PI, 1e-12) << boost::lexical_cast<string>(N)
                                << ": integrate: e^{r^2} r, on [0,\\infty)";

    // test integration \int r \exp{-r^2} r \dd r = 1/2 pi^3/2
    sum = 0;
    for (int i = 0; i < N; ++i) {
      double x = qmaxwell.pts(i);
      sum += qmaxwell.wts(i) * qmaxwell.pts(i) * std::exp(-x*x);
    }
    sum *= 2 * PI;
    EXPECT_NEAR(sum, 0.5 * std::pow(PI, 1.5), 1e-12)
        << boost::lexical_cast<string>(N) << "integrate: e^{r^2} r^2, on [0,\\infty)";
  }
}

/**
 *  @brief Test Gauss-Hermite quadrature to integrate 1, x, x^2
 *
 *  Detailed description
 *
 *  @param param
 *  @return return type
 */
TEST(quadrature, hermite_quad_only)
{
  std::vector<int> Ns = {2, 5, 10, 21, 40, 60, 80, 120};
  int digits = 128;
  cout << "Testing n=" << std::for_each(Ns.begin(), Ns.end(), [](int x) { cout << x << " "; })
       << "\n";

  static const double PI = boost::math::constants::pi<double>();

  for (auto N : Ns) {
    QHermite quad(1.0, /* alpha */
                  N,   /*  num. quad. poins */
                  digits);

    {
      double sum = 0;
      for (int i = 0; i < N; ++i) {
        sum += quad.wts(i);
      }
      EXPECT_NEAR(sum, std::sqrt(PI), 1e-12)
          << boost::lexical_cast<string>(N) << "integrate e^{-x^2} on (-\\infty, \\infty)";
    }
    {
      double sum = 0;
      for (int i = 0; i < N; ++i) {
        sum += quad.wts(i) * quad.pts(i);
      }
      EXPECT_NEAR(sum, 0, 1e-12) << boost::lexical_cast<string>(N)
                                 << "integrate x e^{-x^2} on (-\\infty, \\infty)";
    }
    {
      double sum = 0;
      for (int i = 0; i < N; ++i) {
        sum += quad.wts(i) * quad.pts(i) * quad.pts(i);
      }
      EXPECT_NEAR(sum, std::sqrt(PI) / 2, 1e-12)
          << boost::lexical_cast<string>(N) << "integrate x^2 e^{-x^2} on (-\\infty, \\infty)";
    }
  }
}

/**
 *  @brief Test hermite quadrature rule by computing overlap integrals of Hermite polynomials.
 *
 *
 */
TEST(quadrature, hermite_full)
{
  // hint: quadrature rule for N=1 does not exist!
  std::vector<int> Ns = {2, 5, 10, 21, 40, 60, 80, 120};
  cout << "Testing n=" << std::for_each(Ns.begin(), Ns.end(), [](int x) { cout << x << " "; })
       << "\n";
  typedef double numeric_t;

  for (auto N : Ns) {
    QHermiteW quad(1.0, N);
    auto& wts = quad.wts();
    auto& pts = quad.pts();

    HermiteNW<numeric_t> HW(N);
    HW.compute(quad.pts());

    for (unsigned i = 0; i < N; ++i) {
      double val = 0;
      for (unsigned int q = 0; q < quad.size(); ++q) {
        // const double wh = exp(0.5*quad.pts(q) * quad.pts(q));
        val += (HW.get(i)[q]) * (HW.get(i)[q]) * quad.wts(q);
      }
      EXPECT_NEAR(val, 1, 1e-12)
          << "(h_i, h_i) "
          << "i=" << boost::lexical_cast<string>(i) << ", N=" << boost::lexical_cast<string>(N)
          << " (evaluatued Hermite polynomials _with_ expt weight aka Hermite functions)";
    }
  }
}