/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <string.h>
#include "pair_smd_ulsph.h"
#include "atom.h"
#include "domain.h"
#include "force.h"
#include "update.h"
#include "modify.h"
#include "fix.h"
#include "comm.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "memory.h"
#include "error.h"
#include <stdio.h>
#include <iostream>
#include "smd_material_models.h"
#include "smd_math.h"
#include "smd_kernels.h"

using namespace SMD_Kernels;
using namespace std;
using namespace LAMMPS_NS;
using namespace SMD_Math;

#include <Eigen/Eigen>
using namespace Eigen;

#define ARTIFICIAL_STRESS false
#define FORMAT1 "%60s : %g\n"
#define FORMAT2 "\n.............................. %s \n"

PairULSPH::PairULSPH(LAMMPS *lmp) :
		Pair(lmp) {

	// per-type arrays
	Q1 = NULL;
	eos = viscosity = strength = NULL;
	c0_type = NULL;
	c0 = NULL;
	Lookup = NULL;
	artificial_stress = NULL;
	artificial_pressure = NULL;

	nmax = 0; // make sure no atom on this proc such that initial memory allocation is correct
	stressTensor = L = K = NULL;
	shepardWeight = NULL;
	smoothVel = NULL;
	numNeighs = NULL;
	F = NULL;
	rho = NULL;
	effm = NULL;

	velocity_gradient_required = false; // turn off computation of velocity gradient by default
	density_summation = velocity_gradient = false;

	comm_forward = 18; // this pair style communicates 18 doubles to ghost atoms
	updateFlag = 0;
}

/* ---------------------------------------------------------------------- */

PairULSPH::~PairULSPH() {
	if (allocated) {
		//printf("... deallocating\n");
		memory->destroy(Q1);
		memory->destroy(rho0);
		memory->destroy(eos);
		memory->destroy(viscosity);
		memory->destroy(strength);
		memory->destroy(c0_type);
		memory->destroy(Lookup);
		memory->destroy(artificial_pressure);
		memory->destroy(artificial_stress);

		delete[] onerad_dynamic;
		delete[] onerad_frozen;
		delete[] maxrad_dynamic;
		delete[] maxrad_frozen;

		delete[] K;
		delete[] shepardWeight;
		delete[] c0;
		delete[] smoothVel;
		delete[] stressTensor;
		delete[] L;
		delete[] numNeighs;
		delete[] F;
		delete[] rho;
		delete[] effm;

	}
}

/* ----------------------------------------------------------------------
 *
 * Re-compute mass density from scratch.
 * Only used for plain fluid SPH with no physical viscosity models.
 *
 ---------------------------------------------------------------------- */

void PairULSPH::PreCompute_DensitySummation() {
	double *radius = atom->radius;
	double **x = atom->x;
	double *rmass = atom->rmass;
	int *type = atom->type;
	int *ilist, *jlist, *numneigh;
	int **firstneigh;
	int nlocal = atom->nlocal;
	int inum, jnum, ii, jj, i, itype, jtype, j;
	double h, irad, hsq, rSq, wf;
	Vector3d dx, xi, xj;

	// set up neighbor list variables
	inum = list->inum;
	ilist = list->ilist;
	numneigh = list->numneigh;
	firstneigh = list->firstneigh;

	// zero accumulators
	for (i = 0; i < nlocal; i++) {
		rho[i] = 0.0;
		//shepardWeight[i] = 0.0;
	}

	/*
	 * only recompute mass density if density summation is used.
	 * otherwise, change in mass density is time-integrated
	 */
	for (i = 0; i < nlocal; i++) {
		itype = type[i];
		if (setflag[itype][itype] == 1) {
			// initialize particle density with self-contribution.
			h = 2.0 * radius[i];
			hsq = h * h;
			Poly6Kernel(hsq, h, 0.0, domain->dimension, wf);
			rho[i] = wf * rmass[i]; // / shepardWeight[i];
			//printf("SIC to rho is %f\n", rho[i]);
		}
	}

	for (ii = 0; ii < inum; ii++) {
		i = ilist[ii];
		itype = type[i];
		jlist = firstneigh[i];
		jnum = numneigh[i];
		irad = radius[i];

		xi << x[i][0], x[i][1], x[i][2];

		for (jj = 0; jj < jnum; jj++) {
			j = jlist[jj];
			j &= NEIGHMASK;

			xj << x[j][0], x[j][1], x[j][2];
			dx = xj - xi;
			rSq = dx.squaredNorm();
			h = irad + radius[j];
			hsq = h * h;
			if (rSq < hsq) {

				jtype = type[j];
				Poly6Kernel(hsq, h, rSq, domain->dimension, wf);

				if (setflag[itype][itype] == 1) {
					rho[i] += wf * rmass[j]; // / shepardWeight[i];
				}

				if (j < nlocal) {
					if (setflag[jtype][jtype] == 1) {
						rho[j] += wf * rmass[i]; // / shepardWeight[j];
					}
				}
			} // end if check distance
		} // end loop over j
	} // end loop over i
}

/* ----------------------------------------------------------------------
 *
 * Compute shape matrix for kernel gradient correction and velocity gradient.
 * This is used if material strength or viscosity models are employed.
 *
 ---------------------------------------------------------------------- */

void PairULSPH::PreCompute() {
	double **atom_data9 = atom->smd_data_9;
	double *radius = atom->radius;
	double **x = atom->x;
	double **x0 = atom->x0;
	double **v = atom->vest;
	double *vfrac = atom->vfrac;
	int *type = atom->type;
	int *ilist, *jlist, *numneigh;
	int **firstneigh;
	int nlocal = atom->nlocal;
	int inum, jnum, ii, jj, i, itype, j, idim;
	double wfd, h, irad, r, rSq, wf, ivol, jvol;
	Vector3d dx, dv, g, du;
	Matrix3d Ktmp, Ltmp, Ftmp, K3di, D;
	Vector3d xi, xj, vi, vj, x0i, x0j, dx0;
	Matrix2d K2di, K2d;

	// zero accumulators
	for (i = 0; i < nlocal; i++) {
		itype = type[i];
		if (setflag[itype][itype]) {
			if (gradient_correction_flag) {
				K[i].setZero();
			} else {
				K[i].setIdentity();
			}
			L[i].setZero();
			F[i].setZero();
		}
	}

	// set up neighbor list variables
	inum = list->inum;
	ilist = list->ilist;
	numneigh = list->numneigh;
	firstneigh = list->firstneigh;

	for (ii = 0; ii < inum; ii++) {
		i = ilist[ii];
		itype = type[i];
		jlist = firstneigh[i];
		jnum = numneigh[i];
		irad = radius[i];
		ivol = vfrac[i];

		// initialize Eigen data structures from LAMMPS data structures
		for (idim = 0; idim < 3; idim++) {
			x0i(idim) = x0[i][idim];
			xi(idim) = x[i][idim];
			vi(idim) = v[i][idim];
		}

		for (jj = 0; jj < jnum; jj++) {
			j = jlist[jj];
			j &= NEIGHMASK;

			for (idim = 0; idim < 3; idim++) {
				x0j(idim) = x0[j][idim];
				xj(idim) = x[j][idim];
				vj(idim) = v[j][idim];
			}

			dx = xj - xi;

			rSq = dx.squaredNorm();
			h = irad + radius[j];
			if (rSq < h * h) {

				r = sqrt(rSq);
				jvol = vfrac[j];

				// distance vectors in current and reference configuration, velocity difference
				dv = vj - vi;
				dx0 = x0j - x0i;

				// kernel and derivative
				spiky_kernel_and_derivative(h, r, domain->dimension, wf, wfd);
				//barbara_kernel_and_derivative(h, r, domain->dimension, wf, wfd);

				// uncorrected kernel gradient
				g = (wfd / r) * dx;

				/* build correction matrix for kernel derivatives */
				if (gradient_correction_flag) {
					Ktmp = -g * dx.transpose();
					K[i] += jvol * Ktmp;
				}

				// velocity gradient L
				Ltmp = -dv * g.transpose();
				L[i] += jvol * Ltmp;

				// deformation gradient F in Eulerian frame
				du = dx - dx0;
				Ftmp = dv * g.transpose();
				F[i] += jvol * Ftmp;

				if (j < nlocal) {

					if (gradient_correction_flag) {
						K[j] += ivol * Ktmp;
					}

					L[j] += ivol * Ltmp;
					F[j] += ivol * Ftmp;
				}
			} // end if check distance
		} // end loop over j
	} // end loop over i

	/*
	 * invert shape matrix and compute corrected quantities
	 */

	for (i = 0; i < nlocal; i++) {
		itype = type[i];
		if (setflag[itype][itype]) {
			if (gradient_correction_flag) {
				pseudo_inverse_SVD(K[i]);
				K[i] = LimitEigenvalues(K[i], 2.0);
				L[i] *= K[i];
				F[i] *= K[i];
			} // end if (gradient_correction[itype]) {

			/*
			 * accumulate strain increments
			 * we abuse the atom array "atom_data_9" for this purpose, which was originally designed to hold the deformation gradient.
			 */
			D = update->dt * 0.5 * (L[i] + L[i].transpose());
			atom_data9[i][0] += D(0, 0); // xx
			atom_data9[i][1] += D(1, 1); // yy
			atom_data9[i][2] += D(2, 2); // zz
			atom_data9[i][3] += D(0, 1); // xy
			atom_data9[i][4] += D(0, 2); // xz
			atom_data9[i][5] += D(1, 2); // yz

		} // end if (setflag[itype][itype])
	} // end loop over i = 0 to nlocal

}

/* ---------------------------------------------------------------------- */

void PairULSPH::compute(int eflag, int vflag) {
	double **x = atom->x;
	double **v = atom->vest;
	double **vint = atom->v; // Velocity-Verlet algorithm velocities
	double **f = atom->f;
	double *vfrac = atom->vfrac;
	double *de = atom->de;
	double *rmass = atom->rmass;
	double *radius = atom->radius;
	double *contact_radius = atom->contact_radius;
	double **atom_data9 = atom->smd_data_9;

	int *type = atom->type;
	int nlocal = atom->nlocal;
	int i, j, ii, jj, jnum, itype, jtype, iDim, inum;
	double r, wf, wfd, h, rSq, ivol, jvol;
	double mu_ij, c_ij, rho_ij;
	double delVdotDelR, visc_magnitude, deltaE;
	int *ilist, *jlist, *numneigh;
	int **firstneigh;
	Vector3d fi, fj, dx, dv, f_stress, g, vinti, vintj, dvint;
	Vector3d xi, xj, vi, vj, f_visc, sumForces, f_stress_new;
	Vector3d gamma, f_hg, dx0, du_est, du;
	double r_ref, weight, p;
	//int periodic = (domain->xperiodic || domain->yperiodic || domain->zperiodic);

	double ini_dist;
	Matrix3d S, D, V, eye;
	eye.setIdentity();
	int k;
	SelfAdjointEigenSolver < Matrix3d > es;

	if (eflag || vflag)
		ev_setup(eflag, vflag);
	else
		evflag = vflag_fdotr = 0;

	if (atom->nmax > nmax) {
//printf("... allocating in compute with nmax = %d\n", atom->nmax);
		nmax = atom->nmax;
		delete[] K;
		K = new Matrix3d[nmax];
		delete[] shepardWeight;
		shepardWeight = new double[nmax];
		delete[] c0;
		c0 = new double[nmax];
		delete[] smoothVel;
		smoothVel = new Vector3d[nmax];
		delete[] stressTensor;
		stressTensor = new Matrix3d[nmax];
		delete[] L;
		L = new Matrix3d[nmax];
		delete[] numNeighs;
		numNeighs = new int[nmax];
		delete[] F;
		F = new Matrix3d[nmax];
		delete[] rho;
		rho = new double[nmax];
		delete[] effm;
		effm = new double[nmax];
	}

// zero accumulators
	for (i = 0; i < nlocal; i++) {
		shepardWeight[i] = 0.0;
		smoothVel[i].setZero();
		numNeighs[i] = 0;

		h = 2.0 * radius[i];
		r = 0.0;
		spiky_kernel_and_derivative(h, r, domain->dimension, wf, wfd);
	}

	/*
	 * if this is the very first step, zero the array which holds the accumulated strain
	 */
	if (update->ntimestep == 0) {
		for (i = 0; i < nlocal; i++) {
			itype = type[i];
			if (setflag[itype][itype]) {
				for (j = 0; j < 9; j++) {
					atom_data9[i][j] = 0.0;
				}
			}
		}
	}

	if (density_summation) {
		//printf("dens summ\n");
		PreCompute_DensitySummation();

		for (i = 0; i < nlocal; i++) { //compute volumes from rho
			itype = type[i];
			if (setflag[itype][itype]) {
				vfrac[i] = rmass[i] / rho[i];
			}
		}

	}

	if (velocity_gradient) {
		PairULSPH::PreCompute(); // get velocity gradient and kernel gradient correction
	}

	PairULSPH::AssembleStressTensor();

	/*
	 * QUANTITIES ABOVE HAVE ONLY BEEN CALCULATED FOR NLOCAL PARTICLES.
	 * NEED TO DO A FORWARD COMMUNICATION TO GHOST ATOMS NOW
	 */
	comm->forward_comm_pair(this);

	updateFlag = 0;

	/*
	 * iterate over pairs of particles i, j and assign forces using pre-computed pressure
	 */

// set up neighbor list variables
	inum = list->inum;
	ilist = list->ilist;
	numneigh = list->numneigh;
	firstneigh = list->firstneigh;

	for (ii = 0; ii < inum; ii++) {
		i = ilist[ii];
		itype = type[i];
		jlist = firstneigh[i];
		jnum = numneigh[i];
		ivol = vfrac[i];

		// initialize Eigen data structures from LAMMPS data structures
		for (iDim = 0; iDim < 3; iDim++) {
			xi(iDim) = x[i][iDim];
			vi(iDim) = v[i][iDim];
			vinti(iDim) = vint[i][iDim];
		}

		for (jj = 0; jj < jnum; jj++) {
			j = jlist[jj];
			j &= NEIGHMASK;

			xj(0) = x[j][0];
			xj(1) = x[j][1];
			xj(2) = x[j][2];

			dx = xj - xi;
			rSq = dx.squaredNorm();
			h = radius[i] + radius[j];
			if (rSq < h * h) {

				// initialize Eigen data structures from LAMMPS data structures
				for (iDim = 0; iDim < 3; iDim++) {
					vj(iDim) = v[j][iDim];
					vintj(iDim) = vint[j][iDim];
				}

				r = sqrt(rSq);
				jtype = type[j];
				jvol = vfrac[j];

				// distance vectors in current and reference configuration, velocity difference
				dv = vj - vi;
				dvint = vintj - vinti;

				// kernel and derivative
				spiky_kernel_and_derivative(h, r, domain->dimension, wf, wfd);
				//barbara_kernel_and_derivative(h, r, domain->dimension, wf, wfd);

				// uncorrected kernel gradient
				g = (wfd / r) * dx;

				delVdotDelR = dx.dot(dv) / (r + 0.1 * h); // project relative velocity onto unit particle distance vector [m/s]

				S = stressTensor[i] + stressTensor[j];

				if (artificial_pressure[itype][jtype] > 0.0) {
					p = S.trace();
					if (p > 0.0) { // we are in tension
						r_ref = contact_radius[i] + contact_radius[j];
						weight = Kernel_Cubic_Spline(r, h) / Kernel_Cubic_Spline(r_ref, h);
						weight = pow(weight, 4.0);
						S -= artificial_pressure[itype][jtype] * weight * p * eye;
					}
				}

				/*
				 * artificial stress to control tensile instability
				 * Only works if particles are uniformly spaced initially.
				 */
				if (artificial_stress[itype][jtype] > 0.0) {
					ini_dist = contact_radius[i] + contact_radius[j];
					weight = Kernel_Cubic_Spline(r, h) / Kernel_Cubic_Spline(ini_dist, h);
					weight = pow(weight, 4.0);

					es.compute(S);
					D = es.eigenvalues().asDiagonal();
					for (k = 0; k < 3; k++) {
						if (D(k, k) > 0.0) {
							D(k, k) -= weight * artificial_stress[itype][jtype] * D(k, k);
						}
					}
					V = es.eigenvectors();
					S = V * D * V.inverse();
				}

				// compute forces
				f_stress = -ivol * jvol * S * g; // DO NOT TOUCH SIGN

				/*
				 * artificial viscosity -- alpha is dimensionless
				 * MonaghanBalsara form of the artificial viscosity
				 */

				c_ij = 0.5 * (c0[i] + c0[j]);
				LimitDoubleMagnitude(delVdotDelR, 1.1 * c_ij);

				mu_ij = h * delVdotDelR / (r + 0.1 * h); // units: [m * m/s / m = m/s]
				rho_ij = 0.5 * (rmass[i] / ivol + rmass[j] / jvol);
				visc_magnitude = 0.5 * (Q1[itype] + Q1[jtype]) * c_ij * mu_ij / rho_ij;
				f_visc = -rmass[i] * rmass[j] * visc_magnitude * g;

				if ((Lookup[HOURGLASS_CONTROL_AMPLITUDE][itype] > 0.0) && (Lookup[HOURGLASS_CONTROL_AMPLITUDE][jtype] > 0.0)) {
					f_hg = ComputeHourglassForce(i, itype, j, jtype, dv, dx, g, c_ij, mu_ij, rho_ij);

				} else {
					f_hg.setZero();
				}

				sumForces = f_stress + f_visc + f_hg;

				// energy rate -- project velocity onto force vector
				deltaE = sumForces.dot(dv);

				// apply forces to pair of particles
				f[i][0] += sumForces(0);
				f[i][1] += sumForces(1);
				f[i][2] += sumForces(2);
				de[i] += deltaE;

				// accumulate smooth velocities
				shepardWeight[i] += jvol * wf;
				smoothVel[i] += jvol * wf * dvint;
				numNeighs[i] += 1;

				if (j < nlocal) {
					f[j][0] -= sumForces(0);
					f[j][1] -= sumForces(1);
					f[j][2] -= sumForces(2);
					de[j] += deltaE;

					shepardWeight[j] += ivol * wf;
					smoothVel[j] -= ivol * wf * dvint;
					numNeighs[j] += 1;
				}

				// tally atomistic stress tensor
				if (evflag) {
					ev_tally_xyz(i, j, nlocal, 0, 0.0, 0.0, sumForces(0), sumForces(1), sumForces(2), dx(0), dx(1), dx(2));
				}
			}

		}
	}

	for (i = 0; i < nlocal; i++) {
		itype = type[i];
		if (setflag[itype][itype] == 1) {
			if (shepardWeight[i] != 0.0) {
				smoothVel[i] /= shepardWeight[i];
			} else {
				smoothVel[i].setZero();
			}
		} // end check if particle is SPH-type
	} // end loop over i = 0 to nlocal

	if (vflag_fdotr)
		virial_fdotr_compute();

}

/* ----------------------------------------------------------------------
 Assemble total stress tensor with pressure, material sterength, and
 viscosity contributions.
 ------------------------------------------------------------------------- */
void PairULSPH::AssembleStressTensor() {
	double *radius = atom->radius;
	double *vfrac = atom->vfrac;
	double *rmass = atom->rmass;
	double *eff_plastic_strain = atom->eff_plastic_strain;
	double **tlsph_stress = atom->smd_stress;
	double *e = atom->e;
	int *type = atom->type;
	int i, itype;
	int nlocal = atom->nlocal;
	Matrix3d D, Ddev, W, V, sigma_diag;
	Matrix3d eye, stressRate, StressRateDevJaumann;
	Matrix3d sigmaInitial_dev, d_dev, sigmaFinal_dev, stressRateDev, oldStressDeviator, newStressDeviator;
	double plastic_strain_increment, yieldStress;
	double dt = update->dt;
	double vol, newPressure;
	double G_eff = 0.0; // effective shear modulus
	double K_eff; // effective bulk modulus
	double M, p_wave_speed;
	double rho, effectiveViscosity;
	Matrix3d deltaStressDev;

	dtCFL = 1.0e22;
	eye.setIdentity();

	for (i = 0; i < nlocal; i++) {
		itype = type[i];
		if (setflag[itype][itype] == 1) {
			newStressDeviator.setZero();
			newPressure = 0.0;
			stressTensor[i].setZero();
			vol = vfrac[i];
			rho = rmass[i] / vfrac[i];
			effectiveViscosity = 0.0;
			K_eff = 0.0;
			G_eff = 0.0;

			//printf("rho = %f\n", rho);

			switch (eos[itype]) {
			default:
				error->one(FLERR, "unknown EOS.");
				break;
			case NONE:
				c0[i] = 1.0;
				break;
			case EOS_TAIT:
				TaitEOS_density(Lookup[EOS_TAIT_EXPONENT][itype], Lookup[REFERENCE_SOUNDSPEED][itype],
						Lookup[REFERENCE_DENSITY][itype], rho, newPressure, c0[i]);
				//printf("new pressure =%f\n", newPressure);

				break;
			case EOS_PERFECT_GAS:
				PerfectGasEOS(Lookup[EOS_PERFECT_GAS_GAMMA][itype], vol, rmass[i], e[i], newPressure, c0[i]);
				break;
			case EOS_LINEAR:
				newPressure = Lookup[BULK_MODULUS][itype] * (rho / Lookup[REFERENCE_DENSITY][itype] - 1.0);
				//printf("p=%f, rho0=%f, rho=%f\n", newPressure, Lookup[REFERENCE_DENSITY][itype], rho);
				c0[i] = Lookup[REFERENCE_SOUNDSPEED][itype];
				break;
			}

			K_eff = c0[i] * c0[i] * rho; // effective bulk modulus

			/*
			 * ******************************* STRENGTH MODELS ************************************************
			 */

			if (strength[itype] != NONE) {
				/*
				 * initial stress state: given by the unrotateted Cauchy stress.
				 * Assemble Eigen 3d matrix from stored stress state
				 */
				oldStressDeviator(0, 0) = tlsph_stress[i][0];
				oldStressDeviator(0, 1) = tlsph_stress[i][1];
				oldStressDeviator(0, 2) = tlsph_stress[i][2];
				oldStressDeviator(1, 1) = tlsph_stress[i][3];
				oldStressDeviator(1, 2) = tlsph_stress[i][4];
				oldStressDeviator(2, 2) = tlsph_stress[i][5];
				oldStressDeviator(1, 0) = oldStressDeviator(0, 1);
				oldStressDeviator(2, 0) = oldStressDeviator(0, 2);
				oldStressDeviator(2, 1) = oldStressDeviator(1, 2);

				D = 0.5 * (L[i] + L[i].transpose());
				W = 0.5 * (L[i] - L[i].transpose()); // spin tensor:: need this for Jaumann rate
				d_dev = Deviator(D);

				switch (strength[itype]) {
				default:
					error->one(FLERR, "unknown strength model.");
					break;
				case STRENGTH_LINEAR:

					// here in a version with pressure part
//					stressRateDev = Lookup[BULK_MODULUS][itype] * d_iso * eye + 2.0 * Lookup[SHEAR_MODULUS][itype] * d_dev;
//					c0[i] = Lookup[REFERENCE_SOUNDSPEED][itype];
//					newPressure = 0.0;

					// here only stress deviator
					stressRateDev = 2.0 * Lookup[SHEAR_MODULUS][itype] * d_dev;
					//cout << "stress rate deviator is " << endl << stressRateDev << endl;
					break;

				case STRENGTH_LINEAR_PLASTIC:
					yieldStress = Lookup[YIELD_STRENGTH][itype] + Lookup[HARDENING_PARAMETER][itype] * eff_plastic_strain[i];
					LinearPlasticStrength(Lookup[SHEAR_MODULUS][itype], yieldStress, oldStressDeviator, d_dev, dt,
							newStressDeviator, stressRateDev, plastic_strain_increment);
					eff_plastic_strain[i] += plastic_strain_increment;

					break;
				}

				//double m = effective_longitudinal_modulus(itype, dt, d_iso, p_rate, d_dev, stressRate_dev, damage);

				StressRateDevJaumann = stressRateDev - W * oldStressDeviator + oldStressDeviator * W;
				newStressDeviator = oldStressDeviator + dt * StressRateDevJaumann;

				tlsph_stress[i][0] = newStressDeviator(0, 0);
				tlsph_stress[i][1] = newStressDeviator(0, 1);
				tlsph_stress[i][2] = newStressDeviator(0, 2);
				tlsph_stress[i][3] = newStressDeviator(1, 1);
				tlsph_stress[i][4] = newStressDeviator(1, 2);
				tlsph_stress[i][5] = newStressDeviator(2, 2);

				// estimate effective shear modulus for time step stability
				deltaStressDev = oldStressDeviator - newStressDeviator;
				G_eff = effective_shear_modulus(d_dev, deltaStressDev, dt, itype);

			} // end if (strength[itype] != NONE)

			if (viscosity[itype] != NONE) {
				D = 0.5 * (L[i] + L[i].transpose());
				d_dev = Deviator(D);

				switch (viscosity[itype]) {
				default:
					error->one(FLERR, "unknown viscosity model.");
					break;
				case VISCOSITY_NEWTON:
					effectiveViscosity = Lookup[VISCOSITY_MU][itype];
//					double shear_rate = 2.0
//							* sqrt(d_dev(0, 1) * d_dev(0, 1) + d_dev(0, 2) * d_dev(0, 2) + d_dev(1, 2) * d_dev(1, 2)); // 3d
					//cout << "shear rate: " << shear_rate << endl;
					//effectiveViscosity = PA6_270C(shear_rate);
					//if (effectiveViscosity > 178.062e-6) {
					//	printf("effective visc is %f\n", effectiveViscosity);
					//}
					newStressDeviator = 2.0 * effectiveViscosity * d_dev; // newton original
					//cout << "this is Ddev " << endl << d_dev << endl << endl;
					break;
				}
			} // end if (viscosity[itype] != NONE)

			/*
			 * assemble stress Tensor from pressure and deviatoric parts
			 */

			stressTensor[i] = -newPressure * eye + newStressDeviator;

			/*
			 * stable timestep based on speed-of-sound
			 */

			M = K_eff + 4.0 * G_eff / 3.0;
			p_wave_speed = sqrt(M / rho);
			effm[i] = G_eff;
			dtCFL = MIN(2 * radius[i] / p_wave_speed, dtCFL);

			/*
			 * stable timestep based on viscosity
			 */
			if (viscosity[itype] != NONE) {
				dtCFL = MIN(4 * radius[i] * radius[i] * rho / effectiveViscosity, dtCFL);
			}

			/*
			 * kernel gradient correction
			 */
			if (gradient_correction_flag) {
				stressTensor[i] *= K[i];
			}
		}
		// end if (setflag[itype][itype] == 1)
	} // end loop over nlocal

//printf("stable timestep = %g\n", 0.1 * hMin * MaxBulkVelocity);
}

/* ----------------------------------------------------------------------
 allocate all arrays
 ------------------------------------------------------------------------- */

void PairULSPH::allocate() {

	allocated = 1;
	int n = atom->ntypes;

	memory->create(setflag, n + 1, n + 1, "pair:setflag");

	memory->create(Q1, n + 1, "pair:Q1");
	memory->create(rho0, n + 1, "pair:Q2");
	memory->create(c0_type, n + 1, "pair:c0_type");
	memory->create(eos, n + 1, "pair:eosmodel");
	memory->create(viscosity, n + 1, "pair:viscositymodel");
	memory->create(strength, n + 1, "pair:strengthmodel");

	memory->create(Lookup, MAX_KEY_VALUE, n + 1, "pair:LookupTable");

	memory->create(artificial_pressure, n + 1, n + 1, "pair:artificial_pressure");
	memory->create(artificial_stress, n + 1, n + 1, "pair:artificial_stress");
	memory->create(cutsq, n + 1, n + 1, "pair:cutsq");		// always needs to be allocated, even with granular neighborlist

	/*
	 * initialize arrays to default values
	 */

	for (int i = 1; i <= n; i++) {
		for (int j = i; j <= n; j++) {
			artificial_pressure[i][j] = 0.0;
			artificial_stress[i][j] = 0.0;
			setflag[i][j] = 0;
		}
	}

	onerad_dynamic = new double[n + 1];
	onerad_frozen = new double[n + 1];
	maxrad_dynamic = new double[n + 1];
	maxrad_frozen = new double[n + 1];

}

/* ----------------------------------------------------------------------
 global settings
 ------------------------------------------------------------------------- */

void PairULSPH::settings(int narg, char **arg) {
	if (narg != 3) {
		printf("narg = %d\n", narg);
		error->all(FLERR, "Illegal number of arguments for pair_style ulsph");
	}

	if (comm->me == 0) {
		printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
		printf("... SMD / ULSPH PROPERTIES\n\n");
	}

	if (strcmp(arg[0], "*DENSITY_SUMMATION") == 0) {
		density_summation = true;
		density_continuity = false;
		if (comm->me == 0)
			printf("... density summation active\n");
	} else if (strcmp(arg[0], "*DENSITY_CONTINUITY") == 0) {
		density_continuity = true;
		density_summation = false;
		if (comm->me == 0)
			printf("... density continuity active\n");
	} else {
		error->all(FLERR,
				"Illegal settings keyword for first keyword of pair style ulsph. Must be either *DENSITY_SUMMATION or *DENSITY_CONTINUITY");
	}

	if (strcmp(arg[1], "*VELOCITY_GRADIENT") == 0) {
		velocity_gradient = true;
		if (comm->me == 0)
			printf("... computation of velocity gradients active\n");
	} else if (strcmp(arg[1], "*NO_VELOCITY_GRADIENT") == 0) {
		velocity_gradient = false;
		if (comm->me == 0)
			printf("... computation of velocity gradients NOT active\n");
	} else {
		error->all(FLERR,
				"Illegal settings keyword for first keyword of pair style ulsph. Must be either *VELOCITY_GRADIENT or *NO_VELOCITY_GRADIENT");
	}

	if (strcmp(arg[2], "*GRADIENT_CORRECTION") == 0) {
		gradient_correction_flag = true;
		if (comm->me == 0)
			printf("... first order correction of kernel gradients is active\n");
	} else if (strcmp(arg[2], "*NO_GRADIENT_CORRECTION") == 0) {
		gradient_correction_flag = false;
		if (comm->me == 0)
			printf("... first order correction of kernel gradients is NOT active\n");
	} else {
		error->all(FLERR, "Illegal settings keyword for pair style ulsph");
	}

// error check
	//if ((gradient_correction_flag == true) && (density_summation)) {
	//	error->all(FLERR, "Cannot use *DENSITY_SUMMATION in combination with *YES_GRADIENT_CORRECTION");
	//}

	if (comm->me == 0)
		printf(">>========>>========>>========>>========>>========>>========>>========>>========\n");

}

/* ----------------------------------------------------------------------
 set coeffs for one or more type pairs
 ------------------------------------------------------------------------- */

void PairULSPH::coeff(int narg, char **arg) {
	int ioffset, iarg, iNextKwd, itype, jtype;
	char str[128];
	std::string s, t;

	if (narg < 3) {
		sprintf(str, "number of arguments for pair ulsph is too small!");
		error->all(FLERR, str);
	}
	if (!allocated)
		allocate();

	/*
	 * if parameters are give in i,i form, i.e., no a cross interaction, set material parameters
	 */

	if (force->inumeric(FLERR, arg[0]) == force->inumeric(FLERR, arg[1])) {

		itype = force->inumeric(FLERR, arg[0]);
		eos[itype] = viscosity[itype] = strength[itype] = NONE;

		if (comm->me == 0) {
			printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
			printf("...SMD / ULSPH PROPERTIES OF PARTICLE TYPE %d\n\n", itype);
		}

		/*
		 * read parameters which are common -- regardless of material / eos model
		 */

		ioffset = 2;
		if (strcmp(arg[ioffset], "*COMMON") != 0) {
			sprintf(str, "common keyword missing!");
			error->all(FLERR, str);
		} else {
		}

		t = string("*");
		iNextKwd = -1;
		for (iarg = ioffset + 1; iarg < narg; iarg++) {
			s = string(arg[iarg]);
			if (s.compare(0, t.length(), t) == 0) {
				iNextKwd = iarg;
				break;
			}
		}

		//printf("keyword following *COMMON is %s\n", arg[iNextKwd]);

		if (iNextKwd < 0) {
			sprintf(str, "no *KEYWORD terminates *COMMON");
			error->all(FLERR, str);
		}

		if (iNextKwd - ioffset != 5 + 1) {
			sprintf(str, "expected 5 arguments following *COMMON but got %d\n", iNextKwd - ioffset - 1);
			error->all(FLERR, str);
		}

		Lookup[REFERENCE_DENSITY][itype] = force->numeric(FLERR, arg[ioffset + 1]);
		Lookup[REFERENCE_SOUNDSPEED][itype] = force->numeric(FLERR, arg[ioffset + 2]);
		Q1[itype] = force->numeric(FLERR, arg[ioffset + 3]);
		Lookup[HEAT_CAPACITY][itype] = force->numeric(FLERR, arg[ioffset + 4]);
		Lookup[HOURGLASS_CONTROL_AMPLITUDE][itype] = force->numeric(FLERR, arg[ioffset + 5]);

		Lookup[BULK_MODULUS][itype] = Lookup[REFERENCE_SOUNDSPEED][itype] * Lookup[REFERENCE_SOUNDSPEED][itype]
				* Lookup[REFERENCE_DENSITY][itype];

		if (comm->me == 0) {
			printf("material unspecific properties for SMD/ULSPH definition of particle type %d:\n", itype);
			printf(FORMAT1, "reference density", Lookup[REFERENCE_DENSITY][itype]);
			printf(FORMAT1, "reference speed of sound", Lookup[REFERENCE_SOUNDSPEED][itype]);
			printf(FORMAT1, "linear viscosity coefficient", Q1[itype]);
			printf(FORMAT1, "heat capacity [energy / (mass * temperature)]", Lookup[HEAT_CAPACITY][itype]);
			printf(FORMAT1, "bulk modulus", Lookup[BULK_MODULUS][itype]);
			printf(FORMAT1, "hourglass control amplitude", Lookup[HOURGLASS_CONTROL_AMPLITUDE][itype]);
		}

		/*
		 * read following material cards
		 */

//		if (comm->me == 0) {
//			printf("next kwd is %s\n", arg[iNextKwd]);
//		}
		while (true) {
			if (strcmp(arg[iNextKwd], "*END") == 0) {
//				if (comm->me == 0) {
//					sprintf(str, "found *END");
//					error->message(FLERR, str);
//				}
				break;
			}

			ioffset = iNextKwd;
			if (strcmp(arg[ioffset], "*EOS_TAIT") == 0) {

				/*
				 * Tait EOS
				 */

				eos[itype] = EOS_TAIT;
				//printf("reading *EOS_TAIT\n");

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *EOS_TAIT");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *EOS_TAIT but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				Lookup[EOS_TAIT_EXPONENT][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "Tait EOS");
					printf(FORMAT1, "Exponent", Lookup[EOS_TAIT_EXPONENT][itype]);
				}
			} // end Tait EOS

			else if (strcmp(arg[ioffset], "*EOS_PERFECT_GAS") == 0) {

				/*
				 * Perfect Gas EOS
				 */

				eos[itype] = EOS_PERFECT_GAS;
				//printf("reading *EOS_PERFECT_GAS\n");

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *EOS_PERFECT_GAS");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *EOS_PERFECT_GAS but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				Lookup[EOS_PERFECT_GAS_GAMMA][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "Perfect Gas EOS");
					printf(FORMAT1, "Heat Capacity Ratio Gamma", Lookup[EOS_PERFECT_GAS_GAMMA][itype]);
				}
			} // end Perfect Gas EOS
			else if (strcmp(arg[ioffset], "*EOS_LINEAR") == 0) {

				/*
				 * Linear EOS
				 */

				eos[itype] = EOS_LINEAR;

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *EOS_LINEAR");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 0 + 1) {
					sprintf(str, "expected 0 arguments following *EOS_LINEAR but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				if (comm->me == 0) {
					printf(FORMAT2, "Linear EOS");
					printf(FORMAT1, "Bulk modulus", Lookup[BULK_MODULUS][itype]);
				}
			} // end Linear EOS
			else if (strcmp(arg[ioffset], "*STRENGTH_LINEAR_PLASTIC") == 0) {

				if (velocity_gradient != true) {
					error->all(FLERR, "A strength model was requested but *VELOCITY_GRADIENT is not set");
				}

				/*
				 * linear elastic / ideal plastic material model with strength
				 */

				strength[itype] = STRENGTH_LINEAR_PLASTIC;
				velocity_gradient_required = true;
				//printf("reading *LINEAR_PLASTIC\n");

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *STRENGTH_LINEAR_PLASTIC");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 3 + 1) {
					sprintf(str, "expected 3 arguments following *STRENGTH_LINEAR_PLASTIC but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				Lookup[SHEAR_MODULUS][itype] = force->numeric(FLERR, arg[ioffset + 1]);
				Lookup[YIELD_STRENGTH][itype] = force->numeric(FLERR, arg[ioffset + 2]);
				Lookup[HARDENING_PARAMETER][itype] = force->numeric(FLERR, arg[ioffset + 3]);

				if (comm->me == 0) {
					printf(FORMAT2, "linear elastic / ideal plastic material mode");
					printf(FORMAT1, "yield_strength", Lookup[YIELD_STRENGTH][itype]);
					printf(FORMAT1, "constant hardening parameter", Lookup[HARDENING_PARAMETER][itype]);
					printf(FORMAT1, "shear modulus", Lookup[SHEAR_MODULUS][itype]);
				}
			} // end *STRENGTH_LINEAR_PLASTIC
			else if (strcmp(arg[ioffset], "*STRENGTH_LINEAR") == 0) {

				if (velocity_gradient != true) {
					error->all(FLERR, "A strength model was requested but *VELOCITY_GRADIENT is not set");
				}

				/*
				 * linear elastic / ideal plastic material model with strength
				 */

				strength[itype] = STRENGTH_LINEAR;
				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *STRENGTH_LINEAR");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *STRENGTH_LINEAR but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				Lookup[SHEAR_MODULUS][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "linear elastic strength model");
					printf(FORMAT1, "shear modulus", Lookup[SHEAR_MODULUS][itype]);
				}
			} // end *STRENGTH_LINEAR
			else if (strcmp(arg[ioffset], "*VISCOSITY_NEWTON") == 0) {

				if (velocity_gradient != true) {
					error->all(FLERR, "A viscosity model was requested but *VELOCITY_GRADIENT is not set");
				}

				/*
				 * linear elastic / ideal plastic material model with strength
				 */

				viscosity[itype] = VISCOSITY_NEWTON;
				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *VISCOSITY_NEWTON");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *VISCOSITY_NEWTON but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				Lookup[VISCOSITY_MU][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "Newton viscosity model");
					printf(FORMAT1, "viscosity mu", Lookup[VISCOSITY_MU][itype]);
				}
			} // end *STRENGTH_VISCOSITY_NEWTON

			else if (strcmp(arg[ioffset], "*ARTIFICIAL_PRESSURE") == 0) {

				/*
				 * use Monaghan's artificial pressure to prevent particle clumping
				 */

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *ARTIFICIAL_PRESSURE");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *ARTIFICIAL_PRESSURE but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				artificial_pressure[itype][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "Artificial Pressure is enabled.");
					printf(FORMAT1, "Artificial Pressure amplitude", artificial_pressure[itype][itype]);
				}
			} // end *ARTIFICIAL_PRESSURE

			else if (strcmp(arg[ioffset], "*ARTIFICIAL_STRESS") == 0) {

				/*
				 * use Monaghan's artificial stress to prevent particle clumping
				 */

				t = string("*");
				iNextKwd = -1;
				for (iarg = ioffset + 1; iarg < narg; iarg++) {
					s = string(arg[iarg]);
					if (s.compare(0, t.length(), t) == 0) {
						iNextKwd = iarg;
						break;
					}
				}

				if (iNextKwd < 0) {
					sprintf(str, "no *KEYWORD terminates *ARTIFICIAL_STRESS");
					error->all(FLERR, str);
				}

				if (iNextKwd - ioffset != 1 + 1) {
					sprintf(str, "expected 1 arguments following *ARTIFICIAL_STRESS but got %d\n", iNextKwd - ioffset - 1);
					error->all(FLERR, str);
				}

				artificial_stress[itype][itype] = force->numeric(FLERR, arg[ioffset + 1]);

				if (comm->me == 0) {
					printf(FORMAT2, "Artificial Stress is enabled.");
					printf(FORMAT1, "Artificial Stress amplitude", artificial_stress[itype][itype]);
				}
			} // end *ARTIFICIAL_STRESS

			else {
				sprintf(str, "unknown *KEYWORD: %s", arg[ioffset]);
				error->all(FLERR, str);
			}

		}

		/*
		 * copy data which is looked up in inner pairwise loops from slow maps to fast arrays
		 */

		rho0[itype] = Lookup[REFERENCE_DENSITY][itype];
		c0_type[itype] = Lookup[REFERENCE_SOUNDSPEED][itype];
		setflag[itype][itype] = 1;

		/*
		 * error checks
		 */

		if ((viscosity[itype] != NONE) && (strength[itype] != NONE)) {
			sprintf(str, "cannot have both a strength and viscosity model for particle type %d", itype);
			error->all(FLERR, str);
		}

		if (eos[itype] == NONE) {
			sprintf(str, "must specify an EOS for particle type %d", itype);
			error->all(FLERR, str);
		}

	} else {
		/*
		 * we are reading a cross-interaction line for particle types i, j
		 */

		itype = force->inumeric(FLERR, arg[0]);
		jtype = force->inumeric(FLERR, arg[1]);

		if (strcmp(arg[2], "*CROSS") != 0) {
			sprintf(str, "ulsph cross interaction between particle type %d and %d requested, however, *CROSS keyword is missing",
					itype, jtype);
			error->all(FLERR, str);
		}

		if (setflag[itype][itype] != 1) {
			sprintf(str,
					"ulsph cross interaction between particle type %d and %d requested, however, properties of type %d  have not yet been specified",
					itype, jtype, itype);
			error->all(FLERR, str);
		}

		if (setflag[jtype][jtype] != 1) {
			sprintf(str,
					"ulsph cross interaction between particle type %d and %d requested, however, properties of type %d  have not yet been specified",
					itype, jtype, jtype);
			error->all(FLERR, str);
		}

		setflag[itype][jtype] = 1;
		setflag[jtype][itype] = 1;

		if ((artificial_pressure[itype][itype] > 0.0) && (artificial_pressure[jtype][jtype] > 0.0)) {
			artificial_pressure[itype][jtype] = 0.5 * (artificial_pressure[itype][itype] + artificial_pressure[jtype][jtype]);
			artificial_pressure[jtype][itype] = artificial_pressure[itype][jtype];
		} else {
			artificial_pressure[itype][jtype] = artificial_pressure[jtype][itype] = 0.0;
		}

		if ((artificial_stress[itype][itype] > 0.0) && (artificial_stress[jtype][jtype] > 0.0)) {
			artificial_stress[itype][jtype] = 0.5 * (artificial_stress[itype][itype] + artificial_stress[jtype][jtype]);
			artificial_stress[jtype][itype] = artificial_stress[itype][jtype];
		} else {
			artificial_stress[itype][jtype] = artificial_stress[jtype][itype] = 0.0;
		}

		if (comm->me == 0) {
			printf(">>========>>========>>========>>========>>========>>========>>========>>========\n");
		}

	}
}

/* ----------------------------------------------------------------------
 init for one type pair i,j and corresponding j,i
 ------------------------------------------------------------------------- */

double PairULSPH::init_one(int i, int j) {

	if (!allocated)
		allocate();

	if (setflag[i][j] == 0)
		error->all(FLERR, "All pair coeffs are not set");

// cutoff = sum of max I,J radii for
// dynamic/dynamic & dynamic/frozen interactions, but not frozen/frozen

	double cutoff = maxrad_dynamic[i] + maxrad_dynamic[j];
	cutoff = MAX(cutoff, maxrad_frozen[i] + maxrad_dynamic[j]);
	cutoff = MAX(cutoff, maxrad_dynamic[i] + maxrad_frozen[j]);
//printf("cutoff for pair sph/fluid = %f\n", cutoff);
	return cutoff;
}

/* ----------------------------------------------------------------------
 init specific to this pair style
 ------------------------------------------------------------------------- */

void PairULSPH::init_style() {
	int i;

//printf(" in init style\n");
// request a granular neighbor list
	int irequest = neighbor->request(this);
	neighbor->requests[irequest]->half = 0;
	neighbor->requests[irequest]->gran = 1;

// set maxrad_dynamic and maxrad_frozen for each type
// include future Fix pour particles as dynamic

	for (i = 1; i <= atom->ntypes; i++)
		onerad_dynamic[i] = onerad_frozen[i] = 0.0;

	double *radius = atom->radius;
	int *type = atom->type;
	int nlocal = atom->nlocal;

	for (i = 0; i < nlocal; i++)
		onerad_dynamic[type[i]] = MAX(onerad_dynamic[type[i]], radius[i]);

	MPI_Allreduce(&onerad_dynamic[1], &maxrad_dynamic[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
	MPI_Allreduce(&onerad_frozen[1], &maxrad_frozen[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);

}

/* ----------------------------------------------------------------------
 neighbor callback to inform pair style of neighbor list to use
 optional granular history list
 ------------------------------------------------------------------------- */

void PairULSPH::init_list(int id, NeighList *ptr) {
	if (id == 0)
		list = ptr;
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based arrays
 ------------------------------------------------------------------------- */

double PairULSPH::memory_usage() {

//printf("in memory usage\n");

	return 11 * nmax * sizeof(double);

}

/* ---------------------------------------------------------------------- */

int PairULSPH::pack_forward_comm(int n, int *list, double *buf, int pbc_flag, int *pbc) {
	double *vfrac = atom->vfrac;
	double *eff_plastic_strain = atom->eff_plastic_strain;
	int i, j, m;

//printf("packing comm\n");
	m = 0;
	for (i = 0; i < n; i++) {
		j = list[i];
		buf[m++] = vfrac[j];
		buf[m++] = c0[j]; //2

		buf[m++] = stressTensor[j](0, 0); // pack symmetric stress tensor
		buf[m++] = stressTensor[j](1, 1);
		buf[m++] = stressTensor[j](2, 2);
		buf[m++] = stressTensor[j](0, 1);
		buf[m++] = stressTensor[j](0, 2);
		buf[m++] = stressTensor[j](1, 2); // 2 + 6 = 8

		buf[m++] = F[j](0, 0); // F is not symmetric
		buf[m++] = F[j](0, 1);
		buf[m++] = F[j](0, 2);
		buf[m++] = F[j](1, 0);
		buf[m++] = F[j](1, 1);
		buf[m++] = F[j](1, 2);
		buf[m++] = F[j](2, 0);
		buf[m++] = F[j](2, 1);
		buf[m++] = F[j](2, 2); // 8 + 9 = 17

		buf[m++] = eff_plastic_strain[j]; // 18
	}
	return m;
}

/* ---------------------------------------------------------------------- */

void PairULSPH::unpack_forward_comm(int n, int first, double *buf) {
	double *vfrac = atom->vfrac;
	double *eff_plastic_strain = atom->eff_plastic_strain;
	int i, m, last;

	m = 0;
	last = first + n;
	for (i = first; i < last; i++) {
		vfrac[i] = buf[m++];
		c0[i] = buf[m++]; // 2

		stressTensor[i](0, 0) = buf[m++];
		stressTensor[i](1, 1) = buf[m++];
		stressTensor[i](2, 2) = buf[m++];
		stressTensor[i](0, 1) = buf[m++];
		stressTensor[i](0, 2) = buf[m++];
		stressTensor[i](1, 2) = buf[m++]; // 2 + 6 = 8
		stressTensor[i](1, 0) = stressTensor[i](0, 1);
		stressTensor[i](2, 0) = stressTensor[i](0, 2);
		stressTensor[i](2, 1) = stressTensor[i](1, 2);

		F[i](0, 0) = buf[m++];
		F[i](0, 1) = buf[m++];
		F[i](0, 2) = buf[m++];
		F[i](1, 0) = buf[m++];
		F[i](1, 1) = buf[m++];
		F[i](1, 2) = buf[m++];
		F[i](2, 0) = buf[m++];
		F[i](2, 1) = buf[m++];
		F[i](2, 2) = buf[m++]; // 8 + 9 = 17

		eff_plastic_strain[i] = buf[m++]; // 18
	}
}

/*
 * EXTRACT
 */

void *PairULSPH::extract(const char *str, int &i) {
//printf("in extract\n");
	if (strcmp(str, "smd/ulsph/smoothVel_ptr") == 0) {
		return (void *) smoothVel;
	} else if (strcmp(str, "smd/ulsph/stressTensor_ptr") == 0) {
		return (void *) stressTensor;
	} else if (strcmp(str, "smd/ulsph/velocityGradient_ptr") == 0) {
		return (void *) L;
	} else if (strcmp(str, "smd/ulsph/numNeighs_ptr") == 0) {
		return (void *) numNeighs;
	} else if (strcmp(str, "smd/ulsph/dtCFL_ptr") == 0) {
//printf("dtcfl = %f\n", dtCFL);
		return (void *) &dtCFL;
	} else if (strcmp(str, "smd/ulsph/updateFlag_ptr") == 0) {
		return (void *) &updateFlag;
	} else if (strcmp(str, "smd/ulsph/effective_modulus_ptr") == 0) {
		return (void *) effm;
	} else if (strcmp(str, "smd/ulsph/shape_matrix_ptr") == 0) {
		return (void *) K;
	}

	return NULL;
}

/* ----------------------------------------------------------------------
 compute effective shear modulus by dividing rate of deviatoric stress with rate of shear deformation
 ------------------------------------------------------------------------- */

double PairULSPH::effective_shear_modulus(const Matrix3d d_dev, const Matrix3d deltaStressDev, const double dt, const int itype) {
	double G_eff; // effective shear modulus, see Pronto 2d eq. 3.4.7
	double deltaStressDevSum, shearRateSq, strain_increment;

	if (domain->dimension == 3) {
		deltaStressDevSum = deltaStressDev(0, 1) * deltaStressDev(0, 1) + deltaStressDev(0, 2) * deltaStressDev(0, 2)
				+ deltaStressDev(1, 2) * deltaStressDev(1, 2);
		shearRateSq = d_dev(0, 1) * d_dev(0, 1) + d_dev(0, 2) * d_dev(0, 2) + d_dev(1, 2) * d_dev(1, 2);
	} else {
		deltaStressDevSum = deltaStressDev(0, 1) * deltaStressDev(0, 1);
		shearRateSq = d_dev(0, 1) * d_dev(0, 1);
	}

	strain_increment = dt * dt * shearRateSq;

	if (strain_increment > 1.0e-12) {
		G_eff = 0.5 * sqrt(deltaStressDevSum / strain_increment);
	} else {
		if (strength[itype] != NONE) {
			G_eff = Lookup[SHEAR_MODULUS][itype];
		} else {
			G_eff = 0.0;
		}
	}

	return G_eff;

}

/* ----------------------------------------------------------------------
 hourglass force for updated Lagrangian SPH
 input: particles indices i, j, particle types ityep, jtype
 ------------------------------------------------------------------------- */

Vector3d PairULSPH::ComputeHourglassForce(const int i, const int itype, const int j, const int jtype, const Vector3d dv,
		const Vector3d xij, const Vector3d g, const double c_ij, const double mu_ij, const double rho_ij) {

	double *rmass = atom->rmass;
	Vector3d dv_est, f_hg;
	double visc_magnitude;

	dv_est = -0.5 * (F[i] + F[j]) * xij;
	double hurz = dv_est.dot(dv) / (dv_est.norm() * dv.norm() + 1.0e-16);
	if (hurz < 0.0) {

		visc_magnitude = 0.5 * (Q1[itype] + Q1[jtype]) * c_ij * mu_ij / rho_ij;
		f_hg = -rmass[i] * rmass[j] * visc_magnitude * g;
//		printf(" f_hg   = %f %f %f\n", f_hg(0), f_hg(1), f_hg(2));
//		printf("\nnegative\n");
//		printf(" dv_est = %f %f %f\n", dv_est(0), dv_est(1), dv_est(2));
//		printf(" dv     = %f %f %f\n", dv(0), dv(1), dv(2));
	} else {
		f_hg.setZero();
	}

	return f_hg;

}


// Copyright (c) 2018 The PIVX developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "masternode-budget.h"
#include "tinyformat.h"
#include "utilmoneystr.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(budget_tests)

void CheckBudgetValue(int nHeight, std::string strNetwork, CAmount nExpectedValue)
{
    CBudgetManager budget;
    CAmount nBudget = budget.GetTotalBudget(nHeight);
    std::string strError = strprintf("Budget is not as expected for %s. Result: %s, Expected: %s", strNetwork, FormatMoney(nBudget), FormatMoney(nExpectedValue));
    BOOST_CHECK_MESSAGE(nBudget == nExpectedValue, strError);
}

BOOST_AUTO_TEST_CASE(budget_value)
{
    SelectParams(CBaseChainParams::TESTNET);
    int nHeightTest = 500;
    CheckBudgetValue(nHeightTest, "testnet", 7300*COIN);

    SelectParams(CBaseChainParams::MAIN);
    nHeightTest = 0;
    CheckBudgetValue(nHeightTest, "mainnet", 0);
    nHeightTest = 50000;
    CheckBudgetValue(nHeightTest, "mainnet", 249972480*COIN);
    nHeightTest = 100000;
    CheckBudgetValue(nHeightTest, "mainnet", 4320000*COIN);
    nHeightTest = 200000;
    CheckBudgetValue(nHeightTest, "mainnet", 3888000*COIN);
    nHeightTest = 300000;
    CheckBudgetValue(nHeightTest, "mainnet", 3456000*COIN);
    nHeightTest = 400000;
    CheckBudgetValue(nHeightTest, "mainnet", 2160000*COIN);
    nHeightTest = 500000;
    CheckBudgetValue(nHeightTest, "mainnet", 1296000*COIN);
    nHeightTest = 600000;
    CheckBudgetValue(nHeightTest, "mainnet", 216000*COIN);
    nHeightTest = 1000000;
    CheckBudgetValue(nHeightTest, "mainnet", 129600*COIN);
    nHeightTest = 6000000;
    CheckBudgetValue(nHeightTest, "mainnet", 69120*COIN);
}

BOOST_AUTO_TEST_SUITE_END()


#include "bicgstab.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <iostream>
#include <unsupported/Eigen/IterativeSolvers>

namespace {
template <typename Scalar> class CustomMatrix;
}

namespace Eigen {
namespace internal {
template <typename Scalar>
struct traits<CustomMatrix<Scalar>>
    : public Eigen::internal::traits<Eigen::SparseMatrix<Scalar>> {};
} // namespace internal
} // namespace Eigen

namespace {
template <typename T>
class CustomMatrix : public Eigen::EigenBase<CustomMatrix<T>> {
public:
  using Scalar = T;
  using RealScalar = typename Eigen::SparseMatrix<Scalar>::RealScalar;
  using Index = typename Eigen::SparseMatrix<Scalar>::Index;
  using StorageIndex = typename Eigen::SparseMatrix<Scalar>::StorageIndex;
  enum {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false
  };

  auto rows() const noexcept -> Index { return _settings->dimension; }
  auto cols() const noexcept -> Index { return _settings->dimension; }

  template <typename Rhs>
  auto operator*(Eigen::MatrixBase<Rhs> const &x) const
      -> Eigen::Product<CustomMatrix<Scalar>, Rhs, Eigen::AliasFreeProduct> {
    return Eigen::Product<CustomMatrix<Scalar>, Rhs, Eigen::AliasFreeProduct>{
        *this, x.derived()};
  }

  CustomMatrix(bicgstab_hs_parameters const &settings) : _settings(&settings) {}

  bicgstab_hs_parameters const *_settings;
};

constexpr auto type_of(float const *) noexcept -> DLDataType {
  return DLDataType{kDLFloat, 32, 1};
}

constexpr auto type_of(double const *) noexcept -> DLDataType {
  return DLDataType{kDLFloat, 64, 1};
}

struct StaticTensor {
  DLTensor tensor;
  int64_t shape[1];
  int64_t strides[1];
};

template <typename Scalar, typename Index>
StaticTensor vector_as_tensor(Scalar *data, Index const size,
                              Index const stride) noexcept {
  auto t = StaticTensor{
      DLTensor{const_cast<typename std::remove_const<Scalar>::type *>(data),
               DLDevice{kDLCPU, 0}, 1, type_of(data), nullptr, nullptr, 0},
      {size},
      {stride}};
  t.tensor.shape = t.shape;
  t.tensor.strides = t.strides;
  return t;
}

template <typename Scalar>
auto tensor_as_vector(DLTensor const &t)
    -> Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Unaligned,
                  Eigen::OuterStride<>> {
  return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Unaligned,
                    Eigen::OuterStride<>>{static_cast<Scalar *>(t.data),
                                          t.shape[0],
                                          Eigen::OuterStride<>{t.strides[0]}};
}
} // namespace

// Implementation of CustomMatrix * Eigen::DenseVector though a
// specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {
template <typename Scalar, typename Rhs>
struct generic_product_impl<CustomMatrix<Scalar>, Rhs, SparseShape, DenseShape,
                            GemvProduct>
    : generic_product_impl_base<
          CustomMatrix<Scalar>, Rhs,
          generic_product_impl<CustomMatrix<Scalar>, Rhs>> {
  template <typename Dest>
  static void scaleAndAddTo(Dest &dst, const CustomMatrix<Scalar> &lhs,
                            const Rhs &rhs, const Scalar &alpha) {
    // This method should implement "dst += alpha * lhs * rhs" inplace
    auto x = vector_as_tensor(rhs.data(), rhs.size(), rhs.stride());
    auto y = vector_as_tensor(dst.data(), dst.size(), dst.stride());
    std::cerr << "Calling matrix ...\n";
    auto const status = (*lhs._settings->matrix)(&x.tensor, &y.tensor);
    if (status != 0) {
      throw std::runtime_error{"shoooot!"};
    }
    if (alpha != Scalar{1}) {
      dst *= alpha;
    }
  }
};
} // namespace internal
} // namespace Eigen

namespace {
template <typename Scalar> int solve(bicgstab_hs_parameters const *settings) {
  CustomMatrix<Scalar> matrix(*settings);
  Eigen::BiCGSTAB<CustomMatrix<Scalar>, Eigen::IdentityPreconditioner> bicg;
  std::cerr << "Calling bicg.compute ...\n";
  bicg.compute(matrix);
  bicg.setMaxIterations(settings->max_iters);
  bicg.setTolerance(static_cast<Scalar>(settings->tol));
  auto b = tensor_as_vector<Scalar>(*settings->b);
  auto x = tensor_as_vector<Scalar>(*settings->x);
  std::cerr << "b = " << b << '\n';
  if (settings->x0 != nullptr) {
    auto x0 = tensor_as_vector<Scalar>(*settings->x0);
    x.noalias() = bicg.solveWithGuess(b, x0);
  } else {
    std::cerr << "Calling bicg.solve ...\n";
    x.noalias() = bicg.solve(b);
    std::cerr << "Obtained: " << x << '\n';
  }
  std::cerr << "A * x = " << (matrix * x) << '\n';
  return 0;
}

constexpr auto operator==(DLDataType a, DLDataType b) noexcept -> bool {
  return a.code == b.code && a.bits == b.bits && a.lanes == b.lanes;
}
} // namespace

extern "C" int bicgstab_hs_solve(bicgstab_hs_parameters const *settings) {
  if (settings->b->dtype == type_of(static_cast<const float *>(nullptr))) {
    return solve<float>(settings);
  }
  if (settings->b->dtype == type_of(static_cast<const double *>(nullptr))) {
    return solve<double>(settings);
  }
  return -1;
}


/*
Transform single data point or entire point cloud from camera frame to robot base frame using Hand-Eye calibration
matrix.

This example shows how to utilize the result of Hand-Eye calibration to transform either (picking) point coordinates
or the entire point cloud from the camera frame to the robot base frame.

For both Eye-To-Hand and Eye-In-Hand, there is a Zivid gem placed approx. 500 mm away from the robot base (see below).
The (picking) point is the Zivid gem centroid, defined as image coordinates in the camera frame and hard-coded
in this code example. Open the ZDF files in Zivid Studio to inspect the gem's 2D and corresponding 3D coordinates.

Eye-To-Hand
- ZDF file: ZividGemEyeToHand.zdf
- 2D image coordinates: (1035,255)
- Corresponding 3D coordinates: (37.77 -145.92 1227.1)
- Corresponding 3D coordinates (robot base frame): (-12.4  514.37 -21.79)

Eye-In-Hand:
- ZDF file: ZividGemEyeInHand.zdf
- 2D image coordinates: (1460,755)
- Corresponding 3D coordinates (camera frame): (83.95  28.84 305.7)
- Corresponding 3D coordinates (robot base frame): (531.03  -5.44 164.6)

For verification, check that the Zivid gem centroid 3D coordinates are the same as above after the transformation.

The YAML files for this sample can be found under the main instructions for Zivid samples.
*/

#include <Zivid/Zivid.h>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <cmath>
#include <iostream>

namespace
{
    enum class Command
    {
        transformSinglePoint,
        transformPointCloud,
        unknown
    };

    enum class RobotCameraConfiguration
    {
        eyeToHand,
        eyeInHand,
        unknown
    };

    std::string getInput()
    {
        std::string command;
        std::getline(std::cin, command);
        return command;
    }

    Command enterCommand()
    {
        std::cout << "Enter command, s (to transform single point) or p (to transform point cloud): ";
        const auto command = getInput();

        if(command == "S" || command == "s")
        {
            return Command::transformSinglePoint;
        }
        if(command == "P" || command == "p")
        {
            return Command::transformPointCloud;
        }
        return Command::unknown;
    }

    RobotCameraConfiguration enterRobotCameraConfiguration()
    {
        std::cout << "Enter type of calibration, eth (for eye-to-hand) or eih (for eye-in-hand): ";
        const auto command = getInput();

        if(command == "eth" || command == "ETH")
        {
            return RobotCameraConfiguration::eyeToHand;
        }
        if(command == "eih" || command == "EIH")
        {
            return RobotCameraConfiguration::eyeInHand;
        }
        return RobotCameraConfiguration::unknown;
    }

    Eigen::MatrixXf cvToEigen(const cv::Mat &cvMat)
    {
        if(cvMat.dims != 2)
        {
            throw std::invalid_argument("Invalid matrix dimensions. Expected 2D.");
        }

        Eigen::MatrixXf eigenMat(cvMat.rows, cvMat.cols);
        for(int i = 0; i < cvMat.rows; i++)
        {
            for(int j = 0; j < cvMat.cols; j++)
            {
                eigenMat(i, j) = cvMat.at<float>(i, j);
            }
        }
        return eigenMat;
    }

    Zivid::Matrix4x4 cvToZivid(const cv::Mat &cvMat)
    {
        const auto zividMat = Zivid::Matrix4x4(cvMat.ptr<float>(0), cvMat.ptr<float>(0) + 16);
        return zividMat;
    }

    cv::Mat readTransform(const std::string &transformFile)
    {
        auto fileStorage = cv::FileStorage();

        if(!fileStorage.open(transformFile, cv::FileStorage::Mode::READ))
        {
            throw std::invalid_argument("Could not open " + transformFile);
        }
        try
        {
            const auto poseStateNode = fileStorage["PoseState"];

            if(poseStateNode.empty())
            {
                throw std::invalid_argument("PoseState not found in file " + transformFile);
            }

            const auto rows = poseStateNode.mat().rows;
            const auto cols = poseStateNode.mat().cols;
            if(rows != 4 || cols != 4)
            {
                throw std::invalid_argument(
                    "Expected 4x4 matrix in " + transformFile + ", but got " + std::to_string(cols) + "x"
                    + std::to_string(rows));
            }

            auto poseState = poseStateNode.mat();
            fileStorage.release();
            return poseState;
        }
        catch(...)
        {
            fileStorage.release();
            throw;
        }
    }
} // namespace

int main()
{
    try
    {
        Zivid::Application zivid;

        std::string fileName;
        size_t imageCoordinateX = 0;
        size_t imageCoordinateY = 0;
        cv::Mat transformBaseToCameraCV;

        bool loopContinue = true;
        while(loopContinue)
        {
            switch(enterRobotCameraConfiguration())
            {
                case RobotCameraConfiguration::eyeToHand:
                {
                    fileName = "/ZividGemEyeToHand.zdf";

                    // The (picking) point is defined as image coordinates in camera frame. It is hard-coded for the
                    // ZividGemEyeToHand.zdf (1035,255) X: 37.77 Y: -145.92 Z: 1227.1
                    imageCoordinateX = 1035;
                    imageCoordinateY = 255;

                    const auto eyeToHandTransformFile = "/EyeToHandTransform.yaml";

                    std::cout << "Reading camera pose in robot base frame (result of eye-to-hand calibration"
                              << std::endl;
                    transformBaseToCameraCV =
                        readTransform(std::string(ZIVID_SAMPLE_DATA_DIR) + eyeToHandTransformFile);

                    loopContinue = false;
                    break;
                }
                case RobotCameraConfiguration::eyeInHand:
                {
                    fileName = "/ZividGemEyeInHand.zdf";

                    // The (picking) point is defined as image coordinates in camera frame. It is hard-coded for the
                    // ZividGemEyeInHand.zdf (1460,755) X: 83.95 Y: 28.84 Z: 305.7
                    imageCoordinateX = 1357;
                    imageCoordinateY = 666;

                    const auto eyeInHandTransformFile = "/EyeInHandTransform.yaml";
                    const auto robotTransformFile = "/RobotTransform.yaml";

                    std::cout << "Reading camera pose in end-effector frame (result of eye-in-hand calibration)"
                              << std::endl;
                    const cv::Mat transformEndEffectorToCamera =
                        readTransform(std::string(ZIVID_SAMPLE_DATA_DIR) + eyeInHandTransformFile);

                    std::cout << "Reading end-effector pose in robot base frame" << std::endl;
                    const cv::Mat transformBaseToEndEffector =
                        readTransform(std::string(ZIVID_SAMPLE_DATA_DIR) + robotTransformFile);

                    std::cout << "Computing camera pose in robot base frame" << std::endl;
                    transformBaseToCameraCV = transformBaseToEndEffector * transformEndEffectorToCamera;

                    loopContinue = false;
                    break;
                }
                case RobotCameraConfiguration::unknown:
                {
                    std::cout << "Entered unknown Hand-Eye calibration type" << std::endl;
                    break;
                }
            }
        }

        const auto dataFile = std::string(ZIVID_SAMPLE_DATA_DIR) + fileName;
        std::cout << "Reading ZDF frame from file: " << dataFile << std::endl;
        const auto frame = Zivid::Frame(dataFile);
        auto pointCloud = frame.pointCloud();

        loopContinue = true;
        while(loopContinue)
        {
            switch(enterCommand())
            {
                case Command::transformSinglePoint:
                {
                    std::cout << "Transforming single point" << std::endl;

                    const auto xyz = pointCloud.copyPointsXYZW();

                    const Eigen::Vector4f pointInCameraFrame(
                        xyz(imageCoordinateY, imageCoordinateX).x,
                        xyz(imageCoordinateY, imageCoordinateX).y,
                        xyz(imageCoordinateY, imageCoordinateX).z,
                        xyz(imageCoordinateY, imageCoordinateX).w);

                    std::cout << "Point coordinates in camera frame: " << pointInCameraFrame.x() << " "
                              << pointInCameraFrame.y() << " " << pointInCameraFrame.z() << std::endl;

                    // Converting to Eigen matrix for easier computation
                    const Eigen::MatrixXf transformBaseToCamera = cvToEigen(transformBaseToCameraCV);

                    std::cout << "Transforming (picking) point from camera to robot base frame" << std::endl;
                    const Eigen::Vector4f pointInBaseFrame = transformBaseToCamera * pointInCameraFrame;

                    std::cout << "Point coordinates in robot base frame: " << pointInBaseFrame.x() << " "
                              << pointInBaseFrame.y() << " " << pointInBaseFrame.z() << std::endl;

                    loopContinue = false;
                    break;
                }
                case Command::transformPointCloud:
                {
                    std::cout << "Transforming point cloud" << std::endl;

                    const auto transformBaseToCamera = cvToZivid(transformBaseToCameraCV);
                    pointCloud.transform(transformBaseToCamera);

                    const auto saveFile = "ZividGemTransformed.zdf";
                    std::cout << "Saving frame to file: " << saveFile << std::endl;
                    frame.save(saveFile);

                    loopContinue = false;
                    break;
                }
                case Command::unknown:
                {
                    std::cout << "Entered unknown command" << std::endl;
                    break;
                }
            }
        }
    }
    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        std::cout << "Press enter to exit." << std::endl;
        std::cin.get();
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}


#ifndef RHF_HPP
#define RHF_HPP

#include <armadillo>
#include "EigenSolver.hpp"
#include "Integrals.hpp"

namespace willow { namespace qcmol {

class RHF
{
public:

  RHF(const std::vector<libint2::Atom>& atoms,
      const libint2::BasisSet& bs,
      const Integrals& ints,
      const int qm_chg = 0,
      const bool l_print = true,
      const std::vector<QAtom>& atoms_Q = std::vector<QAtom>() );

  arma::mat densityMatrix ();
  arma::mat energyWeightDensityMatrix ();

  double rhf_energy  () { return (E_nuc + E_hf);};
  double get_nocc() { return m_nocc;};
  EigenSolver eig_solver;

private:

  size_t m_nocc;
  double E_nuc;
  double E_hf;

};

} }  // namespace willow::qcmol

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>

template<typename HyperplaneType> void hyperplane(const HyperplaneType& _plane)
{
  /* this test covers the following files:
     Hyperplane.h
  */
  typedef typename HyperplaneType::Index Index;
  const Index dim = _plane.dim();
  enum { Options = HyperplaneType::Options };
  typedef typename HyperplaneType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime, 1> VectorType;
  typedef Matrix<Scalar, HyperplaneType::AmbientDimAtCompileTime,
                         HyperplaneType::AmbientDimAtCompileTime> MatrixType;

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);

  VectorType n0 = VectorType::Random(dim).normalized();
  VectorType n1 = VectorType::Random(dim).normalized();

  HyperplaneType pl0(n0, p0);
  HyperplaneType pl1(n1, p1);
  HyperplaneType pl2 = pl1;

  Scalar s0 = internal::random<Scalar>();
  Scalar s1 = internal::random<Scalar>();

  VERIFY_IS_APPROX( n1.dot(n1), Scalar(1) );

  VERIFY_IS_MUCH_SMALLER_THAN( pl0.absDistance(p0), Scalar(1) );
  VERIFY_IS_APPROX( pl1.signedDistance(p1 + n1 * s0), s0 );
  VERIFY_IS_MUCH_SMALLER_THAN( pl1.signedDistance(pl1.projection(p0)), Scalar(1) );
  VERIFY_IS_MUCH_SMALLER_THAN( pl1.absDistance(p1 +  pl1.normal().unitOrthogonal() * s1), Scalar(1) );

  // transform
  if (!NumTraits<Scalar>::IsComplex)
  {
    MatrixType rot = MatrixType::Random(dim,dim).householderQr().householderQ();
    DiagonalMatrix<Scalar,HyperplaneType::AmbientDimAtCompileTime> scaling(VectorType::Random());
    Translation<Scalar,HyperplaneType::AmbientDimAtCompileTime> translation(VectorType::Random());

    pl2 = pl1;
    VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot).absDistance(rot * p1), Scalar(1) );
    pl2 = pl1;
    VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot,Isometry).absDistance(rot * p1), Scalar(1) );
    pl2 = pl1;
    VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling).absDistance((rot*scaling) * p1), Scalar(1) );
    pl2 = pl1;
    VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*scaling*translation)
                                 .absDistance((rot*scaling*translation) * p1), Scalar(1) );
    pl2 = pl1;
    VERIFY_IS_MUCH_SMALLER_THAN( pl2.transform(rot*translation,Isometry)
                                 .absDistance((rot*translation) * p1), Scalar(1) );
  }

  // casting
  const int Dim = HyperplaneType::AmbientDimAtCompileTime;
  typedef typename GetDifferentType<Scalar>::type OtherScalar;
  Hyperplane<OtherScalar,Dim,Options> hp1f = pl1.template cast<OtherScalar>();
  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),pl1);
  Hyperplane<Scalar,Dim,Options> hp1d = pl1.template cast<Scalar>();
  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),pl1);
}

template<typename Scalar> void lines()
{
  typedef Hyperplane<Scalar, 2> HLine;
  typedef ParametrizedLine<Scalar, 2> PLine;
  typedef Matrix<Scalar,2,1> Vector;
  typedef Matrix<Scalar,3,1> CoeffsType;

  for(int i = 0; i < 10; i++)
  {
    Vector center = Vector::Random();
    Vector u = Vector::Random();
    Vector v = Vector::Random();
    Scalar a = internal::random<Scalar>();
    while (internal::abs(a-1) < 1e-4) a = internal::random<Scalar>();
    while (u.norm() < 1e-4) u = Vector::Random();
    while (v.norm() < 1e-4) v = Vector::Random();

    HLine line_u = HLine::Through(center + u, center + a*u);
    HLine line_v = HLine::Through(center + v, center + a*v);

    // the line equations should be normalized so that a^2+b^2=1
    VERIFY_IS_APPROX(line_u.normal().norm(), Scalar(1));
    VERIFY_IS_APPROX(line_v.normal().norm(), Scalar(1));

    Vector result = line_u.intersection(line_v);

    // the lines should intersect at the point we called "center"
    VERIFY_IS_APPROX(result, center);

    // check conversions between two types of lines
    PLine pl(line_u); // gcc 3.3 will commit suicide if we don't name this variable
    CoeffsType converted_coeffs = HLine(pl).coeffs();
    converted_coeffs *= (line_u.coeffs()[0])/(converted_coeffs[0]);
    VERIFY(line_u.coeffs().isApprox(converted_coeffs));
  }
}

template<typename Scalar> void hyperplane_alignment()
{
  typedef Hyperplane<Scalar,3,AutoAlign> Plane3a;
  typedef Hyperplane<Scalar,3,DontAlign> Plane3u;

  EIGEN_ALIGN16 Scalar array1[4];
  EIGEN_ALIGN16 Scalar array2[4];
  EIGEN_ALIGN16 Scalar array3[4+1];
  Scalar* array3u = array3+1;

  Plane3a *p1 = ::new(reinterpret_cast<void*>(array1)) Plane3a;
  Plane3u *p2 = ::new(reinterpret_cast<void*>(array2)) Plane3u;
  Plane3u *p3 = ::new(reinterpret_cast<void*>(array3u)) Plane3u;

  p1->coeffs().setRandom();
  *p2 = *p1;
  *p3 = *p1;

  VERIFY_IS_APPROX(p1->coeffs(), p2->coeffs());
  VERIFY_IS_APPROX(p1->coeffs(), p3->coeffs());

  #if defined(EIGEN_VECTORIZE) && EIGEN_ALIGN_STATICALLY
  if(internal::packet_traits<Scalar>::Vectorizable)
    VERIFY_RAISES_ASSERT((::new(reinterpret_cast<void*>(array3u)) Plane3a));
  #endif
}

void test_geo_hyperplane()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( hyperplane(Hyperplane<float,2>()) );
    CALL_SUBTEST_2( hyperplane(Hyperplane<float,3>()) );
    CALL_SUBTEST_2( hyperplane(Hyperplane<float,3,DontAlign>()) );
    CALL_SUBTEST_2( hyperplane_alignment<float>() );
    CALL_SUBTEST_3( hyperplane(Hyperplane<double,4>()) );
    CALL_SUBTEST_4( hyperplane(Hyperplane<std::complex<double>,5>()) );
    CALL_SUBTEST_1( lines<float>() );
    CALL_SUBTEST_3( lines<double>() );
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file DecisionTreeFactor.cpp
 * @brief discrete factor
 * @date Feb 14, 2011
 * @author Duy-Nguyen Ta
 * @author Frank Dellaert
 */

#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/base/FastSet.h>

#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

using namespace std;

namespace gtsam {

  /* ******************************************************************************** */
  DecisionTreeFactor::DecisionTreeFactor() {
  }

  /* ******************************************************************************** */
  DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys,
      const ADT& potentials) :
      DiscreteFactor(keys.indices()), Potentials(keys, potentials) {
  }

  /* *************************************************************************/
  DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) :
      DiscreteFactor(c.keys()), Potentials(c) {
  }

  /* ************************************************************************* */
  bool DecisionTreeFactor::equals(const DiscreteFactor& other, double tol) const {
    if(!dynamic_cast<const DecisionTreeFactor*>(&other)) {
      return false;
    }
    else {
      const DecisionTreeFactor& f(static_cast<const DecisionTreeFactor&>(other));
      return Potentials::equals(f, tol);
    }
  }

  /* ************************************************************************* */
  void DecisionTreeFactor::print(const string& s,
      const KeyFormatter& formatter) const {
    cout << s;
    Potentials::print("Potentials:",formatter);
  }

  /* ************************************************************************* */
  DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
    ADT::Binary op) const {
    map<Key,size_t> cs; // new cardinalities
    // make unique key-cardinality map
    BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j);
    BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j);
    // Convert map into keys
    DiscreteKeys keys;
    BOOST_FOREACH(const DiscreteKey& key, cs)
      keys.push_back(key);
    // apply operand
    ADT result = ADT::apply(f, op);
    // Make a new factor
    return DecisionTreeFactor(keys, result);
  }

  /* ************************************************************************* */
  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals,
    ADT::Binary op) const {

    if (nrFrontals > size()) throw invalid_argument(
        (boost::format(
            "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d")
            % nrFrontals % size()).str());

    // sum over nrFrontals keys
    size_t i;
    ADT result(*this);
    for (i = 0; i < nrFrontals; i++) {
      Key j = keys()[i];
      result = result.combine(j, cardinality(j), op);
    }

    // create new factor, note we start keys after nrFrontals
    DiscreteKeys dkeys;
    for (; i < keys().size(); i++) {
      Key j = keys()[i];
      dkeys.push_back(DiscreteKey(j,cardinality(j)));
    }
    return boost::make_shared<DecisionTreeFactor>(dkeys, result);
  }

  /* ************************************************************************* */
  DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(const Ordering& frontalKeys,
    ADT::Binary op) const {

    if (frontalKeys.size() > size()) throw invalid_argument(
        (boost::format(
            "DecisionTreeFactor::combine: invalid number of frontal keys %d, nr.keys=%d")
            % frontalKeys.size() % size()).str());

    // sum over nrFrontals keys
    size_t i;
    ADT result(*this);
    for (i = 0; i < frontalKeys.size(); i++) {
      Key j = frontalKeys[i];
      result = result.combine(j, cardinality(j), op);
    }

    // create new factor, note we collect keys that are not in frontalKeys
    // TODO: why do we need this??? result should contain correct keys!!!
    DiscreteKeys dkeys;
    for (i = 0; i < keys().size(); i++) {
      Key j = keys()[i];
      // TODO: inefficient!
      if (std::find(frontalKeys.begin(), frontalKeys.end(), j) != frontalKeys.end())
        continue;
      dkeys.push_back(DiscreteKey(j,cardinality(j)));
    }
    return boost::make_shared<DecisionTreeFactor>(dkeys, result);
  }

/* ************************************************************************* */
} // namespace gtsam


/*

 * ex10.cpp

 *

 *

 *  Author: Daniel Rehfeldt

 *

 *  Compile: g++ ex10.cpp -o ex10 -O3 -march=native -std=c++14 -lboost_timer  -lboost_system  -pedantic -Wall
 *
 *  Compile with OpenMP:  g++ ex10.cpp -o ex10 -O3 -march=native -std=c++14 -lboost_timer  -lboost_system  -pedantic -Wall -fopenmp
 *
 *
 */

#define NDEBUG
#define SEQHEAP

#include <limits>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <cassert>
#include <memory>
#include <string>
#include <array>
#include <algorithm>
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>
#ifdef SEQHEAP
#include "knheap.C"
#else
#include "binheap.h"
#endif
#ifdef _OPENMP
#include <omp.h>
#endif

using Weight = long long;

struct IntInt
{
      int index;
      int prededge;
};

class Graph{

    public:
        int* deg = nullptr;
        int* term = nullptr;
        int* head = nullptr;
        int* tail = nullptr;
        int* headcprs = nullptr;
        int* tailcprs = nullptr;
        int* startcprs = nullptr;
        int* weightcprs = nullptr;
        int* orgedgecprs = nullptr;
        Weight* edgeweight = nullptr;
        Weight* edgeweightcprs = nullptr;

        int nterms = -1;
        int nnodes = -1;
        int nedges = -1;
        int nmaxedges = -1;

        Graph(int nodes, int maxedges)
        {
            assert(nodes >= 1);
            assert(maxedges >= 0);
            nedges = 0;
            nterms = 0;
            nnodes = nodes;
            nmaxedges = maxedges;

            if( maxedges > 0 )
            {
               // initialize with 0
               deg = new int[nnodes]();
               term = new int[nnodes]();

               head = new int[maxedges];
               tail = new int[maxedges];
               edgeweight = new Weight[maxedges];
            }
        }

        bool isTerm(int i) const;

        void makeTerm(int i);

        void addEdge(int etail, int ehead, Weight eweight);

        // initialize compressed data structures
        void initCompressed();

        // check whether compression was successful
        bool compressedValid() const;

        bool isConnected() const;

        ~Graph()
        {
           delete[] orgedgecprs;
           delete[] weightcprs;
           delete[] edgeweightcprs;
           delete[] headcprs;
           delete[] tailcprs;
           delete[] startcprs;
           delete[] edgeweight;
           delete[] deg;
           delete[] head;
           delete[] tail;
        }
};

bool Graph::isConnected() const
{
     assert(startcprs);
     assert(headcprs);

     bool* visited = new bool[nnodes];
     int* stack = new int[nnodes];

     int size = 0;

     std::fill_n(visited, nnodes, false);

     stack[size++] = 0;
     visited[0] = true;

     int nodecount = 0;

     // DFS
     while( size )
     {
        const int k = stack[--size];
        nodecount++;

        //traverse adjacent edges of k
        const int end = startcprs[k + 1];
        for( int e = startcprs[k]; e != end; e++ )
        {
            const int head = headcprs[e];
            if( !visited[head] )
            {
               visited[head] = true;
               stack[size++] = head;
            }
        }
     }

     delete[] visited;
     delete[] stack;

     if( nodecount != nnodes )
         return false;

     return true;
}

inline bool Graph::isTerm(
     int i
) const
{
   assert(i >= 0 && i < nnodes);
   assert(term);

   return (term[i] != 0);
}

inline void Graph::makeTerm(
    int i
)
{
   assert(i >= 0 && i < nnodes);
   assert(term);

   if( !term[i] )
   {
      nterms++;
      term[i] = 1;
   }
}

inline void Graph::addEdge(
    int etail,
    int ehead,
    Weight eweight
)
{
    assert(ehead >= 0 && ehead < nnodes );
    assert(etail >= 0 && etail < nnodes );
    assert(nedges < nmaxedges);
    assert(eweight >= Weight(0));

    head[nedges] = ehead;
    tail[nedges] = etail;
    edgeweight[nedges++] = eweight;
    deg[ehead]++;
    deg[etail]++;
}

void Graph::initCompressed()
{
    tailcprs = new int[2 * nedges];
    headcprs = new int[2 * nedges];
    orgedgecprs = new int[2 * nedges];
    startcprs = new int[nnodes + 1];
    edgeweightcprs = new Weight[2 * nedges];

    startcprs[0] = deg[0];

    for( int i = 1; i < nnodes; i++ )
        startcprs[i] = startcprs[i - 1] + deg[i];

    assert(startcprs[nnodes - 1] == 2 * nedges);

    startcprs[nnodes] = 2 * nedges;

    auto update_cprs = [this](int node, int adjnode, int edge)
    {
        const int e = --startcprs[node];
        assert(e >= 0);
        assert(node == 0 || e >= startcprs[node - 1]);
        edgeweightcprs[e] = edgeweight[edge];
        tailcprs[e] = node;
        headcprs[e] = adjnode;
        orgedgecprs[e] = edge;
    };

    for( int e = 0; e < nedges; e++ )
    {
        update_cprs(tail[e], head[e], e);
        update_cprs(head[e], tail[e], e);
    }

    assert(startcprs[nnodes - 1] + deg[nnodes - 1] == 2 * nedges);
}

bool Graph::compressedValid() const
{
   if( !startcprs || !edgeweightcprs || !tailcprs || !headcprs || !orgedgecprs )
      return false;

   auto is_adjacent = [this](int node, int nodeadj)
   {
      for( int e = startcprs[node], end = startcprs[node + 1]; e != end; e++ )
         if( tailcprs[e] != node )
            return false;

      for( int e = startcprs[node], end = startcprs[node + 1]; e != end; e++ )
         if( headcprs[e] == nodeadj )
            return true;

      return false;
   };

   for( int e = 0; e < nedges; e++ )
       if( !is_adjacent(tail[e], head[e]) || !is_adjacent(head[e], tail[e]) )
          return false;

   return true;
}

// fills array that marks whether edge i is part of the tree (steineredge[i] = true) or not (steineredge[i] = false)
static std::unique_ptr<bool[]> computeSteinerTree(
    const Graph& graph,
    const int root
)
{
    const int nnodes = graph.nnodes;
    const int nedges = graph.nedges;
    const int nterms = graph.nterms;

    assert(nnodes >= 1);
    assert(nedges >= 1);
    assert(nterms >= 1);
    assert(root >= 0 && root < nnodes);

    auto steineredge = std::make_unique<bool[]>(nedges);

    if( nterms == 1 && graph.isTerm(root) )
       return move(steineredge);

    const int* orgarr = graph.orgedgecprs;
    const int* tailarr = graph.tailcprs;
    const int* headarr = graph.headcprs;
    const int* startarr = graph.startcprs;
    const Weight* weightarr = graph.edgeweightcprs;

    assert(orgarr);
    assert(tailarr);
    assert(startarr);
    assert(headarr);
    assert(weightarr);

    // predecessor and distance array to store incoming edge and distance for each node todo unique pointer
    bool* steinernode = new bool[nnodes];
    int* predarr = new int[nnodes];
    Weight* distarr = new Weight[nnodes];

    // initialize array for Dijkstra calculations
    std::fill_n(distarr, nnodes, std::numeric_limits<Weight>::max());
    std::fill_n(predarr, nnodes, -1);
    std::fill_n(steinernode, nnodes, false);

#ifdef SEQHEAP
    KNHeap<Weight, IntInt> heap(std::numeric_limits<Weight>::max(), std::numeric_limits<Weight>::min());
#else
    BinHeap<Weight, IntInt> heap(2 * nedges);
#endif

    assert(heap.getSize() == 0);

    IntInt dummy;
    dummy.index = root;
    dummy.prededge  = -1;
    heap.insert(Weight(0), dummy);

    distarr[root] = Weight(0);
    steinernode[root] = true;

    int termcount = 0;
    if( graph.isTerm(root) )
       termcount++;

    while( heap.getSize() != 0 )
    {
        Weight distk;

#ifdef SEQHEAP
        heap.deleteMin(&distk, &dummy);
#else
        heap.deleteMin(distk, dummy);
#endif

        assert(distk >= Weight(0));

        const int node = dummy.index;

        assert(node >= 0);

        if( distarr[node] >= distk )
        {
           distarr[node] = distk;
           predarr[node] = dummy.prededge;

           if( graph.isTerm(node) && !steinernode[node] )
           {
              int k = node;

              // add path to current tree
              do
              {
                 int pred;
                 steinernode[k] = true;
                 pred = predarr[k];

                 assert(pred >= 0);
                 assert(headarr[pred] == k);

                 steineredge[orgarr[pred]] = true;
                 dummy.index = k;
                 dummy.prededge = pred;

                 distarr[k] = Weight(0);

                 heap.insert(Weight(0), dummy);

                 k = tailarr[pred];

              }
              while( !steinernode[k] );

              // all terminals connected?
              if( ++termcount == nterms )
                 break;
           }

           // traverse adjacent edges of node
           const int end = startarr[node + 1];
           for( int e = startarr[node]; e != end; e++ )
           {
               const int head = headarr[e];

               if( distarr[head] > distk + weightarr[e] )
               {
                  // update
                  distarr[head] = distk + weightarr[e];
                  dummy.index = head;
                  dummy.prededge = e;
                  heap.insert(distarr[head], dummy);
                  //heap.insert(distarr[head], e);
               }
           }
        }
    }

    delete[] steinernode;
    delete[] distarr;
    delete[] predarr;

    return move(steineredge);
}

static bool isTreeValid(
      const Graph& graph,
      bool* steinertree,
      const int root
)
{
   assert(steinertree);

   const int nnodes = graph.nnodes;
   const int nedges = graph.nedges;
   const int nterms = graph.nterms;
   const int* orgarr = graph.orgedgecprs;
   const int* headarr = graph.headcprs;
   const int* startarr = graph.startcprs;

   assert(orgarr);
   assert(startarr);
   assert(headarr);

   bool* visited = new bool[nnodes];
   int* stack = new int[nnodes];

   int size = 0;
   int nsolnodes = 0;
   int termcount = 0;

   std::fill_n(visited, nnodes, false);

   stack[size++] = root;
   visited[root] = true;

   // DFS
   while( size )
   {
      const int k = stack[--size];

      nsolnodes++;

      if( graph.isTerm(k) )
         termcount++;

      //traverse adjacent edges of k
      const int end = startarr[k + 1];
      for( int e = startarr[k]; e != end; e++ )
      {
          const int head = headarr[e];
          if( !visited[head] && steinertree[orgarr[e]] )
          {
             visited[head] = true;
             stack[size++] = head;
          }
      }
   }

   delete[] visited;
   delete[] stack;

   int nsoledges = 0;

   for( int e = 0; e < nedges; e++ )
      if( steinertree[e] )
         nsoledges++;

   if( nsoledges != nsolnodes - 1 )
   {
       std::cout << "not a tree "  << std::endl;
       return false;
   }

   if( termcount != nterms )
   {
       std::cout << "not all terminals reached " << termcount << " < " << nterms <<  std::endl;
       return false;
   }

   return true;
}

// adds prime vertices as terminals
static void addTermsPrime(
      Graph& graph
)
{
   const int nnodes = graph.nnodes;

   // Sieve's prime algorithm

   bool* primecands = new bool[nnodes + 1];

   std::fill_n(primecands, nnodes + 1, true);

   const int sqrn = sqrt(nnodes);

   for( int k = 4; k <= nnodes; k += 2 )
      primecands[k] = false;

   for( int i = 3; i <= sqrn; i += 2 )
      if( primecands[i] )
      {
         const int currprime = i;

         for( int k = currprime * currprime; k <= nnodes; k += 2 * currprime )
            primecands[k] = false;
      }

   if( nnodes >= 2 )
      graph.makeTerm(1);

   for( int i = 3; i <= nnodes; i += 2 )
      if( primecands[i] )
         graph.makeTerm(i - 1);

   delete[] primecands;

}

static Graph loadGraph(
   const char* filename
)
{
    std::ifstream file (filename, std::ifstream::in);

    if( file.fail() )
    {
       std::cerr << "file could not be opened" << std::endl;
       exit(EXIT_FAILURE);
    }

    int nnodes;
    int nedges;

    if( !(file >> nnodes >> nedges) || nedges <= 0 || nnodes <= 1 )
    {
        std::cerr << "file syntax error" << std::endl;
        exit(EXIT_FAILURE);
    }

    Graph graph(nnodes, nedges);

    std::string strline;

    using namespace boost::spirit;
    using qi::int_;
    using qi::phrase_parse;
    using ascii::space;

    int etail, ehead;
    Weight eweight;

    while( std::getline(file, strline) )
    {
        auto it = strline.begin();

        bool success = phrase_parse(it, strline.end(),
                 int_[([&etail](int i){ etail = i; })] >> int_[([&ehead](int i){ ehead = i; })] >> int_[([&eweight](Weight i){ eweight = Weight(i); })]
                          , space);

        if( success && it == strline.end() )
           graph.addEdge(etail - 1, ehead - 1, eweight);
    }

    file.close();

    std::cout << "loaded graph;" << " nodes: " << nnodes << ", edges: " << nedges  << " ... ";

    return graph;
}

int main(
    int argc,
    char* argv[]
)
{
    // check number of parameters
    if( argc != 3 )
    {
       // tell the user how to run the program
       std::cerr << "Usage: " << argv[0] << " filename " << "number_of_starting_nodes"  << std::endl;
       exit(EXIT_FAILURE);
    }

    const int nstarts = std::stoi(argv[2]);

    if( nstarts <= 0 )
    {
       std::cout << "number of starting nodes should be >= 1" << std::endl;
       exit(EXIT_FAILURE);
    }

    Graph graph = loadGraph(argv[1]);
    addTermsPrime(graph);

    std::cout << "terminals: " << graph.nterms << std::endl;

    graph.initCompressed();
    assert(graph.compressedValid());

    if( !graph.isConnected() )
    {
       std::cout << "graph unconnected" << std::endl;
       exit(EXIT_FAILURE);
    }

    const int nedges = graph.nedges;
    const int nruns = std::min(nstarts, graph.nterms);

    bool* incumbent(new bool[nedges]);
    int* startterms(new int[graph.nnodes]);

    for( int i = 0, t = 0; t < nruns; i++ )
       if( graph.isTerm(i) )
          startterms[t++] = i;

    int bestroot = -1;
    Weight bestobj = std::numeric_limits<Weight>::max();

    using namespace boost::timer;
    cpu_timer timer;

    // run heuristic from first nruns many terminals (prime nodes)
    #pragma omp parallel for
    for( int r = 0; r < nruns; r++ )
    {
       const int root = startterms[r];

       assert(graph.isTerm(root));

       std::unique_ptr<bool[]> steinertree = computeSteinerTree(graph, root);

       assert(isTreeValid(graph, steinertree, root));

       Weight obj = Weight(0);
       for( int e = 0; e < nedges; e++ )
          if( steinertree[e] )
             obj += graph.edgeweight[e];

#ifdef _OPENMP
#pragma omp critical
       {
          int t = omp_get_thread_num();
          int tcount = omp_get_num_threads();
          std::cout << "thread " << t  << " of " << tcount << " finished" << std::endl;
       }
#endif

       #pragma omp critical
       if( obj < bestobj )
       {
          bestobj = obj;
          bestroot = root;
          for( int i = 0; i < nedges; i++ )
             incumbent[i] = steinertree[i];
       }
    }

    cpu_times times = timer.elapsed();

    delete[] startterms;

    assert(bestroot >= 0);

    if( !isTreeValid(graph, incumbent, bestroot) )
       std::cout << "FAILED WITH FINAL TREE" << std::endl;
    else
       std::cout << "FINAL TREE OK!" << std::endl;

    delete[] incumbent;
    assert(bestobj < std::numeric_limits<Weight>::max());

    std::cout << "TLEN: " << bestobj << std::endl;
    std::cout << "TIME: "  << times.user / 1e9 << " s" << std::endl;
    std::cout << "WALL: "  << times.wall / 1e9 << " s" << std::endl;

    return EXIT_SUCCESS;
}


#ifndef NETKET_COMMON_TYPES_HPP
#define NETKET_COMMON_TYPES_HPP
/**
 * This header contains standard type aliases to be used throughout the NetKet
 * codebase.
 */

#include <complex>
#include <cstddef>

#include <Eigen/Core>

namespace netket {

using Index = std::ptrdiff_t;
using Complex = std::complex<double>;

using VectorXd = Eigen::VectorXd;
using VectorXcd = Eigen::VectorXcd;

template <class T>
using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

using MatrixXd = Matrix<double>;
using MatrixXcd = Matrix<Complex>;

template <class T>
using RowMatrix =
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

using RowMatrixXd = RowMatrix<double>;
using RowMatrixXcd = RowMatrix<Complex>;

}  // namespace netket

#endif  // NETKET_COMMON_TYPES_HPP


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <stdexcept>
#include <vector>

#include <evaluation/splitgenerators/RandomPermutationAndDivisionSplitGenerator.h>
using namespace evaluation;

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_RandomPermutationAndDivisionSplitGenerator)

BOOST_AUTO_TEST_CASE(constructor_test)
{
  const unsigned int seed = 12345;
  const float ratio = 0.5f;

  BOOST_CHECK_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 0, ratio), std::runtime_error);
  BOOST_CHECK_NO_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 1, ratio));

  BOOST_CHECK_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 1, 0.0f), std::runtime_error);
  BOOST_CHECK_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 1, 1.0f), std::runtime_error);
  BOOST_CHECK_NO_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 1, 0.1f));
  BOOST_CHECK_NO_THROW(RandomPermutationAndDivisionSplitGenerator(seed, 1, 0.9f));
}

BOOST_AUTO_TEST_CASE(generate_splits_test)
{
  const size_t exampleCount = 20;
  const size_t splitCount = 5;
  const float ratio = 0.75f;

  RandomPermutationAndDivisionSplitGenerator gen(12345, splitCount, ratio);
  std::vector<SplitGenerator::Split> splits = gen.generate_splits(exampleCount);

  // Check the sizes of the split halves.
  BOOST_REQUIRE_EQUAL(splits.size(), splitCount);
  for(size_t i = 0; i < splitCount; ++i)
  {
    BOOST_CHECK_EQUAL(splits[i].first.size(), exampleCount * ratio);
    BOOST_CHECK_EQUAL(splits[i].second.size(), exampleCount * (1.0f - ratio));
  }
}

BOOST_AUTO_TEST_SUITE_END()