/*
BSD 3-Clause License

Copyright (c) 2020, Mihai Francu
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "FemPhysicsMatrixFree.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>
#include <Engine/Profiler.h>
#include "PolarDecomposition.h"
#include "ElasticEnergy.h"
#include "LinearSolver.h"
#include "NewtonSolver.h"
#include "NonlinearCG.h"
#include <iostream>

#pragma warning( disable : 4267) // for size_t-uint conversions

// Bibliography:
// [Sifakis] Sifakis, E., The classical FEM method and discretization methodology, Siggraph course, 2012
// [Teran03] Teran, J. et al., Finite Volume Method for the Simulation of Skeletal Muscle, Eurographics, 2003
// [Teran03] Teran, J. et al., Robust Quasistatic Finite Elements and Flesh Simulation, Eurographics, 2005
// [Mueller] Mueller, M. et al., Real Time Physics Class Notes, Chapter 4 - The finite element method, Siggraph course, 2008
// [Bonet] Bonet, J., Wood, R.D., Nonlinear Continuum Mechanics for Finite Element Analysis
// [Erleben] Erleben, K. et al., Physics-based animation

// We are using explicit FEM: lumped masses (particle masses) and lumped body forces (particle forces)
// Also, it is geometrically linear FEM: all elements are linear displacement/constant strain tetrahedra
// P or PK1 is the first Piola-Kirchoff tensor
// S or PK2 is the second Piola-Kirchoff tensor

//#define CACHED_STIFFNESS_MATRIX

namespace FEM_SYSTEM
{
	// node indices for each face
	int faces[4][3] = {
			{ 1, 2, 3 },
			{ 0, 2, 3 },
			{ 0, 1, 3 },
			{ 0, 1, 2 } };

	FemPhysicsMatrixFree::FemPhysicsMatrixFree(const std::vector<Tet>& tetrahedra,
		const std::vector<Node>& allNodes, const FemConfig& config)
		: FemPhysicsBase(config)
		, nodes(allNodes)
		, hasCollisions(false)
	{
		tets.resize(tetrahedra.size());
		for (size_t i = 0; i < tets.size(); i++)
		{
			Tetrahedron& tet = tets.at(i);

			tet.i[0] = tetrahedra[i].idx[0];
			tet.i[1] = tetrahedra[i].idx[1];
			tet.i[2] = tetrahedra[i].idx[2];
			tet.i[3] = tetrahedra[i].idx[3];
		}

#ifndef USE_CONSTRAINT_BCS
		ReshuffleFixedNodes();
#else
		mNumBCs = 0;
		mReshuffleMap.resize(nodes.size());
		for (uint32 i = 0; i < nodes.size(); i++)
		{
			mReshuffleMap[i] = i;
			if (nodes[i].invMass == 0)
			{
				nodes[i].invMass = 1;
				AddDirichletBC(i, AXIS_X | AXIS_Y | AXIS_Z);
			}
		}
#endif

		if (config.mCustomConfig != nullptr)
		{
			const Config* cfg = (Config*)config.mCustomConfig;
			mConfig = *cfg;
		}

		// damping params (partial Rayleigh damping: we reuse the matrix structure from the stiffness matrix)
		// TODO: use Rayleigh damping params instead
		ed = config.mDampingYoungsModulus;
		nud = config.mDampingPoissonRatio;
		real omnd = 1.f - nud;
		real om2nd = 1.f - 2 * nud;
		real fd = ed / (1.f + nud) / om2nd;
		Ed = fd * Matrix3R(omnd, nud, nud,
			nud, omnd, nud,
			nud, nud, omnd);

		// prepare the vector of lumped masses
		std::vector<real> masses(nodes.size(), 0.f);

		// init the test and shape matrices
		mTotalInitialVol = 0;
		for (size_t i = 0; i < tets.size(); i++)
		{
			Tetrahedron& tet = tets.at(i);
			const Vector3R& x0 = nodes.at(tet.i[0]).pos0;
			const Vector3R& x1 = nodes.at(tet.i[1]).pos0;
			const Vector3R& x2 = nodes.at(tet.i[2]).pos0;
			const Vector3R& x3 = nodes.at(tet.i[3]).pos0;
			Vector3R d1 = x1 - x0;
			Vector3R d2 = x2 - x0;
			Vector3R d3 = x3 - x0;
			Matrix3R mat(d1, d2, d3); // this is the reference shape matrix Dm [Sifakis][Teran03]
			tet.X = mat.GetInverse(); // Dm^-1
			tet.Xtr = !tet.X; // Dm^-T; pre-stored but could not be for saving memory
			tet.vol = (mat.Determinant()) / 6.f; // signed volume of the tet
			mTotalInitialVol += tet.vol;

			real lumpedMass = 0.25f * tet.vol * config.mDensity;
			masses[tet.i[0]] += lumpedMass;
			masses[tet.i[1]] += lumpedMass;
			masses[tet.i[2]] += lumpedMass;
			masses[tet.i[3]] += lumpedMass;

			// compute face areas
			Vector3R b[4];
			for (int j = 0; j < 4; j++)
			{
				int i1 = faces[j][0];
				int i2 = faces[j][1];
				int i3 = faces[j][2];

				int j1 = tet.i[i1];
				int j2 = tet.i[i2];
				int j3 = tet.i[i3];
				int j4 = tet.i[j];

				tet.NA[j] = cross(nodes[j2].pos - nodes[j1].pos, nodes[j3].pos - nodes[j1].pos);
				if (dot(tet.NA[j], nodes[j4].pos - nodes[j1].pos) > 0)
					tet.NA[j].Flip();

				Vector3R splitFaceForce = (-1.f / 6.f) * tet.NA[j];
				b[i1] += splitFaceForce;
				b[i2] += splitFaceForce;
				b[i3] += splitFaceForce;
			}
			//tet.Bm = Matrix3R(b[1], b[2], b[3]);

			// compute the gradient of the shape functions [Erleben]
			Vector3R y[4];
			y[1] = tet.X[0];
			y[2] = tet.X[1];
			y[3] = tet.X[2];
			y[0] = y[1] + y[2] + y[3];
			y[0].Flip();

			// compute the Cauchy strain Jacobian matrix according to [Mueller]: H = de/dx
			for (int j = 0; j < 4; j++)
			{
				tet.Hn[j] = Matrix3R(y[j]);
				tet.Hs[j] = Matrix3R(0, y[j].Z(), y[j].Y(),
					y[j].Z(), 0, y[j].X(),
					y[j].Y(), y[j].X(), 0);
			}

			// for linear FEM we can actually precompute the tangent stiffness matrix
			real s = mYoungsModulus / (1.f + mPoissonRatio);
			for (int j = 0; j < 4; j++)
			{
				for (int k = 0; k < 4; k++)
				{
					tet.K[j][k] = tet.vol * (tet.Hn[j] * mNormalElasticityMatrix * tet.Hn[k] + s * tet.Hs[j] * (!tet.Hs[k]));
				}
			}
		}

		// set masses
		for (size_t i = 0; i < nodes.size(); i++)
		{
			if (nodes[i].invMass != 0 && masses[i] != 0)
				nodes[i].invMass = 1.f / masses[i];
		}

		lambdaAcc.resize(tets.size());
		dH.resize(tets.size());

		mForces.resize(nodes.size());

		BuildMassMatrix();
	}

	void FemPhysicsMatrixFree::BuildMassMatrix()
	{
		// build lumped mass matrix
		uint32 numDofs = GetNumFreeNodes() * 3;
		mMassMatrix.resize(numDofs, numDofs);
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			real mass = 1.f / nodes[i + mNumBCs].invMass;
			mMassMatrix.coeffRef(i * 3, i * 3) = mass;
			mMassMatrix.coeffRef(i * 3 + 1, i * 3 + 1) = mass;
			mMassMatrix.coeffRef(i * 3 + 2, i * 3 + 2) = mass;
		}
	}

	void FemPhysicsMatrixFree::ReshuffleFixedNodes()
	{
		std::vector<Node> newNodes;
		mReshuffleMap.resize(nodes.size()); // map from old indices to new ones
		// add fixed nodes first
		mNumBCs = 0;
		for (size_t i = 0; i < nodes.size(); i++)
		{
			if (nodes[i].invMass == 0)
			{
				mReshuffleMap[i] = newNodes.size();
				newNodes.push_back(nodes[i]);
				mNumBCs++;
			}
		}
		// then the other nodes
		for (size_t i = 0; i < nodes.size(); i++)
		{
			if (nodes[i].invMass != 0)
			{
				mReshuffleMap[i] = newNodes.size();
				newNodes.push_back(nodes[i]);
			}
		}
		nodes = newNodes; // replace old nodes with shuffled ones
		// remap tets
		originalTets = tets;
		for (size_t i = 0; i < tets.size(); i++)
		{
			for (int j = 0; j < 4; j++)
			{
				tets[i].i[j] = mReshuffleMap[tets[i].i[j]];
			}
		}
	}

	void FemPhysicsMatrixFree::AddCable(const Cable& cable)
	{
		FemPhysicsBase::AddCable(cable);
		// add new DOF nodes for the unattached spring nodes
		Cable& currCable = mCables[mCables.size() - 1];
		mNumSpringNodes = 0;
		for (uint32 i = 0; i < cable.mCableNodes.size(); i++)
		{
			int elem = cable.mCableNodes[i].elem;
			if (elem < 0)
			{
				const Tetrahedron& tet = tets[-elem];
				real w0 = currCable.mCableNodes[i].bary.x;
				real w1 = currCable.mCableNodes[i].bary.y;
				real w2 = currCable.mCableNodes[i].bary.z;
				real w3 = 1 - w0 - w1 - w2;
				real invMass = w0 * nodes[tet.i[0]].invMass + w1 * nodes[tet.i[1]].invMass + w1 * nodes[tet.i[1]].invMass + w3 * nodes[tet.i[3]].invMass;

				currCable.mCableNodes[i].elem = -((int)nodes.size());
				Node node;
				node.invMass = invMass; // TODO: average node mass
				node.pos = currCable.mCablePositions[i];
				mReshuffleMap.push_back(nodes.size());
				nodes.push_back(node);
				mNumSpringNodes++;
			}
		}
		// rebuild the mass matrix
		BuildMassMatrix();
		mForces.resize(nodes.size());
	}

	void FemPhysicsMatrixFree::Step(real dt)
	{
		if (mSimType == ST_IMPLICIT)
		{
			// save positions before step - breaks dynamic Dirichlet BCs!
			for (size_t i = 0; i < GetNumNodes(); i++)
			{
				nodes[i].pos0 = nodes[i].pos;
			}

			real h = dt / mNumSteps;
			for (int i = 0; i < mNumSteps; i++)
			{
				mTimeStep = h;
				mForceFraction = 1;
				Solve();
				HandleCollisions(h);
			}
		}
		else if (mSimType == ST_EXPLICIT)
		{
			real h = dt / mNumSteps;
			mForceFraction = 1;
			for (int i = 0; i < mNumSteps; i++)
			{
				SubStep(h);
			}
		}
		else if (mSimType == ST_STATIC)
		{
			if (mForceFraction == 0)
			{
				mForceFraction = 1;
				Solve();
			}
		}
		else if (mSimType == ST_QUASI_STATIC)
		{
			if (mForceFraction < 1)
			{
				mForceFraction = std::min(real(1), mForceFraction + mForceStep);
				Solve();
			}
		}

	}

	void FemPhysicsMatrixFree::UpdatePositions(std::vector<Node>& newNodes)
	{
		// TODO: use mNumBCs and mReshuffleMapInv
		for (size_t i = 0; i < newNodes.size(); i++)
		{
			if (newNodes[i].invMass == 0)
				nodes[mReshuffleMap[i]].pos = newNodes[i].pos;
		}
	}

	void FemPhysicsMatrixFree::ComputeDeformationGradient(uint32 e, Matrix3R& F) const
	{
		const Tetrahedron& tet = tets[e];
		// compute deformed/spatial shape matrix Ds [Sifakis]
		const Vector3R& x0 = nodes.at(tet.i[0]).pos;
		const Vector3R& x1 = nodes.at(tet.i[1]).pos;
		const Vector3R& x2 = nodes.at(tet.i[2]).pos;
		const Vector3R& x3 = nodes.at(tet.i[3]).pos;
		Vector3R d1 = x1 - x0;
		Vector3R d2 = x2 - x0;
		Vector3R d3 = x3 - x0;
		Matrix3R Ds(d1, d2, d3);
		// compute deformation gradient
		F = Ds * tet.X;
	}

	// simplest way of doing time dependent FEM - explicit integration of the discretized system
	void FemPhysicsMatrixFree::SubStep(real h)
	{
		for (uint32 i = 0; i < mForces.size(); i++)
			mForces[i].SetZero();
		ElasticEnergy::ComputeForces(this, mForces);

		ComputeSpringForces(mForces);

		// integrate node velocities and positions using Symplectic Euler
		for (size_t i = 0; i < nodes.size(); i++)
		{
			if (nodes[i].invMass == 0)
				continue;
			nodes[i].vel += (h * nodes[i].invMass) * mForces[i] + h * mGravity;
			nodes[i].pos += h * nodes[i].vel;
		}
	}

	inline void AddMatrix3(EigenMatrix& K, int n, int m, const Matrix3R& Ke, real h2)
	{
		int nn = 3 * n;
		int mm = 3 * m;
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
#ifdef SPARSE
				K.coeffRef(nn + i, mm + j) -= h2 * Ke.m[i][j];
#else
				K(nn + i, mm + j) -= h2 * Ke.m[i][j];
#endif
			}
		}
	}

	void FemPhysicsMatrixFree::ComputeForceDifferential(const std::vector<Vector3R>& dx, std::vector<Vector3R>& df) const
	{
		PROFILE_SCOPE("Differential");
		memset(&df[0], 0, df.size() * sizeof(Vector3R));

		// Lame coefficients
		const real mu = GetShearModulus();
		const real lambda = GetLameFirstParam();

		Matrix3R id;
		Vector3R zero;
		#pragma omp parallel for
		for (int i = 0; i < (int)tets.size(); i++)
		{
			const Tetrahedron& tet = tets.at(i);
			// compute deformation gradient increment dF
			const Vector3R& x0 = tet.i[0] < mNumBCs ? zero : dx[tet.i[0] - mNumBCs];
			const Vector3R& x1 = tet.i[1] < mNumBCs ? zero : dx[tet.i[1] - mNumBCs];
			const Vector3R& x2 = tet.i[2] < mNumBCs ? zero : dx[tet.i[2] - mNumBCs];
			const Vector3R& x3 = tet.i[3] < mNumBCs ? zero : dx[tet.i[3] - mNumBCs];
			Vector3R d1 = x1 - x0;
			Vector3R d2 = x2 - x0;
			Vector3R d3 = x3 - x0;
			Matrix3R mat(d1, d2, d3);
			Matrix3R dF = mat * tet.X;

			Matrix3R F;
			ComputeDeformationGradient(i, F); // we don't need it for linear, but never mind

			Matrix3R dP;
			if (mMaterial == MMT_LINEAR || mMaterial == MMT_DISTORTIONAL_LINEAR)
			{
				// stress differential - linear elasticity
				dP = mu * (dF + !dF) + lambda * dF.Trace() * id;
			}
			else if (mMaterial == MMT_COROTATIONAL)
			{
				// corotational
				Matrix3R R, S;
				ComputePolarDecomposition(F, R, S);
				Matrix3R dS = !R * dF;
				dP = 2 * mu * dF + lambda * dS.Trace() * R;
				// nonlinear correction term - comment below to obtain the implicit corotational method in [Mueller]
				real trS = S.Trace();
				Matrix3R A = S - trS * id;
				Vector3R w(dS(1, 2) - dS(2, 1), dS(0, 2) - dS(2, 0), dS(0, 1) - dS(1, 0));
				Vector3R r = A.GetInverse() * w;
				Matrix3R X = Matrix3R::Skew(r);
				Matrix3R dR = R * X;
				dP = dP + (lambda * (trS - 3) - 2 * mu) * dR;
			}
			else if (mMaterial == MMT_STVK)
			{
				Matrix3R E = 0.5f * (!F * F - id);
				Matrix3R dE = 0.5f * (!dF * F + !F * dF);
				dP = dF * (2 * mu * E + lambda * E.Trace() * id)
					+ F * (2 * mu * dE + lambda * dE.Trace() * id);
			}
			else if (mMaterial == MMT_NEO_HOOKEAN)
			{
				Matrix3R Finv = F.GetInverse();
				Matrix3R Finvtr = !Finv;
				real J = F.Determinant();
				dP = mu * dF + (mu - lambda * log(J)) * Finvtr * !dF * Finvtr + lambda * (Finv * dF).Trace() * Finvtr;
			}

			// compute the double contraction of dF and dP to check PD
			//real indicator = Matrix3R::DoubleContraction(dF, dP);
			//if (indicator < 0)
			//{
			//	Printf("Stiffness matrix is not PD (dF:dP = %g).\n", indicator);
			//}

			dH[i] = -tet.vol * dP * tet.Xtr;
		}

		for (size_t i = 0; i < tets.size(); i++)
		{
			const Tetrahedron& tet = tets.at(i);
			Vector3R f3;
			for (int j = 1; j < 4; j++)
			{
				Vector3R f = dH[i](j - 1);
				if (tet.i[j] >= mNumBCs)
					df[tet.i[j] - mNumBCs] += f;
				f3 -= f;
			}
			if (tet.i[0] >= mNumBCs)
				df[tet.i[0]- mNumBCs] += f3;
		}
	}

	real FemPhysicsMatrixFree::GetTotalVolume() const
	{
		real totalVol = 0;
		for (size_t e = 0; e < tets.size(); e++)
		{
			// compute current volume
			const Tetrahedron& tet = tets.at(e);
			int i0 = tet.i[0];
			int i1 = tet.i[1];
			int i2 = tet.i[2];
			int i3 = tet.i[3];
			const Vector3R& x0 = nodes.at(i0).pos;
			const Vector3R& x1 = nodes.at(i1).pos;
			const Vector3R& x2 = nodes.at(i2).pos;
			const Vector3R& x3 = nodes.at(i3).pos;
			Vector3R d1 = x1 - x0;
			Vector3R d2 = x2 - x0;
			Vector3R d3 = x3 - x0;
			Matrix3R mat(d1, d2, d3); // this is the spatial shape matrix Dm [Sifakis][Teran03]
			real vol = (mat.Determinant()) / 6.f; // signed volume of the tet
			totalVol += vol;
		}
		return totalVol;
	}

	void FemPhysicsMatrixFree::MatrixVectorMultiply(const std::vector<Vector3R>& d, std::vector<Vector3R>& df) const
	{
		PROFILE_SCOPE("Matrix mult");
		// compute M * d / h^2 - K * d (only the second term for quasi-static, identified by mTimeStep = 0)
		ComputeForceDifferential(d, df);
		const real invHSqr = mTimeStep == 0 ? 0 : 1.f / (mTimeStep * mTimeStep);
		for (size_t i = 0; i < GetNumFreeNodes(); i++)
		{
			real mass = 1.f / nodes[i + mNumBCs].invMass;
			df[i] = invHSqr * mass * d[i] - df[i];
		}
	}

	void FemPhysicsMatrixFree::ComputeGradients(std::vector<Vector3R>& r)
	{
		PROFILE_SCOPE("Gradients");
		// compute negative gradient r = f(x, v) + M * g
		for (uint32 i = 0; i < mForces.size(); i++)
			mForces[i].SetZero();
		ComputePressureForces(mForces, mTractionStiffnessMatrix);
		ElasticEnergy::ComputeForces(this, mForces);
		ComputeSpringForces(mForces);
		const real invH = mTimeStep == 0 ? 0 : 1.f / mTimeStep;
		const real invHSqr = mTimeStep == 0 ? 0 : 1.f / (mTimeStep * mTimeStep);
		for (size_t i = 0; i < GetNumFreeNodes(); i++)
		{
			if (nodes[i + mNumBCs].invMass == 0)
				continue; // this is for USE_CONSTRAINT_BCS but does no harm
			real mass = 1.f / nodes[i + mNumBCs].invMass;
			r[i] = mForces[i + mNumBCs];
			r[i] += mass * mForceFraction * mGravity;
			if (i < GetNumFreeNodes() - mNumSpringNodes) // do not apply gravity or inertia to free cable nodes
			{
				r[i] += mass * invH * nodes[i + mNumBCs].vel;
				r[i] -= mass * invHSqr * (nodes[i + mNumBCs].pos - nodes[i + mNumBCs].pos0);
			}
		}
	}

	void FemPhysicsMatrixFree::UpdatePosAndComputeGradients(const std::vector<Vector3R>& pos, std::vector<Vector3R>& r)
	{
		for (size_t i = 0; i < GetNumFreeNodes(); i++)
			nodes[i + mNumBCs].pos = pos[i];

		ComputeGradients(r);
	}

	bool FemPhysicsMatrixFree::Solve()
	{
		MEASURE_TIME("Nonlinear FEM solve");
		PROFILE_SCOPE("Solve");

		bool ret = true;
		if (mConfig.mSolver == NST_NEWTON)
			SolveNewton();
		else if (mConfig.mSolver == NST_NEWTON_LS)
			SolveNewtonLS();
		else if (mConfig.mSolver == NST_NEWTON_CG)
			SolveNewtonCG();
		else if (mConfig.mSolver == NST_NONLINEAR_CG)
			SolveNonlinearConjugateGradient();
		else if (mConfig.mSolver == NST_STEEPEST_DESCENT)
			SolveNonlinearSteepestDescent();
		else if (mConfig.mSolver == NST_GRADIENT_DESCENT)
			SolveGradientDescent(mConfig.mDescentRate);

		if (mTimeStep != 0 && mSimType == ST_IMPLICIT)
		{
			// compute new velocities
			const real invH = 1.f / mTimeStep;
			for (size_t i = 0; i < GetNumFreeNodes(); i++)
			{
				nodes.at(i + mNumBCs).vel = invH * (nodes[i + mNumBCs].pos - nodes[i + mNumBCs].pos0);
			}
		}

		//Printf("final energy: %g\n", ComputeEnergy());

		return ret;
	}

	void FemPhysicsMatrixFree::SolveNewtonCG()
	{
		std::vector<Vector3R> delta(GetNumFreeNodes()); // allocation!
		std::vector<Vector3R> b(GetNumFreeNodes()); // allocation!

		// no need for Newton for linear elasticity
		int numIters = /*mMaterial == MMT_LINEAR ? 1 : */mOuterIterations;

		// Newton steps (outer loop)
		for (int iter = 0; iter < numIters; iter++)
		{
			ComputeGradients(b);

			// solve K * dx = b by Conjugate Gradient
			std::fill(delta.begin(), delta.end(), Vector3R()); // reset the guess to zero; TODO: warm start
			SolveConjugateGradientMF<real, Vector3R, FemPhysicsMatrixFree>(*this, b, delta, mInnerIterations);

			// add dx
			for (size_t i = 0; i < GetNumFreeNodes(); i++)
			{
				//mTotalDisplacements[i] += delta[i];
				nodes.at(i + mNumBCs).pos += delta[i];
			}
		}
	}

	real FemPhysicsMatrixFree::MeritResidual(const EigenVector& rhs)
	{
		if (mConfig.mOptimizer)
			return ComputeEnergy();
		else
			return 0.5 * rhs.squaredNorm();
	}

	EigenVector FemPhysicsMatrixFree::ComputeRhs(const EigenVector& sol)
	{
		// set node positions
		auto solVecs = GetStdVector(sol);
		ASSERT(solVecs.size() == GetNumFreeNodes());
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			nodes[i + mNumBCs].pos = solVecs[i];
		}

		std::vector<Vector3R> b(GetNumFreeNodes()); // allocation!
		ComputeGradients(b);
		EigenVector rhs = GetEigenVector(b);

		// add penalty Dirichlet BC forces
		if (!mDirichletIndices.empty())
		{
			// count the constraints
			int count = 0;
			for (size_t i = 0; i < mDirichletIndices.size(); i++)
			{
				if (mDirichletAxes[i] & AXIS_X) count++;
				if (mDirichletAxes[i] & AXIS_Y) count++;
				if (mDirichletAxes[i] & AXIS_Z) count++;
			}

			EigenVector dirichletErrors(count);
			count = 0;
			for (uint32 i = 0; i < mDirichletIndices.size(); i++)
			{
				// the i'th BC
				uint32 flags = mDirichletAxes[i];
				uint32 idx0 = mDirichletIndices[i];
				uint32 idx = idx0 - mNumBCs; // affecting node idx
				// compute the BC error
				if (flags & AXIS_X)
				{
					dirichletErrors(count++) = nodes[idx].pos.x - nodes[idx].pos0.x;
				}
				if (flags & AXIS_Y)
				{
					dirichletErrors(count++) = nodes[idx].pos.y - nodes[idx].pos0.y;
				}
				if (flags & AXIS_Z)
				{
					dirichletErrors(count++) = nodes[idx].pos.z - nodes[idx].pos0.z;
				}
			}
			//Printf("Dirichlet BCs error: %g\n", dirichletErrors.lpNorm<Eigen::Infinity>());
			rhs -= mDirichletStiffness * mDirichletJacobian.transpose() * dirichletErrors;
			// TODO: we can build the rhs term directly without the Jacobian
		}

		//Printf("E=%g\n", ComputeEnergy());
		return rhs;
	}

	template<class MATRIX>
	void FemPhysicsMatrixFree::ComputeSystemMatrix(const EigenVector& s, const EigenVector& y, MATRIX& K)
	{
		ElasticEnergy::AssembleStiffnessMatrix(this, K);
		//K -= mTractionStiffnessMatrix;
		if (mTimeStep != 0 && mSimType == ST_IMPLICIT)
			K += (1.f / mTimeStep / mTimeStep) * mMassMatrix;

		if (!mDirichletIndices.empty())
		{
			K += mDirichletStiffness * mDirichletJacobian.transpose() * mDirichletJacobian;
		}
		if (!mCables.empty())
		{
			MATRIX Ks;
			ComputeSpringStiffnessMatrix(Ks);
			K -= Ks;
		}
		if (mConfig.mOptimizer)
			Printf("energy: %g\n", ComputeEnergy());
	}

	EigenVector FemPhysicsMatrixFree::SolveLinearSystem(EigenMatrix& K, const EigenVector& rhs, const EigenVector& s, const EigenVector& y)
	{
		LinearSolver solver;
		solver.Init(K, LST_LU_PARDISO);
		//solver.SetTolerance(0.1);
		//solver.Init(K, LST_CG);
		return solver.Solve(rhs);
	}

	EigenVector FemPhysicsMatrixFree::SolveLinearSystem(SparseMatrix& K, const EigenVector& rhs, const EigenVector& s, const EigenVector& y)
	{
		LinearSolver solver;
		solver.Init(K, LST_LU_PARDISO);
		return solver.Solve(rhs);
	}

	bool FemPhysicsMatrixFree::SolveNewtonLS()
	{
		AssembleDynamicContributions();

		NewtonSolverBackTrack<FemPhysicsMatrixFree, SparseMatrix> solver;
		solver.mNumIterations = mOuterIterations;
		solver.mVerbose = mVerbose ? VL_MINIMUM : VL_NONE;
		solver.mResidualThreshold = mAbsNewtonResidualThreshold;
		solver.mUseProblemSolver = true;
		if (mConfig.mOptimizer)
		{
			solver.mLSCondition = LSC_ARMIJO;
			//solver.mAlpha = 1e-3;
		}
		//solver.mUseBFGS = false;

		// prepare the initial guess with the current configuration
		uint32 size = GetNumFreeNodes() * NUM_POS_COMPONENTS;
		EigenVector solution(size);
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			const Vector3R& p = nodes[i + mNumBCs].pos;
			solution(i * NUM_POS_COMPONENTS) = p.x;
			solution(i * NUM_POS_COMPONENTS + 1) = p.y;
			solution(i * NUM_POS_COMPONENTS + 2) = p.z;
		}

		solver.Solve(*this, size, solution);
		if (mVerbose)
		{
			real vol = GetTotalVolume();
			Printf("vol err: %.2f%%\n", abs(vol - mTotalInitialVol) / mTotalInitialVol * 100);
		}

		// set node positions
		auto solVecs = GetStdVector(solution);
		ASSERT(solVecs.size() == GetNumFreeNodes());
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			nodes[i + mNumBCs].pos = solVecs[i];
		}

		CheckForInversion(true);

		// store Hessian
		mHessian = solver.mB;

		return true;
	}

	bool FemPhysicsMatrixFree::SolveNewton()
	{
		AssembleDynamicContributions();

		NewtonSolver<FemPhysicsMatrixFree, SparseMatrix> solver;
		solver.mNumIterations = mOuterIterations;
		solver.mVerbose = mVerbose ? VL_MINIMUM : VL_NONE;
		solver.mResidualThreshold = mAbsNewtonResidualThreshold;
		solver.mSolverType = LST_LU_PARDISO;
		//solver.mUseFiniteDiff = true;

		// prepare the initial guess with the current configuration
		uint32 size = GetNumFreeNodes() * NUM_POS_COMPONENTS;
		//Printf("#dofs: %d\n", size);
		EigenVector solution(size);
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			const Vector3R& p = nodes[i + mNumBCs].pos;
			solution(i * NUM_POS_COMPONENTS) = p.x;
			solution(i * NUM_POS_COMPONENTS + 1) = p.y;
			solution(i * NUM_POS_COMPONENTS + 2) = p.z;
		}

		solver.Solve(*this, size, solution);
		if (mVerbose)
		{
			real vol = GetTotalVolume();
			Printf("vol err: %.2f%%\n", abs(vol - mTotalInitialVol) / mTotalInitialVol * 100);
		}

		// set node positions
		auto solVecs = GetStdVector(solution);
		ASSERT(solVecs.size() == GetNumFreeNodes());
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
		{
			nodes[i + mNumBCs].pos = solVecs[i];
		}

		CheckForInversion(true);

		return true;
	}

	void FemPhysicsMatrixFree::SolveGradientDescent(real alpha)
	{
		std::vector<Vector3R> b(GetNumFreeNodes()); // allocation!
		for (uint32 iter = 0; iter < mOuterIterations; iter++)
		{
			ComputeGradients(b);

			// perform the gradient descent step (very similar to explicit integration)
			for (size_t i = 0; i < GetNumFreeNodes(); i++)
			{
				Vector3R delta = alpha * b[i];
				nodes[i + mNumBCs].pos += delta;
			}
		}
	}

	void FemPhysicsMatrixFree::SolveNonlinearSteepestDescent()
	{
		// the residual vector
		std::vector<Vector3R> r(GetNumFreeNodes()); // allocation!
		// the differential vector
		std::vector<Vector3R> df(GetNumFreeNodes()); // allocation!

		for (uint32 iter = 0; iter < mOuterIterations; iter++)
		{
			ComputeGradients(r);
			MatrixVectorMultiply(r, df);
			real delta = InnerProduct<Vector3R, real>(r, r);
			if (delta == 0)
				break;
			real alpha = delta / InnerProduct<Vector3R, real>(r, df);
			if (alpha == 0)
				break;

			// perform the gradient descent step (very similar to explicit integration)
			for (size_t i = 0; i < GetNumFreeNodes(); i++)
			{
				Vector3R delta = alpha * r[i];
				nodes[i + mNumBCs].pos += delta;
			}
		}
	}

	void FemPhysicsMatrixFree::SolveNonlinearConjugateGradient()
	{
		std::vector<Vector3R> pos(GetNumFreeNodes()); // allocation!
		for (uint32 i = 0; i < GetNumFreeNodes(); i++)
			pos[i] = nodes[i + mNumBCs].pos;

		NonlinearConjugateGradientMinimizer<FemPhysicsMatrixFree, std::vector<Vector3R>> minimizer;
		minimizer.mOuterIterations = mOuterIterations;
		minimizer.mInnerIterations = mInnerIterations;
		minimizer.mAbsResidualThreshold = mAbsNewtonResidualThreshold;
		minimizer.Solve(*this, GetNumFreeNodes(), pos);
	}

	real FemPhysicsMatrixFree::ComputeEnergy()
	{
		real val = ComputeElasticEnergy();
		// add the gravitational and kinetic part
		real invHSqr = mTimeStep != 0 ? 0.5f / mTimeStep / mTimeStep : 0;
		for (size_t i = 0; i < GetNumFreeNodes(); i++)
		{
			real mass = 1.f / nodes[i + mNumBCs].invMass;
			val -= mass * mForceFraction * mGravity.y * nodes[i + mNumBCs].pos.y; // gravitational
			val -= invHSqr * mass * GetTotalDisplacement(i).LengthSquared(); // kinetic (delta)
		}
		// add the pressure part (WIP)
		for (size_t i = 0; i < mTractionSurface.size() / 3; i += 3)
		{
			int base = i * 3;
			// global (shuffled) indices of nodes
			uint32 i1 = mTractionSurface[base];
			uint32 i2 = mTractionSurface[base + 1];
			uint32 i3 = mTractionSurface[base + 2];

			// compute triangle area and normal
			const Vector3R& p1 = GetDeformedPosition(i1);
			const Vector3R& p2 = GetDeformedPosition(i2);
			const Vector3R& p3 = GetDeformedPosition(i3);

			// compute volume formed with the origin
			real vol = (1.f / 6.f) * triple(p1, p2, p3);
			val += abs(vol) * mAppliedPressure;
		}
		// add the springs
		val += ComputeSpringEnergy();

		//Printf("energy: %g\n", val);
		return val;
	}

	real FemPhysicsMatrixFree::ComputeEnergy(int level)
	{
		// level 0 -> elastic
		// level 1 -> gravitational
		// level 2 -> vol
		// level 3 -> kinetic

		real val = 0;

		if (level == 0)
			val += ComputeElasticEnergy();

		// add the gravitational and kinetic part
		real invHSqr = mTimeStep != 0 ? 0.5f / mTimeStep / mTimeStep : 0;

		for (size_t i = 0; i < GetNumFreeNodes(); i++)
		{
			real mass = 1.f / nodes[i + mNumBCs].invMass;
			if (level == 1)
				val += mass * mForceFraction * mGravity.y * nodes[i + mNumBCs].pos.y; // gravitational

			if (level == 3)
				val += invHSqr * mass * GetTotalDisplacement(i).LengthSquared(); // kinetic
		}

		//// add the pressure part (WIP)
		//for (size_t i = 0; i < mTractionSurface.size() / 3; i += 3)
		//{
		//	int base = i * 3;
		//	// global (shuffled) indices of nodes
		//	uint32 i1 = mTractionSurface[base];
		//	uint32 i2 = mTractionSurface[base + 1];
		//	uint32 i3 = mTractionSurface[base + 2];

		//	// compute triangle area and normal
		//	const Vector3R& p1 = GetDeformedPosition(i1);
		//	const Vector3R& p2 = GetDeformedPosition(i2);
		//	const Vector3R& p3 = GetDeformedPosition(i3);

		//	// compute volume formed with the origin
		//	real vol = (1.f / 6.f) * triple(p1, p2, p3);
		//	val += abs(vol) * mAppliedPressure;
		//}

		//Printf("energy: %f\n", val);
		return val;
	}

	void FemPhysicsMatrixFree::ComputePressureForces(Vector3Array& fout, EigenMatrix& Kout) const
	{
		if (mTractionSurface.empty())
			return;

		if (mUseImplicitPressureForces)
		{
			uint32 numDofs = GetNumFreeNodes() * NUM_POS_COMPONENTS;
			Kout.resize(numDofs, numDofs);
			Kout.setZero();
		}

		auto computeForce = [&](const Vector3R& p1, const Vector3R& p2, const Vector3R& p3)->Vector3R
		{
			Vector3R a = p2 - p1;
			Vector3R b = p3 - p1;
			Vector3R normal = cross(a, b);
			real area = 0.5f * normal.Length();
			normal.Normalize();

			Vector3R traction = mAppliedPressure * normal;
			Vector3R force = (area / 3.0f) * traction;
			return force;
		};

		// go through all inner boundary triangles
		for (uint32 t = 0; t < mTractionSurface.size() / 3; t++)
		{
			int base = t * 3;
			// global (shuffled) indices of nodes
			uint32 i1 = mTractionSurface[base];
			uint32 i2 = mTractionSurface[base + 1];
			uint32 i3 = mTractionSurface[base + 2];

			// compute triangle area and normal
			const Vector3R& p1 = GetDeformedPosition(i1);
			const Vector3R& p2 = GetDeformedPosition(i2);
			const Vector3R& p3 = GetDeformedPosition(i3);

			// apply traction to triangle nodes
			Vector3R force = mForceFraction * computeForce(p1, p2, p3);
			i1 = mReshuffleMap[i1];
			i2 = mReshuffleMap[i2];
			i3 = mReshuffleMap[i3];
			fout[i1] += force;
			fout[i2] += force;
			fout[i3] += force;

			if (mUseImplicitPressureForces)
			{
				// compute local stiffness matrix
				Matrix3R K[3];
				Vector3R a = p2 - p1;
				Vector3R b = p3 - p1;
				a.Scale(mAppliedPressure / 6.0f);
				b.Scale(mAppliedPressure / 6.0f);
				K[2] = Matrix3R::Skew(a);
				K[1] = Matrix3R::Skew(-b);
				K[0] = -K[1] - K[2];

				i1 -= mNumBCs;
				i2 -= mNumBCs;
				i3 -= mNumBCs;

				// assemble global stiffness matrix
				for (uint32 x = 0; x < 3; x++)
				{
					for (uint32 y = 0; y < 3; y++)
					{
						Kout(i1 * 3 + x, i1 * 3 + y) += K[0](x, y);
						Kout(i1 * 3 + x, i2 * 3 + y) += K[1](x, y);
						Kout(i1 * 3 + x, i3 * 3 + y) += K[2](x, y);
						Kout(i2 * 3 + x, i1 * 3 + y) += K[0](x, y);
						Kout(i2 * 3 + x, i2 * 3 + y) += K[1](x, y);
						Kout(i2 * 3 + x, i3 * 3 + y) += K[2](x, y);
						Kout(i3 * 3 + x, i1 * 3 + y) += K[0](x, y);
						Kout(i3 * 3 + x, i2 * 3 + y) += K[1](x, y);
						Kout(i3 * 3 + x, i3 * 3 + y) += K[2](x, y);
					}
				}
			}
		}
	}

	void FemPhysicsMatrixFree::SetBoundaryConditionsSurface(const std::vector<uint32>& triangleList, real pressure)
	{
		mAppliedPressure = pressure;
		mTractionSurface.resize(triangleList.size());

		for (uint32 i = 0; i < triangleList.size(); i++)
		{
			mTractionSurface[i] = triangleList[i];
		}
	}

	real FemPhysicsMatrixFree::ComputeSpringEnergy(Cable& cable)
	{
		if (cable.mCableNodes.empty() || cable.mActuation == 0)
			return 0;
		uint32 numNodes = cable.mCableNodes.size();
		uint32 numSprings = numNodes - 1;
		// compute interpolated spring nodes
		cable.mCablePositions.resize(numNodes);
		Vector3Array vels(numNodes);
		for (uint32 i = 0; i < numNodes; i++)
		{
			int elem = cable.mCableNodes[i].elem;
			if (elem < 0)
			{
				cable.mCablePositions[i] = nodes[-elem].pos;
				continue;
			}
			const Tetrahedron& tet = tets[elem];
			const Vector3R& x0 = nodes[tet.i[0]].pos;
			const Vector3R& x1 = nodes[tet.i[1]].pos;
			const Vector3R& x2 = nodes[tet.i[2]].pos;
			const Vector3R& x3 = nodes[tet.i[3]].pos;
			real w0 = cable.mCableNodes[i].bary.x;
			real w1 = cable.mCableNodes[i].bary.y;
			real w2 = cable.mCableNodes[i].bary.z;
			real w3 = 1 - w0 - w1 - w2;
			cable.mCablePositions[i] = w0 * x0 + w1 * x1 + w2 * x2 + w3 * x3;
			vels[i] = w0 * nodes[tet.i[0]].vel + w1 * nodes[tet.i[1]].vel + w2 * nodes[tet.i[2]].vel + w3 * nodes[tet.i[3]].vel;
		}
		// TODO: reuse cable positions
		// 1. Compute spring errors and potentials
		std::vector<Vector3R> springForces(numSprings);
		const real eps = cable.mCableRestLength * 0.01;
		real energy = 0;
		for (uint32 i = 0; i < numSprings; i++)
		{
			Vector3R y = cable.mCablePositions[i + 1] - cable.mCablePositions[i];
			real len = y.Length();
			Vector3R dir = (1.0 / len) * y;
			real err = len - cable.mCableRestLength * cable.mActuation;
			real potential = cable.mCableStiffness * (err * err + eps * eps / 3);
			// Bern tension model (unilateral spring + twice differentiable)
			if (err < -eps)
				potential = 0;
			else if (err >= -eps && err <= eps)
				potential = 0.5 * cable.mCableStiffness * (err * err * err / 3 / eps + err * err + eps * err + eps * eps / 3);
			energy += potential;
		}
		return energy;
	}

	real FemPhysicsMatrixFree::ComputeSpringEnergy()
	{
		real energy = 0;
		for (Cable& cable : mCables)
		{
			energy += ComputeSpringEnergy(cable);
		}
		return energy;
	}

} // namespace FEM_SYSTEM


/**
# Copyright 2018 D-Wave Systems Inc.
#
#    Licensed under the Apache License, Version 2.0 (the "License");
#    you may not use this file except in compliance with the License.
#    You may obtain a copy of the License at
#
#        http://www.apache.org/licenses/LICENSE-2.0
#
#    Unless required by applicable law or agreed to in writing, software
#    distributed under the License is distributed on an "AS IS" BASIS,
#    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#    See the License for the specific language governing permissions and
#    limitations under the License.
#
# ================================================================================================
*/
#include "fix_variables.hpp"
#include "compressed_matrix.hpp"

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <string>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/strong_components.hpp>

//for debugging only
#include <iostream>
#include <iomanip>
#include <fstream>
#include <time.h>

namespace
{

class compClass
{
public:
	bool operator()(const std::pair<int, int>& a, const std::pair<int, int>& b)
	{
		if (a.second != b.second)
			return !(a.second < b.second);
		else
			return a.first < b.first;
	}
};

bool compareAbs(double a, double b)
{
	return std::abs(a) < std::abs(b);
}

//the index is 1-based, according to the paper
struct Posiform
{
	std::vector<std::pair<std::pair<int, int>, long long int> > quadratic;
	std::vector<std::pair<int, long long int> > linear;
	long long int cst;
	int numVars;
};

struct SC
{
	std::vector<int> original;
	std::vector<int> positive;
	std::vector<int> negative;
};

struct SCRet
{
	std::vector<SC> S;
	compressed_matrix::CompressedMatrix<long long int> G;
};

Posiform BQPToPosiform(const compressed_matrix::CompressedMatrix<long long int>& Q, bool makeRandom = false)
{
	int numVariables = Q.numRows();
	Posiform ret;
	ret.numVars = numVariables;
	std::vector<long long int> q(numVariables);

	//q = diag(Q);
	for (int i = 0; i < numVariables; i++)
		q[i] = Q.get(i, i);

	//tmpQ = triu(Q,1);
	std::vector<int> tmpQRowOffsets(numVariables + 1);
	std::vector<int> tmpQColIndices; tmpQColIndices.reserve(Q.nnz());
	std::vector<long long int> tmpQValues; tmpQValues.reserve(Q.nnz());
	int index = 0;
	for (int i = 0; i < Q.numRows(); i++)
	{
		int start = Q.rowOffsets()[i];
		int end = Q.rowOffsets()[i + 1];
		tmpQRowOffsets[i] = index;
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = Q.colIndices()[j];
			if (r != c)
			{
				tmpQColIndices.push_back(c);
				tmpQValues.push_back(Q.values()[j]); //Q.values()[j] is Q(r, c)
				++index;
			}
		}
	}
	tmpQRowOffsets.back() = index; //number of non-zero elements

	compressed_matrix::CompressedMatrix<long long int> tmpQ(numVariables, numVariables, tmpQRowOffsets, tmpQColIndices, tmpQValues);

	if (!makeRandom) //make a deterministic posiform from Q
	{
		//cPrime = q + sum(tmpQ.*(tmpQ<0),2); and quadratic term
		std::vector<long long int> cPrime = q;
		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] < 0)
				{
					cPrime[i] += tmpQ.values()[j];
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, -(c + 1)), -tmpQ.values()[j])); //+1 makes it 1-based
				}
				else if (tmpQ.values()[j] > 0)
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, c + 1), tmpQ.values()[j])); //+1 makes it 1-based
			}
		}

		//constant term and linear term
		ret.cst = 0;
		for (int i = 0; i < numVariables; i++)
		{
			if (cPrime[i] < 0)
			{
				ret.cst += cPrime[i]; //constant term
				ret.linear.push_back(std::make_pair(-(i + 1), -cPrime[i])); //+1 makes it 1-based
			}
			else if (cPrime[i] > 0)
				ret.linear.push_back(std::make_pair(i + 1, cPrime[i])); //+1 makes it 1-based
		}
	}
	else
	{
		std::vector<long long int> linear = q;
		std::set<std::pair<int, int> > negPairs;
		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] < 0)
				{
					if (rand() % 2 == 0)
						negPairs.insert(std::make_pair(-(r + 1), c + 1)); //+1 makes it 1-based
					else
						negPairs.insert(std::make_pair(r + 1, -(c + 1))); //+1 makes it 1-based
				}
			}
		}

		for (int i = 0; i < tmpQ.numRows(); i++)
		{
			int start = tmpQ.rowOffsets()[i];
			int end = tmpQ.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = tmpQ.colIndices()[j];
				if (tmpQ.values()[j] > 0)
					ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, c + 1), tmpQ.values()[j]));
				else
				{
					if (negPairs.find(std::make_pair(r + 1, -(c + 1)))!=negPairs.end())
						ret.quadratic.push_back(std::make_pair(std::make_pair(r + 1, -(c + 1)), -tmpQ.values()[j]));
					else
						ret.quadratic.push_back(std::make_pair(std::make_pair(-(r + 1), c + 1), -tmpQ.values()[j]));
				}
			}
		}

		for (std::set<std::pair<int, int> >::iterator it = negPairs.begin(); it != negPairs.end(); ++it)
		{
			if (it->first > 0)
				linear[it->first - 1] += tmpQ(it->first - 1, -it->second - 1);
			else
				linear[it->second - 1] += tmpQ(-it->first - 1, it->second - 1);
		}

		ret.cst = 0;

		for (int i = 0; i < numVariables; i++)
		{
			if (linear[i] < 0)
			{
				ret.cst += linear[i];
				ret.linear.push_back(std::make_pair(-(i + 1), -linear[i]));
			}
			else if (linear[i] > 0)
				ret.linear.push_back(std::make_pair(i + 1, linear[i]));
		}
	}

	return ret;
}

compressed_matrix::CompressedMatrix<long long int> posiformToImplicationNetwork_1(const Posiform& p)
{
	int n = p.numVars;
	int numVertices = 2 * n + 2;
	int source = 0;
	int sink = n + 1;

	//n = p.numVars
	//0: source;
	//1 to n: x_1 to x_n
	//n+1: sink
	//n+2 to 2*n+1: \overline_x_1 to \overline_x_n
	std::map<std::pair<int, int>, long long int> m;
	for (int i = 0; i < p.linear.size(); i++)
	{
		int v = p.linear[i].first;

		long long int capacity = p.linear[i].second; // originally p.linear[i].second/2
		if (v > 0)
		{
			m[std::make_pair(source, v + n + 1)] = capacity;
			m[std::make_pair(v, sink)] = capacity;
		}
		else
		{
			v = std::abs(v);
			m[std::make_pair(source, v)] = capacity;
			m[std::make_pair(v + n + 1, sink)] = capacity;
		}
	}

	for (int i = 0; i < p.quadratic.size(); i++)
	{
		int v_1 = p.quadratic[i].first.first;
		int v_2 = p.quadratic[i].first.second;

		long long int capacity = p.quadratic[i].second; // originally p.quadratic[i].second/2
		if (v_1 < 0)
		{
			v_1 = std::abs(v_1);
			m[std::make_pair(v_1 + n + 1, v_2 + n + 1)] = capacity;
			m[std::make_pair(v_2, v_1)] = capacity;
		}
		else if (v_2 < 0)
		{
			v_2 = std::abs(v_2);
			m[std::make_pair(v_1, v_2)] = capacity;
			m[std::make_pair(v_2 + n + 1, v_1 + n + 1)] = capacity;
		}
		else
		{
			m[std::make_pair(v_1, v_2 + n + 1)] = capacity;
			m[std::make_pair(v_2, v_1 + n + 1)] = capacity;
		}
	}

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, m);
}

compressed_matrix::CompressedMatrix<long long int> posiformToImplicationNetwork_2(const Posiform& p)
{
	int n = p.numVars;
	int numVertices = 2 * n + 2;
	int source = n;
	int sink = 2 * n + 1;

	//n = p.numVars
	//0 to n-1: x_1 to x_n
	//n: source;
	//n+1 to 2*n: \overline_x_1 to \overline_x_n
	//2*n+1: sink
	std::map<std::pair<int, int>, long long int> m;
	for (int i = 0; i < p.linear.size(); i++)
	{
		int v = p.linear[i].first;
		long long int capacity = p.linear[i].second; // originally p.linear[i].second/2
		if (v > 0)
		{
			m[std::make_pair(source, v + n)] = capacity;
			m[std::make_pair(v - 1, sink)] = capacity;
		}
		else
		{
			v = std::abs(v);
			m[std::make_pair(source, v - 1)] = capacity;
			m[std::make_pair(v + n, sink)] = capacity;
		}
	}

	for (int i = 0; i < p.quadratic.size(); i++)
	{
		int v_1 = p.quadratic[i].first.first;
		int v_2 = p.quadratic[i].first.second;
		long long int capacity = p.quadratic[i].second; // originally p.quadratic[i].second/2
		if (v_1 < 0)
		{
			v_1 = std::abs(v_1);
			m[std::make_pair(v_1 + n, v_2 + n)] = capacity;
			m[std::make_pair(v_2 - 1, v_1 - 1)] = capacity;
		}
		else if (v_2 < 0)
		{
			v_2 = std::abs(v_2);
			m[std::make_pair(v_1 - 1, v_2 - 1)] = capacity;
			m[std::make_pair(v_2 + n, v_1 + n)] = capacity;
		}
		else
		{
			m[std::make_pair(v_1 - 1, v_2 + n)] = capacity;
			m[std::make_pair(v_2 - 1, v_1 + n)] = capacity;
		}
	}

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, m);
}

//this version returns R instead of F
compressed_matrix::CompressedMatrix<long long int> maxFlow(const compressed_matrix::CompressedMatrix<long long int>& A)
{
	int numVertices = A.numRows();
	int numVariables = numVertices / 2 - 1;

	//clock_t curr_1 = clock();
	//clock_t curr_2;

	using namespace boost;

	typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
	typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, long long int, property<edge_residual_capacity_t, long long int, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is long long int
	//typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, double, property<edge_residual_capacity_t, double, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is double

	Graph g;

	property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
	property_map<Graph, edge_reverse_t>::type reverse_edge = get(edge_reverse, g);
	property_map<Graph, edge_residual_capacity_t>::type residual_capacity = get(edge_residual_capacity, g);

	std::vector<Traits::vertex_descriptor> verts(numVertices);
	for (int i = 0; i < numVertices; ++i)
		verts[i] = add_vertex(g);

	Traits::vertex_descriptor s = verts[0];
	Traits::vertex_descriptor t = verts[numVariables + 1];

	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int cap = A.values()[j];

			Traits::edge_descriptor e1, e2;
			bool in1, in2;
			boost::tie(e1, in1) = add_edge(verts[r], verts[c], g);
			boost::tie(e2, in2) = add_edge(verts[c], verts[r], g);

			capacity[e1] = cap;
			capacity[e2] = 0;
			reverse_edge[e1] = e2;
			reverse_edge[e2] = e1;
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_constructing_graph_for_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	push_relabel_max_flow(g, s, t);

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_for_boost_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<int> RRowOffsets(numVertices+1);
	std::vector<int> RColIndices;
	std::vector<long long int> RValues;
	int offset = 0;
	int currRow = 0;
	graph_traits<Graph>::vertex_iterator u_iter, u_end;
	graph_traits<Graph>::out_edge_iterator ei, e_end;
	for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
		for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
		{
			if (capacity[*ei] > 0)
			{
				if (currRow <= *u_iter)
				{
					for (int i = currRow; i <= *u_iter; i++)
						RRowOffsets[i] = offset;
					currRow = static_cast<int>((*u_iter) + 1);
				}

				RColIndices.push_back(static_cast<int>(target(*ei, g)));
				RValues.push_back(residual_capacity[*ei]);
				++offset;
			}
		}
	for (int i = currRow; i < RRowOffsets.size(); i++)
		RRowOffsets[i] = offset;

	compressed_matrix::CompressedMatrix<long long int> R(numVertices, numVertices, RRowOffsets, RColIndices, RValues); //residual

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_get_R: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::map<std::pair<int, int>, long long int> rm = compressed_matrix::compressedMatrixToMap(R);
	std::map<std::pair<int, int>, long long int> rmMissing;

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_copy_R_to_map: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			long long int Arc = A.get(r, c);
			long long int Rcr = R.get(c, r);
			if (Arc != 0 && R.values()[j] + Rcr - Arc != 0) //it->second is: R(r, c); here it means R(r, c)+R(c, r)!=A(r, c), so R(c, r) is missing
				rmMissing.insert(std::make_pair(std::make_pair(c, r), Arc - R.values()[j]));
		}
	}

	for (std::map<std::pair<int, int>, long long int>::iterator it = rmMissing.begin(), end = rmMissing.end(); it != end; ++it)
		rm.insert(*it);

	//curr_2 = clock();
	//mexPrintf("inside maxFlow int version: Time elapsed_get_missing_elements_in_R: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, rm);
}

std::vector<int> bfs_for_method_2(const compressed_matrix::CompressedMatrix<long long int>& g, int s)
{
	std::queue<int> q;
	q.push(s);
	int numVertices = g.numRows();
	std::vector<int> visited(numVertices, -1);
	visited[s] = 1;

	while (!q.empty())
	{
		int curr = q.front(); q.pop();
		int start = g.rowOffsets()[curr];
		int end = g.rowOffsets()[curr + 1];
		for (int j = start; j < end; j++)
		{
			int c = g.colIndices()[j];
			if (g.values()[j] != 0 && visited[c] == -1)
			{
				visited[c] = 1;
				q.push(c);
			}
		}
	}

	return visited;
}

compressed_matrix::CompressedMatrix<long long int> makeResidualSymmetric(const compressed_matrix::CompressedMatrix<long long int>& R)
{
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	int numVertices = R.numRows();
	int numVariables = numVertices / 2 - 1;
	std::map<std::pair<int, int>, long long int> MsymR;
	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			if (r != c)
			{
				MsymR[std::make_pair(r, c)] += R.values()[j];
				int compR;
				if (r <= numVariables)
					compR = r + (numVariables + 1);
				else
					compR = r - (numVariables + 1);
				int compC;
				if (c <= numVariables)
					compC = c + (numVariables + 1);
				else
					compC = c - (numVariables + 1);
				MsymR[std::make_pair(compC, compR)] += R.values()[j];
			}
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside makeResidualSym: Time elapsed_get_MsymR: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//remove extra 0s in MsymR, required because there will possibly be 0 in MsymR and the 0 will add non-exist edges in stronglyConnectedComponents()
	//cause the connected components results incorrct !!!
	std::map<std::pair<int, int>, long long int> MsymRWithoutZero;
	for (std::map<std::pair<int, int>, long long int>::const_iterator it = MsymR.begin(), end = MsymR.end(); it != end; ++it)
	{
		if (it->second != 0 && it->first.first != numVariables + 1 && it->first.second != 0) //add clearing R here !!!
			MsymRWithoutZero.insert(*it);
	}

	//curr_2 = clock();
	//mexPrintf("inside makeResidualSym: Time elapsed_remove_zero_in_MsymR: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return compressed_matrix::CompressedMatrix<long long int>(numVertices, numVertices, MsymRWithoutZero);
}

SCRet stronglyConnectedComponents(const compressed_matrix::CompressedMatrix<long long int>& R)
{
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	int numVertices = R.numRows();
	int numVariables = numVertices / 2 - 1;

	using namespace boost;

	typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
	typedef adjacency_list < vecS, vecS, directedS,
		property < vertex_name_t, std::string,
		property < vertex_index_t, long,
		property < vertex_color_t, boost::default_color_type,
		property < vertex_distance_t, long,
		property < vertex_predecessor_t, Traits::edge_descriptor > > > > >
	> Graph;

	typedef graph_traits<Graph>::vertex_descriptor Vertex;

	Graph G;

	std::vector<Traits::vertex_descriptor> root(numVertices);
	for (int i = 0; i < numVertices; ++i)
		root[i] = add_vertex(G);

	std::vector<int> component(num_vertices(G)), discover_time(num_vertices(G));
	std::vector<default_color_type> color(num_vertices(G));

	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			add_edge(root[r], root[c], G);
		}
	}

	//curr_2 = clock();
	//mexPrintf("Time elapsed_constructing_graph_for_strongly_connected_components_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//int num = strong_components(G, &component[0], root_map(&root[0]).color_map(&color[0]).discover_time_map(&discover_time[0]));
	int num = strong_components(G, boost::make_iterator_property_map(component.begin(), boost::get(boost::vertex_index, G)),
			                                root_map(boost::make_iterator_property_map(root.begin(), boost::get(boost::vertex_index, G))).color_map(boost::make_iterator_property_map(color.begin(), boost::get(boost::vertex_index, G))).discover_time_map(boost::make_iterator_property_map(discover_time.begin(), boost::get(boost::vertex_index, G))));

	//curr_2 = clock();
	//mexPrintf("Time elapsed_boost_strongly_connected_components_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//here original's content is from 0 to 2*n+1, 0: source, 1 to n, variables, n+1: sink, n+2 to 2*n+1: bar variables
	//positive: from 1 to n (1-based variables index)
	//negative: from 1 to n (1-based variables index)
	std::vector<SC> vsc(num);
	for (int i = 0; i < component.size(); i++)
	{
		int whichComponent = component[i];
		vsc[whichComponent].original.push_back(i);
		if (vsc[whichComponent].original.back() <= numVariables)
			vsc[whichComponent].positive.push_back(vsc[whichComponent].original.back());
		else
			vsc[whichComponent].negative.push_back(vsc[whichComponent].original.back() - (numVariables + 1));
	}

	std::map<std::pair<int, int>, long long int> M;

	//speed up the program a lot !!!
	//build the graph G for strongly connected components
	for (int i = 0; i < R.numRows(); i++)
	{
		int start = R.rowOffsets()[i];
		int end = R.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = R.colIndices()[j];
			int sc1 = component[r];
			int sc2 = component[c];
			if (sc1 != sc2 && R.values()[j] > 0 && M.find(std::make_pair(sc1, sc2)) == M.end())
				M.insert(std::make_pair(std::make_pair(sc1, sc2), 1));
		}
	}

	//curr_2 = clock();
	//mexPrintf("Time elapsed_after_boost_scc: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	SCRet ret;
	ret.S = vsc;
	ret.G = compressed_matrix::CompressedMatrix<long long int>(num, num, M);

	return ret;
}

std::vector<int> classifyStronglyConnectedComponents(const std::vector<SC>& S)
{
	std::vector<int> ret(S.size()); //0: self-complement, 1: non-self-complement

	for (int i = 0; i < S.size(); i++)
	{
		bool flag = true;
		std::set<int> pos(S[i].positive.begin(), S[i].positive.end());
		for (int j = 0; j < S[i].negative.size(); j++)
		{
			if (pos.find(S[i].negative[j]) != pos.end())
			{
				flag = false;
				break;
			}
		}

		if (flag)
			ret[i] = 1;
		else
			ret[i] = 0;
	}

	return ret;
}

void shortestPath(int src, const compressed_matrix::CompressedMatrix<long long int>& g, std::vector<int>& visited)
{
	std::queue<int> q;
	q.push(src);
	visited.resize(g.numRows(), 0);
	visited[src] = 1;

	while (!q.empty())
	{
		int curr = q.front(); q.pop();

		int start = g.rowOffsets()[curr];
		int end = g.rowOffsets()[curr + 1];
		for (int j = start; j < end; j++)
		{
			int c = g.colIndices()[j];
			if (g.values()[j] != 0 && !visited[c])
			{
				visited[c] = 2;
				q.push(c);
			}
		}
	}
}

std::vector<std::pair<int, int> > fixVariables(const std::vector<int>& classifiedSC, const SCRet& scRet, int numVariables)
{
	std::vector<std::pair<int, int> > ret;

	int x0Location;
	int x0BarLocation;
	std::vector<int> I;

	for (int i = 0; i < classifiedSC.size(); i++)
	{
		if (classifiedSC[i] == 1) //1: non self-complement
		{
			if (scRet.S[i].original[0] == 0)
				x0Location = i;
			else if (scRet.S[i].original[0] == numVariables + 1)
				x0BarLocation = i;
			else
				I.push_back(i);
		}
	}

	//calculate if there is a path from x0 to other nodes
	std::vector<int> visited;
	shortestPath(x0Location, scRet.G, visited); //speed up the program

	//find complement pairs
	std::vector<int> positive(numVariables + 1, -1);
	std::vector<int> negative(numVariables + 1, -1);
	for (int i = 0; i < I.size(); i++)
	{
		for (int k = 0; k < scRet.S[I[i]].positive.size(); k++)
			positive[scRet.S[I[i]].positive[k]] = I[i];
		for (int k = 0; k < scRet.S[I[i]].negative.size(); k++)
			negative[scRet.S[I[i]].negative[k]] = I[i];
	}

	std::map<int, int> complementPairs;
	for (int i = 0; i < positive.size(); i++)
	{
		if (positive[i] != -1 && negative[i] != -1)
		{
			complementPairs[positive[i]] = negative[i];
			complementPairs[negative[i]] = positive[i];
		}
	}

	std::queue<int> q;

	std::map<std::pair<int, int>, long long int> GTransMap;
	for (int i = 0; i < scRet.G.numRows(); i++)
	{
		int start = scRet.G.rowOffsets()[i];
		int end = scRet.G.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
			GTransMap.insert(std::make_pair(std::make_pair(scRet.G.colIndices()[j], i), scRet.G.values()[j]));
	}
	compressed_matrix::CompressedMatrix<long long int> GTrans(scRet.G.numRows(), scRet.G.numRows(), GTransMap);

	std::vector<int> outDegrees(scRet.G.numRows(), -1);
	for (int i = 0; i < classifiedSC.size(); i++)
	{
		if (classifiedSC[i] == 1) //non self-complement components
		{
			outDegrees[i] = scRet.G.rowOffsets()[i + 1] - scRet.G.rowOffsets()[i];
			if (outDegrees[i] == 0 && i != x0Location && i != x0BarLocation) //need to push the original outdegree 0 nodes into q !!!
				q.push(i);
		}
	}
	outDegrees[x0Location] = -1;
	outDegrees[x0BarLocation] = -1;

	for (int i = 0; i < visited.size(); i++)
	{
		if (visited[i] == 2) //exclude x0
		{
			for (int k = 0; k < scRet.S[i].positive.size(); k++)
				ret.push_back(std::make_pair(scRet.S[i].positive[k], 1));

			for (int k = 0; k < scRet.S[i].negative.size(); k++)
				ret.push_back(std::make_pair(scRet.S[i].negative[k], 0));

			int complement = complementPairs[i];

			outDegrees[i] = -1;
			outDegrees[complement] = -1;
		}
	}

	//decrease the outdegrees of node which has outgoing edges to i and to complement
	//push node which has 0 outdegrees into the queue
	for (int i = 0; i < visited.size(); i++)
	{
		if (visited[i] == 2) //exclude x0
		{
			int start = GTrans.rowOffsets()[i];
			int end = GTrans.rowOffsets()[i + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}

			int complement = complementPairs[i];
			start = GTrans.rowOffsets()[complement];
			end = GTrans.rowOffsets()[complement + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}
		}
	}

	while (!q.empty())
	{
		int curr = q.front(); q.pop();
		if (outDegrees[curr] == 0)
		{
			outDegrees[curr] = -1;
			int complement = complementPairs[curr];
			outDegrees[complement] = -1;

			//fixed all variables in component curr
			for (int k = 0; k < scRet.S[curr].positive.size(); k++)
				ret.push_back(std::make_pair(scRet.S[curr].positive[k], 1));

			for (int k = 0; k < scRet.S[curr].negative.size(); k++)
				ret.push_back(std::make_pair(scRet.S[curr].negative[k], 0));

			//decrease the outdegrees of node which has outgoing edges to i and to complement
			//push node which has 0 outdegrees into the queue
			int start = GTrans.rowOffsets()[curr];
			int end = GTrans.rowOffsets()[curr + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}

			start = GTrans.rowOffsets()[complement];
			end = GTrans.rowOffsets()[complement + 1];
			for (int k = start; k < end; k++)
			{
				if (outDegrees[GTrans.colIndices()[k]] > 0)
				{
					--outDegrees[GTrans.colIndices()[k]];
					if (outDegrees[GTrans.colIndices()[k]] == 0)
						q.push(GTrans.colIndices()[k]);
				}
			}
		}
	}

	std::sort(ret.begin(), ret.end(), compClass());

	return ret;
}

compressed_matrix::CompressedMatrix<double> computeNewQAndOffset(const compressed_matrix::CompressedMatrix<double>& Q, const std::vector<std::pair<int, int> >& fixed, double& offset)
{
	//fixed now is 1-based
	if (!fixed.empty())
	{
		int numVariables = Q.numRows();

		std::map<int, int> mI;
		for (int i = 0; i < fixed.size(); i++)
			mI[fixed[i].first - 1] = i; //-1 to make it 0-based

		std::vector<int> J;
		std::map<int, int> mJ;
		int cnt = 0;
		for (int i = 0; i < numVariables; i++)
		{
			if (mI.find(i) == mI.end())
			{
				J.push_back(i);
				mJ[i] = cnt++;
			}
		}

		std::map<std::pair<int, int>, double> QII;
		std::map<std::pair<int, int>, double> QJJ;
		std::map<std::pair<int, int>, double> QIJ;
		std::map<std::pair<int, int>, double> QJI;

		for (int i = 0; i < Q.numRows(); i++)
		{
			int start = Q.rowOffsets()[i];
			int end = Q.rowOffsets()[i + 1];
			for (int j = start; j < end; j++)
			{
				int r = i;
				int c = Q.colIndices()[j];
				if (mI.find(r) != mI.end() && mI.find(c) != mI.end())
					QII[std::make_pair(mI[r], mI[c])] = Q.values()[j];
				if (mJ.find(r) != mJ.end() && mJ.find(c) != mJ.end())
					QJJ[std::make_pair(mJ[r], mJ[c])] = Q.values()[j];
				if (mI.find(r) != mI.end() && mJ.find(c) != mJ.end())
					QIJ[std::make_pair(mI[r], mJ[c])] = Q.values()[j];
				if (mJ.find(r) != mJ.end() && mI.find(c) != mI.end())
					QJI[std::make_pair(mJ[r], mI[c])] = Q.values()[j];
			}
		}

		//off_set = x0'*Q_II*x0;
		offset = 0;
		for (std::map<std::pair<int, int>, double>::const_iterator it = QII.begin(), end = QII.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			offset += fixed[r].second * it->second * fixed[c].second;
		}

		//x0'*Q_IJ
		std::vector<double> tmp2(numVariables - fixed.size(), 0);
		for (std::map<std::pair<int, int>, double>::const_iterator it = QIJ.begin(), end = QIJ.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			tmp2[c] += it->second * fixed[r].second;
		}

		//Q_JI*x0
		std::vector<double> tmp3(numVariables - fixed.size(), 0);
		for (std::map<std::pair<int, int>, double>::const_iterator it = QJI.begin(), end = QJI.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			tmp3[r] += it->second * fixed[c].second;
		}

		for (int i = 0; i < (int)tmp3.size(); i++)
			tmp2[i] += tmp3[i];

		cnt = 0;
		for (int i = 0; i < (int)mJ.size() * (int)mJ.size(); i += numVariables - static_cast<int>(fixed.size()) + 1)
		{
			int r = i / static_cast<int>(mJ.size());
			int c = i % mJ.size();
			QJJ[std::make_pair(r, c)] += tmp2[cnt++];
		}

		std::map<std::pair<int, int>, double> mfixedQ;
		for (std::map<std::pair<int, int>, double>::const_iterator it = QJJ.begin(), end = QJJ.end(); it != end; ++it)
		{
			int r = it->first.first;
			int c = it->first.second;
			if (it->second != 0)
				mfixedQ[std::make_pair(J[r], J[c])] = it->second;
		}

		return compressed_matrix::CompressedMatrix<double>(numVariables, numVariables, mfixedQ);
	}
	else
	{
		offset = 0;
		return Q;
	}
}

std::vector<std::pair<int, int> > applyImplication(const compressed_matrix::CompressedMatrix<long long int>& A)
{
	int numVertices = A.numRows();
	int numVariables = numVertices / 2 - 1;

	//debuging only
	//clock_t curr_1 = clock();
	//clock_t curr_2;

	using namespace boost;

	typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
	typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, long long int, property<edge_residual_capacity_t, long long int, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is long long int
	//typedef adjacency_list<vecS, vecS, directedS, property<vertex_name_t, std::string>, property<edge_capacity_t, double, property<edge_residual_capacity_t, double, property<edge_reverse_t, Traits::edge_descriptor> > > > Graph; //for edge capacity is double

	Graph g;

	property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
	property_map<Graph, edge_reverse_t>::type reverse_edge = get(edge_reverse, g);
	property_map<Graph, edge_residual_capacity_t>::type residual_capacity = get(edge_residual_capacity, g);

	std::vector<Traits::vertex_descriptor> verts(numVertices);
	for (int i = 0; i < numVertices; ++i)
		verts[i] = add_vertex(g);

	Traits::vertex_descriptor s = verts[numVariables];
	Traits::vertex_descriptor t = verts[2 * numVariables + 1];

	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int cap = A.values()[j];

			Traits::edge_descriptor e1, e2;
			bool in1, in2;
			boost::tie(e1, in1) = add_edge(verts[r], verts[c], g);
			boost::tie(e2, in2) = add_edge(verts[c], verts[r], g);

			capacity[e1] = cap;
			capacity[e2] = 0;
			reverse_edge[e1] = e2;
			reverse_edge[e2] = e1;
		}
	}

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_building_graph: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	push_relabel_max_flow(g, s, t);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_max_flow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//residual
	std::vector<int> RRowOffsets(numVertices+1);
	std::vector<int> RColIndices;
	std::vector<long long int> RValues;
	//flow
	std::vector<int> FRowOffsets(numVertices+1);
	std::vector<int> FColIndices;
	std::vector<long long int> FValues;

	int offset = 0;
	int currRow = 0;
	graph_traits<Graph>::vertex_iterator u_iter, u_end;
	graph_traits<Graph>::out_edge_iterator ei, e_end;
	for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
		for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
		{
			if (capacity[*ei] > 0)
			{
				if (currRow <= *u_iter)
				{
					for (int i = currRow; i <= *u_iter; i++)
					{
						RRowOffsets[i] = offset;
						FRowOffsets[i] = offset;
					}
					currRow = static_cast<int>((*u_iter) + 1);
				}
				RColIndices.push_back(static_cast<int>(target(*ei, g)));
				RValues.push_back(residual_capacity[*ei]);
				FColIndices.push_back(static_cast<int>(target(*ei, g)));
				FValues.push_back(capacity[*ei] - residual_capacity[*ei]);
				++offset;
			}
		}
	for (int i = currRow; i < RRowOffsets.size(); i++)
		RRowOffsets[i] = offset;
	for (int i = currRow; i < FRowOffsets.size(); i++)
		FRowOffsets[i] = offset;

	compressed_matrix::CompressedMatrix<long long int> R(numVertices, numVertices, RRowOffsets, RColIndices, RValues); //residual
	compressed_matrix::CompressedMatrix<long long int> F(numVertices, numVertices, FRowOffsets, FColIndices, FValues); //flow

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_get_RF: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	//resid = (A>0).*R + F'.*(A'>0);
	std::map<std::pair<int, int>, long long int> residM;
	for (int i = 0; i < A.numRows(); i++)
	{
		int start = A.rowOffsets()[i];
		int end = A.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = A.colIndices()[j];
			long long int RValue = R.get(r, c);
			if (A.values()[j] > 0 && RValue != 0 && r != 2 * numVariables + 1 && c != numVariables)
				residM[std::make_pair(r, c)] += RValue;

			long long int FTValue = F.get(r, c); //not F(c, r) !!!
			if (A.values()[j] > 0 && FTValue != 0 && c != 2 * numVariables + 1 && r != numVariables)
				residM[std::make_pair(c, r)] += FTValue;
		}
	}

	compressed_matrix::CompressedMatrix<long long int> resid(numVertices, numVertices, residM);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_finally_get_resid: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<int> forced = bfs_for_method_2(resid, numVariables);

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_bfs_for_method_2: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	std::vector<std::pair<int, int> > fixed;
	for (int i = 0; i < forced.size(); i++)
	{
		if (forced[i] > 0)
		{
			if (i <= numVariables - 1)
				fixed.push_back(std::make_pair(i + 1, 1)); //1-based
			else if (i >= numVariables + 1 && i <= 2 * numVariables)
				fixed.push_back(std::make_pair(i - numVariables, 0)); //1-based
		}
	}

	std::sort(fixed.begin(), fixed.end(), compClass());

	//curr_2 = clock();
	//mexPrintf("inside applyImplication int version: Time elapsed_fix_vars_and_sorting: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return fixed;
}

} // anonymous namespace

namespace fix_variables_
{

FixVariablesResult fixQuboVariables(const compressed_matrix::CompressedMatrix<double>& Q, int method)
{
	//Q needs to be a square matrix
	if (Q.numRows() != Q.numCols())
		throw FixVariablesException("Q's size is not correct.");

	if (!(method == 1 || method == 2))
		throw FixVariablesException("method must be an integer of 1 or 2.");

	FixVariablesResult ret;

	//check if Q is empty
	if (Q.numRows() == 0 || Q.numCols() == 0)
	{
		ret.offset = 0;
		return ret;
	}

	int numVariables = Q.numRows();

	//clock_t curr_1 = clock();
	//clock_t curr_2;

	//uTriQ = triu(Q) + tril(Q,-1)'; //make upper triangular
	std::map<std::pair<int, int>, double> uTriQMap;
	std::set<int> usedVariables;
	for (int i = 0; i < Q.numRows(); i++)
	{
		int start = Q.rowOffsets()[i];
		int end = Q.rowOffsets()[i + 1];
		for (int j = start; j < end; j++)
		{
			int r = i;
			int c = Q.colIndices()[j];
			uTriQMap[std::make_pair(std::min(r, c), std::max(r, c))] += Q.values()[j];
			usedVariables.insert(r);
			usedVariables.insert(c);
		}
	}

	compressed_matrix::CompressedMatrix<double> uTriQ(numVariables, numVariables, uTriQMap);

	double maxAbsValue = 0;

	if (!uTriQ.values().empty())
		maxAbsValue = std::fabs(*std::max_element(uTriQ.values().begin(), uTriQ.values().end(), compareAbs));

	double ratio = 1;

	if (maxAbsValue != 0)
		ratio = static_cast<double>(std::numeric_limits<long long int>::max()) / maxAbsValue;

	ratio /= static_cast<double>(1LL << 10);

	if (ratio < 1)
		ratio = 1;

	std::map<std::pair<int, int>, long long int> uTriQMapLLI;

	for (std::map<std::pair<int, int>, double>::iterator it = uTriQMap.begin(), end = uTriQMap.end(); it != end; ++it)
		uTriQMapLLI[it->first] = static_cast<long long int>(it->second * ratio);

	compressed_matrix::CompressedMatrix<long long int> uTriQLLI(numVariables, numVariables, uTriQMapLLI);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_make upper triangular: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	Posiform p = BQPToPosiform(uTriQLLI);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_BQPToPosiform: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	if (method == 1)
	{
		compressed_matrix::CompressedMatrix<long long int> A = posiformToImplicationNetwork_1(p);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_posiformToImplicationNetwork_1: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		compressed_matrix::CompressedMatrix<long long int> R = maxFlow(A);  //use this

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_maxFlow: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		compressed_matrix::CompressedMatrix<long long int> symR = makeResidualSymmetric(R); //use this

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_makeRSym: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		//add clearing R in makeResidualSym, so here just use symR directly !!!
		SCRet scRet = stronglyConnectedComponents(symR);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_stronglyConnectedComponents: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		std::vector<int> classifiedSC = classifyStronglyConnectedComponents(scRet.S);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_classifyStrongComponents: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		ret.fixedVars = fixVariables(classifiedSC, scRet, numVariables);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_fixVarsUsingOutDegree: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;
	}
	else if (method == 2)
	{
		compressed_matrix::CompressedMatrix<long long int> A = posiformToImplicationNetwork_2(p);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_posiformToImplicationNetwork_2: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;

		ret.fixedVars = applyImplication(A);

		//curr_2 = clock();
	    //mexPrintf("Time elapsed_applyImplication: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	    //curr_1 = curr_2;
	}

	if (ret.fixedVars.size() > numVariables)
		throw FixVariablesException("ret.fixedVars has wrong size.");

	// remove unused variables from ret.fixedVars
	std::vector<std::pair<int, int> > updatedFixedVars;
	for (int i = 0; i < ret.fixedVars.size(); ++i)
	{
		if (usedVariables.find(ret.fixedVars[i].first - 1) != usedVariables.end()) // -1 to make it 0-based since usedVariables is 0-based
			updatedFixedVars.push_back(ret.fixedVars[i]);
	}

	ret.fixedVars = updatedFixedVars;

	ret.newQ = computeNewQAndOffset(uTriQ, ret.fixedVars, ret.offset);

	//curr_2 = clock();
	//mexPrintf("Time elapsed_computeNewQAndOffset: %f\n", ((double)curr_2 - curr_1) / CLOCKS_PER_SEC);
	//curr_1 = curr_2;

	return ret;
}

std::vector<std::pair<int,  int> > fixQuboVariablesMap(std::map<std::pair<int, int>, double> QMap, int QSize, int mtd)
{
    compressed_matrix::CompressedMatrix<double> QInput(QSize, QSize, QMap);

    FixVariablesResult ret = fixQuboVariables(QInput, mtd);

    return ret.fixedVars;
}

} // namespace fix_variables_


#pragma once

#include "optimizationtools/indexed_set.hpp"
#include "optimizationtools/utils.hpp"

#include <iostream>
#include <fstream>

#include <boost/filesystem.hpp>
#include <boost/regex.hpp>

/**
 * Time-dependent orienteering problem.
 *
 * Input:
 * - n locations; for each location j = 1..n, a profit pⱼ
 * - a function t(j₁, j₂, s) which returns the time to travel from location j₁
 *   to location j₂ starting at time s
 * - a time limit tₘₐₓ
 * Problem:
 * - Find a path starting at location 1 and ending at location n such that:
 *   - each location is visited at most once
 *   - the arrival at location n is before tₘₐₓ
 * Objective:
 * - Maximize the total profit of the visited locations
 *
 */

namespace orproblems
{

namespace timedependentorienteering
{

using LocationId = int64_t;
using LocationPos = int64_t;
using ArcCategory = int64_t;
using TimePeriod = int64_t;
using Time = double;
using Length = double;
using Profit = double;

struct Location
{
    Length x;
    Length y;
    Profit profit;
};

struct Arc
{
    ArcCategory category;
    Length length;
};

class Instance
{

public:

    Instance(LocationId n):
        locations_(n),
        arcs_(n, std::vector<Arc>(n)) { }
    void set_maximum_duration(Time maximum_duration) { maximum_duration_ = maximum_duration; }

    Instance(std::string instance_path, std::string format = "")
    {
        std::ifstream file(instance_path);
        if (!file.good())
            throw std::runtime_error(
                    "Unable to open file \"" + instance_path + "\".");
        if (format == "" || format == "verbeeck2014") {
            read_verbeeck2014(file, instance_path);
        } else {
            throw std::invalid_argument(
                    "Unknown instance format \"" + format + "\".");
        }
        file.close();
    }

    virtual ~Instance() { }

    inline LocationId number_of_locations() const { return locations_.size(); }
    inline const Location& location(LocationId j) const { return locations_[j]; }
    inline Time maximum_duration() const { return maximum_duration_; }
    inline Time arrival_time(LocationId j1, LocationId j2, Time start) const
    {
        Time current_time = start;
        Length remaining_length = arcs_[j1][j2].length;
        ArcCategory arc_category = arcs_[j1][j2].category;
        TimePeriod time_period =
            (current_time < 9 - 7)? 0:
            (current_time < 17 - 7)? 1:
            (current_time < 19 - 7)? 2:
            3;
        for (;;) {
            Time time_period_end =
                (time_period == 0)? 9 - 7:
                (time_period == 1)? 17 - 7:
                (time_period == 2)? 19 - 7:
                std::numeric_limits<Time>::max();
            double speed = speed_matrix_[arc_category][time_period];
            Time at = current_time + remaining_length / speed;
            if (at <= time_period_end) {
                //std::cout << "j1 " << j1
                //    << " j2 " << j2
                //    << " x1 " << locations_[j1].x
                //    << " y1 " << locations_[j1].y
                //    << " x2 " << locations_[j2].x
                //    << " y2 " << locations_[j2].y
                //    << " length " << arcs_[j1][j2].length
                //    << " cat " << arc_category
                //    << " start " << start
                //    << " dur " << at - start
                //    << " arrival " << at << " / " << time_period_end
                //    << std::endl;
                return at;
            }
            remaining_length -= (time_period_end - current_time) * speed;
            //std::cout << "remaining_length " << remaining_length << std::endl;
            current_time = time_period_end;
            time_period++;
        }
        return -1;
    }

    std::pair<bool, Time> check(
            std::string certificate_path,
            int verbose = 1) const
    {
        // Initial display.
        if (verbose >= 1) {
            std::cout
                << "Checker" << std::endl
                << "-------" << std::endl;
        }

        std::ifstream file(certificate_path);
        if (!file.good())
            throw std::runtime_error(
                    "Unable to open file \"" + certificate_path + "\".");

        LocationId n = number_of_locations();
        LocationId j = -1;
        LocationId j_prec = 0;
        optimizationtools::IndexedSet locations(n);
        locations.add(0);
        locations.add(n - 1);
        LocationPos number_of_duplicates = 0;
        Time current_time = 0;
        Profit profit = location(0).profit;
        while (file >> j) {
            if (locations.contains(j)) {
                number_of_duplicates++;
                if (verbose == 2)
                    std::cout << "Location " << j << " is already scheduled." << std::endl;
            }
            locations.add(j);
            current_time = arrival_time(j_prec, j, current_time);
            profit += location(j).profit;
            if (verbose == 2)
                std::cout << "Location: " << j
                    << "; Time: " << current_time
                    << "; Profit: " << profit << " (" << location(j).profit << ")"
                    << std::endl;
            j_prec = j;
        }
        current_time = arrival_time(j_prec, n - 1, current_time);
        profit += location(n - 1).profit;

        bool feasible
            = (current_time <= maximum_duration())
            && (number_of_duplicates == 0);
        if (verbose == 2)
            std::cout << "---" << std::endl;
        if (verbose >= 1) {
            std::cout << "Number of locations:       " << locations.size() << " / " << n  << std::endl;
            std::cout << "Number of duplicates:      " << number_of_duplicates << std::endl;
            std::cout << "Duraction:                 " << current_time << " / " << maximum_duration() << std::endl;
            std::cout << "Feasible:                  " << feasible << std::endl;
            std::cout << "Profit:                    " << profit << std::endl;
        }
        return {feasible, profit};
    }

private:

    void read_verbeeck2014(
            std::ifstream& file,
            std::string instance_path)
    {
        std::string tmp;
        LocationId n = -1;
        file
            >> tmp >> n
            >> tmp >> tmp
            >> tmp >> maximum_duration_
            ;
        locations_ = std::vector<Location>(n);
        for (LocationId j = 0; j < n; ++j)
            file >> locations_[j].x >> locations_[j].y >> locations_[j].profit;
        arcs_ = std::vector<std::vector<Arc>>(n, std::vector<Arc>(n));
        for (LocationId j1 = 0; j1 < n; ++j1) {
            for (LocationId j2 = 0; j2 < n; ++j2) {
                Length dx = locations_[j1].x - locations_[j2].x;
                Length dy = locations_[j1].y - locations_[j2].y;
                Length dxy = std::sqrt(dx * dx + dy * dy);
                dxy /= 5;
                arcs_[j1][j2].length = dxy;
            }
        }

        // Read speed matrix.
        auto speed_matrix_path = boost::filesystem::path(instance_path)
            .parent_path().parent_path().parent_path() /= "speedmatrix.txt";
        std::ifstream speed_matrix_file(speed_matrix_path.string());
        speed_matrix_ = std::vector<std::vector<double>>(
                5, std::vector<double>(4, 0.0));
        for (ArcCategory arc_category = 0; arc_category < 5; ++arc_category)
            for (TimePeriod time_period = 0; time_period < 4; ++time_period)
                speed_matrix_file >> speed_matrix_[arc_category][time_period];

        // Read arc category matrix.
        boost::filesystem::directory_iterator it_end;
        auto arc_category_directory = boost::filesystem::path(instance_path)
            .parent_path().parent_path();
        const boost::regex filter("arc_cat_.*\\.txt");
        for (boost::filesystem::directory_iterator it(arc_category_directory); it != it_end; ++it) {
            if (!boost::filesystem::is_regular_file(it->status()))
                continue;
            boost::smatch what;
            // For V2:
            //if(!boost::regex_match(it->leaf(), what, filter))
            //    continue;
            // For V3:
            if (!boost::regex_match(it->path().filename().string(), what, filter))
                continue;
            // File matches, store it
            std::ifstream arc_category_file(it->path().string());
            for (LocationId location_id_1 = 0; location_id_1 < number_of_locations(); ++location_id_1)
                for (LocationId location_id_2 = 0; location_id_2 < number_of_locations(); ++location_id_2)
                    arc_category_file >> arcs_[location_id_1][location_id_2].category;
        }
    }

    std::vector<Location> locations_;
    std::vector<std::vector<Arc>> arcs_;
    std::vector<std::vector<double>> speed_matrix_;
    Time maximum_duration_ = 0;

};

static inline std::ostream& operator<<(
        std::ostream &os, const Instance& instance)
{
    os << "number of locations: " << instance.number_of_locations() << std::endl;
    for (LocationId j = 0; j < instance.number_of_locations(); ++j) {
        os << "location " << j
            << " profit " << instance.location(j).profit;
        os << std::endl;
    }
    return os;
}

}

}


/**
 * @file yaml_eigen.h
 * @author Alexander Herzog
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2015-02-27
 *
 * @brief Add support for eigen frm the yaml-cpp standard package.
 */

#pragma once

#include <type_traits>
#include <iomanip>
#include <Eigen/Eigen>
#include <yaml-cpp/yaml.h>

namespace robot_math{
template<class T>
struct MovableEigenVector;
template<class T>
struct MovableEigenMatrix;
}

namespace YAML {

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct EigenVectorConverter{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> VectorType;

  static void resize_if_needed(int rows, int cols, VectorType& rhs){
    if(rhs.size() != rows*cols)
    {
      if (VectorType::SizeAtCompileTime == Eigen::Dynamic &&
          VectorType::MaxSizeAtCompileTime == Eigen::Dynamic)
      {
        rhs.resize(rows*cols);
      }else
      {
        std::ostringstream error;
        error << "ERROR: The fixed sized vector of size (" << rhs.size()
              << ") is of different size than the input yaml data vector of "
              << "size (" << rows * cols << ")." ;
        throw(std::runtime_error(error.str()));
      }
    }
  }

  static Scalar& access_element(int r, int c, VectorType& rhs)
  {
    return rhs(r+c);
  }
};

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct EigenMatrixConverter{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> MatrixType;

  static void resize_if_needed(int rows, int cols, MatrixType& rhs){
    if(rhs.rows() != rows || rhs.cols() != cols)
    {
      if (MatrixType::SizeAtCompileTime == Eigen::Dynamic &&
          MatrixType::MaxSizeAtCompileTime == Eigen::Dynamic)
      {
        rhs.resize(rows, cols);
      }else
      {
        std::ostringstream error;
        error << "ERROR: The fixed sized matrix of dim (" << rhs.rows() << ","
              << rhs.cols() << ") is of different dim than the input yaml "
              << "data matrix of dim (" << rows << "," << cols << ")." ;
        throw(std::runtime_error(error.str()));
      }
    }
  }

  static Scalar& access_element(int r, int c, MatrixType& rhs){
    return rhs(r, c);
  }
};

template<class Scalar, int Rows, int Cols, int Align, int RowsAtCompileTime, int ColsAtCompileTime>
struct convert<Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> >{
  typedef Eigen::Matrix<Scalar,  Rows,  Cols,  Align,  RowsAtCompileTime,  ColsAtCompileTime> Eigen_Type_;
  const static bool is_vector_type_ = Rows == 1 || Cols == 1;
  typedef typename std::conditional<is_vector_type_,
      EigenVectorConverter<Scalar, Rows, Cols, Align, RowsAtCompileTime, ColsAtCompileTime>,
      EigenMatrixConverter<Scalar, Rows, Cols, Align, RowsAtCompileTime, ColsAtCompileTime>>::type Converter_;

  static Node encode(const Eigen_Type_& rhs) {
    Eigen::IOFormat yaml_format(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", "[", "]", "[", "]");
    std::stringstream ss;
    ss << std::setprecision (std::numeric_limits<double>::digits10 + 1) << rhs.format(yaml_format);
    return Load(ss.str());
  }

  static bool decode_2d(const Node& node, Eigen_Type_& rhs){
    const size_t n_rows = node.size();
    const size_t n_cols = node[0].size();
    Converter_::resize_if_needed(n_rows, n_cols, rhs);

    for (size_t r=0;r<n_rows;++r){
      const Node& yaml_row = node[r];
      if(yaml_row.size() != n_cols)
        return false;
      for (size_t c=0;c<n_cols;++c)
        Converter_::access_element(r, c, rhs) = yaml_row[c].as<Scalar>();
    }

    return true;
  }

  static bool decode_1d(const Node& node, Eigen_Type_& rhs){
    const size_t n_size = node.size();
    Converter_::resize_if_needed(n_size, 1, rhs);
    for (size_t r=0;r<n_size;++r)
      Converter_::access_element(r, 0, rhs) = node[r].as<Scalar>();
    return true;
  };

  static bool decode(const Node& node, Eigen_Type_& rhs){
    if(!node.IsSequence())
      return false;
    if(node.size() > 0)
    {
      if(!node[0].IsSequence())
        return decode_1d(node, rhs);
      else
        return decode_2d(node, rhs);
    }
    else
    {
      Converter_::resize_if_needed(0, 0, rhs);
      return true;
    }
  }
};

template<class T>
struct convert<robot_math::MovableEigenVector<T>> {
  static Node encode(const robot_math::MovableEigenVector<T>& mp){
    return convert<T>::encode(static_cast<const T&>(mp));
  }
  static bool decode(const Node& node, robot_math::MovableEigenVector<T>& mp){
    return convert<T>::decode(node, static_cast<T&>(mp));
  }
};

template<>
struct convert<Eigen::Quaterniond> {
  static Node encode(const Eigen::Quaterniond& mp){
    return convert<Eigen::Vector4d>::encode(Eigen::Vector4d(mp.w(), mp.x(), mp.y(), mp.z()));
  }
  static bool decode(const Node& node, Eigen::Quaterniond& mp){
    Eigen::Vector4d v4;
    if(!convert<Eigen::Vector4d>::decode(node, v4)) return false;
    mp.w() = v4[0];
    mp.x() = v4[1];
    mp.y() = v4[2];
    mp.z() = v4[3];
    return true;
  }
};

} // namespace YAML


/// @file metropolis_hastings.hpp A generic templated Metropolis-Hastings sampler implementation

#ifndef BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP
#define BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP

#include <cmath>
#include <boost/tuple/tuple.hpp>

#include "../partition_sampler_items.hpp"
#include "../detail/random.hpp"
#include "../detail/fun.hpp"
#include "../mh_moves/mh_moves.hpp"

namespace biggles { namespace sampling {

/// \brief attributes of the current sampling state
struct mh_state_observer {
    /// @brief The log probability density of the current sample.
    ///
    /// this is either last_sample_log_density (if proposal was rejected)
    /// or last_proposal_log_density (if proposal was accepted)
    float sample_log_density;

    /// @brief last log(alpha) where alpha is uniformly distributed between 0 and 1
    float last_log_alpha;

    /// @brief log density of the last proposed move
    float last_proposal_log_density;

    /// \brief the density of the last sample p(T|theta, y)
    float last_sample_log_density;

    /// @brief the last proposal density ratio Q(T|T*)/Q(T*|T)
    float last_pdr;

    /// @brief The number of samples which have been accepted during the lifetime of this sampler. This will always be
    /// less than or equal to \c n_proposed_.
    size_t n_accepted;

    /// @brief The number of samples which have been proposed during the lifetime of this sampler.
    size_t n_proposed;

    /// @brief was the last proposal accepted?
    bool accepted;

    mh_state_observer() :
        sample_log_density(-std::numeric_limits<float>::max()),
        last_log_alpha(-std::numeric_limits<float>::max()),
        last_proposal_log_density(-std::numeric_limits<float>::max()),
        last_sample_log_density(-std::numeric_limits<float>::max()),
        last_pdr(-std::numeric_limits<float>::max()),
        n_accepted(1),
        n_proposed(1),
        accepted(false) {}

    mh_state_observer(const mh_state_observer& o) :
        sample_log_density(o.sample_log_density),
        last_log_alpha(o.last_log_alpha),
        last_proposal_log_density(o.last_proposal_log_density),
        last_sample_log_density(o.last_sample_log_density),
        last_pdr(o.last_pdr),
        n_accepted(o.n_accepted),
        n_proposed(o.n_proposed),
        accepted(o.accepted) {}

    const mh_state_observer& operator = (const mh_state_observer& o) {
        if (&o == this) return *this;
        sample_log_density = o.sample_log_density;
        last_log_alpha = o.last_log_alpha;
        last_proposal_log_density = o.last_proposal_log_density;
        last_sample_log_density = o.last_sample_log_density;
        last_pdr = o.last_pdr;
        n_accepted = o.n_accepted;
        n_proposed = o.n_proposed;
        accepted = o.accepted;
        return *this;
    }

    /// \brief the total accptance rate
    float acceptance_rate() const { return static_cast<float>(n_accepted) / static_cast<float>(n_proposed); }

};

/// @brief A Metropolis-Hastings sampler.
///
/// Metropolis-Hastings is an algorithm which can sample from an arbitrary probability density function, \f$ P(x) \f$,
/// given only that the PDF can be evaluated. In fact it is even more general: the function may return some constant
/// multiple of the true PDF meaning that M-H is exceptionally useful when the PDF is known up to some normalisation
/// factor. The PDF \f$ P(x) \f$ is termed the target distribution.
///
/// In addition to being able to evaluate the target distribution, one must be able to draw samples from a proposal
/// distribution, \f$ Q(x' | x) \f$. In this case \f$ x' \f$ is a new sample and \f$ x \f$ is the sample we currently
/// are at.
///
/// To use the sampler you must provide at a minimum a functor which can evaluate the logarithm of the target PDF at a
/// given sample and a proposal functor. The proposal functor takes a current sample, \f$ x \f$, and returns a new
/// sample, \f$ x' \f$, conditioned on it. In addition, it returns
///
/// \f[
/// \log\left(\frac{Q(x|x')}{Q(x'|x)}\right) = \log(Q(x|x')) - \log(Q(x'|x)).
/// \f]
///
/// This logarithm may of course be zero to indicate a symmetric proposal distribution.
///
/// In addition you may optionally provide the C++ type of the sample and a functor which can sample uniformly from the
/// interval [0,1]. The boost library is used to provide a Mersenne twister-based uniform sampler if you do not specify
/// an alternative.
///
/// As an example of use, consider sampling the Rosenbrock function:
///
/// @code
/// typedef boost::tuples::tuple<float, float> point;
///
/// inline float rosenbrock(const point& p)
/// {
///     float x, y;
///     boost::tuples::tie(x,y) = p;
///     return (1.f-x)*(1.f-x) + 100.f*(y-x*x)*(y-x*x);
/// }
/// @endcode
///
/// Firstly, we need to convert this function which has a minimum at (1,1) to a log-PDF with a maximum at (1,1). Since
/// we do not need to worry about normalisation, this is quite easy:
///
/// @code
/// // a cooked-up PDF which has a maximum where the Rosenbrock function has a minimum.
/// inline float rosenbrock_log_pdf(const point& p)
/// {
///     return -rosenbrock(p);
/// }
/// @endcode
///
/// We also need a proposal function. The proposal function should accept a sample, in this case a \c point, and return
/// a tuple containing a new sample and the log proposal density ration outlined above. We shall use a simple Guassian
/// proposal function which moves the sample according to a Gaussian distribution centred on the current sample with a
/// standard deviation of 0.2. Since this proposal distribution is symmetric, we can return 0 as the log proposal ratio:
///
/// @code
/// typedef boost::tuples::tuple<point, float> proposal_result_t;
///
/// inline proposal_result_t propose_gaussian(const point& p)
/// {
///     static boost::mt19937 rng;
///     static boost::normal_distribution<float> norm(0.f, 0.2f);
///     static boost::variate_generator<boost::mt19937&, boost::normal_distribution<float> > variate(rng, norm);
///
///     point new_p(p.get<0>() + variate(), p.get<1>() + variate());
///
///     return proposal_result_t(new_p, 0.f);
/// }
/// @endcode
///
/// Since these functions are plain C++ functions, we shall wrap them in the functor classes available in the standard
/// library's \c functor header. We firstly need to define a type for them:
///
/// @code
/// typedef std::pointer_to_unary_function<const point&, float> pdf_t;
/// typedef std::pointer_to_unary_function<const point&, proposal_result_t> proposal_func_t;
/// @endcode
///
/// Using the standard library \c std::ptr_fun function, we can now declare our sampler.
///
/// @code
/// // MH sampler for Rosenbrock function. An example of using a function pointer directly.
/// sampler<pdf_t, proposal_func_t> s(std::ptr_fun(rosenbrock_log_pdf),
///                                   std::ptr_fun(propose_gaussian));
/// @endcode
///
/// In this case, the sample type has been inferred directly from the types of the proposal and target functions. You
/// may also specify it directly if necessary.
///
/// Calling draw() on \c s will now draw point locations from the Rosenbrock PDF we defined earlier. In real-world use,
/// you should keep an eye on the value returned from acceptance_rate() to check it is around 0.25.
///
/// @sa http://en.wikipedia.org/wiki/Metropolis%E2%80%93Hastings_algorithm
///
class metropolis_hastings_sampler {
public:
    /// @brief Initialise the sampler.
    ///
    /// @param target A functor representing the target distribution. This is copied internally.
    /// @param propose A functor for proposing new samples. This is copied internally.
    /// @param initial_sample An initial starting sample.
    /// @param uniform A functor for generating uniform reals on the interval [0,1].
    metropolis_hastings_sampler(const partition_distribution& target, const partition_proposal& propose,
            const partition_sampler_sample& initial_sample )
        : target_(target)
        , propose_(propose)
        , sample_(initial_sample)
        , last_proposal_(partition_proposal::result_type(initial_sample, -std::numeric_limits<float>::max()))
        , temperature_(1.0f)
        , acceptance_seq_(5000)
        , internal_records_ptr_(new capability_recorder(initial_sample.partition_sample_ptr->first_time_stamp(),
                            initial_sample.partition_sample_ptr->last_time_stamp(),
                            initial_sample.partition_sample_ptr->tracks()))
    {
        //state_.sample_log_density = target(initial_sample);
    }

    /// @brief Draw the next sample from the sampler.
    ///
    /// @return A reference to the new sample. This can be retrieved before the next call to draw() via last_sample().
    const partition_sampler_sample& draw();

    /// @brief Obtain a reference to the last sample drawn from the sampler.
    const partition_sampler_sample& last_sample() const { return sample_; }

    /// @brief Obtain the log probability density of the last sample drawn from the sampler. log(P(x| theta, D)
    float current_sample_log_density() const { return state_.sample_log_density; }

    /// @brief Obtain the current acceptance rate of the sampler.
    ///
    /// The acceptance rate is defined as the ratio of accepted proposals to total number of proposals. The usual rule
    /// of thumb for Metropolis-Hastings is that this value, often termed \f$ \alpha \f$, should be around 0.25.
    float acceptance_rate() const { return state_.acceptance_rate(); }

    /// @brief Obtain the recent acceptance rate of the sampler.
    ///
    /// This acceptance rate only takes the last samples into account instead of the whole sampling history
    float recent_acceptance_rate() const {
        return std::accumulate(acceptance_seq_.begin(), acceptance_seq_.end(), 0.f)/float(acceptance_seq_.size());
    }

    /// @brief The last proposed sample
    partition_sampler_sample proposed_sample() const { return last_proposal_.partition_sample; }

    /*
    /// @brief The last proposed move
    mh_moves::move_type proposed_move() const { return proposed_sample().executed_move; }
    */

    /// @brief last log alpha
    float last_log_alpha() const { return state_.last_log_alpha; }

    /// @brief log density of the last proposed partition log(P(x'|theta, D))
    float last_log_density() const { return state_.last_proposal_log_density; }

    /// @brief last proposal density ratio Q(T|T*)/Q(T*|T)
    float last_pdr() const {return state_.last_pdr; }

    bool last_proposal_accepted() const { return state_.accepted; }

    mh_state_observer current_state() const { return state_; }
    internal_containers_observer current_internals() const { return intern_; }

    void set_temperature(float temperature) {
        if (temperature > 1.f or temperature <= 0.f) {
            throw std::runtime_error("temperature must be in (0, 1].");
        }
        temperature_ = temperature;
    }

protected:
    /// @brief The target distribution functor.
    partition_distribution target_;

    /// @brief The proposal function and PDF. See class documentation.
    partition_proposal propose_;

    /// @brief The current sample.
    partition_sampler_sample sample_;

    /// @brief The last (possibly not accepted) proposal
    partition_proposal::result_type last_proposal_;

    mh_state_observer state_;
    internal_containers_observer intern_;

    float temperature_;

    fqueue<int> acceptance_seq_;

    capability_recorder_ptr internal_records_ptr_;

};

} }

#endif // BIGGLES_SAMPLING_METROPOLIS_HASTINGS_HPP