#include "Solver.h"

#include "RigidBodyMain.h"
#include "ChronoTimer.h"

#ifndef EIGEN_NO_STATIC_ASSERT
#define EIGEN_NO_STATIC_ASSERT
#endif

#ifdef REDMAX_PARDISO
#include <Eigen/PardisoSupport>
#endif

#include <Eigen/SparseCholesky>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <fstream>
#include <iomanip>
#include <chrono>
int count = 0;

void Solver::loadMSparse(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S)
{
	int numObj = (int)LS->blocks.size();

	int index;
	std::shared_ptr<Block> block;
	for (int i = 0; i < LS->blocks.size(); ++i)
	{
		block = LS->blocks[i].second;
		index = block->joint->jindex;

		for (int j = 0; j < 6; ++j)
		{
			for (int k = 0; k < 6; ++k)
			{
				if (S->M[index](j, k) != 0)
				{
					SS->LHSlist.push_back(Eigen::Triplet<double>(index * 6 + j, index * 6 + k, S->M[index](j, k) * (1 + SS->alpha*SS->h)));
				}
			}
		}
	}
}

void Solver::computeRHS(std::unique_ptr<StateSolve>& SS, const std::unique_ptr<LinkageSystem>& LS, const std::unique_ptr<State>& S)
{
	Matrix6d spcp = Matrix6d::Zero();
	Vector6d bodyForces = Vector6d::Zero();
	int i;
	for (int index = 0; index < LS->blocks.size(); ++index)
	{
		if (simtype == simType::PCG)
			i = S->jindex[index];
		else
			i = LS->blocks[index].second->joint->jindex;

		// Define the spatial cross product matrix
		spcp.block<3, 3>(0, 0) << Rigid::bracket3(S->v.segment<3>(i * 6));
		spcp.block<3, 3>(3, 3) << spcp.block<3, 3>(0, 0);

		// Define body forces
		bodyForces.segment<3>(3) << S->E[i].block<3, 3>(0, 0).transpose()*SS->grav*S->mass[i];

		// Compute RHS vector
		if (SS->fn.size() != 0)
		{
			SS->fn.segment<6>(i * 6) += S->M[i] * S->v.segment<6>(i * 6) + SS->h*(spcp.transpose()*S->M[i] * S->v.segment<6>(i * 6) + bodyForces);
		}

		// Compute force vector
		if (SS->f.size() != 0)
		{
			SS->f.segment<6>(i * 6) += spcp.transpose()*S->M[i] * S->v.segment<6>(i * 6) + bodyForces;
		}
	}
}

void Solver::pcdSaad2003(Eigen::VectorXd &result, const Eigen::VectorXd &LHSx0, const Eigen::VectorXd &b, std::unique_ptr<StateSolve> &SS, std::unique_ptr<LinkageSystem> &LS, std::unique_ptr<State> &S, std::shared_ptr<State::local_mt> lmt, double tol, int maxit)
{
	//function[x, iter, xs, rs] = pcgSaad2003(A, b, tol, maxit, M, x0)
	//	% pcgSaad2003 Algorithm 9.1 from[Saad 2003]
	std::vector<double> rs;

	Eigen::VectorXd xj = S->qdot;
	Eigen::VectorXd rj = b - LHSx0;
	Eigen::VectorXd zj = ConstraintJoint::computeMinv_x(rj, SS, LS, S, lmt);

	//if (zj.hasNaN())
	//	std::cout << "minv init" << std::endl;

	Eigen::VectorXd pj = zj;
	double tolsq = tol * tol;
	double res1 = rj.dot(rj);
	double res0 = res1;
	Eigen::VectorXd xs = xj;
	rs.push_back(res1);

	auto start = std::chrono::steady_clock::now();
	Eigen::VectorXd J_p;
	Eigen::VectorXd q;
	Eigen::VectorXd Apj;
	Eigen::VectorXd rj1;
	Eigen::VectorXd zj1;
	int j = 1;
	for (; j < maxit; ++j)
	{
		ConstraintJoint::computeJ_x(J_p, pj, SS, LS, S);
		Apj = ConstraintJoint::computeJT_x(
			ConstraintJoint::computeLHS_x(J_p, SS, LS, S),
			SS, LS, S, lmt);

		//if (J_p.hasNaN())
		//	std::cout << "J * qd" << std::endl;
		//if (Apj.hasNaN())
		//	std::cout << "LHS or JT" << std::endl;

		q = S->q + SS->h * pj;
		// Joint stiffness and damping
		ConstraintJoint::computeStiffnessDampingJoint(Apj, pj, S->q, SS, LS, S);

		//if (Apj.hasNaN())
		//	std::cout << "stiffdampjoint" << std::endl;

		double aj = rj.dot(zj) / (Apj).dot(pj);
		if ((Apj).dot(pj) == 0)
		{
			xj = S->qdot;
			break;
		}
		xj = xj + aj * pj;
		rj1 = rj - aj * Apj;
		res1 = rj1.dot(rj1);
		rs.push_back(res1);
		if (res1 < tolsq*res0)
			break;

		zj1 = ConstraintJoint::computeMinv_x(rj1, SS, LS, S, lmt);

		//if (zj1.hasNaN())
		//	std::cout << "minv" << std::endl;
		//std::cout << j << std::endl;

		double bj = rj1.dot(zj1) / rj.dot(zj);
		pj = zj1 + bj * pj;
		rj = rj1;
		zj = zj1;
	}
	auto end = std::chrono::steady_clock::now();
	auto diff = end - start;
	auto time = std::chrono::duration<double, std::nano>(diff).count();
	trackLastTimestep.time_in_solve = time;
	trackLastTimestep.num_iterations = j;

	if (false)//std::abs(S->t - 1.01)) < 1e-5)
	{
		for (int i = 0; i < rs.size(); ++i)
		{
			std::cout << rs[i] << " ";
		}
		std::cout << std::endl;
		std::cout << "---------" << j << std::endl;
	}
	result = xj;
}

void Solver::pcdSaad2003_unopt(Eigen::VectorXd & result, const Eigen::VectorXd & LHSx0, const Eigen::VectorXd & b, std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S, std::shared_ptr<State::local_mt> lmt, double tol, int maxit)
{
	//function[x, iter, xs, rs] = pcgSaad2003(A, b, tol, maxit, M, x0)
	//	% pcgSaad2003 Algorithm 9.1 from[Saad 2003]
	std::vector<double> rs;

	Eigen::VectorXd xj = S->qdot;
	Eigen::VectorXd rj = b - LHSx0;
	Eigen::VectorXd zj = ConstraintJoint::computeMinv_x(rj, SS, LS, S, lmt);
	Eigen::VectorXd pj = zj;
	double tolsq = tol * tol;
	double res1 = rj.dot(rj);
	double res0 = res1;
	Eigen::VectorXd xs = xj;
	rs.push_back(res1);

	auto start = std::chrono::steady_clock::now();
	Eigen::VectorXd J_p;
	Eigen::VectorXd q;
	Eigen::VectorXd Apj;
	Eigen::VectorXd rj1;
	Eigen::VectorXd zj1;
	int j = 1;
	for (; j < maxit; ++j)
	{
		ConstraintJoint::computeJ_x_unopt(J_p, pj, SS, LS, S);
		Apj = ConstraintJoint::computeJT_x_unopt(
			ConstraintJoint::computeLHS_x_unopt(J_p, SS, LS, S),
			SS, LS, S, lmt);

		q = S->q + SS->h * pj;
		// Joint stiffness and damping
		ConstraintJoint::computeStiffnessDampingJoint_unopt(Apj, pj, S->q, SS, LS, S);

		double aj = rj.dot(zj) / (Apj).dot(pj);
		if ((Apj).dot(pj) == 0)
		{
			xj = S->qdot;
			break;
		}
		xj = xj + aj * pj;
		rj1 = rj - aj * Apj;
		res1 = rj1.dot(rj1);
		rs.push_back(res1);
		if (res1 < tolsq*res0)
			break;

		zj1 = ConstraintJoint::computeMinv_x_unopt(rj1, SS, LS, S, lmt);
		double bj = rj1.dot(zj1) / rj.dot(zj);
		pj = zj1 + bj * pj;
		rj = rj1;
		zj = zj1;
	}
	auto end = std::chrono::steady_clock::now();
	auto diff = end - start;
	auto time = std::chrono::duration<double, std::nano>(diff).count();
	trackLastTimestep.time_in_solve = time;
	trackLastTimestep.num_iterations = j;

	if (false)
	{
		for (int i = 0; i < rs.size(); ++i)
		{
			std::cout << rs[i] << " ";
		}
		std::cout << std::endl;
		std::cout << "---------" << j << std::endl;
	}
	result = xj;
}
#ifdef REDMAX_PARDISO
Eigen::VectorXd Solver::solvePardiso(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S)
{
	//ChronoTimer ctime("ldlt", 5);
	//ChronoTimer ltime("load", 7);
	//ctime.tic(0);
	//ctime.tic(1);
	// Preprocessing, eliminate matrix resizing
	//ltime.tic(0);
 	int constraints = ConstraintJoint::getConstraintNumReduced(LS);
	int additional_constraints = ConstraintJoint::getConstraintNumAdditnl(LS);
	int numObj = (int)LS->blocks.size();

	// Initialize containers
	SS->J = Eigen::MatrixXd::Zero(numObj * 6, constraints); // jacobian
	SS->Jd = Eigen::MatrixXd::Zero(numObj * 6, constraints); // jacobian time derivative
	SS->f = Eigen::VectorXd::Zero(numObj * 6); // load with fm
	SS->fr = Eigen::VectorXd::Zero(constraints);
	SS->gm = Eigen::VectorXd::Zero(additional_constraints);
	SS->Dmlist.clear();
	SS->Kmlist.clear();
	SS->LHSlist.clear();
	SS->Jlist.clear();
	SS->Jdlist.clear();
	SS->JTlist.clear();
	SS->LHSGlist.clear();
	SS->Gmlist.clear();
	SS->row = 0;

	// init extra constraints for joint solver
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		LS->constraints[i]->update(SS, LS, S);
	}
	//if (SS->f.hasNaN())
	//	std::cout << "0 f" << std::endl;

	// Compute fn and M
	SS->alpha = 0;
	int index;
	double value;
	std::shared_ptr<Block> block;
	Matrix6d spcp = Matrix6d::Zero();
	Vector6d bodyForces = Vector6d::Zero();
	for (int i = 0; i < LS->blocks.size(); ++i)
	{
		block = LS->blocks[i].second;
		index = block->joint->jindex;

		for (int r = 0; r < 6; ++r)
		{
			value = S->M[index](r, r) * (1 + SS->alpha*SS->h);
			if (std::abs(value) > THRESHOLD)
				SS->LHSlist.push_back(Eigen::Triplet<double>(index * 6 + r, index * 6 + r, value));
		}

		// Define the spatial cross product matrix
		spcp.block<3, 3>(0, 0) << Rigid::bracket3(S->v.segment<3>(i * 6));
		spcp.block<3, 3>(3, 3) << spcp.block<3, 3>(0, 0);

		// Define body forces
		bodyForces.segment<3>(3) << S->E[i].block<3, 3>(0, 0).transpose()*SS->grav*S->mass[i];

		// Compute force vector
		SS->f.segment<6>(i * 6) += spcp.transpose()*S->M[i] * S->v.segment<6>(i * 6) + bodyForces;

		// Handle joint stiffness
		// update is in constraintJoint class for CG solver
		int ci = block->joint->constraint_index;
		for (int cnum = 0; cnum < block->joint->constraintNum; ++cnum)
		{
			SS->fr[ci + cnum] -= block->joint->k*(S->q[ci + cnum] - block->joint->q0[cnum]);

			//if(isnan(S->q[ci + cnum]))
			//	std::cout << "q" << std::endl;
		}
	}

	// Load joint constraints
	ConstraintJoint::update(SS, LS, S);

	// solve
	Eigen::SparseMatrix<double> Mr(constraints, constraints);
	Eigen::SparseMatrix<double> Mrtilde(constraints, constraints);
	Eigen::VectorXd frtilde;

	Eigen::SparseMatrix<double> J(numObj * 6, constraints);
	J.setFromTriplets(SS->Jlist.begin(), SS->Jlist.end());
	Eigen::SparseMatrix<double> JT(constraints, numObj * 6);
	JT.setFromTriplets(SS->JTlist.begin(), SS->JTlist.end());
	Eigen::SparseMatrix<double> Jdot(numObj * 6, constraints);
	Jdot.setFromTriplets(SS->Jdlist.begin(), SS->Jdlist.end());

	Eigen::SparseMatrix<double> LHS(numObj * 6, numObj * 6);
	LHS.setFromTriplets(SS->LHSlist.begin(), SS->LHSlist.end());
	Eigen::SparseMatrix<double> Km(numObj * 6, numObj * 6);
	Km.setFromTriplets(SS->Kmlist.begin(), SS->Kmlist.end());
	Eigen::SparseMatrix<double> Dm(numObj * 6, numObj * 6);
	Dm.setFromTriplets(SS->Dmlist.begin(), SS->Dmlist.end());

	Eigen::SparseMatrix<double> KDMm(numObj * 6, numObj * 6);
	KDMm = LHS + SS->h * SS->h * Km + SS->h * Dm + SS->h * SS->bDm;
	Eigen::SparseMatrix<double> KDr(constraints, constraints);
	KDr = SS->h * SS->Dr + SS->h * SS->h * SS->Kr;

	//Mr_sparse = 0.5*(Mr_sparse + Eigen::SparseMatrix<double>(Mr_sparse.transpose()));
	//Mrtilde_sparse = Mr_sparse + KDr_sparse;
	Mr = JT * LHS * J;
	Mrtilde = JT * KDMm * J + KDr;

	SS->fr += JT * (SS->f - LHS * (Jdot * S->qdot));
	frtilde = Mr * S->qdot + SS->h * SS->fr;

	// solve for velocity
	auto start = std::chrono::steady_clock::now();
	if (additional_constraints == 0)
	{
		Eigen::PardisoLDLT< Eigen::SparseMatrix<double>> solver;
		S->qdot = solver.compute(Mrtilde).solve(frtilde);
	}
	else
	{
		Eigen::SparseMatrix<double> Gm(additional_constraints, numObj * 6);
		Gm.setFromTriplets(SS->Gmlist.begin(), SS->Gmlist.end());
		//Eigen::SparseMatrix<double> GmT(numObj * 6, numObj * 6);
		//GmT.setFromTriplets(SS->GmTlist.begin(), SS->GmTlist.end());

		Eigen::SparseMatrix<double> Gr = Gm * J;
		//Eigen::SparseMatrix<double> GrT = JT * GmT;

		std::vector<Eigen::Triplet<double> > tripletList;
		tripletList.reserve(Mrtilde.nonZeros() + Gr.nonZeros() + Gr.nonZeros());
		for (int k = 0; k < Mrtilde.outerSize(); ++k)
		{
			// Mrtilde
			for (Eigen::SparseMatrix<double>::InnerIterator it(Mrtilde, k); it; ++it)
			{
				tripletList.push_back(Eigen::Triplet<double>(it.row(), it.col(), it.value()));
			}
		}
		for (int k = 0; k < Gr.outerSize(); ++k)
		{
			for (Eigen::SparseMatrix<double>::InnerIterator it(Gr, k); it; ++it)
			{
				// Gr
				tripletList.push_back(Eigen::Triplet<double>(it.row() + Mrtilde.rows(), it.col(), it.value()));
				// GrT
				tripletList.push_back(Eigen::Triplet<double>(it.col(), it.row() + Mrtilde.rows(), it.value()));
			}
		}
		Eigen::SparseMatrix<double> LHSG(constraints + additional_constraints, constraints + additional_constraints);
		LHSG.setFromTriplets(tripletList.begin(), tripletList.end());

		Eigen::VectorXd frG = Eigen::VectorXd::Zero(constraints + additional_constraints);
		frG.segment(0, frtilde.rows()) = frtilde;
		frG.segment(frtilde.rows(), additional_constraints) = -SS->gm * SS->baumgarte[2];

		Eigen::PardisoLU< Eigen::SparseMatrix<double>> solver;
		Eigen::VectorXd result = solver.compute(LHSG).solve(frG);
		S->qdot = result.segment(0,constraints);

		if (false)//std::abs(S->t - 1.01)) < 1e-5)
		{
			// name of file
			std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
			if (decimals.size() == 1)
				decimals = "0" + decimals;
			std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			std::ofstream outfile(filename, std::ios::out);
			outfile << std::setprecision(16);

			//ConstraintJoint::preprocess_preconditioner(SS, LS, S, true);
			//Eigen::MatrixXd preconditioner = Eigen::MatrixXd::Zero(constraints, constraints);
			//Eigen::VectorXd temp;
			//Eigen::VectorXd z;
			//for (int k = 0; k < constraints; ++k)
			//{
			//	temp = Eigen::VectorXd::Zero(constraints);
			//	temp[k] = 1.0;
			//	z = ConstraintJoint::computeMinv_x(temp, SS, LS, S);
			//	for (int l = 0; l < constraints; ++l)
			//	{
			//		preconditioner(l, k) = z[l];
			//	}
			//}

			outfile << "h = " << SS->h << ";\n" << std::endl;
			addSparseToFile(outfile, "Mr_c", Mr);
			addSparseToFile(outfile, "Mrtilde_c", Mrtilde);
			addSparseToFile(outfile, "Mm_c", LHS);
			addSparseToFile(outfile, "J_c", J);
			addSparseToFile(outfile, "Jdot_c", Jdot);

			Eigen::SparseMatrix<double> bDm = Dm + SS->bDm;
			addSparseToFile(outfile, "Dm_c", bDm);
			//addSparseToFile(outfile, "MG_c", LHSGt);
			//addSparseToFile(outfile, "Gr_c", Gr);
			addSparseToFile(outfile, "Km_c", Km);
			addSparseToFile(outfile, "LHSG_c", LHSG);
			addSparseToFile(outfile, "Gm_c", Gm);
			addSparseToFile(outfile, "Dr_c", SS->Dr);
			addSparseToFile(outfile, "Kr_c", SS->Kr);
			addSparseToFile(outfile, "Jmr_c", SS->J);
			addSparseToFile(outfile, "Jdotmr_c", SS->Jd);
			addVectorToFile(outfile, "f_c", SS->f);
			addVectorToFile(outfile, "fr_c", SS->fr);
			addVectorToFile(outfile, "frG_c", frG);
			addVectorToFile(outfile, "fm_c", SS->f);
			addVectorToFile(outfile, "gm_c", SS->gm);
			//addVectorToFile(outfile, "frg_c", frGt);
			addVectorToFile(outfile, "frtilde_c", frtilde);
			addVectorToFile(outfile, "rhs_c", frtilde);
			///Eigen::VectorXd qdot0 = S->qdot;
			//addVectorToFile(outfile, "qdot0_c", qdot0);
			addVectorToFile(outfile, "qdot1_c", S->qdot);
			//addSparseToFile(outfile, "P_c", preconditioner);

			//SS->fr += JT * (SS->f - LHS * (Jdot * S->qdot));

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

	}
	auto end = std::chrono::steady_clock::now();
	auto diff = end - start;
	auto time = std::chrono::duration<double, std::nano>(diff).count();
	trackLastTimestep.time_in_solve = time;

	// update joint angles
	S->q = S->q + SS->h * S->qdot;

	if (false)//std::abs(S->t - 1.01)) < 1e-5)
	{
		// name of file
		std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
		if (decimals.size() == 1)
			decimals = "0" + decimals;
		std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
		//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
		std::ofstream outfile(filename, std::ios::out);
		outfile << std::setprecision(16);

		//ConstraintJoint::preprocess_preconditioner(SS, LS, S, true);
		//Eigen::MatrixXd preconditioner = Eigen::MatrixXd::Zero(constraints, constraints);
		//Eigen::VectorXd temp;
		//Eigen::VectorXd z;
		//for (int k = 0; k < constraints; ++k)
		//{
		//	temp = Eigen::VectorXd::Zero(constraints);
		//	temp[k] = 1.0;
		//	z = ConstraintJoint::computeMinv_x(temp, SS, LS, S);
		//	for (int l = 0; l < constraints; ++l)
		//	{
		//		preconditioner(l, k) = z[l];
		//	}
		//}

		outfile << "h = " << SS->h << ";\n" << std::endl;
		addSparseToFile(outfile, "Mr_c", Mr);
		addSparseToFile(outfile, "Mrtilde_c", Mrtilde);
		addSparseToFile(outfile, "Mm_c", LHS);

		Eigen::SparseMatrix<double> bDm = Dm + SS->bDm;
		addSparseToFile(outfile, "Dm_c", bDm);
		//addSparseToFile(outfile, "MG_c", LHSGt);
		//addSparseToFile(outfile, "Gr_c", Gr);
		addSparseToFile(outfile, "Km_c", Km);
		addSparseToFile(outfile, "Dr_c", SS->Dr);
		addSparseToFile(outfile, "Kr_c", SS->Kr);
		addSparseToFile(outfile, "Jmr_c", SS->J);
		addSparseToFile(outfile, "Jdotmr_c", SS->Jd);
		addVectorToFile(outfile, "fr_c", SS->fr);
		addVectorToFile(outfile, "fm_c", SS->f);
		//addVectorToFile(outfile, "frg_c", frGt);
		addVectorToFile(outfile, "frtilde_c", frtilde);
		addVectorToFile(outfile, "rhs_c", frtilde);
		///Eigen::VectorXd qdot0 = S->qdot;
		//addVectorToFile(outfile, "qdot0_c", qdot0);
		addVectorToFile(outfile, "qdot1_c", S->qdot);
		//addSparseToFile(outfile, "P_c", preconditioner);

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

	if (false)
	{
		std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
		if (decimals.size() == 1)
			decimals = "0" + decimals;
		std::string filename = "../matricies/qdot" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
		std::ofstream outfile(filename, std::ios::out);
		outfile << std::setprecision(16);

		addSparseToFile(outfile, "Mrtilde_c", Mrtilde);
		addSparseToFile(outfile, "Mm_c", LHS);
		addSparseToFile(outfile, "Km_c", Km);
		addSparseToFile(outfile, "Jmr_c", SS->J);
		addVectorToFile(outfile, "qdot1_c", S->qdot);

		outfile.close();

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

	for (int i = 0; i < (int)LS->blocks.size(); ++i)
	{
		// update pos and vel
		LS->joints[LS->joint_map[i]]->update(LS, S);
	}

	//if (LS->blocks.size() == 603 && count < 3)
	//{
	//	count++;
	//	for (int i = 0; i < S->qdot.size(); ++i)
	//	{
	//		std::cout << S->qdot[i] << " ";
	//	}
	//	std::cout << std::endl;
	//}

	return S->qdot;
}
#endif

Eigen::VectorXd Solver::solvePCG(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S)
{
	// Preprocessing, eliminate matrix resizing
	int constraints = SS->Mr_dimension;
	int additional_constraints = SS->Gr_dimension;
	int numObj = (int)LS->blocks.size();

	// Initialize containers
	SS->f = Eigen::VectorXd::Zero(numObj * 6); // load with fm
	SS->fr = Eigen::VectorXd::Zero(constraints);
	SS->Gm = Eigen::MatrixXd::Zero(additional_constraints, numObj * 6);
	SS->GmT = Eigen::MatrixXd::Zero(numObj * 6, additional_constraints);
	SS->gm = Eigen::VectorXd::Zero(additional_constraints);

	// Precomputations for SoA
	ConstraintJoint::preprocess_PCG(SS, LS, S);

	// load constraint RHS update and CG joint preprocessing
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		LS->constraints[i]->updateJoint(SS, LS, S);
	}
	// Update maximal body forces (RHS)
	computeRHS(SS, LS, S);

	// preprocessing
	ConstraintJoint::preprocess_PCG_preconditioner(SS, LS, S, true);

	/// preconditioned
	//Eigen::VectorXd z;
	//Eigen::VectorXd p;
	//if (simtype != redCGNoMat_noprec)
	//{
	//	if (simtype == redCGNoMat_noblkdiag)
	//		ConstraintJoint::preprocess_PCG_preconditioner(SS, LS, S, false);
	//	else
	//		ConstraintJoint::preprocess_PCG_preconditioner(SS, LS, S, true);

	//	z = ConstraintJoint::computeMinv_x(r, SS, LS, S);
	//	p = z;
	//}
	//// NOT preconditioned
	//else
	//{
	//	p = r;
	//}

	// solve
	Eigen::VectorXd qd = S->qdot;		// initial guess is prev. sol.
	Eigen::VectorXd J_x;
	Eigen::VectorXd Jdot_x;

	// J and Jdot in parallel
	ConstraintJoint::computeJ_Jdot_x(J_x, Jdot_x, qd, SS, LS, S);

	Eigen::VectorXd LHSqd = ConstraintJoint::computeJT_x(
		ConstraintJoint::computeLHS_x(J_x, SS, LS, S),
		SS, LS, S);

	// Joint stiffness and damping
	ConstraintJoint::computeStiffnessDampingJoint(LHSqd, qd, S->q, SS, LS, S);

	Eigen::VectorXd Mqd = ConstraintJoint::computeJT_x(
		ConstraintJoint::computeM_x(J_x, SS, LS, S),
		SS, LS, S);
	Eigen::VectorXd fr_save = SS->fr;
	SS->fr += ConstraintJoint::computeJT_x(
		(SS->f - ConstraintJoint::computeM_x(Jdot_x, SS, LS, S)),
		SS, LS, S);
	Eigen::VectorXd frtilde = Mqd + SS->h * SS->fr;

	//////////////////////////////////////////////////////////////////////////
	// Implement Shin's refactoring so JT is only applied once!
	//////////////////////////////////////////////////////////////////////////
	// error accumulation from somewhere
	//// J and Jdot in parallel
	//ConstraintJoint::computeJ_Jdot_x(J_x, Jdot_x, qd, SS, LS, S);
	//Eigen::VectorXd LHSJqd = ConstraintJoint::computeLHS_x(J_x, SS, LS, S);
	//// Joint stiffness and damping
	//ConstraintJoint::computeStiffnessDampingJoint(LHSJqd, qd, S->q, SS, LS, S);
	//Eigen::VectorXd MJqd = ConstraintJoint::computeM_x(J_x, SS, LS, S);
	//Eigen::VectorXd f_MJdqd = SS->f - ConstraintJoint::computeM_x(Jdot_x, SS, LS, S);
	//Eigen::VectorXd Mrtilde_qdot0;
	//Eigen::VectorXd Mr_qdot0;
	//ConstraintJoint::computeJT_x_parallel(Mrtilde_qdot0, LHSJqd, Mr_qdot0, MJqd, SS->fr, f_MJdqd, SS, LS, S);
	//Eigen::VectorXd frtilde = Mr_qdot0 + SS->h * SS->fr;

	if (additional_constraints == 0)
	{
		// Mrtilde\frtilde = qdot
		pcdSaad2003(S->qdot, LHSqd, frtilde, SS, LS, S);
	}
	else if (additional_constraints == 2)
	{
		// Bridge scene has one loop closing constraint
		Eigen::MatrixXd GrT = Eigen::MatrixXd::Zero(constraints, 2);

		Eigen::VectorXd Gmrow1 = SS->Gm.row(0);
		Eigen::VectorXd Gmrow2 = SS->Gm.row(1);
		Eigen::VectorXd GrTcol1 = ConstraintJoint::computeJT_x(Gmrow1, SS, LS, S);
		Eigen::VectorXd GrTcol2 = ConstraintJoint::computeJT_x(Gmrow2, SS, LS, S);

        // Run pcg for each row of G
        Eigen::VectorXd MiGt1;
		Eigen::VectorXd MiGt2;
		pcdSaad2003(MiGt1, LHSqd, GrTcol1, SS, LS, S);
		pcdSaad2003(MiGt2, LHSqd, GrTcol2, SS, LS, S);

		// Form LHS and RHS
		Eigen::VectorXd qdot1unc;
		pcdSaad2003(qdot1unc, LHSqd, frtilde, SS, LS, S);
		Eigen::VectorXd RHS = SS->baumgarte[2] * SS->gm;
		Eigen::MatrixXd LHS = Eigen::MatrixXd::Zero(2, 2);
		for (int r = 0; r < constraints; ++r)
		{
			LHS(0, 0) += GrTcol1[r] * MiGt1[r];
			LHS(0, 1) += GrTcol1[r] * MiGt2[r];
			LHS(1, 0) += GrTcol2[r] * MiGt1[r];
			LHS(1, 1) += GrTcol2[r] * MiGt2[r];

			RHS[0] += GrTcol1[r] * qdot1unc[r];
			RHS[1] += GrTcol2[r] * qdot1unc[r];
		}

		// Solve 2-by-2 system for lambda
		Eigen::VectorXd lambda = LHS.ldlt().solve(RHS);

		// Solve!
		Eigen::VectorXd frtilde_GTlambda = frtilde;
		for (int r = 0; r < constraints; ++r)
		{
			frtilde_GTlambda[r] -= GrTcol1[r] * lambda[0] + GrTcol2[r] * lambda[1];
		}
		pcdSaad2003(S->qdot, LHSqd, frtilde_GTlambda, SS, LS, S);
	}
	else
	{
		// Handle larger scenes
		Eigen::SparseMatrix<double> Gr = Eigen::SparseMatrix<double>(additional_constraints, constraints);
		Eigen::SparseMatrix<double> GrT = Eigen::SparseMatrix<double>(constraints, additional_constraints);
		Eigen::SparseMatrix<double> MiGt = Eigen::SparseMatrix<double>(constraints, additional_constraints);
		std::vector< Eigen::Triplet<double> > Grlist;
		std::vector< Eigen::Triplet<double> > GrTlist;
		std::vector< Eigen::Triplet<double> > MiGtlist;

		// Run PCG for each row of G
		int num_joints = (int)LS->joints.size();
#pragma omp parallel for //private(Gmrowx, GrTcolx, MiGtx)
		for (int i = 0; i < additional_constraints; ++i)
		{
			Eigen::VectorXd Gmrowx;
			Eigen::VectorXd GrTcolx;
			Eigen::VectorXd MiGtx;
			std::shared_ptr<State::local_mt> lmt = std::make_shared<State::local_mt>();
			lmt->alpha_.resize(num_joints);
			lmt->Bhat_.resize(num_joints);
			lmt->beta_.resize(num_joints);
			lmt->Vdot_.resize(num_joints);
			lmt->ST_Bhat_.resize(num_joints);

			Gmrowx = SS->Gm.row(i);
			GrTcolx = ConstraintJoint::computeJT_x(Gmrowx, SS, LS, S, lmt);
			pcdSaad2003(MiGtx, LHSqd, GrTcolx, SS, LS, S, lmt);

			for (int j = 0; j < constraints; ++j)
			{
				if (std::abs(GrTcolx[j]) > THRESHOLD)
				{
#pragma omp critical
					Grlist.push_back(Eigen::Triplet<double>(i, j, GrTcolx[j]));
#pragma omp critical
					GrTlist.push_back(Eigen::Triplet<double>(j, i, GrTcolx[j]));
				}
				if (std::abs(MiGtx[j]) > THRESHOLD)
				{
#pragma omp critical
					MiGtlist.push_back(Eigen::Triplet<double>(j, i, MiGtx[j]));
				}
			}
		}

		// Form LHS and RHS
		Eigen::VectorXd qdot1unc;
		pcdSaad2003(qdot1unc, LHSqd, frtilde, SS, LS, S);
		Eigen::VectorXd RHS = SS->baumgarte[2] * SS->gm;
		Eigen::SparseMatrix<double> LHS = Eigen::SparseMatrix<double>(additional_constraints, additional_constraints);

		Gr.setFromTriplets(Grlist.begin(), Grlist.end());
		MiGt.setFromTriplets(MiGtlist.begin(), MiGtlist.end());

		LHS = Gr * MiGt;
		RHS += Gr * qdot1unc;

		// Solve x-by-x system for lambda
#ifdef REDMAX_PARDISO
		Eigen::PardisoLDLT< Eigen::SparseMatrix<double>> solver;
        Eigen::VectorXd lambda = solver.compute(LHS).solve(RHS);
#else
        Eigen::SparseLU< Eigen::SparseMatrix<double>> solver;
        solver.compute(LHS);
        Eigen::VectorXd lambda = solver.solve(RHS);
#endif

		// Solve!
		GrT.setFromTriplets(GrTlist.begin(), GrTlist.end());
		frtilde -= GrT * lambda;
		pcdSaad2003(S->qdot, LHSqd, frtilde, SS, LS, S);

		if (false)//std::abs(S->t - 1.01)) < 1e-5)
		{
			std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
			if (decimals.size() == 1)
				decimals = "0" + decimals;
			std::string filename = "../matricies/redcgSoA_" + std::to_string(LS->blocks.size()) + "links_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			std::ofstream outfile(filename, std::ios::out);
			outfile << std::setprecision(16);

			outfile << "h = " << SS->h << ";\n" << std::endl;
			//addVectorToFile(outfile, "MiGt1_c", MiGt1);
			//addVectorToFile(outfile, "MiGt2_c", MiGt2);
			//addVectorToFile(outfile, "GrTcol1_c", GrTcol1);
			//addVectorToFile(outfile, "GrTcol2_c", GrTcol2);
			addSparseToFile(outfile, "MiGt_p", MiGt);
			addVectorToFile(outfile, "Mrtildeqd_p", LHSqd);
			addSparseToFile(outfile, "LHS_p", LHS);
			addSparseToFile(outfile, "Gr_p", Gr);
			addSparseToFile(outfile, "Gm_p", SS->Gm);
			addSparseToFile(outfile, "GmT_p", SS->GmT);
			addVectorToFile(outfile, "gm_p", SS->gm);
			addVectorToFile(outfile, "qdot1unc_p", qdot1unc);
			addVectorToFile(outfile, "RHS_p", RHS);
			addVectorToFile(outfile, "fr_p", SS->fr);
			addVectorToFile(outfile, "lambda_p", lambda);
			addVectorToFile(outfile, "GTlambda_p", frtilde);
			addVectorToFile(outfile, "fm_p", SS->f);
			addVectorToFile(outfile, "qdot1_p", S->qdot);
			//addVectorToFile(outfile, "oJT_p", onesJT);
			//addVectorToFile(outfile, "oJ_p", onesJ);

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

	// update joint angles
	S->q = S->q + SS->h * S->qdot;

	if (false)
	{
		std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
		if (decimals.size() == 1)
			decimals = "0" + decimals;
		std::string filename = "../matricies/pcgSoA_qdot" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
		std::ofstream outfile(filename, std::ios::out);
		outfile << std::setprecision(16);
		//outfile << "qdot1_p" << " = [\n";
		//for (int i = 0; i < (int)LS->blocks.size(); ++i)
		//{
		//	Eigen::VectorXd tt = LS->joints[LS->joint_map[i]]->qdot;
		//	for (int j = 0; j < tt.size(); j++)
		//	{
		//		outfile << tt[j] << " ";
		//	}
		//}
		//outfile << "]';\n" << std::endl;

		//Eigen::VectorXd rj = frtilde_save - LHSqd;
		//Eigen::VectorXd zj = ConstraintJoint::computeMinv_x(rj, SS, LS, S);
		//Eigen::VectorXd check = ConstraintJoint::computeMinv_x(rj, SS, LS, S);

		Eigen::VectorXd qdc = Eigen::VectorXd::Zero(S->qdot.size());
		for (int i = 0; i < qdc.size(); ++i)
			qdc[i] = 1;// (((double)rand() / (RAND_MAX)) - 0.5) * 4;
		Eigen::VectorXd checkc = ConstraintJoint::computeMinv_x(qdc, SS, LS, S);

		Eigen::VectorXd MJdqd = ConstraintJoint::computeM_x(Jdot_x, SS, LS, S);

		Eigen::VectorXd J_ones;
		Eigen::VectorXd Jdot_ones;
		ConstraintJoint::computeJ_Jdot_x(J_ones, Jdot_ones, qdc, SS, LS, S);

		addVectorToFile(outfile, "Mrtildeqd_l", LHSqd);
		addVectorToFile(outfile, "frtilde_l", frtilde);
		addVectorToFile(outfile, "Mqd_l", Mqd);
		addVectorToFile(outfile, "fr_l", fr_save);
		addVectorToFile(outfile, "fmr_l", SS->fr);
		addVectorToFile(outfile, "fm_l", SS->f);
		//addVectorToFile(outfile, "zj_l", zj);
		addVectorToFile(outfile, "qdot1_l", S->qdot);
		addVectorToFile(outfile, "check_l", checkc);
		addVectorToFile(outfile, "MJdqd_l", MJdqd);
		addVectorToFile(outfile, "Jdqd_l", Jdot_x);
		addVectorToFile(outfile, "Jqd_l", J_x);
		addVectorToFile(outfile, "Jones_l", J_ones);
		addVectorToFile(outfile, "Jdones_l", Jdot_ones);

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

	for (int i = 0; i < (int)LS->blocks.size(); ++i)
	{
		// update pos and vel
		LS->joints[LS->joint_map[i]]->update(LS, S);
	}
	return S->qdot;
}

Eigen::VectorXd Solver::solvePCG_unopt(std::unique_ptr<StateSolve>& SS, std::unique_ptr<LinkageSystem>& LS, std::unique_ptr<State>& S)
{
	// Preprocessing, eliminate matrix resizing
	int constraints = SS->Mr_dimension;
	int additional_constraints = SS->Gr_dimension;
	int numObj = (int)LS->blocks.size();

	// Initialize containers
	SS->f = Eigen::VectorXd::Zero(numObj * 6); // load with fm
	SS->fr = Eigen::VectorXd::Zero(constraints);
	SS->Gm = Eigen::MatrixXd::Zero(additional_constraints, numObj * 6);
	SS->GmT = Eigen::MatrixXd::Zero(numObj * 6, additional_constraints);
	SS->gm = Eigen::VectorXd::Zero(additional_constraints);

	// load constraint RHS update and CG joint preprocessing
	for (int i = 0; i < LS->constraints.size(); ++i)
	{
		LS->constraints[i]->updateJoint(SS, LS, S);
	}
	// Update maximal body forces (RHS)
	computeRHS(SS, LS, S);

	// preprocessing
	ConstraintJoint::preprocess_preconditioner_unopt(SS, LS, S, true);

	//if (simtype == redCGNoMat_noblkdiag)
	//	ConstraintJoint::preprocess_preconditioner_unopt(SS, LS, S, false);
	//else
	//	ConstraintJoint::preprocess_preconditioner_unopt(SS, LS, S, true);

	// solve
	Eigen::VectorXd qd = S->qdot;		// initial guess is prev. sol.
	Eigen::VectorXd J_x;
	Eigen::VectorXd Jdot_x;

	// J and Jdot in parallel
	ConstraintJoint::computeJ_Jdot_x(J_x, Jdot_x, qd, SS, LS, S);

	Eigen::VectorXd LHSqd = ConstraintJoint::computeJT_x(
		ConstraintJoint::computeLHS_x(J_x, SS, LS, S),
		SS, LS, S);
	// Joint stiffness and damping
	ConstraintJoint::computeStiffnessDampingJoint(LHSqd, qd, S->q, SS, LS, S);

	Eigen::VectorXd Mqd = ConstraintJoint::computeJT_x(
		ConstraintJoint::computeM_x(J_x, SS, LS, S),
		SS, LS, S);
	SS->fr += ConstraintJoint::computeJT_x(
		(SS->f - ConstraintJoint::computeM_x(Jdot_x, SS, LS, S)),
		SS, LS, S);
	Eigen::VectorXd frtilde = Mqd + SS->h * SS->fr;
	Eigen::VectorXd frtilde_save = frtilde;

	//////////////////////////////////////////////////////////////////////////
	// Implement Shin's refactoring so JT is only applied once!
	//////////////////////////////////////////////////////////////////////////

	// error accumulation from somewhere
	//// J and Jdot in parallel
	//ConstraintJoint::computeJ_Jdot_x(J_x, Jdot_x, qd, SS, LS, S);
	//Eigen::VectorXd LHSJqd = ConstraintJoint::computeLHS_x(J_x, SS, LS, S);
	//// Joint stiffness and damping
	//ConstraintJoint::computeStiffnessDampingJoint(LHSJqd, qd, S->q, SS, LS, S);
	//Eigen::VectorXd MJqd = ConstraintJoint::computeM_x(J_x, SS, LS, S);
	//Eigen::VectorXd f_MJdqd = SS->f - ConstraintJoint::computeM_x(Jdot_x, SS, LS, S);
	//Eigen::VectorXd Mrtilde_qdot0;
	//Eigen::VectorXd Mr_qdot0;
	//ConstraintJoint::computeJT_x_parallel(Mrtilde_qdot0, LHSJqd, Mr_qdot0, MJqd, SS->fr, f_MJdqd, SS, LS, S);
	//Eigen::VectorXd frtilde = Mr_qdot0 + SS->h * SS->fr;

	if (additional_constraints == 0)
	{
		// Mrtilde\frtilde = qdot
		pcdSaad2003(S->qdot, LHSqd, frtilde, SS, LS, S);
	}
	else if (additional_constraints == 2)
	{
		// Bridge scene has one loop closing constraint
		Eigen::MatrixXd GrT = Eigen::MatrixXd::Zero(constraints, 2);

		Eigen::VectorXd Gmrow1 = SS->Gm.row(0);
		Eigen::VectorXd Gmrow2 = SS->Gm.row(1);
		Eigen::VectorXd GrTcol1 = ConstraintJoint::computeJT_x(Gmrow1, SS, LS, S);
		Eigen::VectorXd GrTcol2 = ConstraintJoint::computeJT_x(Gmrow2, SS, LS, S);

		// Run pcg for each row of G
		Eigen::VectorXd MiGt1;
		Eigen::VectorXd MiGt2;
		pcdSaad2003(MiGt1, LHSqd, GrTcol1, SS, LS, S);
		pcdSaad2003(MiGt2, LHSqd, GrTcol2, SS, LS, S);

		// Form LHS and RHS
		Eigen::VectorXd qdot1unc;
		pcdSaad2003(qdot1unc, LHSqd, frtilde, SS, LS, S);
		Eigen::VectorXd RHS = SS->baumgarte[2] * SS->gm;
		Eigen::MatrixXd LHS = Eigen::MatrixXd::Zero(2, 2);
		for (int r = 0; r < constraints; ++r)
		{
			LHS(0, 0) += GrTcol1[r] * MiGt1[r];
			LHS(0, 1) += GrTcol1[r] * MiGt2[r];
			LHS(1, 0) += GrTcol2[r] * MiGt1[r];
			LHS(1, 1) += GrTcol2[r] * MiGt2[r];

			RHS[0] += GrTcol1[r] * qdot1unc[r];
			RHS[1] += GrTcol2[r] * qdot1unc[r];
		}

		// Solve 2-by-2 system for lambda
		Eigen::VectorXd lambda = LHS.ldlt().solve(RHS);

		// Solve!
		Eigen::VectorXd frtilde_GTlambda = frtilde;
		for (int r = 0; r < constraints; ++r)
		{
			frtilde_GTlambda[r] -= GrTcol1[r] * lambda[0] + GrTcol2[r] * lambda[1];
		}
		pcdSaad2003(S->qdot, LHSqd, frtilde_GTlambda, SS, LS, S);

		if (false)//std::abs(S->t - 1.01)) < 1e-5)
		{
			std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
			if (decimals.size() == 1)
				decimals = "0" + decimals;
			std::string filename = "../matricies/redcg_" + std::to_string(LS->blocks.size()) + "links_2case_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			std::ofstream outfile(filename, std::ios::out);
			outfile << std::setprecision(16);

			outfile << "h = " << SS->h << ";\n" << std::endl;
			addVectorToFile(outfile, "MiGt1_c", MiGt1);
			addVectorToFile(outfile, "MiGt2_c", MiGt2);
			addVectorToFile(outfile, "GrTcol1_c", GrTcol1);
			addVectorToFile(outfile, "GrTcol2_c", GrTcol2);
			addSparseToFile(outfile, "LHS_c", LHS);
			addSparseToFile(outfile, "GrT_c", GrT);
			addSparseToFile(outfile, "Gm_c", SS->Gm);
			addSparseToFile(outfile, "GmT_c", SS->GmT);
			addVectorToFile(outfile, "gm_c", SS->gm);
			addVectorToFile(outfile, "qdot1unc_c", qdot1unc);
			addVectorToFile(outfile, "RHS_c", RHS);
			addVectorToFile(outfile, "fr_c", SS->fr);
			addVectorToFile(outfile, "lambda_c", lambda);
			addVectorToFile(outfile, "GTlambda_c", frtilde_GTlambda);
			addVectorToFile(outfile, "fm_c", SS->f);
			addVectorToFile(outfile, "qdot1_c", S->qdot);

			outfile.close();
			std::cout << "Printed" << std::endl;
		}
	}
	else
	{
		// Handle larger scenes
		Eigen::SparseMatrix<double> Gr = Eigen::SparseMatrix<double>(additional_constraints, constraints);
		Eigen::SparseMatrix<double> GrT = Eigen::SparseMatrix<double>(constraints, additional_constraints);
		Eigen::SparseMatrix<double> MiGt = Eigen::SparseMatrix<double>(constraints, additional_constraints);
		std::vector< Eigen::Triplet<double> > Grlist;
		std::vector< Eigen::Triplet<double> > GrTlist;
		std::vector< Eigen::Triplet<double> > MiGtlist;

		// Run PCG for each row of G
		int num_joints = LS->joints.size();
#pragma omp parallel for //private(Gmrowx, GrTcolx, MiGtx)
		for (int i = 0; i < additional_constraints; ++i)
		{
			Eigen::VectorXd Gmrowx;
			Eigen::VectorXd GrTcolx;
			Eigen::VectorXd MiGtx;
			std::shared_ptr<State::local_mt> lmt = std::make_shared<State::local_mt>();
			lmt->alpha_.resize(num_joints);
			lmt->Bhat_.resize(num_joints);
			lmt->beta_.resize(num_joints);
			lmt->Vdot_.resize(num_joints);
			lmt->ST_Bhat_.resize(num_joints);

			Gmrowx = SS->Gm.row(i);
			GrTcolx = ConstraintJoint::computeJT_x(Gmrowx, SS, LS, S, lmt);
			pcdSaad2003(MiGtx, LHSqd, GrTcolx, SS, LS, S, lmt);

			for (int j = 0; j < constraints; ++j)
			{
				if (std::abs(GrTcolx[j]) > THRESHOLD)
				{
#pragma omp critical
					Grlist.push_back(Eigen::Triplet<double>(i, j, GrTcolx[j]));
#pragma omp critical
					GrTlist.push_back(Eigen::Triplet<double>(j, i, GrTcolx[j]));
				}
				if (std::abs(MiGtx[j]) > THRESHOLD)
				{
#pragma omp critical
					MiGtlist.push_back(Eigen::Triplet<double>(j, i, MiGtx[j]));
				}
			}
		}

		// Form LHS and RHS
		Eigen::VectorXd qdot1unc;
		pcdSaad2003(qdot1unc, LHSqd, frtilde, SS, LS, S);
		Eigen::VectorXd RHS = SS->baumgarte[2] * SS->gm;
		Eigen::SparseMatrix<double> LHS = Eigen::SparseMatrix<double>(additional_constraints, additional_constraints);

		Gr.setFromTriplets(Grlist.begin(), Grlist.end());
		MiGt.setFromTriplets(MiGtlist.begin(), MiGtlist.end());

		LHS = Gr * MiGt;
		RHS += Gr * qdot1unc;

		// Solve x-by-x system for lambda
#ifdef REDMAX_PARDISO
        Eigen::PardisoLDLT< Eigen::SparseMatrix<double>> solver;
        Eigen::VectorXd lambda = solver.compute(LHS).solve(RHS);
#else
        Eigen::SparseLU< Eigen::SparseMatrix<double>> solver;
        solver.compute(LHS);
        Eigen::VectorXd lambda = solver.solve(RHS);
#endif
		// Solve!
		GrT.setFromTriplets(GrTlist.begin(), GrTlist.end());
		frtilde -= GrT * lambda;
		pcdSaad2003(S->qdot, LHSqd, frtilde, SS, LS, S);

		if (false)//std::abs(S->t - 1.01)) < 1e-5)
		{
			std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
			if (decimals.size() == 1)
				decimals = "0" + decimals;
			std::string filename = "../matricies/redcg_" + std::to_string(LS->blocks.size()) + "links_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			std::ofstream outfile(filename, std::ios::out);
			outfile << std::setprecision(16);

			outfile << "h = " << SS->h << ";\n" << std::endl;
			//addVectorToFile(outfile, "MiGt1_c", MiGt1);
			//addVectorToFile(outfile, "MiGt2_c", MiGt2);
			//addVectorToFile(outfile, "GrTcol1_c", GrTcol1);
			//addVectorToFile(outfile, "GrTcol2_c", GrTcol2);
			addSparseToFile(outfile, "MiGt_l", MiGt);
			addVectorToFile(outfile, "Mrtildeqd_l", LHSqd);
			addSparseToFile(outfile, "LHS_l", LHS);
			addSparseToFile(outfile, "Gr_l", Gr);
			addSparseToFile(outfile, "Gm_l", SS->Gm);
			addSparseToFile(outfile, "GmT_l", SS->GmT);
			addVectorToFile(outfile, "gm_l", SS->gm);
			addVectorToFile(outfile, "qdot1unc_l", qdot1unc);
			addVectorToFile(outfile, "RHS_l", RHS);
			addVectorToFile(outfile, "fr_l", SS->fr);
			addVectorToFile(outfile, "lambda_l", lambda);
			addVectorToFile(outfile, "GTlambda_l", frtilde);
			addVectorToFile(outfile, "frtilde_l", frtilde);
			addVectorToFile(outfile, "fm_l", SS->f);
			addVectorToFile(outfile, "qdot1_l", S->qdot);
			//addVectorToFile(outfile, "oJT_p", onesJT);
			//addVectorToFile(outfile, "oJ_p", onesJ);

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

		if (false)//std::abs(S->t - 1.01)) < 1e-5)
		{
			std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
			if (decimals.size() == 1)
				decimals = "0" + decimals;
			std::string filename = "../matricies/redcg_" + std::to_string(LS->blocks.size()) + "links_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			//std::string filename = "../matricies/redmax_" + std::to_string(LS->blocks.size()) + "Preconditioner_noblkdiag_t" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
			std::ofstream outfile(filename, std::ios::out);
			outfile << std::setprecision(16);

			outfile << "h = " << SS->h << ";\n" << std::endl;
			//addVectorToFile(outfile, "MiGt1_c", MiGt1);
			//addVectorToFile(outfile, "MiGt2_c", MiGt2);
			//addVectorToFile(outfile, "GrTcol1_c", GrTcol1);
			//addVectorToFile(outfile, "GrTcol2_c", GrTcol2);
			addSparseToFile(outfile, "LHS_c", LHS);
			addSparseToFile(outfile, "GrT_c", GrT);
			//addSparseToFile(outfile, "Gm_c", SS->Gm);
			addSparseToFile(outfile, "GmT_c", SS->GmT);
			addVectorToFile(outfile, "gm_c", SS->gm);
			addVectorToFile(outfile, "qdot1unc_c", qdot1unc);
			addVectorToFile(outfile, "RHS_c", RHS);
			addVectorToFile(outfile, "fr_c", SS->fr);
			addVectorToFile(outfile, "lambda_c", lambda);
			addVectorToFile(outfile, "GTlambda_c", frtilde);
			addVectorToFile(outfile, "fm_c", SS->f);
			addVectorToFile(outfile, "qdot1_c", S->qdot);

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

	if (false)
	{
		std::string decimals = std::to_string((int)(100 * (S->t - (int)std::floor(S->t))));
		if (decimals.size() == 1)
			decimals = "0" + decimals;
		std::string filename = "../matricies/pcg_qdot" + std::to_string((int)std::floor(S->t)) + "_" + decimals + ".m";
		std::ofstream outfile(filename, std::ios::out);
		outfile << std::setprecision(16);

		Eigen::VectorXd rj = frtilde_save - LHSqd;
		Eigen::VectorXd zj = ConstraintJoint::computeMinv_x(rj, SS, LS, S);
		Eigen::VectorXd check = ConstraintJoint::computeMinv_x(rj, SS, LS, S);

		Eigen::VectorXd qdc = Eigen::VectorXd::Zero(S->qdot.size());
		for (int i = 0; i < qdc.size(); ++i)
			qdc[i] = 1;// (((double)rand() / (RAND_MAX)) - 0.5) * 4;
		Eigen::VectorXd checkc = ConstraintJoint::computeMinv_x(qdc, SS, LS, S);

		addVectorToFile(outfile, "Mrtildeqd_l", LHSqd);
		addVectorToFile(outfile, "frtilde_l", frtilde_save);
		addVectorToFile(outfile, "Mqd_l", Mqd);
		addVectorToFile(outfile, "fr_l", SS->fr);
		addVectorToFile(outfile, "zj_l", zj);
		addVectorToFile(outfile, "qdot1_l", S->qdot);
		addVectorToFile(outfile, "check_l", checkc);

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

	// update joint angles
	S->q = S->q + SS->h * S->qdot;

	for (int i = 0; i < (int)LS->blocks.size(); ++i)
	{
		// update pos and vel
		LS->joints[LS->joint_map[i]]->update(LS, S);
	}
	return S->qdot;
}

Solver::Solver()
{
	trackLastTimestep = SolverDataTracker();
}

Eigen::VectorXd Solver::solve(std::unique_ptr<StateSolve> &SS, std::unique_ptr<LinkageSystem> &LS, std::unique_ptr<State> &S)
{
	Eigen::VectorXd result;

	trackLastTimestep.num_iterations = 0;
	if (simtype == simType::PCG)
		result = solvePCG(SS, LS, S);
	else if (simtype == simType::PCG_unopt)
		result = solvePCG_unopt(SS, LS, S);
#ifdef REDMAX_PARDISO
	else if (simtype == simType::Pardiso)
		result = solvePardiso(SS, LS, S);
#endif
	else
		std::cerr << "Specified simtype solver has not been set up" << std::endl;

	return result;
}

void Solver::printDenseToFile(Eigen::MatrixXd & MG, Eigen::VectorXd & f, Eigen::VectorXd & res, std::string id)
{
	std::string filename = "../matricies/" + id + ".m";
	std::ofstream outfile(filename, std::ios::out);

	outfile << "MG = [\n";
	for (int r = 0; r < MG.rows(); r++)
	{
		for (int c = 0; c < MG.cols(); c++)
		{
			outfile << MG(r, c) << " ";
		}
		outfile << ";\n";
	}
	outfile << "];\n\nf = [\n";

	for (int i = 0; i < f.size(); i++)
	{
		outfile << f[i] << "; ";
	}
	outfile << "];\n\nres = [";

	for (int i = 0; i < res.size(); i++)
	{
		outfile << res[i] << "; ";
	}
	outfile << "];\n";
}

void Solver::printSparseToFile(std::vector<Eigen::Triplet<double>>& MG, Eigen::VectorXd & f, Eigen::VectorXd & res)
{
	std::string filename = "../matricies/r_sparse.m";
	std::ofstream outfile(filename, std::ios::out);

	outfile << "i = [\n";
	for (int i = 0; i < MG.size(); i++)
	{
		outfile << MG[i].row() + 1 << " ";
	}
	outfile << "]';\n\nj= [\n";
	for (int i = 0; i < MG.size(); i++)
	{
		outfile << MG[i].col() + 1 << " ";
	}
	outfile << "]';\n\nv = [\n";
	for (int i = 0; i < MG.size(); i++)
	{
		outfile << MG[i].value() << " ";
	}
	outfile << "]';\n" << std::endl;

	outfile << "MG = sparse(i,j,v);\n" << std::endl;

	outfile << "f = [\n";
	for (int i = 0; i < f.size(); i++)
	{
		outfile << f[i] << "; ";
	}
	outfile << "];\n\nres = [";
	for (int i = 0; i < res.size(); i++)
	{
		outfile << res[i] << "; ";
	}
	outfile << "];\n";
}

void Solver::addSparseToFile(std::ofstream & outfile, std::string name, Eigen::MatrixXd &M)
{
	std::vector<int> ilist;
	std::vector<int> jlist;
	std::vector<double> vlist;

	for (int r = 0; r < M.rows(); ++r)
	{
		for (int c = 0; c < M.cols(); ++c)
		{
			if (M(r, c) != 0)
			{
				ilist.push_back(r);
				jlist.push_back(c);
				vlist.push_back(M(r,c));
			}
		}
	}

	outfile << "i = [\n";
	for (int i = 0; i < ilist.size(); i++)
	{
		outfile << ilist[i] + 1 << " ";
	}
	outfile << "]';\n\nj= [\n";
	for (int i = 0; i < jlist.size(); i++)
	{
		outfile << jlist[i] + 1 << " ";
	}
	outfile << "]';\n\nv = [\n";
	for (int i = 0; i < vlist.size(); i++)
	{
		outfile << vlist[i] << " ";
	}
	outfile << "]';\n" << std::endl;

	int m = (int)M.rows();
	int n = (int)M.cols();

	outfile << name << " = sparse(i,j,v," + std::to_string(m) + "," + std::to_string(n) + ");\n" << std::endl;
}

void Solver::addSparseToFile(std::ofstream & outfile, std::string name, Eigen::SparseMatrix<double>& M)
{
    // https://stackoverflow.com/questions/28685877/convert-an-eigen-matrix-to-triplet-form-c
    std::string entry = " = sparse(";
    outfile << "ijv = [" << std::endl;
    int e = 1;
    for(int k = 0; k < M.outerSize(); ++k) {
        for(Eigen::SparseMatrix<double>::InnerIterator it(M,k); it; ++it) {
            double v = it.value();
            if(std::abs(v) > 1e-10) {
                outfile << it.row()+1 << " "; // row index
                outfile << it.col()+1 << " "; // col index (here it is equal to k)
                outfile << v << std::endl;
                entry.append("ijv(:," + std::to_string(e) + "),");
                ++e;
            }
        }
    }
    outfile << "];" << std::endl;
    outfile << name <<  entry + std::to_string(M.rows()) + "," + std::to_string(M.cols()) + ");\n" << std::endl;
}

void Solver::addVectorToFile(std::ofstream & outfile, std::string name, Eigen::VectorXd & M)
{
	outfile << name << " = [\n";

	for (int i = 0; i < M.size(); i++)
	{
		outfile << M[i] << " ";
	}
	outfile << "]';\n" << std::endl;
}


//
// Created by gongsf on 15/5/19.
//

#include <float.h>
#include <boost/algorithm/string/trim.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <unordered_set>
#include <vector>

using namespace std;
using namespace boost;

long unfind(vector<long>& un_father, long u) {
  return un_father[u] == u ? u : (un_father[u] = unfind(un_father, un_father[u]));
}
void ununion(vector<long>& un_father, long u, long v) {
  un_father[u] = v;
}

void fennel(int argc, char *argv[]) {
  cout << "fennel start !!!!!!!!!!!" << endl;
  string input;
  string output;
  int part_num = -1;
  long vertex_num = -1;
  long edge_num = -1;

  for (int i = 0; i < argc; i++) {
    string arg = argv[i];
    if (arg == "--in") {
      input = argv[++i];
    } else if (arg == "--out") {
      output = argv[++i];
    } else if (arg == "--part_num") {
      part_num = atoi(argv[++i]);
    } else if (arg == "--vertex_num") {
      vertex_num = atol(argv[++i]);
    } else if (arg == "--edge_num") {
      edge_num = atol(argv[++i]);
    }
  }

  if (part_num == -1 || vertex_num == -1 || edge_num == -1) {
    cout << "invalie parameters" << endl;
    cout << part_num << endl;
    cout << vertex_num << endl;
    cout << edge_num << endl;
    exit(0);
  }

  ifstream infile(input.c_str());
  if (infile.is_open()) {
    string line;
    edge_num = edge_num / 2;
    float gamma = 1.5;
    float alpha = sqrt(part_num) * edge_num / pow(vertex_num, gamma);
    float v = 1.1;
    float miu = v * vertex_num / part_num;
    vector<long> part_info = vector<long>(vertex_num, 0);
    vector<long> vertex_num_part = vector<long>(part_num, 0);

    vector<long> un_father(vertex_num);
    vector<long> un_first(part_num, -1);
    for (long i = 0; i < vertex_num; ++i) un_father[i] = i;

    // the format each line is: u\tv1 v2 v3 ....
    for (long i = 0; getline(infile, line); i++) {
      if (i % 1000 == 0) cout << i << endl;
      trim(line);
      long pos = line.find_first_of("\t");
      long id = atoi(line.substr(0, pos).c_str());
      line = line.substr(pos + 1);
      vector<long> neighbor;
      long to = 0;
      for (size_t p = 0; p < line.length(); ++p) {
        if (line[p] >= '0' && line[p] <= '9') {
          to = to*10+(line[p] - '0');
        }
        if (p == line.length() -1 || line[p] == ' ') {
          neighbor.push_back(to);
          to = 0;
        }
      }

      float max_score = -FLT_MAX;
      int max_part = 0;
      for (long i = 0; i < part_num; i++) {
        if (vertex_num_part[i] <= miu) {
          double delta_c = alpha * (pow(vertex_num_part[i] + 1, gamma) -
                                    pow(vertex_num_part[i], gamma));
          float score = 0;
          for (long j = 0; j < neighbor.size(); j++) {
            long nid = neighbor[j];
            // this partition contains nid
            if (un_first[i] >= 0 && unfind(un_father, un_first[i]) == unfind(un_father, nid)) {
              score += 1;
            }
          }
          score = score - delta_c;
          if (max_score < score) {
            max_score = score;
            max_part = i;
          }
        }
      }
      if (un_first[max_part] < 0) un_first[max_part] = id;
      ununion(un_father, id, un_first[max_part]);
      vertex_num_part[max_part] += 1;
      part_info[id] = max_part;
    }
    ofstream outfile(output.c_str());
    for (long i = 0; i < vertex_num; i++) {
      outfile << part_info[i] << endl;
    }
    outfile.close();
  }
  infile.close();
}

int main(int argc, char *argv[]) {
  if (argc == 1) {
    std::cout << "./fennel --in [] --out [] --part_num [] --vertex_num [] "
                 "--edge_num []"
              << std::endl;
    return -1;
  }
  fennel(argc, argv);
  return 0;
}


#include "../include/Bigram.h"

#include <sstream>
#include <iostream>
#include <fstream>

#include <boost/lexical_cast.hpp>

#include "../include/exceptions.h"
#include "../include/Timer.h"
#include "../include/utility.h"

using namespace boost;

string Bigram::ending = "bigram";

Bigram::Bigram(Corpus* co) : Model<Bigram>(co)
{
    size1 = co->getEwords()->size();
    size2 = size1;
    model = new float*[size1];
    for (int i = 0; i < size1; i++)
    {
        model[i] = new float[size2];
        for (int j = 0; j < size2; j++)
        {
            model[i][j] = SMOOTH;
        }
    }

    int word;
    int prevword;
    int count[size1];
    for(int i=0; i<size1; i++)
    {
        count[i] = SMOOTH*size1;
    }
    vector<int*>* lines = co->getElines();
    systemMessage("Counting Bigrams");
    for(vector<int*>::iterator it = lines->begin(); it != lines->end(); it++)
    {
        prevword = (*it)[1];
        for(int i=2; i< (*it)[0]; i++)
        {
            word = (*it)[i];
            count[prevword]++;
            model[prevword][word]++;
            prevword = word;
        }
    }
    float* ptr;
    systemMessage("Normalizing Bigram Model");
    for(int i = 0; i<size1; i++)
    {
        for(int j = 0; j<size2; j++)
        {
            ptr = &model[i][j];
            if(*ptr != 0.0)
            {
                (*ptr) = (*ptr)/count[i];
            }
        }
    }
    saveModel();
}


// Copyright (c) 2019 herenvarno
//
// This software is released under the MIT License.
// https://opensource.org/licenses/MIT

#ifndef __ALL_TOPOLOGICAL_SORTS_HPP__
#define __ALL_TOPOLOGICAL_SORTS_HPP__

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/property_map/property_map.hpp>
#include <queue>
#include <set>
#include <stack>
#include <unordered_map>
#include <vector>

using namespace std;

namespace bglex {

/**
 * Find all topolotical sorts in a graph
 */
template <typename G>
vector<vector<typename G::vertex_descriptor>> all_topological_sorts(G g_) {

  using namespace boost;
  typedef typename G::vertex_descriptor VD;
  typedef typename G::edge_descriptor ED;
  typedef typename G::vertex_iterator VI;
  typedef typename G::edge_iterator EI;
  typedef typename G::in_edge_iterator IEI;
  typedef typename G::out_edge_iterator OEI;

  typedef boost::adjacency_list<vecS, vecS, bidirectionalS> BG;

  // Define a aux function _ats_util()
  std::function<void(vector<vector<BG::vertex_descriptor>> & ats, BG g,
                     vector<BG::vertex_descriptor> & res,
                     std::unordered_map<VD, bool> visited)>
      _ats_util;
  _ats_util = [&](vector<vector<BG::vertex_descriptor>> &ats, BG g,
                  vector<BG::vertex_descriptor> &res,
                  std::unordered_map<VD, bool> visited) {
    bool flag = false;

    BG::vertex_iterator vi, vi_end;
    for (tie(vi, vi_end) = vertices(g); vi != vi_end; vi++) {
      if (in_degree(*vi, g) == 0 && !visited[*vi]) {
        res.push_back(*vi);
        visited[*vi] = true;

        BG g0;
        copy_graph(g, g0);
        clear_out_edges(*vi, g0);
        _ats_util(ats, g0, res, visited);

        visited[*vi] = false;
        res.erase(res.end() - 1);
        flag = true;
      }
    }

    if (!flag) {
      ats.push_back(res);
    }
  };

  BG bg;
  copy_graph(g_, bg);

  vector<vector<VD>> ret;
  vector<vector<BG::vertex_descriptor>> ret1;
  std::unordered_map<BG::vertex_descriptor, bool> visited;
  BG::vertex_iterator vi, vi_end;
  for (tie(vi, vi_end) = vertices(bg); vi != vi_end; vi++) {
    visited[*vi] = false;
  }
  vector<BG::vertex_descriptor> res;

  _ats_util(ret1, bg, res, visited);

  for (auto &res : ret1) {
    vector<VD> vec;
    for (auto &v : res) {
      vec.push_back(v);
    }
    ret.push_back(vec);
  }

  return ret;
}

} // namespace bglex

#endif


/*
	COL 719 Assignment 1
	Author - Shreshth Tuli

	Determine if a circuit is combinational or sequential
*/

#include <iostream>
#include <vector>
#include <fstream>
#include <deque>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>

using namespace boost;
using namespace std;

typedef adjacency_list<listS, vecS, directedS> Graph;
typedef graph_traits<Graph>::vertex_descriptor vertex_t;

deque<vertex_t> topologicalSorted;

void topologicalSort(Graph &graph)
{
    try{
        topological_sort(graph, front_inserter(topologicalSorted));
    }
    catch (not_a_dag){
        cout << "Sequential Circuit\n"; return;
    }

    cout << "Combinational Circuit\n";
}

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

	ifstream infile(argv[1]);

	int a, b, n;
	infile >> n;

	if(n==0){
		cout << "Combinational Circuit";
		return 0;
	}

	Graph g;

	while(infile >> a >> b)
		add_edge(a, b, g);

	topologicalSort(g);
	return 0;
};


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/betweenness_centrality.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <vector>
#include <stack>
#include <queue>
#include <boost/property_map/property_map.hpp>
#include <boost/test/minimal.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost;

const double error_tolerance = 0.001;

typedef property<edge_weight_t, double,
                 property<edge_index_t, std::size_t> > EdgeProperties;

struct weighted_edge
{
  int source, target;
  double weight;
};

template<typename Graph>
void
run_weighted_test(Graph*, int V, weighted_edge edge_init[], int E,
                  double correct_centrality[])
{
  Graph g(V);
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor Edge;

  std::vector<Vertex> vertices(V);
  {
    vertex_iterator v, v_end;
    int index = 0;
    for (boost::tie(v, v_end) = boost::vertices(g); v != v_end; ++v, ++index) {
      put(vertex_index, g, *v, index);
      vertices[index] = *v;
    }
  }

  std::vector<Edge> edges(E);
  for (int e = 0; e < E; ++e) {
    edges[e] = add_edge(vertices[edge_init[e].source],
                        vertices[edge_init[e].target],
                        g).first;
    put(edge_weight, g, edges[e], 1.0);
  }

  std::vector<double> centrality(V);
  brandes_betweenness_centrality(
    g,
    centrality_map(
      make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                                 double()))
    .vertex_index_map(get(vertex_index, g)).weight_map(get(edge_weight, g)));

  for (int v = 0; v < V; ++v) {
    BOOST_CHECK(centrality[v] == correct_centrality[v]);
  }
}

struct unweighted_edge
{
  int source, target;
};

template<typename Graph>
void
run_unweighted_test(Graph*, int V, unweighted_edge edge_init[], int E,
                    double correct_centrality[],
                    double* correct_edge_centrality = 0)
{
  Graph g(V);
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor Edge;

  std::vector<Vertex> vertices(V);
  {
    vertex_iterator v, v_end;
    int index = 0;
    for (boost::tie(v, v_end) = boost::vertices(g); v != v_end; ++v, ++index) {
      put(vertex_index, g, *v, index);
      vertices[index] = *v;
    }
  }

  std::vector<Edge> edges(E);
  for (int e = 0; e < E; ++e) {
    edges[e] = add_edge(vertices[edge_init[e].source],
                        vertices[edge_init[e].target],
                        g).first;
    put(edge_weight, g, edges[e], 1.0);
    put(edge_index, g, edges[e], e);
  }

  std::vector<double> centrality(V);
  std::vector<double> edge_centrality1(E);

  brandes_betweenness_centrality(
    g,
    centrality_map(
      make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                                 double()))
    .edge_centrality_map(
       make_iterator_property_map(edge_centrality1.begin(),
                                  get(edge_index, g), double()))
    .vertex_index_map(get(vertex_index, g)));

  std::vector<double> centrality2(V);
  std::vector<double> edge_centrality2(E);
  brandes_betweenness_centrality(
    g,
    vertex_index_map(get(vertex_index, g)).weight_map(get(edge_weight, g))
    .centrality_map(
       make_iterator_property_map(centrality2.begin(), get(vertex_index, g),
                                  double()))
    .edge_centrality_map(
       make_iterator_property_map(edge_centrality2.begin(),
                                  get(edge_index, g), double())));

  std::vector<double> edge_centrality3(E);
  brandes_betweenness_centrality(
    g,
    edge_centrality_map(
      make_iterator_property_map(edge_centrality3.begin(),
                                 get(edge_index, g), double())));

  for (int v = 0; v < V; ++v) {
    BOOST_CHECK(centrality[v] == centrality2[v]);

    double relative_error =
      correct_centrality[v] == 0.0? centrality[v]
      : (centrality[v] - correct_centrality[v]) / correct_centrality[v];
    if (relative_error < 0) relative_error = -relative_error;
    BOOST_CHECK(relative_error < error_tolerance);
  }

  for (int e = 0; e < E; ++e) {
    BOOST_CHECK(edge_centrality1[e] == edge_centrality2[e]);
    BOOST_CHECK(edge_centrality1[e] == edge_centrality3[e]);

    if (correct_edge_centrality) {
      double relative_error =
        correct_edge_centrality[e] == 0.0? edge_centrality1[e]
        : (edge_centrality1[e] - correct_edge_centrality[e])
        / correct_edge_centrality[e];
      if (relative_error < 0) relative_error = -relative_error;
      BOOST_CHECK(relative_error < error_tolerance);

      if (relative_error >= error_tolerance) {
        std::cerr << "Edge " << e << " has edge centrality "
                  << edge_centrality1[e] << ", should be "
                  << correct_edge_centrality[e] << std::endl;
      }
    }
  }
}

template<typename Graph>
void
run_wheel_test(Graph*, int V)
{
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor Edge;

  Graph g(V);
  Vertex center = *boost::vertices(g).first;

  std::vector<Vertex> vertices(V);
  {
    vertex_iterator v, v_end;
    int index = 0;
    for (boost::tie(v, v_end) = boost::vertices(g); v != v_end; ++v, ++index) {
      put(vertex_index, g, *v, index);
      vertices[index] = *v;
      if (*v != center) {
        Edge e = add_edge(*v, center, g).first;
        put(edge_weight, g, e, 1.0);
      }
    }
  }

  std::vector<double> centrality(V);
  brandes_betweenness_centrality(
    g,
    make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                               double()));

  std::vector<double> centrality2(V);
  brandes_betweenness_centrality(
    g,
    centrality_map(
      make_iterator_property_map(centrality2.begin(), get(vertex_index, g),
                                 double()))
    .vertex_index_map(get(vertex_index, g)).weight_map(get(edge_weight, g)));

  relative_betweenness_centrality(
    g,
    make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                               double()));

  relative_betweenness_centrality(
    g,
    make_iterator_property_map(centrality2.begin(), get(vertex_index, g),
                               double()));

  for (int v = 0; v < V; ++v) {
    BOOST_CHECK(centrality[v] == centrality2[v]);
    BOOST_CHECK((v == 0 && centrality[v] == 1)
               || (v != 0 && centrality[v] == 0));
  }

  double dominance =
    central_point_dominance(
      g,
      make_iterator_property_map(centrality2.begin(), get(vertex_index, g),
                                 double()));
  BOOST_CHECK(dominance == 1.0);
}

template<typename MutableGraph>
void randomly_add_edges(MutableGraph& g, double edge_probability)
{
  typedef typename graph_traits<MutableGraph>::directed_category
    directed_category;
  const bool is_undirected =
    is_same<directed_category, undirected_tag>::value;

  minstd_rand gen;
  uniform_01<minstd_rand, double> rand_gen(gen);

  typedef typename graph_traits<MutableGraph>::vertex_descriptor vertex;
  typename graph_traits<MutableGraph>::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    vertex v = *vi;
    typename graph_traits<MutableGraph>::vertex_iterator wi
      = is_undirected? vi : vertices(g).first;
    while (wi != vi_end) {
      vertex w = *wi++;
      if (v != w) {
        if (rand_gen() < edge_probability) add_edge(v, w, g);
      }
    }
  }
}

template<typename Graph, typename VertexIndexMap, typename CentralityMap>
void
simple_unweighted_betweenness_centrality(const Graph& g, VertexIndexMap index,
                                         CentralityMap centrality)
{
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex;
  typedef typename boost::graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename boost::graph_traits<Graph>::adjacency_iterator adjacency_iterator;
  typedef typename boost::graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename boost::property_traits<CentralityMap>::value_type centrality_type;

  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    put(centrality, *vi, 0);

  vertex_iterator si, si_end;
  for (boost::tie(si, si_end) = vertices(g); si != si_end; ++si) {
    vertex s = *si;

    // S <-- empty stack
    std::stack<vertex> S;

    // P[w] <-- empty list, w \in V
    typedef std::vector<vertex> Predecessors;
    std::vector<Predecessors> predecessors(num_vertices(g));

    // sigma[t] <-- 0, t \in V
    std::vector<vertices_size_type> sigma(num_vertices(g), 0);

    // sigma[s] <-- 1
    sigma[get(index, s)] = 1;

    // d[t] <-- -1, t \in V
    std::vector<int> d(num_vertices(g), -1);

    // d[s] <-- 0
    d[get(index, s)] = 0;

    // Q <-- empty queue
    std::queue<vertex> Q;

    // enqueue s --> Q
    Q.push(s);

    while (!Q.empty()) {
      // dequeue v <-- Q
      vertex v = Q.front(); Q.pop();

      // push v --> S
      S.push(v);

      adjacency_iterator wi, wi_end;
      for (boost::tie(wi, wi_end) = adjacent_vertices(v, g); wi != wi_end; ++wi) {
        vertex w = *wi;

        // w found for the first time?
        if (d[get(index, w)] < 0) {
          // enqueue w --> Q
          Q.push(w);

          // d[w] <-- d[v] + 1
          d[get(index, w)] = d[get(index, v)] + 1;
        }

        // shortest path to w via v?
        if (d[get(index, w)] == d[get(index, v)] + 1) {
          // sigma[w] = sigma[w] + sigma[v]
          sigma[get(index, w)] += sigma[get(index, v)];

          // append v --> P[w]
          predecessors[get(index, w)].push_back(v);
        }
      }
    }

    // delta[v] <-- 0, v \in V
    std::vector<centrality_type> delta(num_vertices(g), 0);

    // S returns vertices in order of non-increasing distance from s
    while (!S.empty()) {
      // pop w <-- S
      vertex w = S.top(); S.pop();

      const Predecessors& w_preds = predecessors[get(index, w)];
      for (typename Predecessors::const_iterator vi = w_preds.begin();
           vi != w_preds.end(); ++vi) {
        vertex v = *vi;
        // delta[v] <-- delta[v] + (sigma[v]/sigma[w])*(1 + delta[w])
        delta[get(index, v)] +=
          ((centrality_type)sigma[get(index, v)]/sigma[get(index, w)])
          * (1 + delta[get(index, w)]);
      }

      if (w != s) {
        // C_B[w] <-- C_B[w] + delta[w]
        centrality[w] += delta[get(index, w)];
      }
    }
  }

  typedef typename graph_traits<Graph>::directed_category directed_category;
  const bool is_undirected =
    is_same<directed_category, undirected_tag>::value;
  if (is_undirected) {
    vertex_iterator v, v_end;
    for(boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
      put(centrality, *v, get(centrality, *v) / centrality_type(2));
    }
  }
}

template<typename Graph>
void random_unweighted_test(Graph*, int n)
{
  Graph g(n);

  {
    typename graph_traits<Graph>::vertex_iterator v, v_end;
    int index = 0;
    for (boost::tie(v, v_end) = boost::vertices(g); v != v_end; ++v, ++index) {
      put(vertex_index, g, *v, index);
    }
  }

  randomly_add_edges(g, 0.20);

  std::cout << "Random graph with " << n << " vertices and "
            << num_edges(g) << " edges.\n";

  std::cout << "  Direct translation of Brandes' algorithm...";
  std::vector<double> centrality(n);
  simple_unweighted_betweenness_centrality(g, get(vertex_index, g),
    make_iterator_property_map(centrality.begin(), get(vertex_index, g),
                               double()));
  std::cout << "DONE.\n";

  std::cout << "  Real version, unweighted...";
  std::vector<double> centrality2(n);
  brandes_betweenness_centrality(g,
     make_iterator_property_map(centrality2.begin(), get(vertex_index, g),
                                double()));
  std::cout << "DONE.\n";

  if (!std::equal(centrality.begin(), centrality.end(),
                  centrality2.begin())) {
    for (std::size_t v = 0; v < centrality.size(); ++v) {
      double relative_error =
        centrality[v] == 0.0? centrality2[v]
        : (centrality2[v] - centrality[v]) / centrality[v];
      if (relative_error < 0) relative_error = -relative_error;
      BOOST_CHECK(relative_error < error_tolerance);
    }
  }

  std::cout << "  Real version, weighted...";
  std::vector<double> centrality3(n);

  for (typename graph_traits<Graph>::edge_iterator ei = edges(g).first;
       ei != edges(g).second; ++ei)
    put(edge_weight, g, *ei, 1);

  brandes_betweenness_centrality(g,
    weight_map(get(edge_weight, g))
    .centrality_map(
       make_iterator_property_map(centrality3.begin(), get(vertex_index, g),
                                  double())));
  std::cout << "DONE.\n";

  if (!std::equal(centrality.begin(), centrality.end(),
                  centrality3.begin())) {
    for (std::size_t v = 0; v < centrality.size(); ++v) {
      double relative_error =
        centrality[v] == 0.0? centrality3[v]
        : (centrality3[v] - centrality[v]) / centrality[v];
      if (relative_error < 0) relative_error = -relative_error;
      BOOST_CHECK(relative_error < error_tolerance);
    }
  }
}

int test_main(int argc, char* argv[])
{
  int random_test_num_vertices = 300;
  if (argc >= 2) random_test_num_vertices = boost::lexical_cast<int>(argv[1]);
  typedef adjacency_list<listS, listS, undirectedS,
                         property<vertex_index_t, int>, EdgeProperties>
    Graph;
  typedef adjacency_list<listS, listS, directedS,
                         property<vertex_index_t, int>, EdgeProperties>
    Digraph;

  struct unweighted_edge ud_edge_init1[5] = {
    { 0, 1 },
    { 0, 3 },
    { 1, 2 },
    { 3, 2 },
    { 2, 4 }
  };
  double ud_centrality1[5] = { 0.5, 1.0, 3.5, 1.0, 0.0 };
  run_unweighted_test((Graph*)0, 5, ud_edge_init1, 5, ud_centrality1);

  // Example borrowed from the JUNG test suite
  struct unweighted_edge ud_edge_init2[10] = {
    { 0, 1 },
    { 0, 6 },
    { 1, 2 },
    { 1, 3 },
    { 2, 4 },
    { 3, 4 },
    { 4, 5 },
    { 5, 8 },
    { 7, 8 },
    { 6, 7 },
  };
  double ud_centrality2[9] = {
    0.2142 * 28,
    0.2797 * 28,
    0.0892 * 28,
    0.0892 * 28,
    0.2797 * 28,
    0.2142 * 28,
    0.1666 * 28,
    0.1428 * 28,
    0.1666 * 28
  };
  double ud_edge_centrality2[10] = {
    10.66666,
    9.33333,
    6.5,
    6.5,
    6.5,
    6.5,
    10.66666,
    9.33333,
    8.0,
    8.0
  };

  run_unweighted_test((Graph*)0, 9, ud_edge_init2, 10, ud_centrality2,
                      ud_edge_centrality2);

  weighted_edge dw_edge_init1[6] = {
    { 0, 1, 1.0 },
    { 0, 3, 1.0 },
    { 1, 2, 0.5 },
    { 3, 1, 1.0 },
    { 3, 4, 1.0 },
    { 4, 2, 0.5 }
  };
  double dw_centrality1[5] = { 0.0, 1.5, 0.0, 1.0, 0.5 };
  run_weighted_test((Digraph*)0, 5, dw_edge_init1, 6, dw_centrality1);

  run_wheel_test((Graph*)0, 15);

  random_unweighted_test((Graph*)0, random_test_num_vertices);

  return 0;
}


#include <boost/heap/d_ary_heap.hpp>

template<typename T>
using d_ary_heap = boost::heap::d_ary_heap<T, boost::heap::arity<2>, boost::heap::mutable_<true>>;


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/enumerable.hpp>
#include <boost/hana/tuple.hpp>

#include <test/auto/base.hpp>
#include <test/auto/enumerable.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <> auto objects<int> = make<Tuple>(0,1,2,3,4,5);
    template <> auto objects<unsigned int> = make<Tuple>(0u,1u,2u,3u,4u,5u);
    template <> auto objects<long> = make<Tuple>(0l,1l,2l,3l,4l,5l);
    template <> auto objects<unsigned long> = make<Tuple>(0ul,1ul,2ul,3ul,4ul,5ul);
}}}

int main() {
    test::laws<Enumerable, int>();
    test::laws<Enumerable, unsigned int>();

    test::laws<Enumerable, long>();
    test::laws<Enumerable, unsigned long>();

    using T = int;
    constexpr T x = 6;

    // succ
    {
        BOOST_HANA_CONSTEXPR_CHECK(succ(x) == x+1);
    }

    // pred
    {
        BOOST_HANA_CONSTEXPR_CHECK(pred(x) == x-1);
    }
}


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

BOOST_AUTO_TEST_CASE(print_n_first_primes)
{
    std::ostringstream dest;

    print_primes(4, dest);

    std::ostringstream expected;

    expected << "Found primes 4 primes:" << std::endl
             << "2 3 5 7 " << std::endl;

    BOOST_REQUIRE_EQUAL(expected.str(), dest.str());
}

BOOST_AUTO_TEST_CASE(sum_n_first_primes)
{
    unsigned sum = sum_primes(4);

    BOOST_REQUIRE_EQUAL(sum, 2+3+5+7);
}
/*
#include <boost/test/included/prg_exec_monitor.hpp>
int cpp_main(int, char*[])
{
    cout << "Hello, World!" << endl;
    return 0;
}

*/


#include <Discregrid/All>
#include <Eigen/Dense>
#include <cxxopts/cxxopts.hpp>

#include <string>
#include <iostream>
#include <array>

#include "bmp_file.hpp"

using namespace Eigen;

namespace
{
std::array<unsigned char, 3u> doubleToGreenBlueInverse(double v)
{
	if (v >= 0.0)
	{
		return {{0u, static_cast<unsigned char>(std::min(std::max(255.0 * (1.0 - v), 0.0), 255.0)), 0u}};
	}
	return {{0u, 0u, static_cast<unsigned char>(std::min(std::max(255.0 * (1.0 + v), 0.0), 255.0))}};
}

std::array<unsigned char, 3u> doubleToRedSequential(double v)
{
	return {{static_cast<unsigned char>(std::min(std::max(255.0 * v, 0.0), 255.0)), 0u, 0u}};
}

}

int main(int argc, char* argv[])
{
	cxxopts::Options options(argv[0], "Transforms a slice of a discrete SDF to a bitmap image.");
	options.positional_help("[input SDF file]");

	options.add_options()
	("h,help", "Prints this help text")
	("f,field_id", "ID in which the SDF to export is stored.", cxxopts::value<unsigned int>()->default_value("0"))
	("s,samples", "Number of samples in width direction", cxxopts::value<unsigned int>()->default_value("1024"))
	("p,plane", "Plane in which the image slice is extracted", cxxopts::value<std::string>()->default_value("xy"))
	("d,depth", "Relative depth value between -1 and 1 in direction of the axis orthogonal to the plane", cxxopts::value<double>()->default_value("0"))
	("o,output", "Output (in bmp format)", cxxopts::value<std::string>()->default_value(""))
	("c,colormap", "Color map options: redsequential (rs), green blue inverse diverging (gb) (suitable for visualiztion of signed distance fields)", cxxopts::value<std::string>()->default_value("gb"))
	("input", "SDF file", cxxopts::value<std::vector<std::string>>())
	;

	try
	{
		options.parse_positional("input");
		auto result = options.parse(argc, argv);

		if (result.count("help"))
		{
			std::cout << options.help() << std::endl;
			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
			exit(0);
		}
		if (!result.count("input"))
		{
			std::cout << "ERROR: No input file given." << std::endl;
			std::cout << options.help() << std::endl;
			std::cout << std::endl << std::endl << "Example: SDFToBitmap -p xz file.sdf" << std::endl;
			exit(1);
		}

		auto filename = result["input"].as<std::vector<std::string>>().front();

		if (!std::ifstream(filename).good())
		{
			std::cerr << "ERROR: Input file does not exist!" << std::endl;
			exit(1);
		}

		auto sdf = std::unique_ptr<Discregrid::DiscreteGrid>{};

		auto lastindex = filename.find_last_of(".");
		auto extension = filename.substr(lastindex + 1, filename.length() - lastindex);

		std::cout << "Load SDF...";
		if (extension == "cdf" || extension == "cdm")
		{
			sdf = std::unique_ptr<Discregrid::CubicLagrangeDiscreteGrid>(
				new Discregrid::CubicLagrangeDiscreteGrid(filename));
		}
		std::cout << "DONE" << std::endl;

		auto depth = result["d"].as<double>();
		auto const& domain = sdf->domain();
		auto diag = domain.diagonal().eval();

		auto plane = result["p"].as<std::string>();
		if (plane.length() != 2 && plane[0] != plane[1])
		{
			std::cerr << "ERROR: Invalid option for plane provided. Should be one of the following options: xy, xz, yz, yx" << std::endl;
			exit(1);
		}

		auto dir = Vector3i::Zero().eval();
		if (plane[0] == 'y')
			dir(0) = 1;
		else if (plane[0] == 'z')
			dir(0) = 2;
		if (plane[1] == 'y')
			dir(1) = 1;
		else if (plane[1] == 'z')
			dir(1) = 2;
		if (dir(0) != 1 && dir(1) != 1)
			dir(2) = 1;
		if (dir(0) != 2 && dir(1) != 2)
			dir(2) = 2;

		auto xsamples = result["s"].as<unsigned int>();
		auto ysamples = static_cast<unsigned int>(std::round(diag(dir(1)) / diag(dir(0)) * static_cast<double>(xsamples)));

		auto xwidth = diag(dir(0)) / xsamples;
		auto ywidth = diag(dir(1)) / ysamples;

		auto data = std::vector<double>{};
		data.resize(xsamples * ysamples);

		auto field_id = result["f"].as<unsigned int>();

		std::cout << "Sample field...";
#pragma omp parallel for
		for (int k = 0; k < static_cast<int>(xsamples * ysamples); ++k)
		{
			auto i = k % xsamples;
			auto j = k / xsamples;

			auto xr = static_cast<double>(i) / static_cast<double>(xsamples);
			auto yr = static_cast<double>(j) / static_cast<double>(ysamples);

			auto x = domain.min()(dir(0)) + xr * diag(dir(0)) + 0.5 * xwidth;
			auto y = domain.min()(dir(1)) + yr * diag(dir(1)) + 0.5 * ywidth;

			auto sample = Vector3d{};
			sample(dir(0)) = x;
			sample(dir(1)) = y;
			sample(dir(2)) = domain.min()(dir(2)) + 0.5 * (1.0 + depth) * diag(dir(2));

			data[k] = sdf->interpolate(field_id, sample);
			if (data[k] == std::numeric_limits<double>::max())
			{
				data[k] = 0.0;
			}
		}

		std::cout << "DONE" << std::endl;

		auto min_v = *std::min_element(data.begin(), data.end());
		auto max_v = *std::max_element(data.begin(), data.end());

		auto out_file = result["o"].as<std::string>();
		if (out_file == "")
		{
			out_file = filename;
			if (out_file.find(".") != std::string::npos)
			{
				auto lastindex = out_file.find_last_of(".");
				out_file = out_file.substr(0, lastindex);
			}
			out_file += ".bmp";
		}

		std::cout << "Ouput file: " << out_file << std::endl;

		std::cout << "Export BMP...";
		std::transform(data.begin(), data.end(), data.begin(), [&max_v, &min_v](double v) {return v >= 0.0 ? v / std::abs(max_v) : v / std::abs(min_v); });

		auto pixels = std::vector<std::array<unsigned char, 3u>>(data.size());

		auto cm = result["c"].as<std::string>();
		if (cm != "gb" && cm != "rs")
		{
			std::cerr << "WARNING: Unknown color map option. Fallback to mode 'gb'." << std::endl;
		}

		if (cm == "gb")
			std::transform(data.begin(), data.end(), pixels.begin(), doubleToGreenBlueInverse);
		else if (cm == "rs")
			std::transform(data.begin(), data.end(), pixels.begin(), doubleToRedSequential);

		BmpReaderWriter::saveFile(out_file.c_str(), xsamples, ysamples, &pixels.front()[0]);
		std::cout << "DONE" << std::endl;

		std::cout << std::endl << "Statistics:" << std::endl;
		std::cout << "\tdomain         = " << domain.min().transpose() << ", " << domain.max().transpose() << std::endl;
		std::cout << "\tmin value      = " << min_v << std::endl;
		std::cout << "\tmax value      = " << max_v << std::endl;
		std::cout << "\tbmp resolution = " << xsamples << " x " << ysamples << std::endl;
	}
	catch (cxxopts::OptionException const& e)
	{
		std::cout << "error parsing options: " << e.what() << std::endl;
		exit(1);
	}

	return 0;
}


#include <fstream>
#include <vector>
#include <string>
#include <sstream>
#include <Eigen/Dense>
#include "firmware_generator.h"
#include "firmware_template.h"
#include "control_design_functions.h"

void firmware_generator(const std::vector<model_data> & md)
{
  const int observer_state_size = 4;    // observer is a full state estimator
  const int observer_input_size = 3;    // observer inputs are: steer, roll rate,
                                        //                      steer torque
  //const int observer_output_size = 1;   // estimate of roll angle
  const int plant_model_state_size = 4; // roll, steer, roll rate, steer rate
  const int plant_model_input_size = 1; // torque
  const int plant_model_output_size = 1; // yaw rate

  std::string header_body{};
  header_body += "const uint32_t num_gains = "
              + std::to_string(md.size()) + ";\n";
  header_body += "const uint32_t observer_state_size = "
              + std::to_string(observer_state_size) + ";\n";
  header_body += "const uint32_t observer_input_size = "
              + std::to_string(observer_input_size) + ";\n";
//  header_body += "const uint32_t observer_output_size = "
//              + std::to_string(observer_output_size) + ";\n";
  header_body += "const uint32_t plant_model_state_size = "
              + std::to_string(plant_model_state_size) + ";\n";
  header_body += "const uint32_t plant_model_input_size = "
              + std::to_string(plant_model_input_size) + ";\n";
  header_body += "const uint32_t plant_model_output_size = "
              + std::to_string(plant_model_output_size) + ";\n";

  std::ofstream header_file("gain_schedule.h");
  header_file << firmware_template::preamble
              << header_body << firmware_template::postamble;
  header_file.close();

  std::ofstream source_file("fork_schedule.cpp");
  source_file << generate_source(md, "gain_schedule");
  source_file.close();
}

std::string generate_source(const std::vector<model_data> & md, std::string filename_base)
{
  //std::string source_body{};
  Eigen::IOFormat printfmt(Eigen::FullPrecision, 0, "f, ", "f, ", "", "", "{{", "f}}");
  std::ostringstream out;
  out << "#include \"" << filename_base << ".h\"\n\n";
  out << "namespace control {\n\n";
  out << "const std::array<rt_controller_t, " << md.size();
  out << "> GainSchedule::schedule_ {{\n";

  out.precision(16); out << std::scientific;
  for(auto it = md.begin(); it != md.end(); ++it) {
    out << "\t{\n";
    out << "\t\t" << it->theta_R_dot <<"f, // theta_R_dot\n";
    out << "\t\t{\n"; // start writing out controllers

    out << "\t\t\t{ // StateEstimator\n";
    out << "\t\t\t\t" << it->A_obs_d.format(printfmt) << ",\n";
    out << "\t\t\t\t" << it->B_obs_d.format(printfmt) << "\n";
    out << "\t\t\t},\n"; // StateEstimator End

    out << "\t\t\t{ // LQRController\n"; // LQRController Start
    out << "\t\t\t\t" << it->K_lqr_d.format(printfmt) << "\n";
    out << "\t\t\t},\n"; // LQRController End

    out << "\t\t\t{ // PIController\n"; // PIController Start
    out << "\t\t\t\t" << it->Kp_d << "f, " << it->Ki_d << "f\n";
    out << "\t\t\t},\n"; // PIController End

    out << "\t\t\t{ // Yaw rate measurement matrix\n"; // Yaw rate measurement Start
    out << "\t\t\t\t" << it->C_yaw_rate.format(printfmt) << "\n";
    out << "\t\t\t}\n"; // Yaw rate measurement End

    out << "\t\t}\n"; // controllers end
    if (it + 1 == md.end()) {
      out << "\t}\n";
    } else {
      out << "\t},\n";
    }
  }

  out << "}};\n\n} // namespace control";

  return out.str();
}


#ifndef STAN_MATH_PRIM_PROB_SKEW_NORMAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_SKEW_NORMAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <stan/math/prim/fun/to_ref.hpp>
#include <stan/math/prim/prob/normal_rng.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Skew-normal random variate for the given location, scale,
 * and shape using the specified random number generator.
 *
 * mu, sigma, and alpha can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_loc type of location parameter
 * @tparam T_scale type of scale parameter
 * @tparam T_shape type of shape parameter
 * @tparam RNG type of random number generator
 *
 * @param mu (Sequence of) location parameter(s)
 * @param sigma (Sequence of) scale parameter(s)
 * @param alpha (Sequence of) shape parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Skew-normal random variate(s)
 * @throw std::domain_error if mu is infinite, sigma is nonpositive, or
 * alpha is infinite
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_scale, typename T_shape, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_scale, T_shape>::type
skew_normal_rng(const T_loc& mu, const T_scale& sigma, const T_shape& alpha,
                RNG& rng) {
  using boost::variate_generator;
  using boost::random::normal_distribution;
  static const char* function = "skew_normal_rng";
  check_consistent_sizes(function, "Location parameter", mu, "Scale Parameter",
                         sigma, "Shape Parameter", alpha);
  const auto& mu_ref = to_ref(mu);
  const auto& sigma_ref = to_ref(sigma);
  const auto& alpha_ref = to_ref(alpha);
  check_finite(function, "Location parameter", mu_ref);
  check_positive_finite(function, "Scale parameter", sigma_ref);
  check_finite(function, "Shape parameter", alpha_ref);

  scalar_seq_view<T_loc> mu_vec(mu_ref);
  scalar_seq_view<T_scale> sigma_vec(sigma_ref);
  scalar_seq_view<T_shape> alpha_vec(alpha_ref);
  size_t N = max_size(mu, sigma, alpha);
  VectorBuilder<true, double, T_loc, T_scale, T_shape> output(N);

  variate_generator<RNG&, normal_distribution<> > norm_rng(
      rng, normal_distribution<>(0, 1));
  for (size_t n = 0; n < N; ++n) {
    double r1 = norm_rng();
    double r2 = norm_rng();

    if (r2 > alpha_vec[n] * r1) {
      r1 = -r1;
    }

    output[n] = mu_vec[n] + sigma_vec[n] * r1;
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/array.hpp>
#include <array>
#include <utility>
#include <iterator>
#include <algorithm>
#include <iostream>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g{edges.begin(), edges.end(), 4};

  boost::array<int, 4> distances{{0}};

  boost::breadth_first_search(g, topLeft,
    boost::visitor(
      boost::make_bfs_visitor(
        boost::record_distances(distances.begin(),
          boost::on_tree_edge{}))));

  std::copy(distances.begin(), distances.end(),
    std::ostream_iterator<int>{std::cout, "\n"});
}


/********************************************************************************
 * Copyright 2016 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWSIM_LOG_LOGEQUATIONSYSTEM_HPP_
#define RWSIM_LOG_LOGEQUATIONSYSTEM_HPP_

/**
 * @file LogEquationSystem.hpp
 *
 * \copydoc rwsim::log::LogEquationSystem
 */

#include "SimulatorLogEntry.hpp"

#include <rw/core/Ptr.hpp>

#include <Eigen/Core>

namespace rwsim { namespace log {

    class LogContactSet;

    //! @addtogroup rwsim_log

    //! @{
    /**
     * @brief Log entry for a linear equation system \f$\mathbf{A}\mathbf{x}=\mathbf{b}\f$.
     */
    class LogEquationSystem : public SimulatorLogEntry
    {
      public:
        //! Smart pointer type of LogEquationSystem
        typedef rw::core::Ptr< LogEquationSystem > Ptr;

        //! @copydoc SimulatorLogEntry::SimulatorLogEntry
        LogEquationSystem (SimulatorLogScope* parent);

        //! @brief Destructor.
        virtual ~LogEquationSystem ();

        //! @copydoc SimulatorLogEntry::read
        virtual void read (class rw::common::InputArchive& iarchive, const std::string& id);

        //! @copydoc SimulatorLogEntry::write
        virtual void write (class rw::common::OutputArchive& oarchive, const std::string& id) const;

        //! @copydoc SimulatorLogEntry::getType
        virtual std::string getType () const;

        //! @copydoc SimulatorLogEntry::operator==
        virtual bool operator== (const SimulatorLog& b) const;

        //! @copydoc SimulatorLogEntry::getLinkedEntries
        virtual std::list< SimulatorLogEntry::Ptr > getLinkedEntries () const;

        //! @copydoc SimulatorLogEntry::autoLink
        virtual bool autoLink ();

        //! @copydoc SimulatorLogEntry::createNew
        virtual SimulatorLogEntry::Ptr createNew (SimulatorLogScope* parent) const;

        /**
         * @brief Get the type id of this entry type.
         * @return the type id.
         */
        static std::string getTypeID ();

        /**
         * @brief Set the equation system.
         * @param A [in] the matrix.
         * @param b [in] the right-hand-side.
         */
        void set (const Eigen::MatrixXd& A, const Eigen::VectorXd& b);

        /**
         * @brief Set the solution to the system.
         * @param x [in] the solution.
         */
        void setSolution (const Eigen::VectorXd& x);

        /**
         * @brief Get the matrix for the linear equation system.
         * @return a reference to the matrix.
         */
        const Eigen::MatrixXd& A () const;

        /**
         * @brief Get the right-hand side of the equation system.
         * @return a reference to the right-hand side.
         */
        const Eigen::VectorXd& b () const;

        /**
         * @brief Get the solution.
         * @return a reference to the solution.
         */
        const Eigen::VectorXd& x () const;

      private:
        Eigen::MatrixXd _A;
        Eigen::VectorXd _x;
        Eigen::VectorXd _b;
    };
    //! @}
}}     // namespace rwsim::log
#endif /* RWSIM_LOG_LOGEQUATIONSYSTEM_HPP_ */


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     histogram.cpp
 * \author   Collin Johnson
 *
 * Definition of Histogram.
 */

#include "utils/histogram.h"
#include <algorithm>
#include <boost/algorithm/clamp.hpp>
#include <cassert>
#include <cmath>
#include <iostream>

namespace vulcan
{
namespace utils
{

std::vector<HistogramBin>
  split_data_into_bins(const std::vector<double>& data, double minValue, double maxValue, int numBins);

/////////////// Operators /////////////////////
std::ostream& operator<<(std::ostream& out, const Histogram& hist)
{
    for (auto& bin : hist) {
        out << '[' << bin.minValue << ',' << bin.maxValue << "]: " << bin.count << '\n';
    }

    return out;
}

////////////// Histogram /////////////////////

Histogram::Histogram(int numBins)
: numBins_(numBins)
, bins_(numBins)
, binsAreDirty_(true)
, minValue_(NAN)
, maxValue_(NAN)
{
    assert(numBins_ > 0);
}

Histogram::Histogram(double minValue, double maxValue, int numBins)
: numBins_(numBins)
, bins_(numBins)
, binsAreDirty_(true)
, minValue_(minValue)
, maxValue_(maxValue)
{
    assert(minValue_ < maxValue_);
    assert(numBins_ > 0);
}

void Histogram::addValue(double value)
{
    values_.push_back(value);
    binsAreDirty_ = true;
}

int Histogram::findValueBinIndex(double value) const
{
    computeHistogramIfNeeded();

    auto binIt = std::find_if(bins_.begin(), bins_.end(), [value](const HistogramBin& bin) {
        return (bin.minValue <= value) && (value <= bin.maxValue);
    });

    return (binIt == bins_.end()) ? -1 : std::distance(bins_.begin(), binIt);
}

HistogramBin Histogram::bin(int binIndex) const
{
    computeHistogramIfNeeded();

    assert(binIndex >= 0);
    assert(binIndex < numBins_);

    return bins_[binIndex];
}

void Histogram::normalize(void)
{
    computeHistogramIfNeeded();

    double total = 0.0;

    for (auto& b : bins_) {
        total += b.count;
    }

    if (total > 0.0) {
        for (auto& b : bins_) {
            b.count /= total;
        }
    }
}

void Histogram::clear(void)
{
    values_.clear();
    binsAreDirty_ = true;
}

Histogram::const_iterator Histogram::begin(void) const
{
    computeHistogramIfNeeded();
    return bins_.begin();
}

Histogram::const_iterator Histogram::end(void) const
{
    assert(!binsAreDirty_);
    return bins_.end();
}

math::UnivariateGaussianDistribution Histogram::toGaussian(void) const
{
    return math::UnivariateGaussianDistribution(values_.begin(), values_.end());
}

void Histogram::computeHistogramIfNeeded(void) const
{
    if (binsAreDirty_ && !values_.empty()) {
        if (std::isnan(minValue_) || std::isnan(maxValue_)) {
            minValue_ = *std::min_element(values_.begin(), values_.end());
            maxValue_ = *std::max_element(values_.begin(), values_.end());
        }

        bins_ = split_data_into_bins(values_, minValue_, maxValue_, numBins_);
        binsAreDirty_ = false;
    }
}

std::vector<HistogramBin>
  split_data_into_bins(const std::vector<double>& data, double minValue, double maxValue, int numBins)
{
    if (data.empty() || (numBins == 0)) {
        return std::vector<HistogramBin>();
    }

    double range = maxValue - minValue;
    double binWidth = range / numBins;

    // All bins can't be the same
    if (binWidth == 0.0) {
        return std::vector<HistogramBin>();
    }

    // Construct the bins
    std::vector<HistogramBin> bins(numBins);
    bins[0].minValue = minValue;
    bins[0].maxValue = minValue + binWidth;
    bins[0].count = 0;

    for (int n = 1; n < numBins; ++n) {
        bins[n].minValue = bins[n - 1].maxValue;
        bins[n].maxValue = bins[n].minValue + binWidth;
        bins[n].count = 0;
    }

    // Populate the bins
    for (auto value : data) {
        int binIdx = (value - minValue) / binWidth;
        binIdx = boost::algorithm::clamp(binIdx, 0, numBins - 1);
        bins[binIdx].count++;
    }

    return bins;
}

}   // namespace utils
}   // namespace vulcan