#include "ModalWarpingTrainingModel.h"
#include "Integrator/ImplicitNewMatkSparseIntegrator.h"
#include <random>
#include <fstream>
#include <iostream>
#include <time.h>
#include "Simulator/DeepWarp/ModalRotationMatrix.h"
#include "Integrator/ImplicitModalWarpingIntegrator.h"
#include "Functions/GeoMatrix.h"
#include <Eigen/QR>
#include "Functions/findElementInVector.h"
#include "LoboVolumetricMesh/LoboVolumetriceMeshCore.h"
#include "Simulator/ForceField/RotateForceField.h"

#include "Simulator/ReducedSTVK/ReducedSTVKModel.h"
#include "Simulator/ReducedForceModel/ReducedForceModel.h"
#include "Simulator/ReducedForceModel/ReducedSTVKForceModel.h"
#include "Integrator/ImpicitNewMarkDenseIntegrator.h"

ModalWarpingTrainingModel::ModalWarpingTrainingModel(LoboVolumetricMesh* volumtrciMesh_, VectorXd gravity_, SparseMatrix<double>* modalRotationSparseMatrix_, ModalRotationMatrix* modalrotationMatrix_, MatrixXd* subspaceModes_, int r, double timestep, SparseMatrix<double>* massMatrix_, LoboForceModel* forcemodel_, LoboForceModel* nonlinearforceModel_, int numConstrainedDOFs_, int* constrainedDOFs_, double dampingMassCoef, double dampingStiffnessCoef, bool useStaticSolver) :WarpingMapTrainingModel(subspaceModes_, r, timestep, massMatrix_, forcemodel_, nonlinearforceModel_, numConstrainedDOFs_, constrainedDOFs_, dampingMassCoef, dampingStiffnessCoef, useStaticSolver)
{
	this->volumtrciMesh = volumtrciMesh_;
	this->modalRotationSparseMatrix = modalRotationSparseMatrix_;
	this->modalrotationMatrix = modalrotationMatrix_;
	localOrientationMatrixR = new SparseMatrix<double>();
	matrixR_ = new SparseMatrix<double>();

	this->gravity = gravity_;

	//0.001
	modalwarpingintegrator = new ImplicitModalWarpingIntegrator(modalrotationMatrix, modalRotationSparseMatrix,
		r, 0.001, massMatrix_, loboforceModel, 1, numConstrainedDOFs_, constrainedDOFs_, dampingMassCoef, dampingStiffnessCoef, 1, 1e-7, 0.25, 0.5, false
		);

	if (subspaceModes->size() > 0)
	{
		std::cout << "read modes please" << std::endl;
		reducedMassMatrix = new MatrixXd();
		*reducedMassMatrix = subspaceModes->transpose()*(*massMatrix)**subspaceModes;
		int r = subspaceModes->cols();
		VectorXd reducedgraivty(r);
		int R = volumtrciMesh->getNumVertices() * 3;

		reducedSTVKmodel = new ReducedSTVKModel(this->volumtrciMesh, this->massMatrix, subspaceModes);

		reducedSTVKmodel->computeGravity(&reducedgraivty, massMatrix, R);
		reducedSTVKmodel->setGravityForce(reducedgraivty.data());
		reducedSTVKmodel->setGravity(false);
		reducedSTVKmodel->computeReducedModesCoefficients();

		reducedforcemodel = new ReducedSTVKForceModel(reducedSTVKmodel);

		reducedIntergrator = new ImpicitNewMarkDenseIntegrator(r, timestep, reducedMassMatrix, reducedforcemodel, 0, 0, dampingMassCoef, dampingStiffnessCoef, 1);
	}

	modalwarpingintegrator->setDampingMassCoef(0.1);
	modalwarpingintegrator->setDampingStiffnessCoef(0.1); //0.1

	nonLinearIntegrator->setDampingMassCoef(0.3);
	nonLinearIntegrator->setDampingStiffnessCoef(0.3);
	nonLinearIntegrator->setTimeStep(0.005);

	integrator->setDampingMassCoef(0.8);
	integrator->setDampingStiffnessCoef(0.8);

	nonLinearIntegrator->setMaxInteration(50);
}

ModalWarpingTrainingModel::~ModalWarpingTrainingModel()
{
	delete localOrientationMatrixR;
	delete matrixR_;
	delete reducedIntergrator;
	delete modalwarpingintegrator;
	delete reducedforcemodel;
	delete reducedSTVKmodel;
}

void ModalWarpingTrainingModel::excute()
{
	if (getForcefieldType() == 1)
	{
		modalwarpingintegrator->setTimeStep(0.002);
		modalwarpingintegrator->setDampingMassCoef(0.2);
		modalwarpingintegrator->setDampingStiffnessCoef(0.2); //0.1
		method12Twist();
	}
	else
	if (getForcefieldType() == 0)
	{
		modalwarpingintegrator->setTimeStep(0.002);
		modalwarpingintegrator->setDampingMassCoef(0.5);
		modalwarpingintegrator->setDampingStiffnessCoef(0.5); //0.1
		method10();
	}
	//methodFortwist();
	//method9();
	return;
}

void ModalWarpingTrainingModel::getTraingLinearForce(int index, VectorXd &force)
{
	//force = modalwarpingintegrator->getInteranlForce(trainingNonLinearDis[index]);

	//force = gravity;
	Vector3d nodeacc = forcefieldDirection[index];
	VectorXd fullforce = this->createGravity(nodeacc);
	force = fullforce;
	//force = gravity;
}

void ModalWarpingTrainingModel::getTraingLinearForce(VectorXd& lq, VectorXd &force)
{
	force = modalwarpingintegrator->getInteranlForce(lq);
	//force = gravity;
}

void ModalWarpingTrainingModel::getTraingLinearForce(VectorXd& lq, VectorXd &force, double poisson)
{
	LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
	LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
	enmateria->setNu(poisson);
	modalwarpingintegrator->updateMaterial();
	force = modalwarpingintegrator->getInteranlForce(lq);

}

Eigen::VectorXd ModalWarpingTrainingModel::getNonlinearInternalforce(int disid)
{
	return nonLinearIntegrator->getInteranlForce(trainingNonLinearDis[disid]);
}

void ModalWarpingTrainingModel::subexcute3()
{
	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	trainingLinearDis.clear();
	trainingNonLinearDis.clear();
	int maxIteration = 1000;
	trainingLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	trainingNonLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	VectorXd reducedForce(subspaceModes->cols());
	int dataCount = 0;

	while (dataCount < getNumTrainingSet())
	{
		std::cout << dataCount << std::endl;
		srand(time(NULL) + dataCount);
		std::default_random_engine generator;
		generator.seed(time(NULL) + dataCount);
		std::uniform_real_distribution<double> latitude_distribution(-forceScale, forceScale);
		double scale = (this->forceScale / getNumTrainingSet()) *(getNumTrainingSet() - dataCount) + 0.01;
		std::cout << "scale " << scale << std::endl;
		double angle = (PI_/2 / getNumTrainingSet())*dataCount + 0.01;

		Vector3d axis(0, 0, 1);

		angle *= -1;

		//axis.setRandom();
		//axis.normalize();
		//test

		VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		//fullforce = gravity*scale;

		nonLinearIntegrator->setExternalForces(fullforce.data());
		nonLinearIntegrator->resetToRest();
		nonLinearIntegrator->setSaveStepResidual(true);
		VectorXd q_dis;

		//store training linear dis
		//trainingLinearDis.push_back(q_dis);
		double preNorm = DBL_MAX;
		VectorXd preDis = gravity;
		preDis.setConstant(1000);

		double residual;

		for (int j = 0; j < maxIteration; j++)
		{
			nonLinearIntegrator->doTimeStep();
			double norm_ = nonLinearIntegrator->getVectorq().norm();
			VectorXd cur_dis = nonLinearIntegrator->getVectorq();

			residual = std::abs((preNorm - norm_) / preNorm);
			//We only pick the displacement which has larget difference
			if (residual > 0.001)
			{
				if (norm_ > 0.01)
				{
					trainingNonLinearDis.push_back(cur_dis);

					VectorXd curExternalForce = nonLinearIntegrator->getStep_residual();
					modalwarpingintegrator->setSaveStepResidual(true);
					convergeIntegrationLocal(modalwarpingintegrator, curExternalForce, 1000);
					q_dis = modalwarpingintegrator->getVectorq();

					trainingLinearDis.push_back(q_dis);

					dataCount++;
				}
			}
			else
			{
				break;
			}

			preNorm = norm_;
			if (residual < 1e-5)
			{
				break;
			}

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		nonLinearIntegrator->setSaveStepResidual(false);
	}
}

void ModalWarpingTrainingModel::subexcute()
{
	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	trainingLinearDis.clear();
	trainingNonLinearDis.clear();
	int maxIteration = 1000;
	trainingLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	trainingNonLinearDis.reserve(getNumTrainingSet() + getNumTrainingHighFreq());
	VectorXd reducedForce(subspaceModes->cols());
	int dataCount = 0;
	while (dataCount < getNumTrainingSet())
	{
		std::cout << dataCount << std::endl;
		srand(time(NULL) + dataCount);
		std::default_random_engine generator;
		generator.seed(time(NULL) + dataCount);
		std::uniform_real_distribution<double> latitude_distribution(-forceScale, forceScale);
		double scale = (this->forceScale / getNumTrainingSet()) *(getNumTrainingSet()-dataCount) + 0.01;
		std::cout <<"scale "<< scale << std::endl;
		double angle = (PI_ / getNumTrainingSet())*dataCount - PI_ / 2 + 0.1;

		Vector3d axis(0, 0, 1);
		//axis.setRandom();
		//axis.normalize();
		//test
		angle = PI_ / 2;

		VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		fullforce = gravity*scale;

		modalwarpingintegrator->resetToRest();
		modalwarpingintegrator->setSaveStepResidual(true);
		double preNorm = DBL_MAX;
		double residual = DBL_MAX;
		VectorXd preq = fullforce;
		preq.setConstant(10000);

		for (int j = 0; j < maxIteration; j++)
		{
			VectorXd fullq = modalwarpingintegrator->getVectorq();
			VectorXd w = (*modalRotationSparseMatrix)*fullq;

			//set the external force
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w);
			//VectorXd mergedExternalForce = (*localOrientationMatrixR)*fullforce;
			modalwarpingintegrator->setExternalForces(fullforce.data());
			modalwarpingintegrator->doTimeStep();

			double norm_ = modalwarpingintegrator->getVectorq().norm();

			residual = std::abs((fullq - preq).norm() / preq.norm());

			if (residual > 0.01)
			{
				preNorm = norm_;
				preq = fullq;

				if (norm_ > 0.01)
				{
					bool reset = false;
					if (j == 0)
					{
						reset = true;
					}

					VectorXd q_dis;
					q_dis = modalwarpingintegrator->getVectorq();
					w = (*modalRotationSparseMatrix)*q_dis;
					//store training linear dis
					trainingLinearDis.push_back(q_dis);
					VectorXd curExternalForce = modalwarpingintegrator->getStep_residual();
					modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w,false);
					curExternalForce = (*localOrientationMatrixR)*curExternalForce;

					convergeIntegrationNonLinear(nonLinearIntegrator, curExternalForce, maxIteration, reset);
					q_dis = nonLinearIntegrator->getVectorq();
					trainingNonLinearDis.push_back(q_dis);
					dataCount++;
					std::cout << dataCount << std::endl;
				}
			}

			if (residual < 1e-6)
			{
				break;
			}

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		modalwarpingintegrator->setSaveStepResidual(false);
	}
	std::cout << "excute finished" << std::endl;
}

void ModalWarpingTrainingModel::subexcute2()
{
	std::vector<Vector3d> nodedisplacement;

	generateRotationVectorSample(M_PI * 2, 0, 3,
		M_PI / 2, -M_PI / 2, 1,
		0, M_PI/3, 2, nodedisplacement);

	/*Vector3d firstNode = nodedisplacement[10];
	nodedisplacement.clear();
	nodedisplacement.push_back(firstNode);*/

	int r = this->gravity.rows();

	double maxNodedisdance = -DBL_MAX;
	int maxNodeid = -1;

	for (int j = 0; j < r / 3; j++)
	{
		if (node_distance[j] > maxNodedisdance)
		{
			maxNodedisdance = node_distance[j];
			maxNodeid = j;
		}
	}

	LoboNodeBase* nodep = volumtrciMesh->getNodeRef(maxNodeid);
	int correspondingSize = 3 + nodep->neighbor.size() * 3;
	int Phi_1_Size = nodep->neighbor.size() * 3;
	MatrixXd phi1(3, Phi_1_Size);
	phi1.setZero();
	MatrixXd phi2(3, 3);
	phi2.setZero();
	MatrixXd phitotal(3, Phi_1_Size + 3);

	for (int k = 0; k < modalRotationSparseMatrix->outerSize(); ++k)
		for (SparseMatrix<double>::InnerIterator it(*modalRotationSparseMatrix, k); it; ++it)
		{
			if (it.row() / 3 == maxNodeid)
			{
				int neighborid = it.col() / 3;
				int insideIndex = findElementIndex(nodep->neighbor, neighborid);
				if (insideIndex != -1)
				{
					phi1.data()[(insideIndex * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();
					phitotal.data()[(insideIndex * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();

				}
				if (it.col() / 3 == maxNodeid)
				{
					phi2.data()[(it.col() % 3) * 3 + it.row() % 3] = it.value();
					phitotal.data()[(nodep->neighbor.size() * 3 + it.col() % 3) * 3 + it.row() % 3] = it.value();
				}
			}
		}

	MatrixXd invPhi1 = pseudoinverse(phi1);
	MatrixXd invPhitotal = pseudoinverse(phitotal);

	if (subspaceModes == NULL)
	{
		std::cout << "subspaceModes is NULL" << std::endl;
	}

	MatrixXd subModes(Phi_1_Size + 3, subspaceModes->cols());
	subModes.setZero();
	for (int i = 0; i < nodep->neighbor.size() * 3; i++)
	{
		int neighborid = nodep->neighbor[i / 3];
		int row = neighborid * 3 + i % 3;
		for (int j = 0; j < subspaceModes->cols(); j++)
		{
			subModes.data()[j*subModes.rows() + i] = subspaceModes->data()[j*subspaceModes->rows() + row];
		}
	}

	for (int i = 0; i < 3; i++)
	{
		int row = maxNodeid * 3 + i;
		for (int j = 0; j < subspaceModes->cols(); j++)
		{
			subModes.data()[j*subModes.rows() + i + Phi_1_Size] = subspaceModes->data()[j*subspaceModes->rows() + row];
		}
	}

	MatrixXd phiSubModes = phitotal*subModes;
	MatrixXd invPhiSubModes = pseudoinverse(phiSubModes);
	MatrixXd invSubModes = pseudoinverse(subModes);

	modalwarpingintegrator->setStoreLagrangeMultipliers(true);

	int numscale = 1;

	std::vector<VectorXd> exforceList;

	for (int i = 0; i < subspaceModes->cols(); i++)
	{
		VectorXd lq = subspaceModes->col(i);
		VectorXd w = (*modalRotationSparseMatrix)*lq;
		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
		VectorXd nq = *localOrientationMatrixR*lq;
		VectorXd constrainForce = nonLinearIntegrator->getInteranlForce(nq);
		exforceList.push_back(constrainForce);
	}

	if (0)
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		VectorXd w(r);
		w.setZero();

		Vector3d w_j = nodedisplacement[i];

		for (int j = 0; j < numscale; j++)
		{
			Vector3d uscale = Vector3d(0, -j*0.06, 0);

			VectorXd targetq;

			VectorXd T(nodep->neighbor.size() * 3 + 3);
			T.setZero();

			for (int k = 0; k < T.rows() / 3; k++)
			{
				T.data()[k * 3 + 0] = uscale.data()[0];
				T.data()[k * 3 + 1] = uscale.data()[1];
				T.data()[k * 3 + 2] = uscale.data()[2];
			}

			targetq = invPhitotal*(w_j - phitotal*T);
			targetq += T;

			VectorXd linearq;
			VectorXd reducedq = invPhiSubModes*(w_j);
			linearq = subModes*reducedq;
			targetq = linearq + T;

			SparseMatrix<double> constrainMatrix;
			VectorXd constrainTarget;

			VectorXd lq(r);
			lq.setZero();
			lq = *subspaceModes*invSubModes*targetq;
			w = (*modalRotationSparseMatrix)*lq;
			modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
			VectorXd nq = *localOrientationMatrixR*lq;

			VectorXd constrainForce = nonLinearIntegrator->getInteranlForce(nq);
			/*constrainForce = modalwarpingintegrator->getInteranlForce(lq);
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
			constrainForce = *localOrientationMatrixR*constrainForce;*/
			exforceList.push_back(constrainForce);

			//VectorXd nq;
			////nq = *localOrientationMatrixR*lq;
			//nq = nonLinearIntegrator->getVectorq();

			//trainingLinearDis.push_back(lq);

			//trainingNonLinearDis.push_back(nq);
		}
	}

	int maxIteration = 1000;
	int dataCount = 0;
	/*VectorXd temp = exforceList[0];
	exforceList.clear();
	exforceList.push_back(temp);*/

	this->setNumTrainingSet(exforceList.size());
	this->setNumTrainingHighFreq(0);

	std::cout << "num low fre data." << getNumTrainingSet() << std::endl;
	std::cout << "num high fre data." << getNumTrainingHighFreq() << std::endl;

	for (int i = 0; i < exforceList.size(); i++)
	{
		std::cout <<"--------"<< i <<"----------"<< std::endl;
		VectorXd fullforce = exforceList[i];

		//reducedForce.setRandom();
		//fullforce = (*subspaceModes)*reducedForce;

		//fullforce.setRandom();
		//fullforce *= scale;

		nonLinearIntegrator->setExternalForces(fullforce.data());
		nonLinearIntegrator->resetToRest();
		nonLinearIntegrator->setSaveStepResidual(true);
		VectorXd q_dis;

		//store training linear dis
		//trainingLinearDis.push_back(q_dis);
		double preNorm = DBL_MAX;
		VectorXd preDis = gravity;
		preDis.setConstant(1000);

		double residual;
		VectorXd qn = subspaceModes->col(i);
		bool converged = convergeIntegrationLocal(modalwarpingintegrator, fullforce, 1000, true);
		VectorXd ql = modalwarpingintegrator->getVectorq();
		trainingNonLinearDis.push_back(qn);
		trainingLinearDis.push_back(ql);

		if (0)
		for (int j = 0; j < maxIteration; j++)
		{
			nonLinearIntegrator->doTimeStep();

			double norm_ = nonLinearIntegrator->getVectorq().norm();
			VectorXd cur_dis = nonLinearIntegrator->getVectorq();

			residual = std::abs((preNorm - norm_) / preNorm);
			//We only pick the displacement which has larget difference
			if (residual < 1e-5)
			{
				if (norm_ > 0.01)
				{
					VectorXd curExternalForce = nonLinearIntegrator->getStep_residual();
					modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, cur_dis, true);
					curExternalForce = (*localOrientationMatrixR)*curExternalForce;

					bool ifreset = false;
					if (j == 0)
					{
						ifreset = true;
					}
					bool converged = convergeIntegrationLocal(modalwarpingintegrator, curExternalForce, 1000, ifreset);

					if (!converged)
					{
						continue;
					}

					q_dis = modalwarpingintegrator->getVectorq();
					trainingNonLinearDis.push_back(cur_dis);
					trainingLinearDis.push_back(q_dis);

					dataCount++;
				}
				break;
			}

			preNorm = norm_;

			if (dataCount > getNumTrainingSet())
			{
				break;
			}
		}

		nonLinearIntegrator->setSaveStepResidual(false);
	}

	std::cout << "excute finished" << std::endl;

}

void ModalWarpingTrainingModel::subexcute4()
{
	int numScale = 100;
	this->setNumTrainingSet(numScale);
	this->setNumTrainingHighFreq(0);

	MatrixXd forceMatrix(subspaceModes->rows(), numScale);

	for (int i = 0; i < numScale; i++)
	{
		VectorXd lq = subspaceModes->col(2) * (i*0.1+0.1);

		VectorXd w = *modalRotationSparseMatrix*lq;

		VectorXd extforce = modalwarpingintegrator->getInteranlForce(lq);

		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);

		extforce = *localOrientationMatrixR*extforce;

		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);

		VectorXd guessq = *localOrientationMatrixR*lq;
		VectorXd modalwarpingforce = nonLinearIntegrator->getInteranlForce(guessq);
		std::cout <<"diff force = > "<< (modalwarpingforce - extforce).norm() / extforce.norm() << std::endl;
		forceMatrix.col(i) = extforce;

		//convergeIntegrationNonLinear(nonLinearIntegrator, extforce, 1000, false);

		//VectorXd nq = nonLinearIntegrator->getVectorq();

		trainingNonLinearDis.push_back(guessq);
		trainingLinearDis.push_back(lq);

	}

}

void ModalWarpingTrainingModel::subexcute5()
{
	int numScale = 600;
	int numData = 400;
	this->setNumTrainingSet(numData-1);
	this->setNumTrainingHighFreq(0);

	for (int i = 0; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd lq = ((subspaceModes->col(3)*10)/numScale)*(i+1);
		VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
		VectorXd w = *modalRotationSparseMatrix*lq;
		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
		VectorXd nf = *localOrientationMatrixR*lf;
		modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
		VectorXd wq = *matrixR_*lq;

		if (i / numScale < 0.1)
		{
			nonLinearIntegrator->setState(wq.data());
		}

		nonLinearIntegrator->setMaxInteration(80);
		convergeIntegrationNonLinear(nonLinearIntegrator, nf, 100, false);

		VectorXd nq = nonLinearIntegrator->getVectorq();

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
		}
	}

}

void ModalWarpingTrainingModel::method6()
{

	int numScale = 100;
	int numData = 100;
	this->setNumTrainingSet(numData*2);
	this->setNumTrainingHighFreq(0);

	int numVertex = volumtrciMesh->getNumVertices();
	std::ofstream test("test2.txt");
	VectorXd nodeYaxis(numVertex * 3);
	nodeYaxis.setZero();
	for (int i = 0; i < numVertex; i++)
	{
		nodeYaxis.data()[i * 3 + 1] = 1;
	}

	VectorXd lq = (subspaceModes->col(3) * 10);

	VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
	VectorXd w = *modalRotationSparseMatrix*lq;
	modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
	VectorXd lqorientation = *localOrientationMatrixR*nodeYaxis;

	VectorXd nf = *localOrientationMatrixR*lf;
	modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
	VectorXd wq = *matrixR_*lq;
	modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, wq, false);
	VectorXd nqorientation = *localOrientationMatrixR*nodeYaxis;
	test << lqorientation.transpose() << std::endl;
	test << nqorientation.transpose() << std::endl;
	test.close();
	std::cout << (lqorientation - nqorientation).norm() / nqorientation.norm() << std::endl;

	VectorXd ori_p(numVertex * 3);
	for (int i = 0; i < numVertex; i++)
	{
		Vector3d nodeori = volumtrciMesh->getNodeRestPosition(i);
		ori_p.data()[i * 3 + 0] = nodeori.data()[0];
		ori_p.data()[i * 3 + 1] = nodeori.data()[1];
		ori_p.data()[i * 3 + 2] = nodeori.data()[2];
	}

	for (int i = 0; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd nq = ((*matrixR_*lq) / numScale)*(i + 1);
		VectorXd originlq = lq / numScale*(i + 1);

		VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
		modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, nq, true);
		//modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, true);
		VectorXd warp_nf = *localOrientationMatrixR*real_nf;

		convergeIntegrationLocal(modalwarpingintegrator, warp_nf, 200, true);

		VectorXd real_lq = modalwarpingintegrator->getVectorq();
		//VectorXd real_lq;
		//real_lq = *localOrientationMatrixR*nq;

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(originlq);
		}
	}
}

void ModalWarpingTrainingModel::method7()
{
	int numScale = 1;
	int numData = 1;
	this->setNumTrainingSet(numData * 2);
	this->setNumTrainingHighFreq(0);

	Vector3d axis(0, 0, 1);
	double angle = PI_/2;
	VectorXd fullforce = this->rotationGravity(gravity, angle, axis);

	convergeIntegrationNonLinear(nonLinearIntegrator, fullforce, 1000, true);
	VectorXd targetnq = nonLinearIntegrator->getVectorq();

	for (int i = numScale-1; i < numScale; i++)
	{
		std::cout << "Iteration => " << i << std::endl;
		VectorXd nq = ((targetnq) / numScale)*(i + 1);
		VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
		modalrotationMatrix->computeLocalOrientationByPolarDecomposition(localOrientationMatrixR, nq, true);
		VectorXd warp_nf = *localOrientationMatrixR*real_nf;
		convergeIntegrationLocal(modalwarpingintegrator, warp_nf, 200, true);
		VectorXd real_lq = modalwarpingintegrator->getVectorq();

		//warp real_lq
		VectorXd w = *modalRotationSparseMatrix*real_lq;
		modalrotationMatrix->computeWarpingRotationMatrixR(matrixR_, w);
		VectorXd wq = *matrixR_*real_lq;

		if (numScale - i <= numData)
		{
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(wq);
		}
	}

}

void ModalWarpingTrainingModel::method8()
{
	std::vector<Vector3d> nodedisplacement;
	nodedisplacement.clear();
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	std::vector<bool> ifreset;

	//if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI/3.0, -M_PI / 2.0, 10,
			9.8, 4.9, 2, nodedisplacement, ifreset, potentialDirection);
		samplePoissonRatio(0.3, 0.3, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	/*nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);*/

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	std::vector<VectorXd> ql_list;

	int dataCount = 0;
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		VectorXd nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		convergeIntegrationNonLinearBuffer(nonLinearIntegrator, fullforce, 2000, qn_list, true);

		int range = qn_list.size() / numDataPerSample;
		range = 1;
		for (int j = 0; j <qn_list.size(); j++)
		{
			VectorXd nq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((nq - trainingNonLinearDis.back()).norm() / trainingNonLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			VectorXd real_nf = nonLinearIntegrator->getInteranlForce(nq);
			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, nq, true);
			VectorXd warp_nf = *localOrientationMatrixR*real_nf;

			bool reset = false;

			if (j == 0 && ifreset[i] == true)
			{
				reset = true;
			}

			convergeIntegrationLocal(modalwarpingintegrator, real_nf, 2000, reset);

			VectorXd real_lq = modalwarpingintegrator->getVectorq();

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(real_lq);
			forcefieldDirection.push_back(potentialDirection[i]);
			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());

	std::cout << "finished" << std::endl;
}

void ModalWarpingTrainingModel::method9()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	nodedisplacement.clear();
	ifreset.clear();

	//if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI / 2.0, -M_PI / 2.0, 10,
			4.0, 2.0, 2, nodedisplacement, ifreset, potentialDirection);

		samplePoissonRatio(0.2, 0.2, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	/*nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);*/

	//nodedisplacement.push_back(Vector3d(-4.9, 1.83721e-032, 3.00038e-016));
	//ifreset.push_back(true);
	//potentialDirection.push_back(Vector3d(-1, 0, 0));
	//poissonPerdirection.push_back(0.2);

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout <<"poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	int dataCount = 0;
	nonLinearIntegrator->resetToRest();
	//VectorXd q_test(volumtrciMesh->getNumVertices() * 3);
	//q_test.setRandom();
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		Vector3d nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		//reset = false;
		bool reset = ifreset[i];

		std::cout <<"ifrest"<< reset << std::endl;
		convergeIntegrationLocalBuffer(modalwarpingintegrator, fullforce, 1000, qn_list, reset);

		int range = qn_list.size() / numDataPerSample;
		if (range <= 1)
		{
			range = 2;
		}
		range = 1;

		std::cout <<"range => "<< range << std::endl;
		for (int j = 0; j < qn_list.size(); j += range)
		{
			std::cout << "force " << i << "/" << nodedisplacement.size() - 1 << std::endl;
			std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
			VectorXd lq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			//use ku = f instead of ku = Rf
			VectorXd nq;
			if (getLinearDisOnly())
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
				nq = *localOrientationMatrixR*lq;
			}
			else
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
				VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
				VectorXd nf = *localOrientationMatrixR*lf;

				bool reset = false;

				if (j == 0 && ifreset[i] == true)
				{
					reset = true;
				}

				bool converged = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

				nq = nonLinearIntegrator->getVectorq();

				if (converged == false)
				{
					std::cout << "break" << std::endl;
					break;
				}
			}

			if (dataCount % 2 == 1)
			{
				dataCount++;
				continue;
			}

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
			forcefieldDirection.push_back(potentialDirection[i]);

			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());
}

void ModalWarpingTrainingModel::method10()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	nodedisplacement.clear();
	ifreset.clear();

	if (0)
	{
		generateRotationVectorSample(M_PI, M_PI, 1,
			M_PI / 2.0, -M_PI / 2.0, 10,
			4.0, 2.0, 2, nodedisplacement, ifreset, potentialDirection);

		samplePoissonRatio(0.2, 0.2, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);
	}

	nodedisplacement.push_back(Vector3d(0, -1, 0)*9.8);
	ifreset.push_back(true);
	potentialDirection.push_back(Vector3d(0, -1, 0));
	poissonPerdirection.push_back(0.20);

	//nodedisplacement.push_back(Vector3d(-4.9, 1.83721e-032, 3.00038e-016));
	//ifreset.push_back(true);
	//potentialDirection.push_back(Vector3d(-1, 0, 0));
	//poissonPerdirection.push_back(0.2);

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int numDataPerSample = getNumTrainingSet() / nodedisplacement.size();
	setNumTrainingHighFreq(0);

	std::vector<VectorXd> qn_list;
	int dataCount = 0;
	nonLinearIntegrator->resetToRest();
	//VectorXd q_test(volumtrciMesh->getNumVertices() * 3);
	//q_test.setRandom();
	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		Vector3d nodeacc = nodedisplacement[i];
		VectorXd fullforce = this->createGravity(nodeacc);

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		//nonLinearIntegrator->updateMaterial();

		//reset = false;
		bool reset = ifreset[i];

		std::cout << "ifrest" << reset << std::endl;
		convergeIntegrationLocalBuffer(modalwarpingintegrator, fullforce, 1000, qn_list, reset);

		int range = qn_list.size() / numDataPerSample;
		if (range <= 1)
		{
			range = 2;
		}
		range = 1;

		std::cout << "range => " << range << std::endl;
		for (int j = 0; j < qn_list.size(); j += range)
		{
			std::cout << "force " << i << "/" << nodedisplacement.size() - 1 << std::endl;
			std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
			VectorXd lq = qn_list[j];

			if (trainingLinearDis.size() > 0)
				if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-3)
				{
					//the diff is too small
					continue;
				}

			//use ku = f instead of ku = Rf
			VectorXd nq;
			if (getLinearDisOnly())
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);
				nq = *localOrientationMatrixR*lq;
			}
			else
			{
				VectorXd w = *modalRotationSparseMatrix*lq;
				modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
				VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);
				VectorXd nf = *localOrientationMatrixR*lf;

				bool reset = false;

				if (j == 0 && ifreset[i] == true)
				{
					reset = true;
				}

				VectorXd reducednf = subspaceModes->transpose()*nf;

				bool converged = this->convergeIntegrationNonlinearReduced(reducedIntergrator, nf, 1000, reset);

				nq = nonLinearIntegrator->getVectorq();

				if (converged == false)
				{
					std::cout << "break" << std::endl;
					break;
				}
			}

			if (dataCount % 2 == 1)
			{
				dataCount++;
				continue;
			}

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(lq);
			forcefieldDirection.push_back(potentialDirection[i]);

			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
			dataCount++;
		}
	}
	setNumTrainingSet(trainingNonLinearDis.size());
}

void ModalWarpingTrainingModel::method10Twist()
{
	std::vector<VectorXd> qn_list;
	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.0005);

	convergeIntegrationNonLinearBuffer(nonLinearIntegrator, forcefield, 300, qn_list, true);

	delete forcefield;

	int range = 1;
	std::cout << "range => " << range << std::endl;
	for (int j = 0; j < qn_list.size(); j += range)
	{
		std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
		VectorXd nq = qn_list[j];

		if (trainingNonLinearDis.size() > 0)
			if ((nq - trainingNonLinearDis.back()).norm() / trainingNonLinearDis.back().norm() < 1e-3)
			{
				//the diff is too small
				continue;
			}

		VectorXd nf = nonLinearIntegrator->getInteranlForce(nq);

		bool reset = false;
		if (j == 0)
			reset = true;

		convergeIntegrationLocal(modalwarpingintegrator, nf, 1000, reset);
		VectorXd lq = modalwarpingintegrator->getVectorq();

		trainingNonLinearDis.push_back(nq);
		trainingLinearDis.push_back(lq);
		std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
		dataCount++;
	}
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::method11Twist()
{
	std::vector<VectorXd> qn_list;
	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.002);
	convergeIntegrationLocalBuffer(modalwarpingintegrator, forcefield, 200, qn_list, true);

	delete forcefield;

	//qn_list.clear();
	//
	//double maxscale = 8;
	//for (int i = 0; i < 4000; i++)
	//{
	//	double scale = maxscale / 4000 * i;
	//	qn_list.push_back(subspaceModes->col(2)*scale);
	//}

	int range = 1;
	std::cout << "range => " << range << std::endl;
	for (int j = 0; j < qn_list.size(); j += range)
	{
		std::cout << "q_list " << j << "/" << qn_list.size() - 1 << std::endl;
		VectorXd lq = qn_list[j];

		if (trainingLinearDis.size() > 0)
			if ((lq - trainingLinearDis.back()).norm() / trainingLinearDis.back().norm() < 1e-4)
			{
				continue;
			}

		VectorXd w = *modalRotationSparseMatrix*lq;
		modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
		VectorXd lf = modalwarpingintegrator->getInteranlForce(lq);

		VectorXd nf = *localOrientationMatrixR*lf;

		bool reset = false;
		if (j == 0)
			reset = true;
		reset = false;

		bool converge = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

		VectorXd nq = nonLinearIntegrator->getVectorq();

		if (converge == false)
		{
			break;
		}

		/*if (j % 2 != 0)
		{
			continue;
		}*/

		trainingNonLinearDis.push_back(nq);
		trainingLinearDis.push_back(lq);

		std::cout << dataCount << "/" << getNumTrainingSet() << std::endl;
		dataCount++;
	}
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::method12Twist()
{
	std::vector<Vector3d> nodedisplacement;
	std::vector<Vector3d> potentialDirection;
	std::vector<double> poissonPerdirection;
	nodedisplacement.clear();
	std::vector<bool> ifreset;

	/*generateRotationVectorSample(M_PI/2 , 0, 3,
		M_PI / 2, 0, 3,
		1, 1, 1, nodedisplacement);*/

	generateRotationVectorSample(M_PI, M_PI, 1,
		-M_PI / 2, -M_PI / 2, 1,
		1, 1, 1, nodedisplacement, ifreset, potentialDirection);

	samplePoissonRatio(0.20, 0.20, 1, 1, nodedisplacement, potentialDirection, ifreset, poissonPerdirection);

	//nodedisplacement.clear();
	//nodedisplacement.push_back(Vector3d(1, 0, 0));

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		std::cout << nodedisplacement[i].transpose() << std::endl;
		std::cout << "poisson " << poissonPerdirection[i] << std::endl;
	}

	int dataCount = 0;

	RotateForceField* forcefield = new RotateForceField(volumtrciMesh, Vector3d(1, 0, 0));
	forcefield->setForceMagnitude(0.001);
	forcefield->centeraxis_position = this->getPotentialCenter();

	std::cout << "nodedisplacement.size()" << nodedisplacement.size() << std::endl;

	for (int i = 0; i < nodedisplacement.size(); i++)
	{
		forcefield->centeraxis = nodedisplacement[i].normalized();
		std::cout << forcefield->centeraxis.transpose() << std::endl;

		LoboVolumetricMesh::Material* materia = volumtrciMesh->getMaterialById(0);
		LoboVolumetricMesh::ENuMaterial* enmateria = (LoboVolumetricMesh::ENuMaterial*)materia;
		enmateria->setNu(poissonPerdirection[i]);
		modalwarpingintegrator->updateMaterial();
		nonLinearIntegrator->updateMaterial();

		double preNorm = DBL_MAX;
		double residual = DBL_MAX;

		int r = volumtrciMesh->getNumVertices() * 3;
		int maxIteration = 200;

		VectorXd preq(r);
		VectorXd extForce(r);
		VectorXd nq(r);
		nq.setZero();
		extForce.setZero();
		preq.setConstant(1000);

		bool converged = false;
		std::cout << std::endl;

		modalwarpingintegrator->resetToRest();
		for (int j = 0; j < maxIteration; j++)
		{
			bool reset = false;
			if (j == 0)
				reset = true;

			std::cout << '\r';
			std::cout << j;

			volumtrciMesh->setDisplacement(nq.data());
			forcefield->computeCurExternalForce(extForce);

			VectorXd mergedExternalForce = extForce;
			modalwarpingintegrator->setExternalForces(mergedExternalForce.data());

			modalwarpingintegrator->doTimeStep();

			VectorXd curq = modalwarpingintegrator->getVectorq();

			residual = std::abs((preq - curq).norm() / preq.norm());
			preq = curq;

			VectorXd w = *modalRotationSparseMatrix*curq;

			double maxangle = getMaxRotatedAngleFromW(w);
			std::cout << "  maxangle => " << maxangle << "  ";

			/*if (maxangle > M_PI)
			{
				break;
			}*/

			modalrotationMatrix->computeLocalOrientationMatrixR(localOrientationMatrixR, w, false);
			VectorXd lf = modalwarpingintegrator->getInteranlForce(curq);
			VectorXd nf = *localOrientationMatrixR*lf;

			//bool converge = true;

			bool converge = this->convergeIntegrationNonLinear(nonLinearIntegrator, nf, 1000, reset);

			nq = nonLinearIntegrator->getVectorq();

			if (converge == false)
			{
				std::cout << "not coverged" << " " << std::endl;
				break;
			}

			/*if (j % 2 != 0)
			{
			continue;
			}*/

			trainingNonLinearDis.push_back(nq);
			trainingLinearDis.push_back(curq);
			forcefieldDirection.push_back(forcefield->centeraxis);
			poissonPerDis.push_back(poissonPerdirection[i]);

			std::cout << "         " << trainingNonLinearDis.size() << " ";

			if (residual < 1e-5)
			{
				converged = true;
				std::cout << "converged" << std::endl;
				break;
			}
		}
	}

	std::cout << std::endl;
	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);
}

void ModalWarpingTrainingModel::methodFortwist()
{
	VectorXd baselq = subspaceModes->col(2);
	for (int i = -50; i < 50; i++)
	{
		VectorXd lq = baselq*i*0.1;
		trainingNonLinearDis.push_back(lq);
		trainingLinearDis.push_back(lq);
	}

	setNumTrainingSet(trainingNonLinearDis.size());
	setNumTrainingHighFreq(0);

}

bool ModalWarpingTrainingModel::convergeIntegrationLocal(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset)
{
	if (reset)
	integrator_loc->resetToRest();

	/*modalwarpingintegrator->computeStaticDisplacement(extForce, false);

	return true;*/

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);

	bool converged = false;

	for (int j = 0; j < maxIteration; j++)
	{
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-6)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationLocalBuffer(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);

	bool converged = false;
	qn_list.clear();
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationLocalBuffer(LoboIntegrator* integrator_loc, RotateForceField* forcefiled, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	std::cout << "convergeIntegrationLocal === >" << std::endl;
	double preNorm = DBL_MAX;
	double residual = DBL_MAX;

	int r = volumtrciMesh->getNumVertices() * 3;

	VectorXd preq(r);
	VectorXd extForce(r);
	extForce.setZero();
	preq.setConstant(1000);

	bool converged = false;
	qn_list.clear();
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		VectorXd fullq = integrator_loc->getVectorq();
		//set the external force
		VectorXd w = *modalRotationSparseMatrix*fullq;
		modalrotationMatrix->computeWarpingRotationMatrixR(localOrientationMatrixR, w);

		VectorXd nq = (*localOrientationMatrixR)*fullq;
		volumtrciMesh->setDisplacement(nq.data());
		forcefiled->computeCurExternalForce(extForce);

		VectorXd mergedExternalForce = extForce;
		integrator_loc->setExternalForces(mergedExternalForce.data());

		integrator_loc->doTimeStep();

		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();

		if (residual < 1e-5)
		{
			converged = true;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;
	return converged;
}

bool ModalWarpingTrainingModel::convergeIntegrationNonLinear(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset)
{
	if (reset)
		integrator_loc->resetToRest();

	bool converge = ((ImplicitNewMatkSparseIntegrator*)integrator_loc)->computeStaticDisplacement(extForce, false);

	return converge;

	std::cout << "convergeIntegrationNonLinear === >" << std::endl;

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);
	std::cout << std::endl;

	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;

	std::cout << residual << std::endl;

	return true;
}

bool ModalWarpingTrainingModel::convergeIntegrationNonlinearReduced(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, bool reset /*= true*/)
{
	if (reset)
		integrator_loc->resetToRest();

	bool converge = ((ImpicitNewMarkDenseIntegrator*)integrator_loc)->computeStaticDisplacement(extForce, false);

	return converge;
}

void ModalWarpingTrainingModel::convergeIntegrationNonLinearBuffer(LoboIntegrator* integrator_loc, VectorXd &extForce, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	qn_list.clear();

	if (reset)
		integrator_loc->resetToRest();

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;
	VectorXd preq(extForce.rows());
	preq.setConstant(1000);
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;
		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();
		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = integrator_loc->getVectorq();
		if (residual < 1e-5)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;
	std::cout << residual << std::endl;
}

void ModalWarpingTrainingModel::convergeIntegrationNonLinearBuffer(LoboIntegrator* integrator_loc, RotateForceField* forcefiled, int maxIteration, std::vector<VectorXd> &qn_list, bool reset /*= true*/)
{
	qn_list.clear();

	if (reset)
		integrator_loc->resetToRest();

	double preNorm = DBL_MAX;
	double residual = DBL_MAX;

	int r = volumtrciMesh->getNumVertices() * 3;

	VectorXd preq(r);
	VectorXd extForce(r);

	preq.setConstant(1000);
	std::cout << std::endl;
	for (int j = 0; j < maxIteration; j++)
	{
		std::cout << '\r';
		std::cout << j;

		VectorXd iq = integrator_loc->getVectorq();
		volumtrciMesh->setDisplacement(iq.data());
		forcefiled->computeCurExternalForce(extForce);

		integrator_loc->setExternalForces(extForce.data());
		integrator_loc->doTimeStep();
		VectorXd curq = integrator_loc->getVectorq();

		qn_list.push_back(curq);
		residual = std::abs((preq - curq).norm() / preq.norm());
		preq = curq;
		if (residual < 1e-4)
		{
			std::cout << curq.norm() << std::endl;
			std::cout << "converged" << std::endl;
			break;
		}
	}
	std::cout << std::endl;
	std::cout << residual << std::endl;
}

typedef Eigen::Triplet<double> EIGEN_TRI;

void ModalWarpingTrainingModel::generateConstrainMatrixAndVector(SparseMatrix<double>* constrainMatrix, VectorXd &constrainTarget, LoboNodeBase* nodep, int nodeid, VectorXd target_)
{
	int numVertex = this->volumtrciMesh->getNumVertices();
	int totalConstrain = numConstrainedDOFs + nodep->neighbor.size() * 3 +3;

	constrainMatrix->resize(totalConstrain, numVertex * 3);
	constrainTarget.resize(totalConstrain, 1);
	constrainTarget.setZero();
	std::vector<EIGEN_TRI> entrys;

	for (int i = 0; i < numConstrainedDOFs; i++)
	{
		int row = i;
		int col = constrainedDOFs[i];
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = 0;
	}

	for (int i = 0; i < nodep->neighbor.size()*3; i++)
	{
		int row = i + numConstrainedDOFs;
		int col = nodep->neighbor[i/3]*3 + i % 3;
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = target_.data()[i];
	}

	for (int i = 0; i < 3; i++)
	{
		int row = i + numConstrainedDOFs + nodep->neighbor.size() * 3;
		int col = nodeid * 3 + i;
		entrys.push_back(EIGEN_TRI(row, col, 1));
		constrainTarget.data()[row] = target_.data()[nodep->neighbor.size() * 3+i];
	}
	constrainMatrix->setFromTriplets(entrys.begin(), entrys.end());

}

double ModalWarpingTrainingModel::getMaxRotatedAngleFromW(VectorXd &w)
{
	double maxAngle = -DBL_MAX;
	int numVertex = w.rows() / 3;
	for (int i = 0; i < numVertex; i++)
	{
		Vector3d wi;
		wi.data()[0] = w.data()[i * 3 + 0];
		wi.data()[1] = w.data()[i * 3 + 1];
		wi.data()[2] = w.data()[i * 3 + 2];
		double angle = wi.norm();
		if (angle > maxAngle)
		{
			maxAngle = angle;
		}
	}
	return maxAngle;
}

void ModalWarpingTrainingModel::samplePoissonRatio(double maxPoisson, double minPoisson, int maxNumPoisson, int minNumPoisson, std::vector<Vector3d> &nodeforce, std::vector<Vector3d> &forcedirection, std::vector<bool> &ifreset, std::vector<double> &poissonPerdirection)
{
	int num_direction = nodeforce.size();
	poissonPerdirection.clear();

	Vector3d baseline(-1, 0, 0);
	double maxpoisson = maxPoisson;
	double minpoisson = minPoisson;
	int maxnumpoisson = maxNumPoisson;
	int minnumpoisson = minNumPoisson;
	this->poissonPerDis.clear();

	std::vector<Vector3d> finalnodeforce;
	std::vector<Vector3d> finalforcedirection;
	std::vector<bool> finalifreset;

	for (int i = 0; i < num_direction; i++)
	{
		double angle = forcedirection[i].dot(baseline);
		angle = std::acos(angle);
		int  numpoisson = angle / (M_PI / 2.0)*(minnumpoisson - maxnumpoisson) + maxnumpoisson;
		for (int j = 0; j < numpoisson; j++)
		{
			finalnodeforce.push_back(nodeforce[i]);
			finalforcedirection.push_back(forcedirection[i]);
			finalifreset.push_back(ifreset[i]);

			double interval = (maxpoisson - minpoisson) / (numpoisson - 1);
			if (numpoisson == 1)
			{
				interval = (maxpoisson - minpoisson);
			}
			double poisson = interval*j + minpoisson;
			poissonPerdirection.push_back(poisson);
		}
	}

	nodeforce = finalnodeforce;
	forcedirection = finalforcedirection;
	ifreset = finalifreset;
}


// This file generate the score file .pss and produce a skeleton file
#include <cstdlib>
#include <ctime>
#include <math.h>
#include <stdexcept>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>

#include "urlearning/base/record_file.h"
#include "urlearning/base/bayesian_network.h"
#include "urlearning/base/variable.h"

#include "urlearning/ad_tree/ad_node.h"
#include "urlearning/ad_tree/ad_tree.h"

#include "urlearning/scoring_function/scoring_function.h"
#include "urlearning/scoring_function/score_calculator.h"

#include "urlearning/scoring_function/constraints.h"
#include "urlearning/base/skeleton.hpp"

#include "urlearning/scoring_function/bdeu_scoring_function.h"
#include "urlearning/scoring_function/bic_scoring_function.h"
#include "urlearning/scoring_function/fnml_scoring_function.h"
#include "urlearning/scoring_function/lasso_entropy_scoring_function.h"
#include "urlearning/scoring_function/adaptive_lasso_entropy.h"
#include "urlearning/scoring_function/BIC_OLS.h"
namespace po = boost::program_options;
/**
 * The file containing the data.
 */
std::string inputFile;

/**
 * The delimiter in each line.
 */
char delimiter = ',';

/**
 * The file to write the scores.
 */
std::string outputFile;

/**
 * File specifying constraints on the scores.
 */
std::string constraintsFile;

/**
 * Skeleton File specifies the skeleton superstructure
 */
std::string skeletonFile;

datastructures::Skeleton skeleton;

/**
 * The minimum number of records in the AD-tree.
 */
int rMin = 5;

/**
 * The scoring function to use.
 */
std::string sf = "lasso";

/**
 * which score to use, 0 for lasso, 1 for entropy1, 2 for entropy2
 */
int which = 1;
/**
 * A reference to the scoring function object.
 */
scoring::ScoringFunction *scoringFunction;

/**
 * The ess to use for BDeu.
 */

float equivalentSampleSize = 1.0f;

/**
 * A hard limit on the size of parent sets.
 */
int maxParents = -1;

/**
 * The number of threads to use.
 */
int threadCount = 1;

/**
 * The maximum amount of time to use for each variable.
 */
int runningTime = -1;

/**
 * Whether the data file has variable names in the first row.
 */
bool hasHeader = false;

/**
 * Whether to prune the scores before printing
 */
bool prune = true;

/**
 * Whether to use deCampos-style pruning.
 */
bool enableDeCamposPruning = false;

/**
 * The network information.
 */
datastructures::BayesianNetwork network;

/**
 * Constraints on the allowed scores.
 */
scoring::Constraints *constraints;

inline std::string getTime() {
    time_t now = time(0);
    tm *gmtm = gmtime(&now);
    std::string dt(asctime(gmtm));
    boost::trim(dt);
    return dt;
}

void scoringThread(int thread) {
    scoring::ScoreCalculator scoreCalculator(scoringFunction, maxParents, network.size(), runningTime, constraints);
    const int num = network.size();

    for (int variable = 0; variable < network.size(); variable++) {
        if (variable % threadCount != thread) {
            continue;
        }

        printf("Thread: %d, Variable: %d, Time: %s\n", thread, variable, getTime().c_str());

        FloatMap sc;
        init_map(sc);
	// also include neighbors' neighbors
	varset orig_neighbor_bits = skeleton.get_neighbors(variable);
	varset neighbor_bits = orig_neighbor_bits;
	const int card1 = cardinality(orig_neighbor_bits);
	for(int j=0; j < num; j++)
	{
	  if(VARSET_GET(orig_neighbor_bits, j) and j != variable)
	    neighbor_bits = VARSET_OR( neighbor_bits, skeleton.get_neighbors(j) );
	}
	const int card2 = cardinality(neighbor_bits);
        scoreCalculator.calculateScores( variable, sc, neighbor_bits );

        //#ifdef DEBUG
        int size = sc.size();
        printf("Thread: %d, Variable: %d, Size before pruning: %d, Time: %s, neighbor cardinality %d/%d: %s / %s\n"
	      , thread, variable, size, getTime().c_str(), card1, card2
	      , varsetToString(orig_neighbor_bits).c_str(), varsetToString(neighbor_bits).c_str() );
        //#endif

        //Ni added, print out score
        //printf("Independent and dependent scores, variable # %d, ind_score %f, depend_score %f\n", variable, sc[0], sc[optimal_parents[variable]]);
//    Ni added, commented out on June 10, 2017, prune might not work with continuous case
//        if (prune) {
//            scoreCalculator.prune(sc);
//            int prunedSize = sc.size();
//            printf("Thread: %d, Variable: %d, Size after pruning: %d, Time: %s\n", thread, variable, prunedSize, getTime().c_str());
//        }

        std::string varFilename = outputFile + "." + TO_STRING(variable);
        FILE *varOut = fopen(varFilename.c_str(), "w");

        datastructures::Variable *var = network.get(variable);
        fprintf(varOut, "VAR %s\n", var->getName().c_str());
        fprintf(varOut, "META arity=%d\n", var->getCardinality());

        //fprintf(varOut, "META values=");
        //for (int i = 0; i < var->getCardinality(); i++) {
        //    fprintf(varOut, "%s ", var->getValue(i).c_str());
        //}
        //fprintf(varOut, "\n");

        for (auto score = sc.begin(); score != sc.end(); score++) {
            varset parentSet = (*score).first;
            float s = (*score).second;

            fprintf(varOut, "%f ", s);

            for (int p = 0; p < network.size(); p++) {
                if (VARSET_GET(parentSet, p)) {
                    fprintf(varOut, "%s ", network.get(p)->getName().c_str());
                }
            }

            fprintf(varOut, "\n");
        }

        fprintf(varOut, "\n");
        fclose(varOut);

        sc.clear();
    }
}

int main(int argc, char** argv) {
    boost::timer::auto_cpu_timer t;

    std::string description = std::string("Compute the scores for a csv file.  Example usage: ") + argv[0] + " iris.csv iris.pss";
    po::options_description desc(description);
    double lambda = 0.5; // for lasso
    bool adaptive = false;
    desc.add_options()
            ("input", po::value<std::string > (&inputFile)->required(), "The input file. First positional argument.")
            ("output", po::value<std::string > (&outputFile)->required(), "The output file. Second positional argument.")
            ("delimiter,d", po::value<char> (&delimiter)->required()->default_value(','), "The delimiter of the input file.")
            ("lambda,l", po::value<double> (&lambda), "The lambda in Lasso.")
            ("adaptive,a", "Use adaptive Lasso")
            ("scoreType,w", po::value<int> (&which)->default_value(1), "which score, 0 for lasso, 1 for entropy1, 2 for entropy2")
            ("constraints,c", po::value<std::string > (&constraintsFile), "The file specifying constraints on the scores.")
            ("skeleton,k", po::value<std::string > (&skeletonFile), "The file specifying the skeleton superstructure")
            ("rMin,m", po::value<int> (&rMin)->default_value(5), "The minimum number of records in the AD-tree nodes.")
            ("function,f", po::value<std::string > (&sf)->default_value("BIC"), "The scoring function to use.")
            ("ess,e", po::value<float> (&equivalentSampleSize)->default_value(1.0f), "The equivalent sample size, if BDeu is used.")
            ("maxParents,p", po::value<int> (&maxParents)->default_value(0), "The maximum number of parents for any variable. A value less than 1 means no limit.")
            ("threads,t", po::value<int> (&threadCount)->default_value(1), "The number of separate threads to use for score calculations.")
            ("time,r", po::value<int> (&runningTime)->default_value(-1), "The maximum amount of time to use for each variable. A value less than 1 means no limit.")
            ("hasHeader,s", "Add this flag if the first line of the input file gives the variable names.")
            ("doNotPrune,o", "Add this flag if the scores should NOT be pruned at the end of the search.")
            ("enableDeCamposPruning", "Add this flag to ENABLE DeCampos & Ji (JMLR 2011) pruning for BDeu. This feature is experimental and appears to contain some bugs for sufficiently large parent limits for BDeu. This flag has no effect for BIC or fNML.")
            ("help,h", "Show this help message.")
            ;

    po::positional_options_description positionalOptions;
    positionalOptions.add("input", 1);
    positionalOptions.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc)
            .positional(positionalOptions).run(),
            vm);

    if (vm.count("help") || argc == 1) {
        std::cout << desc;
        return 0;
    }
    if(vm.count("adaptive"))
    {
        adaptive = true;
        std::cout << "Will use adaptive Lasso\n";
    }

    po::notify(vm);

    hasHeader = vm.count("hasHeader");
    prune = (vm.count("doNotPrune") == 0);
    enableDeCamposPruning = vm.count("enableDeCamposPruning");

    if (threadCount < 1) {
        threadCount = 1;
    }

    printf("URLearning, Score Calculator\n");
    printf("Input file: '%s'\n", inputFile.c_str());
    printf("Output file: '%s'\n", outputFile.c_str());
    printf("Delimiter: '%c'\n", delimiter);
    printf("Constraints file: '%s'\n", constraintsFile.c_str());
    printf("r_min: '%d'\n", rMin);
    printf("Scoring function: '%s'\n", sf.c_str());
    printf("ESS: '%f'\n", equivalentSampleSize);
    printf("Maximum parents: '%d'\n", maxParents);
    printf("Threads: '%d'\n", threadCount);
    printf("Running time (per variable): '%d'\n", runningTime);
    printf("Has header: '%s'\n", (hasHeader ? "true" : "false"));
    printf("Enable end-of-scoring pruning: '%s'\n", (prune ? "true" : "False"));
    printf("Enable deCampos-style pruning (experimental for BDeu): '%s'\n", (enableDeCamposPruning ? "true" : "False"));

    printf("Parsing input file.\n");
    datastructures::RecordFile recordFile(inputFile, delimiter, hasHeader);
    recordFile.read();

    printf("Initializing data specifications.\n");
    network.initialize(recordFile);

    printf("Creating AD-tree.\n");
    scoring::ADTree *adTree = new scoring::ADTree(rMin);
    adTree->initialize(network, recordFile);
    adTree->createTree();

    boost::algorithm::to_lower(sf);

    if (maxParents > network.size() || maxParents < 1) {
        maxParents = network.size() - 1;
    }

    if (sf.compare("bic") == 0) { // bic discrete
        int maxParentCount = log(2 * recordFile.size() / log(recordFile.size()));
        if (maxParentCount < maxParents) {
            maxParents = maxParentCount;
        }
    } else if (sf.compare("cbic") == 0) { // bic continuous
    } else if (sf.compare("fnml") == 0) {
    } else if (sf.compare("bdeu") == 0) {
    } else if (sf.compare("lasso") == 0 or sf.compare("entropy") == 0) {  //Ni added
    } else if (sf.compare("adaptive_lasso") == 0 or sf.compare("adaptive") == 0) {  //Ni added
    } else {
        throw std::runtime_error("Invalid scoring function.  Options are: 'lasso','adaptive', 'BIC', 'fNML' or 'BDeu'.");
    }

    scoring::Constraints *constraints = NULL;
    if (constraintsFile.length() > 0) {
        constraints = scoring::parseConstraints(constraintsFile, network);
    }
    printf("Skeleton file %s\n", skeletonFile.c_str());
    if(skeletonFile.size() > 0) //Need parse the skeleton file
    {
    	if(skeletonFile.find(".arc") + 4 == skeletonFile.size())
    		skeleton.read_arc_list_file(skeletonFile, network.size());
    	else
    		skeleton.read_matrix_file(skeletonFile);
    }
    else
    {
        skeleton.set_variable_count(network.size());
    }

    scoringFunction = NULL;
    scoring::LogLikelihoodCalculator *llc = NULL;
    std::vector< std::vector< float >* >* regret = NULL;

    if(sf.compare("bic") == 0 or sf.compare("fnml") == 0)
    {
      std::vector<float> ilogi = scoring::LogLikelihoodCalculator::getLogCache(recordFile.size());
      llc = new scoring::LogLikelihoodCalculator(adTree, network, ilogi);
      regret = scoring::getRegretCache(recordFile.size(), network.getMaxCardinality());
    }

    if (sf.compare("bic") == 0) //bic for discrete variables
    {
        scoringFunction = new scoring::BICScoringFunction(network, recordFile, llc, constraints, enableDeCamposPruning);
    }
    else if(sf.compare("cbic") == 0) // bic for continuous variables
    {
	printf("Creating continuous BIC function\n");
	scoringFunction = new scoring::BIC_OLS_Function(network, inputFile, constraints, enableDeCamposPruning, lambda);
    }
    else if (sf.compare("fnml") == 0)
    {
        scoringFunction = new scoring::fNMLScoringFunction(network, llc, constraints, regret, enableDeCamposPruning);
    }
    else if (sf.compare("bdeu") == 0)
    {
        scoringFunction = new scoring::BDeuScoringFunction(equivalentSampleSize, network, adTree, constraints, enableDeCamposPruning);
    }
    else if (sf.compare("lasso") == 0 or sf.compare("entropy") == 0)
    {
        printf("Creating Entropy/LassoFunction with input file %s\n", inputFile.c_str());
        scoringFunction = new scoring::LassoEntropyScoringFunction(network, which, inputFile, lambda, constraints, enableDeCamposPruning);
    }
    else if (sf.compare("adaptive_lasso") == 0 or sf.compare("adaptive") == 0)
    {
        printf("Creating Adaptive Entropy/LassoFunction with input file %s\n", inputFile.c_str());
        scoringFunction = new scoring::AdaptiveLassoEntropyScoringFunction(network, which, inputFile, lambda, constraints, adaptive, enableDeCamposPruning);
    }

    std::vector<boost::thread*> threads;
    for (int thread = 0; thread < threadCount; thread++) {
        boost::thread *workerThread = new boost::thread(scoringThread, thread);
        threads.push_back(workerThread);
    }

    for (auto it = threads.begin(); it != threads.end(); it++) {
        (*it)->join();
    }

    // concatenate all of the files together
    std::ofstream out(outputFile, std::ios_base::out | std::ios_base::binary);

    // first, the header information
    std::string header = "META pss_version = 0.1\nMETA input_file=" + inputFile + "\nMETA num_records=" + TO_STRING(recordFile.size()) + "\n";
    header += "META parent_limit=" + TO_STRING(maxParents) + "\nMETA score_type=" + sf + "\nMETA ess=" + TO_STRING(equivalentSampleSize) + "\n\n";
    out.write(header.c_str(), header.size());

    for (int variable = 0; variable < network.size(); variable++) {
        std::string varFilename = outputFile + "." + TO_STRING(variable);
        std::ofstream varFile(varFilename, std::ios_base::in | std::ios_base::binary);

        out << varFile.rdbuf();
        varFile.close();

        // and remove the variable file
        remove(varFilename.c_str());
    }

    out.close();
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP
#define STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/identity_constrain.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound.
 *
 *
The transform is
 *
 *
\f$f(x) = U - \exp(x)\f$
 *
 *
where \f$U\f$ is the upper bound.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x);
  }
  return ub - exp(x);
}

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound and increment the specified log
 * probability reference with the log absolute Jacobian
 * determinant of the transform.
 *
 *
The transform is as specified for
 * ub_constrain(T, double).  The log absolute Jacobian
 * determinant is
 *
 *
\f$ \log | \frac{d}{dx} -\mbox{exp}(x) + U |
 *     = \log | -\mbox{exp}(x) + 0 | = x\f$.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x, lp).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @param[in,out] lp log density
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub, T& lp) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x, lp);
  }
  lp += x;
  return ub - exp(x);
}

}  // namespace math

}  // namespace stan

#endif


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP
#define BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP

// Gursoy-Atun graph layout, based on:
// "Neighbourhood Preserving Load Balancing: A Self-Organizing Approach"
// in EuroPar 2000, p. 234 of LNCS 1900
// http://springerlink.metapress.com/link.asp?id=pcu07ew5rhexp9yt

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/throw_exception.hpp>
#include <vector>
#include <exception>
#include <algorithm>

#include <boost/graph/visitors.hpp>
#include <boost/graph/properties.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/topology.hpp>

namespace boost {

namespace detail {

struct over_distance_limit : public std::exception {};

template <typename PositionMap, typename NodeDistanceMap,  typename Topology,
          typename Graph>
struct update_position_visitor {
  typedef typename Topology::point_type Point;
  PositionMap position_map;
  NodeDistanceMap node_distance;
  const Topology& space;
  Point input_vector;
  double distance_limit;
  double learning_constant;
  double falloff_ratio;

  typedef boost::on_examine_vertex event_filter;

  typedef typename graph_traits<Graph>::vertex_descriptor
    vertex_descriptor;

  update_position_visitor(PositionMap position_map,
                          NodeDistanceMap node_distance,
                          const Topology& space,
                          const Point& input_vector,
                          double distance_limit,
                          double learning_constant,
                          double falloff_ratio):
    position_map(position_map), node_distance(node_distance),
    space(space),
    input_vector(input_vector), distance_limit(distance_limit),
    learning_constant(learning_constant), falloff_ratio(falloff_ratio) {}

  void operator()(vertex_descriptor v, const Graph&) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::pow;
#endif

    if (get(node_distance, v) > distance_limit)
      BOOST_THROW_EXCEPTION(over_distance_limit());
    Point old_position = get(position_map, v);
    double distance = get(node_distance, v);
    double fraction =
      learning_constant * pow(falloff_ratio, distance * distance);
    put(position_map, v,
        space.move_position_toward(old_position, fraction, input_vector));
  }
};

template<typename EdgeWeightMap>
struct gursoy_shortest
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      EdgeWeightMap weight)
  {
    boost::dijkstra_shortest_paths(g, s, weight_map(weight).
      visitor(boost::make_dijkstra_visitor(std::make_pair(
       boost::record_distances(node_distance, boost::on_edge_relaxed()),
        update_position))));
  }
};

template<>
struct gursoy_shortest<dummy_property_map>
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      dummy_property_map)
  {
    boost::breadth_first_search(g, s,
      visitor(boost::make_bfs_visitor(std::make_pair(
        boost::record_distances(node_distance, boost::on_tree_edge()),
        update_position))));
  }
};

} // namespace detail

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename Diameter, typename VertexIndexMap,
          typename EdgeWeightMap>
void
gursoy_atun_step
  (const VertexListAndIncidenceGraph& graph,
   const Topology& space,
   PositionMap position,
   Diameter diameter,
   double learning_constant,
   VertexIndexMap vertex_index_map,
   EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<double> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  point_type input_vector = space.random_point();
  vertex_descriptor min_distance_loc
    = graph_traits<VertexListAndIncidenceGraph>::null_vertex();
  double min_distance = 0.0;
  bool min_distance_unset = true;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    double this_distance = space.distance(get(position, *i), input_vector);
    put(distance_from_input, *i, this_distance);
    if (min_distance_unset || this_distance < min_distance) {
      min_distance = this_distance;
      min_distance_loc = *i;
    }
    min_distance_unset = false;
  }
  assert (!min_distance_unset); // Graph must have at least one vertex
  boost::detail::update_position_visitor<
      PositionMap, NodeDistanceMap, Topology,
      VertexListAndIncidenceGraph>
    update_position(position, node_distance, space,
                    input_vector, diameter, learning_constant,
                    exp(-1. / (2 * diameter * diameter)));
  std::fill(node_distance_map_vector.begin(), node_distance_map_vector.end(), 0);
  try {
    typedef detail::gursoy_shortest<EdgeWeightMap> shortest;
    shortest::run(graph, min_distance_loc, node_distance, update_position,
                  weight);
  } catch (detail::over_distance_limit) {
    /* Thrown to break out of BFS or Dijkstra early */
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_refine(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  double diameter_ratio = (double)diameter_final / diameter_initial;
  double learning_constant_ratio =
    learning_constant_final / learning_constant_initial;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<int> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<int>::iterator,
                                       VertexIndexMap, double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  for (int round = 0; round < nsteps; ++round) {
    double part_done = (double)round / (nsteps - 1);
    int diameter = (int)(diameter_initial * pow(diameter_ratio, part_done));
    double learning_constant =
      learning_constant_initial * pow(learning_constant_ratio, part_done);
    gursoy_atun_step(graph, space, position, diameter, learning_constant,
                     vertex_index_map, weight);
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  vertex_iterator i, iend;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    put(position, *i, space.random_point());
  }
  gursoy_atun_refine(graph, space,
                     position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, weight);
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map)
{
  gursoy_atun_layout(graph, space, position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, dummy_property_map());
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final = 1.0,
                        double learning_constant_initial = 0.8,
                        double learning_constant_final = 0.2)
{
  gursoy_atun_layout(graph, space, position, nsteps, diameter_initial,
                     diameter_final, learning_constant_initial,
                     learning_constant_final, get(vertex_index, graph));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  gursoy_atun_layout(graph, space, position, nsteps,
                     sqrt((double)num_vertices(graph)));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position)
{
  gursoy_atun_layout(graph, space, position, num_vertices(graph));
}

template<typename VertexListAndIncidenceGraph, typename Topology,
         typename PositionMap, typename P, typename T, typename R>
void
gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                   const Topology& space,
                   PositionMap position,
                   const bgl_named_params<P,T,R>& params)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  std::pair<double, double> diam(sqrt(double(num_vertices(graph))), 1.0);
  std::pair<double, double> learn(0.8, 0.2);
  gursoy_atun_layout(graph, space, position,
                     choose_param(get_param(params, iterations_t()),
                                  num_vertices(graph)),
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).first,
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).second,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).first,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).second,
                     choose_const_pmap(get_param(params, vertex_index), graph,
                                       vertex_index),
                     choose_param(get_param(params, edge_weight),
                                  dummy_property_map()));
}

} // namespace boost

#endif // BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP


/*
 * DivRightHandSideAdjoint.cpp
 *
 *  Created on: 03.08.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/types.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe.h>
#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/fe/fe_values.h>

#include <base/DiscretizedFunction.h>
#include <forward/DivRightHandSideAdjoint.h>

#include <functional>
#include <vector>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template <int dim>
DivRightHandSideAdjoint<dim>::DivRightHandSideAdjoint(std::shared_ptr<Function<dim>> a,
                                                      std::shared_ptr<Function<dim>> u)
    : a(a), u(u) {}

template <int dim>
DivRightHandSideAdjoint<dim>::AssemblyScratchData::AssemblyScratchData(const FiniteElement<dim> &fe,
                                                                       const Quadrature<dim> &quad)
    : fe_values(fe, quad, update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
DivRightHandSideAdjoint<dim>::AssemblyScratchData::AssemblyScratchData(const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
void DivRightHandSideAdjoint<dim>::copy_local_to_global(Vector<double> &result, const AssemblyCopyData &copy_data) {
  for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i)
    result(copy_data.local_dof_indices[i]) += copy_data.cell_rhs(i);
}

template <int dim>
void DivRightHandSideAdjoint<dim>::local_assemble_dd(const Vector<double> &a, const Vector<double> &u,
                                                     const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                     AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      for (unsigned int ka = 0; ka < dofs_per_cell; ++ka)
        for (unsigned int ku = 0; ku < dofs_per_cell; ++ku)
          copy_data.cell_rhs(i) -= a[copy_data.local_dof_indices[ka]] * scratch_data.fe_values.shape_grad(ka, q_point) *
                                   u[copy_data.local_dof_indices[ku]] * scratch_data.fe_values.shape_grad(ku, q_point) *
                                   scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
}

template <int dim>
void DivRightHandSideAdjoint<dim>::local_assemble_cc(const Function<dim> *const a, const Function<dim> *const u,
                                                     const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                     AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
    auto grad_a = a->gradient(scratch_data.fe_values.quadrature_point(q_point));
    auto grad_u = u->gradient(scratch_data.fe_values.quadrature_point(q_point));

    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      copy_data.cell_rhs(i) -=
          grad_a * grad_u * scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
  }
}

template <int dim>
void DivRightHandSideAdjoint<dim>::create_right_hand_side(const DoFHandler<dim> &dof, const Quadrature<dim> &quad,
                                                          Vector<double> &rhs) const {
  AssertThrow(a != nullptr, ExcZero());
  AssertThrow(u != nullptr, ExcZero());

  a->set_time(this->get_time());
  u->set_time(this->get_time());

  auto a_d = dynamic_cast<DiscretizedFunction<dim> *>(a.get());
  auto u_d = dynamic_cast<DiscretizedFunction<dim> *>(u.get());

  if (a_d != nullptr && u_d != nullptr) {
    Vector<double> ca = a_d->get_function_coefficients(a_d->get_time_index());
    Vector<double> cu = u_d->get_function_coefficients(u_d->get_time_index());

    Assert(ca.size() == dof.n_dofs(), ExcDimensionMismatch(ca.size(), dof.n_dofs()));
    Assert(cu.size() == dof.n_dofs(), ExcDimensionMismatch(cu.size(), dof.n_dofs()));

    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DivRightHandSideAdjoint<dim>::local_assemble_dd, *this, std::ref(ca), std::ref(cu),
                  std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
        std::bind(&DivRightHandSideAdjoint<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  } else
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DivRightHandSideAdjoint<dim>::local_assemble_cc, *this, a.get(), u.get(), std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DivRightHandSideAdjoint<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
}

template <int dim>
DiscretizedFunction<dim> DivRightHandSideAdjoint<dim>::run_adjoint(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  DiscretizedFunction<dim> target(mesh);

  for (size_t i = 0; i < mesh->length(); i++) {
    this->set_time(mesh->get_time(i));
    auto dof_handler = mesh->get_dof_handler(i);

    Vector<double> tmp(dof_handler->n_dofs());
    this->create_right_hand_side(*dof_handler, mesh->get_quadrature(), tmp);
    target[i] = tmp;
  }

  return target;
}

template class DivRightHandSideAdjoint<1>;
template class DivRightHandSideAdjoint<2>;
template class DivRightHandSideAdjoint<3>;

} /* namespace forward */
} /* namespace wavepi */


/*Copyright (c) 2021 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <cmath>
#include <stdlib.h>
#include <vector>
//#include <boost/math/special_functions/gamma.hpp>  remove for now, keep in case we ever end up using boost
#include <vector>
#include "Shell.h"
#include "boxcap.h"
#include "../asa/asa239.h"

using namespace std;

const double PI = 3.14159265358979;
const double sqrt_pi = 1.77245385090552;
const double OOPI3 = 1.0 / (PI * PI * PI);
const double gauss_factor[] = {1.0, 0.5, 0.75, 1.875, 6.5625, 29.53125, 162.421875};

//
// box-CAP helper functions
//
double intmod_r2(double a, int k, double cl, double cu);
int xi (double z, int i);
double intgauss(double alpha, int n, int = 0, const char* = "message ");
vector<double> polmulti(vector<double>& g, int n, vector<double>& h, int m,
	       int = 0, const char* = "message");
void poltrans (vector<double>& f, double alpha, double beta, int n,
	       int = 0, const char* = "message ");
void gauss2(double ao, double alpha, double bo, double beta,
	    double& co, double& expo, double& factor, int = 0);
void print_vector(vector<double>& v, const char msg[]);

/*
     gaussian integral of the softbox potential
     V  = { (x+boxlength)^2  for x < -boxlength
          {        0         for -boxlength < x < boxlength
          { (x-boxlength)^2  for x > boxlength

This is a Fortran to Cpp translation of our old code from the late 1990ties.
Fortran by Uwe Riss, Robin Santra, and Thomas Sommerfeld
Cpp translation by Thomas Sommerfeld, Nov 2018 T. Sommerfeld

The integrals of the so-called box-CAP are computed analytically.
Do not even try to backwards-engineer this, dig out Robin's paper.

@Article{santra99,
author = "R. Santra and L. S. Cederbaum and H.-D. Meyer",
journal = "Chem. Phys. Lett.",
volume = 303,
year =   1999,
pages =  413,
title =  "Electronic decay of molecular clusters: Non-stationary states computed by standard quantum chemistry methods"}

gto1 and gto2 define two primitive Cartesian GTOs including their l value,
say, px, or dx^2.
boxlength[3] are the cutoff parameters defined above
normalized [0] is a flag 0=unnormalized GTOs, 1=normalized GTOs
verbose can be increased to create debug information

*/

double integrate_box_cap(Shell shell1, Shell shell2,
std::array<size_t,3> l1, std::array<size_t,3> l2, double boxlength[3])
{
  double sum = 0;
	for(size_t prim1 =0;prim1<shell1.num_prims;prim1++)
  {
    gto gto1 = {shell1.origin,shell1.exps[prim1],l1};
    for(size_t prim2=0;prim2<shell2.num_prims;prim2++)
    {
      gto gto2 = {shell2.origin,shell2.exps[prim2],l2};
      sum+= shell1.coeffs[prim1] * shell2.coeffs[prim2] * boxcap(gto1,gto2,boxlength,false,0);
    }
  }
  return sum;
}

double boxcap(gto gto1, gto gto2, double boxlength[3],
	      int normalized, int verbose) {

  int debug = verbose;

  double cen[3] = {0, 0, 0};
  double alpha = gto1.a;
  double beta  = gto2.a;
  double d0, delta, fakt;

  double ovl[3] = {0,0,0};
  double dip[3] = {0,0,0};
  for (int k = 0; k < 3; ++k) {
    int m = gto1.l[k];
    int n = gto2.l[k];
    int nm = n+m;
    double ao = gto1.r[k];
    double bo = gto2.r[k];
    gauss2(ao, alpha,  bo, beta, d0, delta, fakt, verbose);

    vector<double> g (m+1,0);
    vector<double> h (n+1,0);
    g[m] = 1;
    h[n] = 1;
    poltrans(g, ao, d0, m, debug, " g = ");
    poltrans(h, bo, d0, n, debug, " h = ");
    vector<double> f = polmulti(g, m, h, n, debug, " f = ");

    double cl = cen[k] - d0 - boxlength[k];
    double cu = cen[k] - d0 + boxlength[k];

    double temp1 = 0;
    double temp2 = 0;
    for (int i = 0; i <= nm; ++i)
    {
      temp1 += f[i] * intgauss( delta, i, debug, "  I = ");
      temp2 += f[i] * intmod_r2(delta, i, cl, cu);
    }
    ovl[k] = fakt * temp1;
    dip[k] = fakt * temp2;
  }
  if (verbose > 1) {
    cout << "Ovl: " << ovl[0] << ", " <<  ovl[1] << ", " <<  ovl[2] << endl;
    cout << "Dip: " << dip[0] << ", " <<  dip[1] << ", " <<  dip[2] << endl;
  }

  double cap = dip[0]*ovl[1]*ovl[2] + ovl[0]*dip[1]*ovl[2] + ovl[0]*ovl[1]*dip[2];
  if (normalized == 0)
    return cap;
  else {
    alpha *= 2;
    beta *= 2;
    double ovl[3] = {0, 0, 0};
    for (int k=0; k < 3; ++k) {
      int m = 2 * gto1.l[k];
      int n = 2 * gto2.l[k];
      ovl[k] = intgauss(alpha,m) * intgauss(beta,n);
    }
    if (verbose > 1)
      cout << "<0>: " << ovl[0] << ", " <<  ovl[1] << ", " <<  ovl[2] << endl;
    return cap / sqrt(ovl[0]*ovl[1]*ovl[2]);
  }
}

//
//
// integrate(x**k * f(x) * exp(-a*x**2), x, -oo, oo)
//
// where
//
//        (x + cl)**2 if x < cl
// f(x) =    0        if cl <= x <= cu
//        (x - cu)**2 if x > cu
//
// This integral can be expressed in terms of the incomplete gamma-function.
// see:
//  @Article{santra99,
//  author = "R. Santra and L. S. Cederbaum and H.-D. Meyer",
//  journal = "Chem. Phys. Lett.",
//  volume = 303,
//  year =   1999,
//  pages =  413,
//  title =  "Electronic decay of molecular clusters:
//            Non-stationary states computed by standard quantum chemistry methods"}
//
//
double intmod_r2(double a, int k, double cl, double cu) {
  int sg = -1;
  if ((k % 2) == 0)
    sg = 1;
  double ql = a*cl*cl;
  double qu = a*cu*cu;
  double result = 0;
  int ifault = 0;
  for (int i = 0; i < 3; ++i) {
    int j = k + i;
    double par = 0.5*(j + 1);
    int ex = 2 - i;
    double dl = pow(cl,ex);
    double du = pow(-cu,ex);
    double term = tgamma(par)*(sg*dl + du);
    double norm_factor = tgamma(par);
    term -= sg*dl*gammad(ql,par,&ifault)*norm_factor*xi(-cl,j);
    term -=    du*gammad(qu,par,&ifault)*norm_factor*xi( cu,j);
    //term -= sg*dl*boost::math::tgamma_lower(par,ql)*xi(-cl,j);  func is not normalized w/ boost
    //term -=    du*boost::math::tgamma_lower(par,qu)*xi( cu,j);  func is not normalized w/ boost
    term /= pow(a,par);
    if (i == 1)
      term *= 2;
    result += term;
  }
  return 0.5*result;
}

//
//  computes a sign needed in intmod_r2()
//
int xi (double z, int i) {
  if (z >= 0)
    return 1;
  else {
    if ((i%2) == 0)
      return -1;
    else
      return 1;
  }
}

//
//  returns int dx x^n exp(-alpha*x^2)
//
double intgauss(double alpha, int n, int debug, const char* msg) {
  double integral = 0.0;
  if (n%2 == 0) {
    int k = n/2;
    double a = pow(alpha,k);
    integral = gauss_factor[k] * sqrt(PI / alpha) / a;
  }
  if (debug > 3)
    cout << msg << integral << endl;
  return integral;
}

//
//   {sum} a_i x^i  *  {sum} b_i x^i  ->  {sum} c_i x^i
//
vector<double> polmulti(vector<double>& g, int n, vector<double>& h, int m,
			int verbose, const char msg[]) {
  vector<double> f (n+m+1,0);
  for (int i = 0; i <= n; ++i) {
    for (int j = 0; j <= m; ++j) {
      f[i+j] += g[i] * h[j];
    }
  }
  if (verbose > 2) {
    print_vector(f, msg);
  }
  return f;
}

//
// {sum} a_i ( x - a )^i   ->   {sum} b_i ( x - b )^i
//
void poltrans (vector<double> &f, double alpha, double beta, int n,
	       int verbose, const char msg[]) {
  double diff = beta - alpha;
  int k = 1;
  for (int j = 0; j <= n; ++j) {
    double w = 0;
    for (int i = n; i >= j; --i) {
      w = w*diff + f[i];
      f[i] *= (i-j);
    }
    f[j] = w / k;
    k = (j+1)*k;
  }
  if (verbose > 2)
    print_vector(f, msg);
}

void gauss2(double ao, double alpha, double bo, double beta,
	    double& co, double& expo, double& factor, int verbose) {
  expo = alpha + beta;
  co = (ao*alpha + bo*beta) / expo;
  factor = alpha * beta * (bo-ao) * (bo-ao) / expo;
  factor = exp(-factor);
  if (verbose > 2)
    cout << "gs2:" << co << "  " << expo << "  "  << factor << endl;
}

void print_vector(vector<double> &v, const char msg[]) {
  int n = static_cast<int>(v.size());
  cout << msg;
  for (int i = 0; i < n; ++i) {
    cout << v[i];
    if ((i+1)%5 == 0)
      cout << endl;
    else
      cout << ", ";
  }
  if (n%5 != 0)
    cout << endl;
}