#include "geometrycentral/quad_cover.h"
#include <Eigen/SparseCholesky>

// ONLY WORKS FOR MESHES WITHOUT BOUNDARY
QuadCover::QuadCover(HalfedgeMesh* m, Geometry<Euclidean>* g) : mesh(m), geom(g), phi(m), r(m), field(m),
                                                                singularities(m), branchCover(m) {
    assert(mesh->nBoundaryLoops() == 0);
}

void QuadCover::setup() {
    VertexData<double> s(mesh);
    // Compute s_i at each vertex
    for (VertexPtr v : mesh->vertices()) {
        if (v.isBoundary()) {
            s[v] = 1.0;
        } else {
            double sum = 0;
            for (HalfedgePtr he : v.outgoingHalfedges()) {
                sum += geom->angle(he.next());
            }
            s[v] = 2*M_PI / sum;
        }
    }

    // Compute transport at edges r_ij <- e^ip_ij
    for (VertexPtr v : mesh->vertices()) {
        HalfedgePtr he = v.halfedge();
        double angle = 0;
        double s_i = s[v];
        do {
            phi[he] = angle;
            angle += s_i * geom->angle(he.next());
            he = he.next().next().twin();
        } while (he != v.halfedge());
    }

    // Compute r_ij
    std::complex<double> i(0, 1);
    for (VertexPtr v : mesh->vertices()) {
        for (HalfedgePtr he : v.outgoingHalfedges()) {
            double theta_ij = phi[he];
            double theta_ji = phi[he.twin()] + M_PI;
            double rho_ij = theta_ij - theta_ji;
            r[he] = std::exp(i * n * rho_ij);
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::assembleM() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> M(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i, i, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j, j, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k, k, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::assembleA() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> A(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        HalfedgePtr he_jk = f.halfedge().next();
        HalfedgePtr he_ki = f.halfedge().prev();

        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_jk.vertex()];
        size_t k = vertexIndices[he_ki.vertex()];

        double a = geom->cotan(he_jk);
        double b = geom->cotan(he_ki);
        double c = geom->cotan(he_ij);

        std::complex<double> r_ij = r[he_ij];
        std::complex<double> r_ji = r[he_ij.twin()];
        std::complex<double> r_jk = r[he_jk];
        std::complex<double> r_kj = r[he_jk.twin()];
        std::complex<double> r_ki = r[he_ki];
        std::complex<double> r_ik = r[he_ki.twin()];

        // row i
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,i,b + c));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,j,-c * r_ij));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,k,-b * r_ik));

        // row j
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,i,-c * r_ji));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,j,c + a));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,k,-a * r_jk));

        // row k
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,i,-b * r_ki));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,j,-a * r_kj));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,k,a + b));
    }
    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

void QuadCover::computeSmoothestField(Eigen::SparseMatrix<std::complex<double>> M, Eigen::SparseMatrix<std::complex<double>> A) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd u = Eigen::MatrixXcd::Random(mesh->nVertices(),1);
    Eigen::MatrixXcd x;

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(M * u);
        std::complex<double> norm2 = (x.transpose() * M * x)(0,0);
        u = x / sqrt(norm2);
    }

    // map resulting vector to VertexData
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        std::complex<double> c = u(vertexIndices[v],0);
        if (std::abs(c) == 0) {
            field[v] = 0;
        } else {
            field[v] = c / std::abs(c);
        }
    }
}

VertexData<std::complex<double>> QuadCover::computeCrossField() {
    std::cout << "Computing Cross Field!" << std::endl;
    // Algorithm 1 : Setup
    setup();

    // Algorithm 2 : Smoothest Field
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    Eigen::SparseMatrix<std::complex<double>> A = assembleA();
    A = A + eps * M;
    computeSmoothestField(M,A);

    std::cout << "Done computing smoothest field!" << std::endl;
    return field;
}

FaceData<int> QuadCover::computeSingularities() {
    std::cout << "Computing Singularities!" << std::endl;
    // first, compute Omega_ijk <- arg(r_ij r_jk r_ki)
    FaceData<double> Omega(mesh);
    for (FacePtr f : mesh->faces()) {
        std::complex<double> r_ij = r[f.halfedge()];
        std::complex<double> r_jk = r[f.halfedge().next()];
        std::complex<double> r_ki = r[f.halfedge().prev()];
        Omega[f] = std::arg(r_ij * r_jk * r_ki);
    }

    // next, compute w_ij for each e_ij, such that u_j = e^iw_ij * r_ij * u_i
    // w_ij = arg(u_j / (r_ij * u_i))
    HalfedgeData<double> w(mesh);
    for (HalfedgePtr he : mesh->allHalfedges()) {
        std::complex<double> u_i = field[he.vertex()];
        std::complex<double> u_j = field[he.twin().vertex()];
        std::complex<double> r_ij = r[he];
        w[he] = std::arg(u_j * r_ij / u_i);
    }

    // finally, compute index for each triangle t
    // (1/2pi) * (w_ij + w_jk + w_ki + Omega_ijk)
    for (FacePtr f : mesh->faces()) {
        double w_ij = w[f.halfedge()];
        double w_jk = w[f.halfedge().next()];
        double w_ki = w[f.halfedge().prev()];
        double Omega_ijk = Omega[f];
        double phi = (w_ij + w_jk + w_ki - Omega_ijk) / (2.0 * M_PI);
        singularities[f] = std::round(phi);
    }
    return singularities;
}

void QuadCover::computeBranchCover() {
    std::cout<< "Computing Branch Cover!" << std::endl;
    std::complex<double> i(0, 1);
    for (FacePtr f : mesh->faces()) {
        int total = 0;
        for (HalfedgePtr he_ij : f.adjacentHalfedges()) {
            HalfedgePtr he_ji = he_ij.twin();

            std::complex<double> f_ij = std::pow(field[he_ij.vertex()], 1.0 / n);
            std::complex<double> f_ji = std::pow(field[he_ji.vertex()], 1.0 / n);

            // we need to recompute r_ij here without raising to the nth power,
            // as raising to the nth power and then taking the nth root is not always an identity operation
            double theta_ij = phi[he_ij];
            double theta_ji = phi[he_ij.twin()] + M_PI;
            double rho_ij = theta_ji - theta_ij;
            std::complex<double> r_ij = std::exp(i * rho_ij);
            std::complex<double> s_ij = f_ji / (f_ij * r_ij);
            double ang = std::arg(s_ij);

            if (ang >= -M_PI_4 && ang < M_PI_4) {
                branchCover[he_ij] = 0;
            } else if (ang >= M_PI_4 && ang < 3.0 * M_PI_4) {
                branchCover[he_ij] = 1;
                total = (total + 1) % 4;
            } else if ((ang >= 3.0 * M_PI_4 && ang <= PI) ||
                       (ang < -3.0 * M_PI_4 && ang >= -PI)) {
                branchCover[he_ij] = 2;
                total = (total + 2) % 4;
            } else {
                assert(ang >= -3.0 * M_PI_4 && ang < -M_PI_4);
                branchCover[he_ij] = 3;
                total = (total + 3) % 4;
            }
            /*
            if ( ang >= -M_PI_2 && ang < M_PI_2 ) {
                branchCover[he_ij.edge()] = 0;
            } else {
                branchCover[he_ij.edge()] = 1;
                total = (total + 1) % 2;
            }*/
        }

        if (singularities[f] != 0 && total == 0) {
            std::cout << "difference at singularity: " << total << std::endl;
        } else if (singularities[f] == 0 && total != 0) {
            std::cout << "difference at non-singularity: " << total << std::endl;
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::buildLaplacian() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> L(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v1 : mesh->vertices()) {
        int index1 = vertexIndices[v1];
        double sum = __DBL_EPSILON__;

        // add neighbor weights
        for (HalfedgePtr heOut : v1.outgoingHalfedges()) {
            VertexPtr v2 = heOut.twin().vertex();
            int index2 = vertexIndices[v2];
            double weight = (geom->cotan(heOut) + geom->cotan(heOut.twin())) / 2;
            sum += weight;

            if (branchCover[heOut] == 1) {
                weight *= -1;
            }
            triplets.push_back(Eigen::Triplet<std::complex<double>>(index1, index2, std::complex<double>(-weight,0)));
        }

        // add diagonal weight
        triplets.push_back(Eigen::Triplet<std::complex<double>>(index1, index1, std::complex<double>(sum,0)));
    }
    L.setFromTriplets(triplets.begin(), triplets.end());
    return L;
}

VertexData<double> QuadCover::computeOffset() {
    std::cout << "Computing Offset!" << std::endl;

    Eigen::SparseMatrix<std::complex<double>> L = buildLaplacian();
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(L);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd u = Eigen::MatrixXcd::Random(mesh->nVertices(),1);
    Eigen::MatrixXcd x;

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(M * u);
        std::complex<double> norm2 = (x.transpose() * M * x)(0,0);
        u = x / sqrt(norm2);
    }

    // store into VertexData
    VertexData<double> offset(mesh);
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        size_t index = vertexIndices[v];
        offset[v] = x(index,0).real();
    }

    emitTriangles(offset);
    return offset;
}

void QuadCover::emitTriangles(VertexData<double> offsets) {
    std::cout << "Emitting Triangles!" << std::endl;

    std::ofstream outfile ("branchcover.obj");
    // write vertices
    for (VertexPtr v : mesh->vertices()) {
        Vector3 pos = geom->position(v) + offsets[v] * geom->normal(v);
        Vector3 neg = geom->position(v) - offsets[v] * geom->normal(v);

        outfile << "v " << pos.x << " " << pos.y << " " << pos.z << std::endl;
        outfile << "v " << neg.x << " " << neg.y << " " << neg.z << std::endl;
    }

    // write face indices
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he = f.halfedge();

       for (int currSheet = 0; currSheet < 2; currSheet++) {
           outfile << "f ";
           do {
               size_t index = vertexIndices[he.vertex()];
               if (currSheet == 0) {
                   outfile << (2*index)+1 << " ";
               } else {
                   outfile << (2*index+1)+1 << " ";
               }
               currSheet = (currSheet + branchCover[he]) % 2;
               he = he.next();
           } while (he != f.halfedge());
           outfile << std::endl;
       }
    }

    outfile.close();
    std::cout << "Done!" << std::endl;
}


#include <iostream>
#include <fstream>
#include <cmath>
#include <complex>
#include <iomanip>
#include <Eigen/Dense>
#include "GaussLegendreEigen.cpp"

/*

single channel non-rel T-matrix solver

g++ -I/usr/local/include/eigen3 ...

*/

using namespace std;
using namespace Eigen;

typedef std::complex<double> Complex;
static const Complex I(0.0,1.0);
double V0,r0,mu;

double deltaExact(double k) {
  // S-wave square well phase shift
  // should be a pos del for attractive potl
  double kp = sqrt(k*k+abs(V0)*2.0*mu);
  return (-k*r0+atan(k*tan(kp*r0)/kp))*180.0/M_PI;
}

Complex SqWell(double k, double p, int ell) {
  // V = V0*theta(r-r0) S-wave only
  double SqReal;
  if (k != p) {
    SqReal = -2.0*M_PI*V0/(k*p)*(sin((k+p)*r0)/(k+p) -
                             sin((k-p)*r0)/(k-p));
  } else {
    SqReal = -2.0*M_PI*V0/(k*k)*(sin(2.0*k*r0)/(2.0*k)-r0);
  }
  return Complex(SqReal,0.0);
}

int main () {
  ofstream ofs;
  ofs.open("T.dat");
  double m1,m2;
  int Ngrid,ell;
  Complex D;

  cout << " enter m1, m2, ell " << endl;
  cin >> m1 >> m2 >> ell;
  cout << " enter V0 (<0 for attractive), r0 " << endl;
  cin >> V0 >> r0;
  cout << " enter Ngrid " << endl;
  cin >> Ngrid;

  mu = m1*m2/(m1+m2);
  VectorXd k(Ngrid+1),w(Ngrid);
  VectorXcd V(Ngrid+1),T(Ngrid+1);
  MatrixXcd M(Ngrid+1,Ngrid+1);

  GaussLegendreEigen(k,w,Ngrid,0.0,M_PI_2);   // create GL grid & weights
  for (int i=0;i<Ngrid;i++) {                                 // map grid
    w(i) = w(i)/pow(cos(k(i)),2);
    k(i) = tan(k(i));
  }
  //  test this
  double s=0.0;
  for (int i=0;i<Ngrid;i++) {
   s += w(i)*k(i)*k(i)*exp(-k(i)*k(i));
  }
  s *= 4/sqrt(M_PI);
  cout << " test integral " << setprecision(20) << s << endl;

  for (int ik=1;ik<50;ik++) {            // loop in scattering momentum
    double k0 = ik*0.04;
    k(Ngrid) = k0;                       // add  k0 to the grid
    for (int a=0;a<=Ngrid;a++) {
       V(a) = SqWell(k(a),k0,ell);
    }
    for (int a=0;a<=Ngrid;a++) {
    for (int b=0;b<=Ngrid;b++) {
      if (b == Ngrid) {
        double Dr = 0.0;
        for (int i=0;i<Ngrid;i++)
          Dr -= w(i)*k0*k0/(k0*k0-k(i)*k(i));
          D = Complex(Dr,-M_PI*k0/2.0);
      } else {
          D = Complex(w(b)*k(b)*k(b)/(k0*k0-k(b)*k(b)),0.0);
      }
      M(a,b) = -mu/(M_PI*M_PI)*SqWell(k(a),k(b),ell)*D;
      if (a == b) M(a,b) = Complex(1.0,0.0) + M(a,b);
    }} // end of a/b loops
    T = M.inverse()*V;                        // invert to obtain  T(k,k0)
    Complex Tonshell = T(Ngrid);
    double delta1 = atan(Tonshell.imag()/Tonshell.real())*180.0/M_PI;
    double delta2 = asin(-mu*k0*Tonshell.real()/M_PI)*90.0/M_PI;
    ofs << k0 << " " << delta1 << " " << delta2 << " " << deltaExact(k0) << endl;
  } // end of k0 loop
  cout << " k, delta1 , delta2, delta_exact in T.dat " << endl;
  ofs.close();
  return 0;
}


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef DKA_HPP
#define DKA_HPP

#include <limits>
#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <kv/constants.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T>
static T inline eval_polynomial (const ub::vector<T>& p, const T& x)
{
	int i;
	T r;
	int s = p.size();

	r = p(s-1);
	for (i=s-2; i>=0; i--) {
		r = r * x + p(i);
	}

	return r;
}

// Durand Kerner Aberth algorithm

template <class T>
bool dka(const ub::vector< kv::complex<T> >& p, ub::vector< kv::complex<T> >& x, T epsilon = std::numeric_limits<T>::epsilon())
{
	int i, j;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex<T> > a(s);
	kv::complex<T> c;
	ub::vector<T> b;
	T r, tmp, norm1, norm2, norm3;
	ub::vector<T> db;
	ub::vector< kv::complex<T> > dx(n);
	T pi = kv::constants<T>::pi();
	kv::complex<T> f, df;

	using std::abs;
	using std::pow;

	if (p(n).real() * p(n).real() + p(n).imag() * p(n).imag() == 0.) {
		return false;
	}

	// make equation b(r) = 0

	for (i=0; i<s; i++) a(i) = p(i);

	c = -a(n-1) / (a(n) * n);
	for (i=1; i<=n; i++) {
		for (j = n; j >= i; j--) {
			a(j-1) += a(j) * c;
		}
	}

	b.resize(s);
	for (i=0; i<n-1; i++) b(i) = -abs(a(i));
	b(n-1) = 0.;
	b(n) = abs(a(n));
	// prepare for the case where b(0)...b(n-2) are 0.
	b(0) = std::min(b(0), -std::numeric_limits<T>::epsilon());

	// initial guess for b(r) = 0
	r = 0.;
	for (i=0; i<n-1; i++) {
		tmp = pow(n * abs(b[i]/b[n]), 1.0/(n-i));
		if (tmp > r) r = tmp;
	}

	// db(r) = b'(r)
	db.resize(n);
	for (i=1; i<s; i++) db(i-1) = b(i) * i;

	// calculate radius by Newton's method for equation b(r) = 0
	while (true) {
		tmp = eval_polynomial(b, r) / eval_polynomial(db, r);
		r -= tmp;
		if (abs(tmp) < n * abs(r) * epsilon) break;
	}

	// set Aberth's initial values

	x.resize(n);
	for (i=0; i<n; i++) {
		x(i) = c + r * exp((2. * i * pi / n + pi / (2. * n)) * kv::complex<T>::i());
	}

	// max(|pi|)
	norm3 = 0.;
	for (i=0; i<s; i++) {
		norm3 = std::max(norm3, abs(p(i)));
	}

	// Durand Kerner algorithm

	while (true) {
		for (i=0; i<n; i++) {
			f = eval_polynomial(p, x(i));
			df = p(n);
			for (j=0; j<n; j++) {
				if (j == i) continue;
				df *= x(i) - x(j);
			}
			if (df.real() * df.real() + df.imag() * df.imag() == 0.) {
				dx(i) = 0.;
			} else {
				dx(i) = f / df;
			}
		}

		x -= dx;

		norm1 = 1.;
		norm2 = 0.;
		for (i=0; i<n; i++) {
			norm1 = std::max(norm1, abs(x(i)));
			norm2 = std::max(norm2, abs(dx(i)));
		}
		if (norm2 <= n * norm1 * epsilon) break;
		if (abs(f) <= n * norm3 * epsilon) break;
	}

	return true;
}

// error estimation using Smith's theorem

template <class T>
ub::vector< kv::complex< interval<T> > > smith_error(const ub::vector< kv::complex< interval<T> > >& p, const ub::vector< kv::complex<T> >& x)
{
	int i, j;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex< interval<T> > > x2, x3(n);
	kv::complex< interval<T> > f, df;
	T err;
	ub::vector<int> unified(n);
	bool flag;

	x2 = x;

	for (i=0; i<n; i++) {
		f = eval_polynomial(p, x2(i));
		df = p(n);
		for (j=0; j<n; j++) {
			if (j == i) continue;
			df *= x2(i) - x2(j);
		}
		if (zero_in(df.real() * df.real() + df.imag() * df.imag())) {
			err = std::numeric_limits<T>::infinity();
		} else {
			err = abs(n * f / df).upper();
		}
		x3(i).real() = x2(i).real() + err * interval<T>(-1., 1.);
		x3(i).imag() = x2(i).imag() + err * interval<T>(-1., 1.);
	}

	/* unify overlapping solutions */

	for (i=0; i<n; i++) unified(i) = -1; /* not unified */

	while (true) {
		flag = true;
		for (i=0; i<n-1; i++) {
			if (unified(i) != -1) continue;
			for (j=i; j<n; j++) {
				if (unified(j) != -1) continue;
				if (overlap(x3(i).real(), x3(j).real()) && overlap(x3(i).imag(), x3(j).imag())) {
					x3(i).real() = interval<T>::hull(x3(i).real(), x3(j).real());
					x3(i).imag() = interval<T>::hull(x3(i).imag(), x3(j).imag());
					unified(j) = i; /* x3(j) is unified to x3(i) */
					flag = false;
				}
			}
		}
		if (flag) break;
	}

	for (i=0; i<n; i++) {
		if (unified(i) != -1) x3(i) = x3(unified(i));
	}

	return x3;
}

// verified Durand Kerner Aberth

template <class T>
bool vdka(const ub::vector< kv::complex< interval<T> > >& p, ub::vector< kv::complex< interval<T> > >& result, T epsilon = std::numeric_limits<T>::epsilon())
{
	int i;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex< T > > p2(s), x2;

	if (zero_in(p(n).real() * p(n).real() + p(n).imag() * p(n).imag())) {
		return false;
	}

	for (i=0; i<s; i++) {
		p2(i).real() = mid(p(i).real());
		p2(i).imag() = mid(p(i).imag());
	}

	dka(p2, x2, epsilon);

	result = smith_error(p, x2);
	return true;
}

} // namespace kv

#endif // DKA_HPP


//Trevor Hickey
//Homework 1: GCD
//Sept. 11

//STD
#include <iostream>
#include <string>
#include <cmath>
#include <functional>

//Boost
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

//My Sources
#include "globals.hpp"         // types & constants
#include "gcd_algorithms.hpp"  // namespace
#include "algorithm.hpp"       // class
#include "test_suite.hpp"      // namespace

//Function Decelerations
void Validate_Algorithm_Correctness(void);
void Test_Algorithm_Speed(void);
void Output_Title_Of_Table(std::vector<Algorithm<>> const& algorithms);
void Perform_Bit_Test(boost::random::mt19937 & gen, int i, std::vector<Algorithm<>> algorithms);
void Output_Results(int i, std::vector<Algorithm<>> const& algorithms);
template <typename IntegerType> void Run_GCD_Performance_Test(Algorithm<IntegerType> & alg, IntegerType a, IntegerType b);

int main(){

	//test that the algorithms work
	Validate_Algorithm_Correctness();

	//output speed performance data for each algorithm
	Test_Algorithm_Speed();

	return EXIT_SUCCESS;
}

//Function Definitions
void Validate_Algorithm_Correctness(void){

	//we generate some random numbers of different bit lengths and check edge cases to
	//make sure that the algorithms all produce the desired results.
	//If there is an issue, the function will report a message through std::cerr
	Test_Suite::Check_Algorithm_Validity ( GCD::Euclid_Recursive    );
	Test_Suite::Check_Algorithm_Validity ( GCD::Euclid_Iterative    );
	Test_Suite::Check_Algorithm_Validity ( GCD::Euclid_Unrolled     );
	Test_Suite::Check_Algorithm_Validity ( GCD::Euclid_Bit          );
	Test_Suite::Check_Algorithm_Validity ( GCD::Binary_Recursive    );
	Test_Suite::Check_Algorithm_Validity ( GCD::Binary_Iterative    );
	Test_Suite::Check_Algorithm_Validity ( GCD::Binary_Iterative_V2 );
	Test_Suite::Check_Algorithm_Validity ( GCD::CTZ                 );
	Test_Suite::Check_Algorithm_Validity ( GCD::Boost               );

	//I'm leaving these algorithms out, because they are slow
	//Test_Suite::Check_Algorithm_Validity (GCD::Brute_Force);
	//Test_Suite::Check_Algorithm_Validity (GCD::Factor);

	return;
}
void Test_Algorithm_Speed(void){

	//a vector containing all of the different algorithms we will be testing
	std::vector<Algorithm<>> algorithms;
	algorithms.push_back(Algorithm<>( "Euclid_Recursive"    ,GCD::Euclid_Recursive    ));
	algorithms.push_back(Algorithm<>( "Euclid_Iterative"    ,GCD::Euclid_Iterative    ));
	algorithms.push_back(Algorithm<>( "Euclid_Unrolled"     ,GCD::Euclid_Unrolled     ));
	algorithms.push_back(Algorithm<>( "Euclid_Bit"          ,GCD::Euclid_Bit          ));
	algorithms.push_back(Algorithm<>( "Binary_Recursive"    ,GCD::Binary_Recursive    ));
	algorithms.push_back(Algorithm<>( "Binary_Iterative"    ,GCD::Binary_Iterative    ));
	algorithms.push_back(Algorithm<>( "Binary_Iterative_V2" ,GCD::Binary_Iterative_V2 ));
	algorithms.push_back(Algorithm<>( "CTZ"                 ,GCD::CTZ                 ));
	algorithms.push_back(Algorithm<>( "Boost"               ,GCD::Boost               ));

	//build a generator and test the algorithm across a range of bit lengths
	boost::random::mt19937 gen(std::time(0));
	Output_Title_Of_Table(algorithms);
	for (auto i = constant::min_bit_length; i < constant::max_bit_length; ++i){
		Perform_Bit_Test(gen,i,algorithms);
	}

	return;
}
void Output_Title_Of_Table(std::vector<Algorithm<>> const& algorithms){

	std::cout << "testing the speed of GCD algorithms in milliseconds (" << constant::num_of_trials << " trials for each bit length)\n";
	//print out a terrible title just so we know what algorithm is in each column
	for (auto const& it: algorithms){std::cout << "|" << it.name << "|";}
	std::cout << '\n';

	return;
}
void Perform_Bit_Test(boost::random::mt19937 & gen, int i, std::vector<Algorithm<>> algorithms){

	//loop through each algorithm and test its speed against random numbers of a particular bit length
	TestType a,b;
	auto dist = Test_Suite::Get_Specified_Bit_Length_Distribution(i);
	for (int i = 0; i < constant::num_of_trials; ++i){
		a = dist(gen);
		b = dist(gen);
		for (auto & it: algorithms){
			Run_GCD_Performance_Test(it,a,b);
		}
	}

	//once data is collected for each algorithm in regards to
	//specific bit length numbers, output the results.
	//NOTE: this data will be lost after the function returns.
	//This is because the algorithm classes will be re-used on different bit lengths.
	Output_Results(i,algorithms);

	return;
}
void Output_Results(int i, std::vector<Algorithm<>> const& algorithms){

	//output the total time it took to run the number of trials
	//*You know what, I'm just going to calculate the average in whatever graph software I use
	std::cout << "bit-length " << i << ">";
	for (auto it: algorithms){
		std::cout << " " << "(" << (it.total_time) << ")";
	}
	std::cout << '\n';

	return;
}
template <typename IntegerType> void Run_GCD_Performance_Test(Algorithm<IntegerType> & alg, IntegerType a, IntegerType b){

	boost::posix_time::ptime startTime, endTime;

	//time the execution of the algorithm
	startTime = boost::posix_time::microsec_clock::local_time();
	alg.fun(a,b);
	endTime = boost::posix_time::microsec_clock::local_time();

	alg.total_time += (endTime-startTime);
	++alg.trials;

	return;
}


//
// Created by egrzrbr on 2019-04-26.
//

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
/*****************
 * Main function *
 *****************/

#include <unistd.h>
#include <time.h>
#include <iostream>

#define EIGEN_STACK_ALLOCATION_LIMIT 0

#include <Eigen/Dense>

using namespace Eigen;

/**********************************
 * Pseudo-random number generator *
 **********************************/

#define EIGEN_COUT(mat) {std::cout << #mat << " = {\n" << mat << "}" << std::endl;}

static uint64_t mat_rng[2] = {11ULL, 1181783497276652981ULL};

static inline uint64_t xorshift128plus(uint64_t s[2]) {

	uint64_t x, y;
	x = s[0], y = s[1];
	s[0] = y;
	x ^= x << 23;
	s[1] = x ^ y ^ (x >> 17) ^ (y >> 26);
	y += s[1];
	return y;
}

double mat_drand(void) {

	return (xorshift128plus(mat_rng) >> 11) * (1.0 / 9007199254740992.0);
}

template<class Matrix_t>
void mat_gen_random_ublas(Matrix_t &m) {

	ssize_t i, j;
	for (i = 0; i < m.rows(); ++i)
		for (j = 0; j < m.cols(); ++j)
			m(i, j) = mat_drand();
}

template<class Matrix_t>
void mat_arange(Matrix_t &m) {

	const ssize_t N = m.rows();
	const ssize_t M = m.cols();
	ssize_t i, j;
	for (i = 0; i < N; ++i)
		for (j = 0; j < M; ++j)
			m(i, j) = j * N + i;
}

template<typename T, size_t N, size_t TRIALS>
void test_trials(const char opt = 'r') {

	int c;
	clock_t t;

	{

		MatrixXf a(N, N), b(N, N), m(N, N);

		if (opt == 'r') {
			mat_gen_random_ublas(a);
			mat_gen_random_ublas(b);
		} else {
			mat_arange(a);
			mat_arange(b);
		}

		t = clock();
		for (int i = 0; i < TRIALS; i++)
			m = a * b;
		fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);

		if (N > 10) return;
		EIGEN_COUT(a)
		EIGEN_COUT(b)
		EIGEN_COUT(c)
	}

	{

		Matrix<float, N, N> a, b, c;

		if (opt == 'r') {
			mat_gen_random_ublas(a);
			mat_gen_random_ublas(b);
		} else {
			mat_arange(a);
			mat_arange(b);
		}

		t = clock();

		for (int i = 0; i < TRIALS; i++)
			c = a * b;
		fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
		if (N > 10) return;
		EIGEN_COUT(a)
		EIGEN_COUT(b)
		EIGEN_COUT(c)
	}

}

template<typename T, size_t N>
void matNN_mul_matNN(const char opt = 'r') {

	std::cout << "matNN_mul_matNN" << std::endl;

	clock_t t;
	Matrix<T, N, N> a;
	Matrix<T, N, N> b;
	Matrix<T, N, N> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N>
void vecN1_mul_vec1N(const char opt = 'r') {

	clock_t t;
	Matrix<T, N, 1> a;
	Matrix<T, 1, N> b;
	Matrix<T, N, N> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}
	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N, size_t M>
void matNM_mul_vecM1(const char opt = 'r') {

	clock_t t;
	Matrix<T, N, M> a;
	Matrix<T, M, 1> b;
	Matrix<T, N, 1> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N>
void vec1N_mul_vecN1(const char opt = 'r') {

	clock_t t;
	Matrix<T, 1, N> a;
	Matrix<T, N, 1> b;
	Matrix<T, 1, 1> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

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

	constexpr int N = 500;
	constexpr int M = 400;
	constexpr int K = 10;
	clock_t t;

	matNN_mul_matNN<float, N>();
	matNM_mul_vecM1<float, N, M>();
	vecN1_mul_vec1N<float, N>();
	vec1N_mul_vecN1<float, N>();

	matNN_mul_matNN<float, 5>(0);
	matNM_mul_vecM1<float, 5, 4>(0);
	vecN1_mul_vec1N<float, 5>(0);
	vec1N_mul_vecN1<float, 5>(0);

	return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef METRO_LOG_SUM_EXP_HPP
#define METRO_LOG_SUM_EXP_HPP

#include <vector>
#include <Eigen/Core>

namespace metro {
	// Compute the log of a sum of exponentials via an algorithm that
	template< typename Matrix, typename Nonmissingness >
	double log_sum_exp( Matrix const& data, Nonmissingness const& nonmissingness ) {
		assert( data.rows() == nonmissingness.rows() ) ;
		assert( data.cols() == nonmissingness.cols() ) ;
		if( data.size() == 0 || nonmissingness.sum() == 0 ) {
			return 0.0 ;
		}
		double max_value = ( data.array() * nonmissingness.array() ).maxCoeff() ;
		if( max_value == -std::numeric_limits< double >::infinity() ) {
			return max_value ;
		}
		Eigen::MatrixXd const exponential = ( data - Eigen::MatrixXd::Constant( data.rows(), data.cols(), max_value ) ).array().exp() ;
		return max_value + std::log( ( exponential.array() * nonmissingness.array() ).sum() ) ;
	}

	// Ditto but assuming all values are present.
	template< typename Matrix >
	double log_sum_exp( Matrix const& data ) {
		if( data.size() == 0 ) {
			return 0.0 ;
		}
		double max_value = data.array().maxCoeff() ;
		if( max_value == -std::numeric_limits< double >::infinity() ) {
			return max_value ;
		}
		Eigen::MatrixXd const exponential = ( data - Eigen::MatrixXd::Constant( data.rows(), data.cols(), max_value ) ).array().exp() ;
		return max_value + std::log( exponential.array().sum() ) ;
	}

	// Ditto but assuming all values are present.
	template<>
	double log_sum_exp( std::vector< double > const& data ) ;

	void rowwise_log_sum_exp( Eigen::MatrixXd const&, Eigen::VectorXd* result ) ;
	void rowwise_log_sum_exp( Eigen::MatrixXd const&, Eigen::MatrixXd const& nonmissingness, Eigen::VectorXd* result ) ;
}

#endif


/***************************************************************************************
    File: perceptron.cpp

    Description:
      Runs an arbitrary-precision rational valued perceptron algorithm.
      This program will infinite loop if the dataset is not linearly separable.
      Reads input from stdin and outputs to stdout.

    Input: a training set
    Example:

    2     // number of training vectors
    2     // number of features per vector
    1     // label 1
    1/2   // vector 1, feature 1
    1.2   // vector 1, feature 2
    0     // label 2
    0     // vector 2, feature 1
    1%3   // vector 2, feature 2

    resulting in the following vectors:
      <1 % 1, 1 % 2, 6 % 5> and <-1 % 1, 0 % 1, 1 % 3>
      // both % & / can be used as fraction.
      // labels are transformed from {0, 1} to {-1, 1} and appended to front of vectors.

    Output:
      This program will either infinite loop or termintate after outputing a weight
      vector representing the hyperplane sperating the data (1 feature per line).

    Example output (for the above example input).
    -1 % 1
    1 % 1
    7 % 5

    which represents the following 2D line.
    y = -7/5x - 1/||<1 % 1, 7 % 5>||
 ***************************************************************************************/
#include <iostream>
#include <string>
#include <vector>
#include <limits>

#include <boost/rational.hpp>
#include <boost/multiprecision/gmp.hpp>

typedef boost::multiprecision::mpz_int Z;
typedef boost::rational<Z> Q;
typedef std::vector<Q> Qvec;

std::istream& operator >> (std::istream& ins, Q& q);
std::ostream& operator << (std::ostream& outs, const Q& q);

// Vector Operations
Q dot (const Qvec& x, const Qvec& y);
Qvec add (const Qvec& x, const Qvec& y);
Qvec mult (const Qvec& x, const Q q);
inline int sign(Q q) { return (0 < q) - (0 > q); } // 1 or -1

Qvec perceptron(const std::vector<Qvec>& features, const std::vector<int>& labels);

int main(int argc, char ** argv){
  std::size_t nvecs, nfeats;
  std::cin >> nvecs >> nfeats;

  std::vector<Qvec> vecs;
  std::vector<int>  lbls;

  for (std::size_t i = 0; i < nvecs; ++i){
    int lbl;
    std::cin >> lbl;
    lbls.push_back(lbl*2-1); //{0,1} => {-1, 1}

    std::cin.ignore(std::numeric_limits<int>::max(),'\n'); //ignore until new line

    Qvec vec(1, Q(1, 1));
    for (std::size_t j = 0; j < nfeats; ++j){
      Q q;
      std::cin >> q;
      vec.push_back(q);
    }
    vecs.push_back(vec);
  }

  Qvec w = perceptron(vecs, lbls);

  if (argc != 1){ // pretty print
    std::cout << "<";
    for (std::size_t i = 0; i < w.size(); ++i){
      std::cout << w[i];
      if (i != w.size() - 1){
        std::cout << ", ";
      }
    }
    std::cout << ">" << std::endl;
  } else {
    for (std::size_t i = 0; i < w.size(); ++i){
      std::cout << w[i] << std::endl;
    }
  }
  return 0;
}

// for now, we assume each number is on it's own line
std::istream& operator >> (std::istream& ins, Q& q){
  Z n, d;
  char c;

  std::string num;

  getline(ins, num);

  std::size_t pos;
  if ((pos = num.find('%')) != std::string::npos ||
      (pos = num.find('/')) != std::string::npos){  // fractional
    n = Z(num.substr(0, pos));
    d = Z(num.substr(pos+1));
  } else if ((pos = num.find('.')) != std::string::npos){ // floating point
    std::string N, D;
    N = num.substr(0, pos);
    D = num.substr(pos+1);
    d = 1;
    for (std::size_t i = 0; i < D.length(); ++i){ // pow doesn't work (it has limited precision)
      d *= 10;
    }
    while (D[0] == '0'){
      D = D.substr(1);
    }
    n = Z(N)*d + Z(D);
  } else { // integral
    n = Z(num);
    d = 1;
  }

  q = Q(n, d);

  return ins;
}

std::ostream& operator << (std::ostream& outs, const Q& q){
  return outs << q.numerator() << " % " << q.denominator();
}

Q dot (const Qvec& x, const Qvec& y){
  Q res(0, 1);
  for (std::size_t i = 0; i < x.size(); ++i){
    res += x[i] * y[i];
  }
  return res;
}

Qvec add (const Qvec& x, const Qvec& y){
  Qvec res(x.size());
  for (std::size_t i = 0; i < x.size(); ++i){
    res[i] = x[i] + y[i];
  }
  return res;
}

Qvec mult (const Qvec& x, const Q q){
  Qvec res(x.size());
  for (std::size_t i = 0; i < x.size(); ++i){
    res[i] = x[i] * q;
  }
  return res;
}

Qvec perceptron(const std::vector<Qvec>& features, const std::vector<int>& labels){
  Qvec weights(features[0].size(), Q(0, 1));

  size_t fuel = 0;
  bool converged;
  do {
    ++fuel;
    converged = true;

    for (std::size_t i = 0; i < features.size(); ++i){
      Q res = dot(weights, features[i]);

      if (sign(res) != labels[i] || res == Q(0,1)) { // wrong sign or res = 0.
        weights = add(weights, mult(features[i], Q(labels[i], 1)));
        converged = false;
      }
    }
  } while (!converged);
  std::cerr << "Fuel: " << fuel << std::endl;
  return weights;
}


#include <boost/test/unit_test.hpp>
#include <complex>
#include "../include/WindowingAnalysis.h"
#include "../../JeanBaptiste/include/tools/RealComplexConversion.h"
#include "../../JeanBaptiste/include/windowing/BartlettWindow.h"
#include "../../JeanBaptiste/include/windowing/BlackmanWindow.h"
#include "../../JeanBaptiste/include/windowing/BlackmanHarrisWindow.h"
#include "../../JeanBaptiste/include/windowing/CosineWindow.h"
#include "../../JeanBaptiste/include/windowing/FlatTopWindow.h"
#include "../../JeanBaptiste/include/windowing/HammingWindow.h"
#include "../../JeanBaptiste/include/windowing/VonHannWindow.h"
#include "../../JeanBaptiste/include/windowing/WelchWindow.h"
#include <string>

namespace ut = boost::unit_test;
namespace jb = jeanbaptiste;
namespace jt = jeanbaptiste::tools;
namespace jw = jeanbaptiste::windowing;

class WindowCalculationFixture
{
protected:
    std::vector<double> workingSet_;
    std::vector<double> expectedOut_;
    static const unsigned kSampleCnt_ = 128;
    Utilities::WindowingAnalysis<double> analysis_;
    jt::Real2Complex<std::complex<double>> real2ComplexConverter_;
    jt::Complex2Real<std::complex<double>> complex2RealConverter_;
    bool initialized_;

public:
    WindowCalculationFixture()
    {
        workingSet_.clear();
        expectedOut_.clear();
    }

    ~WindowCalculationFixture()
    {}
};

BOOST_FIXTURE_TEST_SUITE(WindowCalculationTestSuite, WindowCalculationFixture)

    BOOST_AUTO_TEST_CASE(bartlett)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinBartlettTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Bartlett window samples.");

        jw::BartlettWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> bartlett;

        auto complexData = real2ComplexConverter_(workingSet_);
        bartlett(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(blackman_harris)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinBlackmanHarrisTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Blackman Harris window samples.");

        jw::BlackmanHarrisWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> blackmanHarris;

        auto complexData = real2ComplexConverter_(workingSet_);
        blackmanHarris(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(blackman)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinBlackmanTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Blackman window samples.");

        jw::BlackmanWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> blackman;

        auto complexData = real2ComplexConverter_(workingSet_);
        blackman(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(cosine)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinCosineTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Cosine window samples.");

        jw::CosineWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> cosineWin;

        auto complexData = real2ComplexConverter_(workingSet_);
        cosineWin(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(flat_top)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinFlatTopTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Flat Top window samples.");

        jw::FlatTopWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> flatTopWin;

        auto complexData = real2ComplexConverter_(workingSet_);
        flatTopWin(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(hamming)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinHammingTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Hamming window samples.");

        jw::HammingWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> hammingWin;

        auto complexData = real2ComplexConverter_(workingSet_);
        hammingWin(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(von_hann)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinvonHannTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking von Hann window samples.");

        jw::VonHannWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> vonHannWin;

        auto complexData = real2ComplexConverter_(workingSet_);
        vonHannWin(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

    BOOST_AUTO_TEST_CASE(welch)
    {
        BOOST_TEST((initialized_ = analysis_.initialize("../../test cases/WinWelchTest.xml", "win.in", workingSet_, "win.out", expectedOut_)), "Loading test data failed.");
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Checking Welch window samples.");

        jw::WelchWindow<std::integral_constant<int, kSampleCnt_>, std::complex<double>> welchWin;

        auto complexData = real2ComplexConverter_(workingSet_);
        welchWin(&complexData[0]);
        auto realData = complex2RealConverter_(complexData);

        analysis_.checkOutput(realData, expectedOut_);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    int tc; cin >> tc;
    cpp_int n, ans;
    while(tc--){
        cin >> n;
        ans = 24 + n * (-18 + n * (23 + n * (-6 + n)));
        ans /= 24;
        cout << ans << "\n";
    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/How Many Pieces of Land?.cpp


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.

#ifndef poplibs_support_log_arithmetic_hpp
#define poplibs_support_log_arithmetic_hpp

#ifndef INCLUDE_IN_ASSEMBLER
// Conditionally avoid including functions that use multi_array
#ifndef __POPC__
#include <boost/multi_array.hpp>
#endif

namespace poplibs_support {
namespace log {

inline constexpr auto probabilityOne = 0;
inline constexpr auto probabilityZero = -65504; // Min HALF

// Given log values, perform an equivalent `linear add` operation
template <typename FPType> FPType add(const FPType a_, const FPType b_) {
  FPType a = a_ < b_ ? b_ : a_;
  FPType b = a_ < b_ ? a_ : b_;
  return a + std::log(1 + std::exp(b - a));
}

// Given log values, perform an equivalent `linear sub` operation
template <typename FPType> FPType sub(const FPType a_, const FPType b_) {
  FPType a = a_ < b_ ? b_ : a_;
  FPType b = a_ < b_ ? a_ : b_;
  return a + std::log(1 - std::exp(b - a));
}

// Given log values, perform an equivalent `linear mul` operation
template <typename FPType> FPType mul(const FPType a, const FPType b) {
  return a + b;
}
// Given log values, perform an equivalent `linear divide` operation
template <typename FPType> FPType div(const FPType a, const FPType b) {
  return a - b;
}

#ifndef __POPC__

// TODO Move out of log namespace, or do log(softmax(x)) - this is just
// softmax(x)
// Simply to save having to carefully enter values, use a softmax to
// convert them into probabilities
template <typename FPType>
boost::multi_array<FPType, 2> softMax(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in[0].size(); i++) {
    FPType sum = 0;
    for (unsigned j = 0; j < in.size(); j++) {
      sum += std::exp(in[j][i]);
    }
    for (unsigned j = 0; j < in.size(); j++) {
      out[j][i] = std::exp(in[j][i]) / sum;
    }
  }
  return out;
}

// Converted each individual element to natural log.
// Add a small constant to prevent numeric errors
template <typename FPType>
boost::multi_array<FPType, 2> log(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in.size(); i++) {
    for (unsigned j = 0; j < in[i].size(); j++) {
      out[i][j] = std::log(in[i][j] + 1e-50);
    }
  }
  return out;
}
// Find exp of each individual element
template <typename FPType>
boost::multi_array<FPType, 2> exp(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in.size(); i++) {
    for (unsigned j = 0; j < in[i].size(); j++) {
      out[i][j] = std::exp(in[i][j]);
    }
  }
  return out;
}

#endif // ifndef __POPC__
} // namespace log
} // namespace poplibs_support
#endif // ifndef INCLUDE_IN_ASSEMBLER

#define LOG_PROBABILITY_ZERO_FLOAT 0xC77FE000

#endif // poplibs_support_log_arithmetic_hpp


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

#include "mesh_reader_eigen.hpp"
#include "constants_dense.hpp"
#include "newton_raphson_dense.hpp"

using Eigen::MatrixXd;
typedef Matrix<double, Dynamic, 1> VectorXd;

// Compile command:
// g++ -std=c++14 eigen_dense.cpp -O3 -o dense.o -I/usr/local/include/eigen3
// Run command:
// ./dense.o filename T(in C) eta_u(in %) eta_v(in %)

// u = Cu = concentration O2, v = Cv = concentration CO2

class F
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    MatrixXd& Au_;
    MatrixXd& Av_;
    MatrixXd& B_;
    MatrixXd& C_;
    VectorXd& D_;

  public:
    F(MatrixXd& Au, MatrixXd& Av,MatrixXd& B,MatrixXd& C,VectorXd& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    VectorXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      VectorXd func(2*n);
      func << Au_*u + B_*Ru(u,v) + hu*(C_*u - D_*uamb),Av_*v - B_*Rv(u,v) + hv*(C_*v - D_*vamb);
      return func;
    }
};

class J
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    MatrixXd& Au_;
    MatrixXd& Av_;
    MatrixXd& B_;
    MatrixXd& C_;
    VectorXd& D_;

  public:
    J(MatrixXd& Au, MatrixXd& Av,MatrixXd& B,MatrixXd& C,VectorXd& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    MatrixXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      MatrixXd func(2*n,2*n);
      func << Au_ + B_*dRudu(u,v) + hu*C_,B_*dRudv(u,v),-B_*dRvdu(u,v),Av_ - (B_*dRvdv(u,v)) + hv*C_;
      return func;
    }
};

int main(int argc, char *argv[])
{
  std::cout << std::endl;
  std::cout << "Setting constants ..." << std::endl;
  setConstants(atof(argv[2])+273.15,atof(argv[3])/100,atof(argv[4])/100);

  auto t1 = std::chrono::high_resolution_clock::now();
  std::string file_name = argv[1];
  std::string location = "../triangle/"+ file_name +".1";
  MatrixXd vertices = mesh::read_vertices(location+".node");
  MatrixXd triangles = mesh::read_triangles(vertices,location+".ele");
  MatrixXi boundaries = mesh::read_boundaries(vertices,location+".poly");
  auto t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << "Number of vertices :" << vertices.rows() << std::endl;
  std::cout << "Number of triangles:" << triangles.rows() <<std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  /* Calculate coefficient matrix B related to the respiration kinetics,
  part of right hand side in system of nonlinear equations */

  MatrixXd B = MatrixXd::Zero(vertices.rows(), vertices.rows());
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    B(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(b, a) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(a,b) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(c, a) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(a,c) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    B(b, c) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c,b) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
  }
  B *= (1./60.);

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the first part of the stiffness matrix in the lefthand
  side of the the nonlinear system, A */

  t1 = std::chrono::high_resolution_clock::now();
  MatrixXd A_U = MatrixXd::Zero(vertices.rows(), vertices.rows());
  MatrixXd A_V = MatrixXd::Zero(vertices.rows(), vertices.rows());
  Eigen::Matrix<double,3,2> G;
  Eigen::Matrix<double,3,3> GGT_U;
  Eigen::Matrix<double,3,3> GGT_V;
  Eigen::Matrix<double,2,2> I_U;
  Eigen::Matrix<double,2,2> I_V;
  I_U(0,0) = DU_R;
  I_U(1,0) = 0;
  I_U(0,1) = 0;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  I_V(1,0) = 0;
  I_V(0,1) = 0;
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_U*G.transpose());
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_V*G.transpose());
    #ifdef DEBUG
    std::cout<<"GGT_U" << std::endl << GGT_U << std::endl;
    std::cout<<"GGT_V" << std::endl << GGT_V << std::endl;
    #endif
    A_U(a, a) += GGT_U(0, 0);
    A_U(b, a) += GGT_U(1, 0);
    A_U(a, b) += GGT_U(1, 0);
    A_U(c, a) += GGT_U(2, 0);
    A_U(a, c) += GGT_U(2, 0);
    A_U(b, b) += GGT_U(1, 1);
    A_U(b, c) += GGT_U(1, 2);
    A_U(c, b) += GGT_U(1, 2);
    A_U(c, c) += GGT_U(2, 2);
    A_V(a, a) += GGT_V(0, 0);
    A_V(b, a) += GGT_V(1, 0);
    A_V(a, b) += GGT_V(1, 0);
    A_V(c, a) += GGT_V(2, 0);
    A_V(a, c) += GGT_V(2, 0);
    A_V(b, b) += GGT_V(1, 1);
    A_V(b, c) += GGT_V(1, 2);
    A_V(c, b) += GGT_V(1, 2);
    A_V(c, c) += GGT_V(2, 2);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the second part of the stiffness matrix (C) and the
  second part of the righthand side (D) */

  MatrixXd C = MatrixXd::Zero(vertices.rows(), vertices.rows());
  VectorXd D = VectorXd::Zero(vertices.rows());
  for (unsigned b = 0; b < boundaries.rows(); ++b)
  {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    D(boundaries(b,0)) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    D(boundaries(b,1)) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
  }

  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Solve linearized problem with ambient concentrations as input_field
  to obtain starting concentration values, then solve the nonlinear
  system with Newton-Raphson */

  t1 = std::chrono::high_resolution_clock::now();

  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  VectorXd guess(vertices.rows()*2);
  VectorXd u_0 = (A_U+(Vmu/Kmu)*B+hu*C).colPivHouseholderQr().solve(hu*D*uamb);
  VectorXd v_0 = (A_V+hv*C).colPivHouseholderQr().solve(rq*(Vmu/Kmu)*B*u_0+hv*vamb*D);

  guess << u_0,v_0;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  #ifdef DEBUG
  std::cout << "A_U =" << std::endl;
  std::cout << A_U << std::endl;
  std::cout << "A_V =" << std::endl;
  std::cout << A_V << std::endl;
  std::cout << "B =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "C =" << std::endl;
  std::cout << C << std::endl;
  std::cout << "D =" << std::endl;
  std::cout << D << std::endl;
  std::cout << "u_0" << std::endl;
  std::cout << u_0 << std::endl;
  std::cout << "v_0" << std::endl;
  std::cout << v_0 <<std::endl;
  std::cout << "Initial function value = " << std::endl;
  std::cout << F_funct(guess) <<std::endl;
  std::cout << "Initial Jacobian = " << std::endl;
  std::cout << J_funct(guess);
  #endif

  std::cout << std::endl;
  std::cout<< "Calculating nonlinear system solution ..." << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  t2 = std::chrono::high_resolution_clock::now();

  std::cout<< "Numerical solution nonlinear system calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Write out the result for python matplotlib code */

  t1 = std::chrono::high_resolution_clock::now();
  int n = vertices.rows();
  VectorXd u = guess.head(n);
  VectorXd v = guess.tail(n);
  mesh::write_result(u,v);
  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout<< "Results for u and v written out" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << std::endl;

  return 0;
}


/*
 * Copyright Nick Thompson, 2017
 * 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)
 */
#define BOOST_TEST_MODULE naive_monte_carlo_test
#define BOOST_NAIVE_MONTE_CARLO_DEBUG_FAILURES
#include <cmath>
#include <ostream>
#include <boost/lexical_cast.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/naive_monte_carlo.hpp>

using std::abs;
using std::vector;
using std::pair;
using boost::math::constants::pi;
using boost::math::quadrature::naive_monte_carlo;

template<class Real>
void test_pi_multithreaded()
{
    std::cout << "Testing pi is calculated correctly (multithreaded) using Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real {
        Real r = x[0]*x[0]+x[1]*x[1];
        if (r <= 1) {
          return 4;
        }
        return 0;
    };

    std::vector<std::pair<Real, Real>> bounds{{Real(0), Real(1)}, {Real(0), Real(1)}};
    Real error_goal = 0.0002;
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, error_goal,
                                          /*singular =*/ false,/* threads = */ 2, /* seed = */ 18012);
    auto task = mc.integrate();
    Real pi_estimated = task.get();
    if (abs(pi_estimated - pi<Real>())/pi<Real>() > 0.005) {
        std::cout << "Error in estimation of pi too high, function calls: " << mc.calls() << "\n";
        std::cout << "Final error estimate : " << mc.current_error_estimate() << "\n";
        std::cout << "Error goal           : " << error_goal << "\n";
        BOOST_CHECK_CLOSE_FRACTION(pi_estimated, pi<Real>(), 0.005);
    }
}

template<class Real>
void test_pi()
{
    std::cout << "Testing pi is calculated correctly using Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        Real r = x[0]*x[0]+x[1]*x[1];
        if (r <= 1)
        {
            return 4;
        }
        return 0;
    };

    std::vector<std::pair<Real, Real>> bounds{{Real(0), Real(1)}, {Real(0), Real(1)}};
    Real error_goal = (Real) 0.0002;
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, error_goal,
                                            /*singular =*/ false,/* threads = */ 1, /* seed = */ 128402);
    auto task = mc.integrate();
    Real pi_estimated = task.get();
    if (abs(pi_estimated - pi<Real>())/pi<Real>() > 0.005)
    {
        std::cout << "Error in estimation of pi too high, function calls: " << mc.calls() << "\n";
        std::cout << "Final error estimate : " << mc.current_error_estimate() << "\n";
        std::cout << "Error goal           : " << error_goal << "\n";
        BOOST_CHECK_CLOSE_FRACTION(pi_estimated, pi<Real>(), 0.005);
    }

}

template<class Real>
void test_constant()
{
    std::cout << "Testing constants are integrated correctly using Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const &)->Real
    {
      return 1;
    };

    std::vector<std::pair<Real, Real>> bounds{{Real(0), Real(1)}, { Real(0), Real(1)}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.0001,
                                            /* singular = */ false, /* threads = */ 1, /* seed = */ 87);

    auto task = mc.integrate();
    Real one = task.get();
    BOOST_CHECK_CLOSE_FRACTION(one, 1, 0.001);
    BOOST_CHECK_SMALL(mc.current_error_estimate(), std::numeric_limits<Real>::epsilon());
    BOOST_CHECK(mc.calls() > 1000);
}

template<class Real>
void test_exception_from_integrand()
{
    std::cout << "Testing that a reasonable action is performed by the Monte-Carlo integrator when the integrand throws an exception on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        if (x[0] > 0.5 && x[0] < 0.5001)
        {
            throw std::domain_error("You have done something wrong.\n");
        }
        return (Real) 1;
    };

    std::vector<std::pair<Real, Real>> bounds{{ Real(0), Real(1)}, { Real(0), Real(1)}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.0001);

    auto task = mc.integrate();
    bool caught_exception = false;
    try
    {
      Real result = task.get();
      // Get rid of unused variable warning:
      std::ostream cnull(0);
      cnull << result;
    }
    catch(std::exception const &)
    {
        caught_exception = true;
    }
    BOOST_CHECK(caught_exception);
}

template<class Real>
void test_cancel_and_restart()
{
    std::cout << "Testing that cancellation and restarting works on naive Monte-Carlo integration on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real exact = boost::lexical_cast<Real>("1.3932039296856768591842462603255");
    BOOST_CONSTEXPR const Real A = 1.0 / (pi<Real>() * pi<Real>() * pi<Real>());
    auto g = [&](std::vector<Real> const & x)->Real
    {
        return A / (1.0 - cos(x[0])*cos(x[1])*cos(x[2]));
    };
    vector<pair<Real, Real>> bounds{{ Real(0), pi<Real>()}, { Real(0), pi<Real>()}, { Real(0), pi<Real>()}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.05, true, 1, 888889);

    auto task = mc.integrate();
    mc.cancel();
    double y = task.get();
    // Super low tolerance; because it got canceled so fast:
    BOOST_CHECK_CLOSE_FRACTION(y, exact, 1.0);

    mc.update_target_error((Real) 0.01);
    task = mc.integrate();
    y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, exact, 0.1);
}

template<class Real>
void test_finite_singular_boundary()
{
    std::cout << "Testing that finite singular boundaries work on naive Monte-Carlo integration on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    using std::pow;
    using std::log;
    auto g = [](std::vector<Real> const & x)->Real
    {
        // The first term is singular at x = 0.
        // The second at x = 1:
        return pow(log(1.0/x[0]), 2) + log1p(-x[0]);
    };
    vector<pair<Real, Real>> bounds{{Real(0), Real(1)}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.01, true, 1, 1922);

    auto task = mc.integrate();

    double y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1.0, 0.1);
}

template<class Real>
void test_multithreaded_variance()
{
    std::cout << "Testing that variance computed by naive Monte-Carlo integration converges to integral formula on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real exact_variance = (Real) 1/(Real) 12;
    auto g = [&](std::vector<Real> const & x)->Real
    {
        return x[0];
    };
    vector<pair<Real, Real>> bounds{{ Real(0), Real(1)}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, false, 2, 12341);

    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 0.5, 0.01);
    BOOST_CHECK_CLOSE_FRACTION(mc.variance(), exact_variance, 0.05);
}

template<class Real>
void test_variance()
{
    std::cout << "Testing that variance computed by naive Monte-Carlo integration converges to integral formula on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real exact_variance = (Real) 1/(Real) 12;
    auto g = [&](std::vector<Real> const & x)->Real
    {
        return x[0];
    };
    vector<pair<Real, Real>> bounds{{ Real(0), Real(1)}};
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, false, 1, 12341);

    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 0.5, 0.01);
    BOOST_CHECK_CLOSE_FRACTION(mc.variance(), exact_variance, 0.05);
}

template<class Real, uint64_t dimension>
void test_product()
{
    std::cout << "Testing that product functions are integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [&](std::vector<Real> const & x)->Real
    {
        double y = 1;
        for (uint64_t i = 0; i < x.size(); ++i)
        {
            y *= 2*x[i];
        }
        return y;
    };

    vector<pair<Real, Real>> bounds(dimension);
    for (uint64_t i = 0; i < dimension; ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(0, 1);
    }
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, false, 1, 13999);

    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);
    using std::pow;
    Real exact_variance = pow(4.0/3.0, dimension) - 1;
    BOOST_CHECK_CLOSE_FRACTION(mc.variance(), exact_variance, 0.1);
}

template<class Real, uint64_t dimension>
void test_alternative_rng_1()
{
    std::cout << "Testing that alternative RNGs work correctly using naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [&](std::vector<Real> const & x)->Real
    {
        double y = 1;
        for (uint64_t i = 0; i < x.size(); ++i)
        {
            y *= 2*x[i];
        }
        return y;
    };

    vector<pair<Real, Real>> bounds(dimension);
    for (uint64_t i = 0; i < dimension; ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(0, 1);
    }
    std::cout << "Testing std::mt19937" << std::endl;

    naive_monte_carlo<Real, decltype(g), std::mt19937> mc1(g, bounds, (Real) 0.001, false, 1, 1882);

    auto task = mc1.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);
    using std::pow;
    Real exact_variance = pow(4.0/3.0, dimension) - 1;
    BOOST_CHECK_CLOSE_FRACTION(mc1.variance(), exact_variance, 0.05);

    std::cout << "Testing std::knuth_b" << std::endl;
    naive_monte_carlo<Real, decltype(g), std::knuth_b> mc2(g, bounds, (Real) 0.001, false, 1, 1883);
    task = mc2.integrate();
    y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);

    std::cout << "Testing std::ranlux48" << std::endl;
    naive_monte_carlo<Real, decltype(g), std::ranlux48> mc3(g, bounds, (Real) 0.001, false, 1, 1884);
    task = mc3.integrate();
    y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);
}

template<class Real, uint64_t dimension>
void test_alternative_rng_2()
{
    std::cout << "Testing that alternative RNGs work correctly using naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [&](std::vector<Real> const & x)->Real
    {
        double y = 1;
        for (uint64_t i = 0; i < x.size(); ++i)
        {
            y *= 2*x[i];
        }
        return y;
    };

    vector<pair<Real, Real>> bounds(dimension);
    for (uint64_t i = 0; i < dimension; ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(0, 1);
    }

    std::cout << "Testing std::default_random_engine" << std::endl;
    naive_monte_carlo<Real, decltype(g), std::default_random_engine> mc4(g, bounds, (Real) 0.001, false, 1, 1884);
    auto task = mc4.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);

    std::cout << "Testing std::minstd_rand" << std::endl;
    naive_monte_carlo<Real, decltype(g), std::minstd_rand> mc5(g, bounds, (Real) 0.001, false, 1, 1887);
    task = mc5.integrate();
    y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);

    std::cout << "Testing std::minstd_rand0" << std::endl;
    naive_monte_carlo<Real, decltype(g), std::minstd_rand0> mc6(g, bounds, (Real) 0.001, false, 1, 1889);
    task = mc6.integrate();
    y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);

}

template<class Real>
void test_upper_bound_infinite()
{
    std::cout << "Testing that infinite upper bounds are integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        return 1.0/(x[0]*x[0] + 1.0);
    };

    vector<pair<Real, Real>> bounds(1);
    for (uint64_t i = 0; i < bounds.size(); ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(0, std::numeric_limits<Real>::infinity());
    }
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, true, 1, 8765);
    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, boost::math::constants::half_pi<Real>(), 0.01);
}

template<class Real>
void test_lower_bound_infinite()
{
    std::cout << "Testing that infinite lower bounds are integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        return 1.0/(x[0]*x[0] + 1.0);
    };

    vector<pair<Real, Real>> bounds(1);
    for (uint64_t i = 0; i < bounds.size(); ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(-std::numeric_limits<Real>::infinity(), 0);
    }
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, true, 1, 1208);

    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, boost::math::constants::half_pi<Real>(), 0.01);
}

template<class Real>
void test_lower_bound_infinite2()
{
    std::cout << "Testing that infinite lower bounds (2) are integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        // If x[0] = inf, this should blow up:
        return (x[0]*x[0])/(x[0]*x[0]*x[0]*x[0] + 1.0);
    };

    vector<pair<Real, Real>> bounds(1);
    for (uint64_t i = 0; i < bounds.size(); ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(-std::numeric_limits<Real>::infinity(), 0);
    }
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, true, 1, 1208);
    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, boost::math::constants::half_pi<Real>()/boost::math::constants::root_two<Real>(), 0.01);
}

template<class Real>
void test_double_infinite()
{
    std::cout << "Testing that double infinite bounds are integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        return 1.0/(x[0]*x[0] + 1.0);
    };

    vector<pair<Real, Real>> bounds(1);
    for (uint64_t i = 0; i < bounds.size(); ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(-std::numeric_limits<Real>::infinity(), std::numeric_limits<Real>::infinity());
    }
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, (Real) 0.001, true, 1, 1776);

    auto task = mc.integrate();
    Real y = task.get();
    BOOST_CHECK_CLOSE_FRACTION(y, boost::math::constants::pi<Real>(), 0.01);
}

template<class Real, uint64_t dimension>
void test_radovic()
{
    // See: Generalized Halton Sequences in 2008: A Comparative Study, function g1:
    std::cout << "Testing that the Radovic function is integrated correctly by naive Monte-Carlo on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto g = [](std::vector<Real> const & x)->Real
    {
        using std::abs;
        Real alpha = (Real)0.01;
        Real z = 1;
        for (uint64_t i = 0; i < dimension; ++i)
        {
            z *= (abs(4*x[i]-2) + alpha)/(1+alpha);
        }
        return z;
    };

    vector<pair<Real, Real>> bounds(dimension);
    for (uint64_t i = 0; i < bounds.size(); ++i)
    {
        bounds[i] = std::make_pair<Real, Real>(0, 1);
    }
    Real error_goal = (Real) 0.001;
    naive_monte_carlo<Real, decltype(g)> mc(g, bounds, error_goal, false, 1, 1982);

    auto task = mc.integrate();
    Real y = task.get();
    if (abs(y - 1) > 0.01)
    {
        std::cout << "Error in estimation of Radovic integral too high, function calls: " << mc.calls() << "\n";
        std::cout << "Final error estimate: " << mc.current_error_estimate() << std::endl;
        std::cout << "Error goal          : " << error_goal << std::endl;
        std::cout << "Variance estimate   : " << mc.variance() << std::endl;
        BOOST_CHECK_CLOSE_FRACTION(y, 1, 0.01);
    }
}

BOOST_AUTO_TEST_CASE(naive_monte_carlo_test)
{
   std::cout << "Default hardware concurrency = " << std::thread::hardware_concurrency() << std::endl;
#if !defined(TEST) || TEST == 1
    test_finite_singular_boundary<double>();
    test_finite_singular_boundary<float>();
#endif
#if !defined(TEST) || TEST == 2
    test_pi<float>();
    test_pi<double>();
#endif
#if !defined(TEST) || TEST == 3
    test_pi_multithreaded<float>();
    test_constant<float>();
#endif
    //test_pi<long double>();
#if !defined(TEST) || TEST == 4
    test_constant<double>();
    //test_constant<long double>();
    test_cancel_and_restart<float>();
#endif
#if !defined(TEST) || TEST == 5
    test_exception_from_integrand<float>();
    test_variance<float>();
#endif
#if !defined(TEST) || TEST == 6
    test_variance<double>();
    test_multithreaded_variance<double>();
#endif
#if !defined(TEST) || TEST == 7
    test_product<float, 1>();
    test_product<float, 2>();
#endif
#if !defined(TEST) || TEST == 8
    test_product<float, 3>();
    test_product<float, 4>();
    test_product<float, 5>();
#endif
#if !defined(TEST) || TEST == 9
    test_product<float, 6>();
    test_product<double, 1>();
#endif
#if !defined(TEST) || TEST == 10
    test_product<double, 2>();
#endif
#if !defined(TEST) || TEST == 11
    test_product<double, 3>();
    test_product<double, 4>();
#endif
#if !defined(TEST) || TEST == 12
    test_upper_bound_infinite<float>();
    test_upper_bound_infinite<double>();
#endif
#if !defined(TEST) || TEST == 13
    test_lower_bound_infinite<float>();
    test_lower_bound_infinite<double>();
#endif
#if !defined(TEST) || TEST == 14
    test_lower_bound_infinite2<float>();
#endif
#if !defined(TEST) || TEST == 15
    test_double_infinite<float>();
    test_double_infinite<double>();
#endif
#if !defined(TEST) || TEST == 16
    test_radovic<float, 1>();
    test_radovic<float, 2>();
#endif
#if !defined(TEST) || TEST == 17
    test_radovic<float, 3>();
    test_radovic<double, 1>();
#endif
#if !defined(TEST) || TEST == 18
    test_radovic<double, 2>();
    test_radovic<double, 3>();
#endif
#if !defined(TEST) || TEST == 19
    test_radovic<double, 4>();
    test_radovic<double, 5>();
#endif
#if !defined(TEST) || TEST == 20
    test_alternative_rng_1<float, 3>();
#endif
#if !defined(TEST) || TEST == 21
    test_alternative_rng_1<double, 3>();
#endif
#if !defined(TEST) || TEST == 22
    test_alternative_rng_2<float, 3>();
#endif
#if !defined(TEST) || TEST == 23
    test_alternative_rng_2<double, 3>();
#endif

}


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

#include <array>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

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

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>

#include "advectionfv2d.h"

int main() {
#if SOLUTION
  // Define velocity field beta
  // Note that the problem description requires ||B|| <= 1
  auto beta = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(-x[1], x[0]) / std::sqrt(2.0);
  };

  // Functor for initial bump
  Eigen::Vector2d x0(0.8, 0.2);
  double d = 0.2;
  auto u0 = [x0, d](Eigen::Vector2d x) -> double {
    double dist = (x - x0).norm();
    if (dist < d) {
      return std::pow(std::cos(M_PI / (2.0 * d) * dist), 2);
    } else {
      return 0.0;
    }
  };

  // Task 8-8.o
  // Generate a mesh hierarchy
  double T = 1.0;

  // TODO inconsistancy: g vs. G
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1. / 3.);

  auto mesh_seq_p{
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p, 5)};

  std::vector<int> vector_num_cells;
  std::vector<double> vector_l2error;

  // Iterate over mesh levels starting from thrid refinement
  int num_meshes = mesh_seq_p->NumLevels();
  for (int level = 3; level < num_meshes; level++) {
    std::cout << "Computing L2Error for level: " << level << std::endl;

    // Get the current mesh
    auto cur_mesh = mesh_seq_p->getMesh(level);

    // Create a DOF Hander for the current mesh
    const lf::assemble::UniformFEDofHandler cur_dofh(
        cur_mesh, {{lf::base::RefEl::kPoint(), 0},
                   {lf::base::RefEl::kSegment(), 0},
                   {lf::base::RefEl::kTria(), 1},
                   {lf::base::RefEl::kQuad(), 1}});

    // Compute cell normals
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        Eigen::Matrix<double, 2, Eigen::Dynamic>>>
        normal_vectors = AdvectionFV2D::computeCellNormals(cur_dofh.Mesh());

    // Compute adjecent cells
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        std::array<const lf::mesh::Entity *, 4>>>
        adjacentCells = AdvectionFV2D::getAdjacentCellPointers(cur_dofh.Mesh());

    // Get result from simulation
    Eigen::VectorXd result = AdvectionFV2D::simulateAdvection(
        cur_dofh, beta, u0, adjacentCells, normal_vectors, T);

    // Get exact result at barycenters of cells
    Eigen::VectorXd ref_solution = AdvectionFV2D::refSolution(cur_dofh, u0, T);

    // Compute L2 error in barycenter
    double l2_error = 0;
    for (const lf::mesh::Entity *cell : cur_mesh->Entities(0)) {
      const lf::geometry::Geometry *geo_p = cell->Geometry();
      double area = lf::geometry::Volume(*geo_p);
      int idx = cur_dofh.GlobalDofIndices(*cell)[0];
      l2_error += std::pow((result[idx] - ref_solution[idx]), 2) * area;
    }
    l2_error = std::sqrt(l2_error);
    vector_num_cells.push_back(cur_dofh.NumDofs());
    vector_l2error.push_back(l2_error);
    std::cout << "L2Error at level " << level << ": " << l2_error << std::endl;

    // Writing vtk files (optional part)
    std::string sol = "sol";
    std::string ref = "ref";
    std::string f_end = ".vtk";
    std::ostringstream sol_st;
    std::ostringstream ref_st;
    std::ostringstream sol_st_file;
    std::ostringstream ref_st_file;
    sol_st << sol << level;
    ref_st << ref << level;
    sol_st_file << sol << level << f_end;
    ref_st_file << ref << level << f_end;

    lf::io::VtkWriter vtk_writer1(cur_dofh.Mesh(), sol_st_file.str());
    auto cell_data_ref =
        lf::mesh::utils::make_CodimMeshDataSet<double>(cur_dofh.Mesh(), 0);
    for (const lf::mesh::Entity *cell : cur_dofh.Mesh()->Entities(0)) {
      int row = cur_dofh.GlobalDofIndices(*cell)[0];
      cell_data_ref->operator()(*cell) = result[row];
    }
    vtk_writer1.WriteCellData(sol_st.str(), *cell_data_ref);

    lf::io::VtkWriter vtk_writer2(cur_dofh.Mesh(), ref_st_file.str());
    auto cell_data_sol =
        lf::mesh::utils::make_CodimMeshDataSet<double>(cur_dofh.Mesh(), 0);
    for (const lf::mesh::Entity *cell : cur_dofh.Mesh()->Entities(0)) {
      int row = cur_dofh.GlobalDofIndices(*cell)[0];
      cell_data_sol->operator()(*cell) = ref_solution[row];
    }
    vtk_writer2.WriteCellData(ref_st.str(), *cell_data_sol);
  }

  // Task 8-8.q
  // Compute threshold for fourth refinement level
  int level = 4;
  auto cur_mesh = mesh_seq_p->getMesh(level);

  // Create a DOF Hander for the current mesh
  const lf::assemble::UniformFEDofHandler cur_dofh(
      cur_mesh, {{lf::base::RefEl::kPoint(), 0},
                 {lf::base::RefEl::kSegment(), 0},
                 {lf::base::RefEl::kTria(), 1},
                 {lf::base::RefEl::kQuad(), 1}});

  int threshold = AdvectionFV2D::findCFLthreshold(cur_dofh, beta, T);
  int cfl_thres = int((T / AdvectionFV2D::computeHmin(cur_mesh) + 1));
  std::cout << "Threshold for level: " << level << " is: " << threshold
            << " | Threshold from CFL is: " << cfl_thres << std::endl;

  // Write Output file of Task 8-8.o and 8-8.q
  std::ofstream csv_file;
  csv_file.open("advectionfv2d.csv");
  for (int i = 0; i < vector_num_cells.size(); i++) {
    std::cout << "Cells: " << vector_num_cells.at(i)
              << " | L2Error: " << vector_l2error.at(i) << std::endl;
    csv_file << vector_num_cells.at(i) << "," << vector_l2error.at(i) << "\n";
  }
  csv_file.close();

  // Print convergence rates
  for (int i = 1; i < vector_num_cells.size(); i++) {
    double conv_rate =
        (std::log(vector_l2error[i - 1]) - std::log(vector_l2error[i])) /
        (std::log(vector_num_cells[i]) - std::log(vector_num_cells[i - 1]));
    std::cout << "Conv. Rate " << i << "-" << i + 1 << "  is: " << conv_rate
              << std::endl;
  }

  // Plot results from 8-8.o
  std::system("python3 " CURRENT_SOURCE_DIR
              "/advectionfv2d.py " CURRENT_BINARY_DIR
              "/advectionfv2d.csv " CURRENT_BINARY_DIR "/solution.eps");

  return 0;
#else
  //====================
  // Your code goes here
  //====================
  return 0;
#endif
}


/**
 * @file Unit tests for FBstabDense
 * which is designed to solve QPs of the form:
 *
 * min  0.5 z'Hz + f'z
 * s.t. Az <= b
 *
 */
#include <cmath>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/solvers/fbstab/fbstab_dense.h"

namespace drake {
namespace solvers {
namespace fbstab {
namespace test {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;

/**
 * Tests FBstab with
 *
 * H = [3 1]  f = [10]
 *     [1 1]      [5 ]
 *
 * A = [-1 0] b = [0]
 *     [0  1]     [0]
 *
 * This QP can be solved analytically
 * and has the unique primal(z) - dual(v) solution
 * z = [0 -5],  v = [5 0]
 */
GTEST_TEST(FBstabDense, FeasibleQP) {
  MatrixXd H(2, 2);
  MatrixXd A(2, 2);
  VectorXd f(2);
  VectorXd b(2);

  H << 3, 1, 1, 1;
  f << 10, 5;
  A << -1, 0, 0, 1;
  b << 0, 0;

  int n = f.size();
  int q = b.size();

  FBstabDense::QPData data;
  data.H = &H;
  data.f = &f;
  data.A = &A;
  data.b = &b;

  VectorXd z0 = Eigen::VectorXd::Zero(n);
  VectorXd v0 = Eigen::VectorXd::Zero(q);
  VectorXd y0 = Eigen::VectorXd::Zero(q);

  FBstabDense::QPVariable x0;
  x0.z = &z0;
  x0.v = &v0;
  x0.y = &y0;

  FBstabDense solver(n, q);
  solver.UpdateOption("abs_tol", 1e-8);
  solver.SetDisplayLevel(FBstabAlgoDense::Display::OFF);
  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);

  VectorXd zopt(2);
  VectorXd vopt(2);
  zopt << 0, -5;
  vopt << 5, 0;
  for (int i = 0; i < n; i++) {
    EXPECT_NEAR(z0(i), zopt(i), 1e-8);
  }

  for (int i = 0; i < q; i++) {
    EXPECT_NEAR(v0(i), vopt(i), 1e-8);
  }
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1]
 *     [0 0]      [0]
 *
 * A = [0  0] b = [0 ]
 *     [1  0]     [3 ]
 *     [0  1]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is degenerate with a primal solution set
 * [1] x [1,3]
 */
GTEST_TEST(FBstabDense, DegenerateQP) {
  MatrixXd H(2, 2);
  MatrixXd A(5, 2);
  VectorXd f(2);
  VectorXd b(5);

  H << 1, 0, 0, 0;
  f << 1, 0;

  A << 0, 0, 1, 0, 0, 1, -1, 0, 0, -1;

  b << 0, 3, 3, -1, -1;

  int n = f.size();
  int q = b.size();

  FBstabDense::QPData data;
  data.H = &H;
  data.f = &f;
  data.A = &A;
  data.b = &b;

  VectorXd z0 = Eigen::VectorXd::Zero(n);
  VectorXd v0 = Eigen::VectorXd::Zero(q);
  VectorXd y0 = Eigen::VectorXd::Zero(q);

  FBstabDense::QPVariable x0;
  x0.z = &z0;
  x0.v = &v0;
  x0.y = &y0;

  FBstabDense solver(n, q);
  solver.UpdateOption("abs_tol", 1e-8);
  solver.SetDisplayLevel(FBstabAlgoDense::Display::OFF);
  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  EXPECT_NEAR(z0(0), 1, 1e-8);
  EXPECT_TRUE((z0(1) >= 1) && (z0(1) <= 3));

  // Check satisfaction of KKT conditions.
  VectorXd r1 = H * z0 + f + A.transpose() * v0;
  VectorXd r2 = y0.cwiseMin(v0);

  ASSERT_NEAR(r1.norm() + r2.norm(), 0, 1e-6);
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1 ]
 *     [0 0]      [-1]
 *
 * A = [1  1] b = [0 ]
 *     [1  0]     [3 ]
 *     [0  1]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is infeasible, i.e.,
 * there is no z satisfying Az <= b
 */
GTEST_TEST(FBstabDense, InfeasibleQP) {
  MatrixXd H(2, 2);
  MatrixXd A(5, 2);
  VectorXd f(2);
  VectorXd b(5);

  H << 1, 0, 0, 0;
  f << 1, -1;

  A << 1, 1, 1, 0, 0, 1, -1, 0, 0, -1;

  b << 0, 3, 3, -1, -1;

  int n = f.size();
  int q = b.size();

  FBstabDense::QPData data;
  data.H = &H;
  data.f = &f;
  data.A = &A;
  data.b = &b;

  VectorXd z0 = Eigen::VectorXd::Zero(n);
  VectorXd v0 = Eigen::VectorXd::Zero(q);
  VectorXd y0 = Eigen::VectorXd::Zero(q);

  FBstabDense::QPVariable x0;
  x0.z = &z0;
  x0.v = &v0;
  x0.y = &y0;

  FBstabDense solver(n, q);
  solver.UpdateOption("abs_tol", 1e-8);
  solver.SetDisplayLevel(FBstabAlgoDense::Display::OFF);
  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::PRIMAL_INFEASIBLE);
}

/**
 * Tests FBstab with
 *
 * H = [1 0]  f = [1 ]
 *     [0 0]      [-1]
 *
 * A = [0  0] b = [0 ]
 *     [1  0]     [3 ]
 *     [-1 0]     [-1]
 *     [0 -1]     [-1]
 *
 * This QP is unbounded below, i.e.,
 * its optimal value is -infinity
 */
GTEST_TEST(FBstabDense, UnboundedQP) {
  MatrixXd H(2, 2);
  MatrixXd A(4, 2);
  VectorXd f(2);
  VectorXd b(4);

  H << 1, 0, 0, 0;
  f << 1, -1;

  A << 0, 0, 1, 0, -1, 0, 0, -1;

  b << 0, 3, -1, -1;

  int n = f.size();
  int q = b.size();

  FBstabDense::QPData data;
  data.H = &H;
  data.f = &f;
  data.A = &A;
  data.b = &b;

  VectorXd z0 = Eigen::VectorXd::Zero(n);
  VectorXd v0 = Eigen::VectorXd::Zero(q);
  VectorXd y0 = Eigen::VectorXd::Zero(q);

  FBstabDense::QPVariable x0;
  x0.z = &z0;
  x0.v = &v0;
  x0.y = &y0;

  FBstabDense solver(n, q);
  solver.UpdateOption("abs_tol", 1e-8);
  solver.SetDisplayLevel(FBstabAlgoDense::Display::OFF);
  SolverOut out = solver.Solve(data, &x0);

  ASSERT_EQ(out.eflag, ExitFlag::DUAL_INFEASIBLE);
}

}  // namespace test
}  // namespace fbstab
}  // namespace solvers
}  // namespace drake