#include "precompile.h"

#pragma warning( disable : 4101)
#pragma warning( disable : 4244)
#pragma warning( disable : 4267)

#include <iostream>

#include <fstream>
#include "nis_solver_base.h"
//#define IGL_SOLVE

#ifdef IGL_SOLVE
#include "igl/mosek/mosek_quadprog.h"
#endif

//#define ENABLE_MATLAB_DEBUGGING // comment this out if you don't want matlab debugger
#ifdef ENABLE_MATLAB_DEBUGGING
#include "../matlab_debugger.h"
extern MatlabDebugger* g_debugger;
#endif

#include "../timer_wrapper.h"

#include <cmath>

// external global variables
extern ScalarType test1;
extern ScalarType test2;
extern ScalarType test3;
extern ScalarType test4;
extern ScalarType test5;

// global functions
ScalarType exterior_points_psi(ScalarType t, ScalarType k)
{
	ScalarType tk = k*t;
	ScalarType y;
	if (tk > -0.5)
	{
		y = log(tk+1);
	}
	else
	{
		y = -2*tk*tk + log(0.5) + 0.5;
	}
	y = y/k;

	return y;
}
ScalarType exterior_points_d_psi(ScalarType t, ScalarType k)
{
	ScalarType dy;
	ScalarType tk = k*t;

	if (tk>-0.5)
	{
		dy = 1.0/(tk+1);
	}
	else
	{
		dy = -4*tk;
	}
	return dy;
}
ScalarType exterior_points_d2_psi(ScalarType t, ScalarType k)
{
	ScalarType d2y;
	ScalarType tk = k*t;
	if (tk>-0.5)
	{
		d2y = -k/pow((tk+1),2);
	}
	else
	{
		d2y = -4*k;
	}
	return d2y;
}

VectorX exterior_points_psi(const VectorX& t, ScalarType k)
{
	VectorX y;
	y.resize(t.size());
	VectorX tk = k*t;
	ScalarType log_one_half = std::log(0.5);
	for (unsigned int i = 0; i != tk.size(); ++i)
	{
		if (tk(i)>-0.5)
		{
			y(i) = log(tk(i)+1);
		}
		else
		{
			y(i) = -2*tk(i)*tk(i) + log_one_half + 0.5;
		}
	}
	y = y / k;

	return y;
}
VectorX exterior_points_d_psi(const VectorX& t, ScalarType k)
{
	VectorX dy;
	dy.resize(t.size());
	VectorX tk = k*t;
	for (unsigned int i = 0; i != tk.size(); ++i)
	{
		if (tk(i)>-0.5)
		{
			dy(i) = 1.0/(tk(i)+1);
		}
		else
		{
			dy(i) = -4*tk(i);
		}
	}
	return dy;
}
VectorX exterior_points_d2_psi(const VectorX& t, ScalarType k)
{
	VectorX d2y;
	d2y.resize(t.size());
	VectorX tk = k*t;
	for (unsigned int i = 0; i != tk.size(); ++i)
	{
		if (tk(i)>-0.5)
		{
			d2y(i) = -k/pow((tk(i)+1),2);
		}
		else
		{
			d2y(i) = -4*k;
		}
	}
	return d2y;
}

ScalarType function_log_barrier(ScalarType x) {return -std::log(x);}
ScalarType derivative_log_barrier(ScalarType x) {return -1/x;}
ScalarType derivative2_log_barrier(ScalarType x) {return 1/(x*x);}

ScalarType function_log_plus_linear_barrier(ScalarType x) {return -std::log(x)+x;}
ScalarType derivative_log_plus_linear_barrier(ScalarType x) {return -1/x+1;}
ScalarType derivative2_log_plus_linear_barrier(ScalarType x) {return 1/(x*x);}

ScalarType penalty_function_quadratic_on_negative_psi(ScalarType x)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON; 
	return (x<0)?0.5*x*x:0;
}
ScalarType penalty_function_quadratic_on_negative_d_psi(ScalarType x)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?x:0;
}
ScalarType penalty_function_quadratic_on_negative_d2_psi(ScalarType x) 
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?1:0;
}


ScalarType penalty_function_quadratic_on_negative_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON; 
	return (x<0)?0.5*k*x*x:0;
}
ScalarType penalty_function_quadratic_on_negative_d_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?k*x:0;
}
ScalarType penalty_function_quadratic_on_negative_d2_psi(ScalarType x, ScalarType k) 
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?k:0;
}

//ScalarType penalty_function_quadratic_on_negative_psi(ScalarType x, ScalarType k)
//{
//	// shift to positive a little bit
//	x = x-1; 
//	return 0.5*k*x*x;
//}
//ScalarType penalty_function_quadratic_on_negative_d_psi(ScalarType x, ScalarType k)
//{
//	// shift to positive a little bit
//	x = x-1;
//	return k*x;
//}
//ScalarType penalty_function_quadratic_on_negative_d2_psi(ScalarType x, ScalarType k) 
//{
//	// shift to positive a little bit
//	x = x-1;
//	return k;
//}

//ScalarType penalty_function_quadratic_on_negative_psi(ScalarType x, ScalarType k){return (x<0.1)?0.5*k*(x-0.1)*(x-0.1):0;}
//ScalarType penalty_function_quadratic_on_negative_d_psi(ScalarType x, ScalarType k){return (x<0.1)?k*(x-0.1):0;}
//ScalarType penalty_function_quadratic_on_negative_d2_psi(ScalarType x, ScalarType k) {return (x<0.1)?k:0;}
//
ScalarType penalty_function_cubic_on_negative_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?-k*(x)*(x)*(x):0;
}
ScalarType penalty_function_cubic_on_negative_d_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?-3*k*(x)*(x):0;
}
ScalarType penalty_function_cubic_on_negative_d2_psi(ScalarType x, ScalarType k) 
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	return (x<0)?-6*k*(x):0;
}

ScalarType penalty_function_spline_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return -std::log(k*x+1)/k;
	}
	else
	{
		return (2*k*k*x*x-std::log(0.5)-0.5)/k;
	}

}
ScalarType penalty_function_spline_d_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return -1.0/(k*x+1);
	}
	else
	{
		return 4*k*x;
	}
}
ScalarType penalty_function_spline_d2_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x-LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return k/std::pow(k*x+1, 2);
	}
	else
	{
		return 4*k;
	}
}

ScalarType sp_function_spline_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x+LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return x + 0.5*k*x*x;
	}
	else
	{
		return (-0.25*std::log(-2*k*x)-0.375)/k;
	}
}
ScalarType sp_function_spline_d_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x+LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return 1+k*x;
	}
	else
	{
		return -0.25/(k*x);
	}
}
ScalarType sp_function_spline_d2_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x+LARGER_EPSILON;
	if (k*x > -0.5)
	{
		return k;
	}
	else
	{
		return 0.25/(k*x*x);
	}
}
ScalarType pennon_function(ScalarType x, ScalarType k)
{
	x = x + LARGER_EPSILON;
	if (k*x > 0.5)
	{
		return (2 * k*k*x*x - std::log(0.5) - 0.5) / k;
	}
	else
	{
		return -std::log(-k*x + 1) / k;
	}
}
ScalarType pennon_function_d_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x + LARGER_EPSILON;
	if (k*x > 0.5)
	{
		return 4*k*x;
	}
	else
	{
		return 1.0/(-k*x+1);
	}
}
ScalarType pennon_function_d2_psi(ScalarType x, ScalarType k)
{
	// shift to positive a little bit
	x = x + LARGER_EPSILON;
	if (k*x > 0.5)
	{
		return 4*k;
	}
	else
	{
		return k / std::pow(-k*x + 1, 2);
	}
}

#define SPLINE_SJ 0.5
ScalarType function_spline_barrier(ScalarType x) 
{
	if (x < 0)
	{
		return 0.5*INFINITY * x * x;
	}
	else if (x < SPLINE_SJ)
	{
		return 1/(pow((x/SPLINE_SJ-1), 3)+1) - 1;
	}
	else
	{
		return 0;
	}
}
ScalarType derivative_spline_barrier(ScalarType x)
{
	if (x < 0)
	{
		return INFINITY*x;
	}
	else if (x < SPLINE_SJ)
	{
		ScalarType xdsm1 = x/SPLINE_SJ - 1.0;
		return (-3/(pow((pow(xdsm1, 3) + 1), 2))*pow(xdsm1, 2))/SPLINE_SJ;
	}
	else
	{
		return 0;
	}
}
ScalarType derivative2_spline_barrier(ScalarType x)
{
	if (x < 0)
	{
		return INFINITY;
	}
	else if (x < SPLINE_SJ)
	{
		ScalarType xdsm1 = x/SPLINE_SJ - 1.0;
		return (18/pow(pow(xdsm1, 3)+1, 3)*pow(xdsm1, 4) - 6/pow(pow(xdsm1, 3)+1, 2) * xdsm1)/(SPLINE_SJ*SPLINE_SJ);
	}
	else
	{
		return 0;
	}
}

ScalarType function_modified_log_barrier(ScalarType x) {return std::log(x+1);}
ScalarType derivative_modified_log_barrier(ScalarType x) {return 1.0/(x+1);}
ScalarType derivative2_modified_log_barrier(ScalarType x) {return -1.0/std::pow((x+1), 2);}

ScalarType function_NR_trasformation_function(ScalarType x)
{
	if (x > -0.5)
	{
		return std::log(x+1);
	}
	else
	{
		return -2*x*x+std::log(0.5)+0.5;
	}
}
ScalarType derivative_NR_transformation_function(ScalarType x)
{
	if (x > -0.5)
	{
		return 1.0/(x+1);
	}
	else
	{
		return -4*x;
	}
}
ScalarType derivative2_NR_transformation_function(ScalarType x)
{
	if (x > -0.5)
	{
		return -1.0/std::pow(x+1, 2);
	}
	else
	{
		return -4;
	}
}

ScalarType function_LSL(ScalarType x)
{
	return 2*(std::log(2.0)+x-std::log(1+std::exp(x)));
}

ScalarType derivative_LSL(ScalarType x)
{
	ScalarType et = std::exp(x);
	return (2-2*et/(1+et));
}

ScalarType derivative2_LSL(ScalarType x)
{
	ScalarType et = std::exp(x);
	ScalarType et_divide_etplus1 = et/(et+1);
	return 2*(et_divide_etplus1*et_divide_etplus1 - et_divide_etplus1);
}

// protected functions and utilities

bool NISSolverBase::updateGradientDescent(VectorX& x)
{
	// evaluate gradient
	VectorX gradient;
	evaluateGradient(x, gradient);
	if (gradient.norm() < EPSILON)
		return true;

	// solve for descent direction
	VectorX descent_dir = -gradient;

	// linesearch
	ScalarType step_size = lineSearch(x, gradient, descent_dir);

	// update x
	x = x + descent_dir * step_size;

	return false;
}
bool NISSolverBase::updateLocalGlobal(VectorX& x)
{
	bool convergence = false;
	// prefactorization, will be done only once.
	localGlobalPrefactorization(x);

	m_iteration_num --;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num ++;
		ScalarType old_multipiler = m_multiplier;
		unsigned int multiplier_ind = localGlobalChooseMultiplier(m_multiplier);
		m_multiplier = m_local_global_multipliers[multiplier_ind];

		// gradient descent
		localGlobalLocalStep(x);

		VectorX gradient;
		evaluateGradientLocalGlobal(x, gradient);
		VectorX descent_dir;

		// solve for descent direction
		descent_dir = -m_local_global_llt_solvers[multiplier_ind].solve(gradient);

		//std::cout << "Progress  = " << -descent_dir.dot(gradient) << std::endl;

		// update x
		x = x + descent_dir;
		m_multiplier = old_multipiler;

		if (-descent_dir.dot(gradient) < gradient.size()*EVEN_LARGER_EPSILON && m_auto_multiplier)
		{
			m_multiplier *= m_multiplier_mu;
			if (m_multiplier > 1e15)
			{
				m_multiplier = 1e15;
				convergence = true;
				break;
			}
		}
	}

	//std::cout << "Local Global Energy = " << evaluateEnergyLocalGlobal(x) << std::endl;
	//std::cout << "Newton Energy = " << evaluateEnergyPenaltyMethod(x, penalty_function_quadratic_on_negative_psi) << std::endl;

	//localGlobalLocalStep(x);
	//VectorX gradient;
	//evaluateGradientLocalGlobal(x, gradient);

	//EigenSparseMatrix H;
	//evaluateHessianLocalGlobal(x, H);

	//Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> H_llt;
	//factorizeDirectSolverLLT(H, H_llt, "Local Global");

	//VectorX descent_dir = -H_llt.solve(gradient);

	//x = x + descent_dir;

	return convergence;
}
bool NISSolverBase::updatePenaltyMethod(VectorX& x)
{
	bool convergence = false;
	VectorX gradient;
	EigenSparseMatrix H;
	VectorX descent_dir;
	ScalarType step_size;

	ScalarType (*psi)(ScalarType, ScalarType);
	ScalarType (*d_psi)(ScalarType, ScalarType);
	ScalarType (*d2_psi)(ScalarType, ScalarType);

	switch(m_penalty_type)
	{
	case PENALTY_TYPE_QUADRATIC:
		psi = penalty_function_quadratic_on_negative_psi;
		d_psi = penalty_function_quadratic_on_negative_d_psi;
		d2_psi = penalty_function_quadratic_on_negative_d2_psi;
		break;
	case PENALTY_TYPE_CUBIC:
		psi = penalty_function_cubic_on_negative_psi;
		d_psi = penalty_function_cubic_on_negative_d_psi;
		d2_psi = penalty_function_cubic_on_negative_d2_psi;
		break;
	case PENALTY_TYPE_SPLINE:
		psi = penalty_function_spline_psi;
		d_psi = penalty_function_spline_d_psi;
		d2_psi = penalty_function_spline_d2_psi;
		break;
	}

	m_iteration_num --;
	bool all_element_zero_penalty = false;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num ++;

		//std::cout << "Iteration" << m_iteration_num << std::endl;
		//std::cout << "Multiplier" << m_multiplier << std::endl;

#ifndef OPEN_MP_SUPPORT
		// squential implementation
		evaluateGradientPenaltyMethod(x, gradient, d_psi, m_multiplier);
		evaluateHessianPenaltyMethod(x, H, d_psi, d2_psi, m_multiplier);
#ifdef PARDISO_SUPPORT
		if (m_penalty_hessian_symbolic_factorization_flag)
		{
			factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
		}
		else
		{
			factorizePardisoLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
			m_penalty_hessian_symbolic_factorization_flag = true;
		}
#else
		//// symbolic factorization
		//if (m_penalty_hessian_symbolic_factorization_flag)
		//{
		//	factorizeDirectSolverLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
		//}
		//else
		//{
		//	factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
		//	m_penalty_hessian_symbolic_factorization_flag = true;
		//}
		// no symbolic factorization
		factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
#endif
		descent_dir = -m_penalty_hessian_llt_solver.solve(gradient);
		step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, psi, m_multiplier);
#else
		// parallel implementation
		evaluateEnergyGradientHessianPenaltyMethod(x, psi, d_psi, d2_psi, m_multiplier);
		retrieveGradientPenaltyMethod(gradient, x.size());
		retrieveHessianPenaltyMethod(H, x.size());
#ifdef PARDISO_SUPPORT
		if (m_penalty_hessian_symbolic_factorization_flag)
		{
			factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
		}
		else
		{
			factorizePardisoLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
			m_penalty_hessian_symbolic_factorization_flag = true;
		}
#else
		//// symbolic factorization
		//if (m_penalty_hessian_symbolic_factorization_flag)
		//{
		//	factorizeDirectSolverLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
		//}
		//else
		//{
		//	factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
		//	m_penalty_hessian_symbolic_factorization_flag = true;
		//}
		// no symbolic factorization
		factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
#endif
		descent_dir = -m_penalty_hessian_llt_solver.solve(gradient);
		step_size = lineSearchPenaltyMethod(retrieveEnergyPenaltyMethod(), x, gradient, descent_dir, psi, m_multiplier);
#endif

		x = x+descent_dir*step_size;

		//std::cout << "step size = " << step_size << std::endl;
		//std::cout << "newton decrement = " << -descent_dir.dot(gradient) << std::endl;
		//std::cout << "Newton Energy = " << evaluateEnergyPenaltyMethod(x, penalty_function_quadratic_on_negative_psi) << std::endl;
		ScalarType newton_decrement_square = -descent_dir.dot(gradient);
		if (newton_decrement_square < gradient.size()*EVEN_LARGER_EPSILON/*std::pow((gradient.size()*EVEN_LARGER_EPSILON), 2)*/)
		{
			if (m_enable_non_inversion && m_auto_multiplier)
			{
				m_multiplier *= m_multiplier_mu;
				all_element_zero_penalty = allElementsHasZeroPenaltyEnergy(x, psi);
				if (all_element_zero_penalty)
				{
					convergence = true;
					break;
				}
				//if (m_multiplier > 1e15)
				//{
				//	m_multiplier = 1e15;
				//	convergence = true;
				//	break;
				//}
				if (m_multiplier > 1e10)
				{
					m_multiplier = 1e10;
					convergence = true;
					break;
				}
			}
			else
			{
				convergence = true;
				break;
			}
		}
	}

	return convergence;
}
bool NISSolverBase::updateSpectralSDP(VectorX& x)
{
	bool convergence = false;
	VectorX gradient;
	EigenSparseMatrix H;
	VectorX descent_dir;
	ScalarType step_size;

	ScalarType (*psi)(ScalarType, ScalarType);
	ScalarType (*d_psi)(ScalarType, ScalarType);
	ScalarType (*d2_psi)(ScalarType, ScalarType);

	//switch(m_penalty_type)
	//{
	//case PENALTY_TYPE_QUADRATIC:
	//	psi = penalty_function_quadratic_on_negative_psi;
	//	d_psi = penalty_function_quadratic_on_negative_d_psi;
	//	d2_psi = penalty_function_quadratic_on_negative_d2_psi;
	//	break;
	//case PENALTY_TYPE_CUBIC:
	//	psi = penalty_function_cubic_on_negative_psi;
	//	d_psi = penalty_function_cubic_on_negative_d_psi;
	//	d2_psi = penalty_function_cubic_on_negative_d2_psi;
	//	break;
	//case PENALTY_TYPE_SPLINE:
	//	psi = penalty_function_spline_psi;
	//	d_psi = penalty_function_spline_d_psi;
	//	d2_psi = penalty_function_spline_d2_psi;
	//	break;
	//}

	psi = sp_function_spline_psi;
	d_psi = sp_function_spline_d_psi;
	d2_psi = sp_function_spline_d2_psi;

	m_iteration_num --;
	bool all_element_zero_penalty = false;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num ++;

		//std::cout << "Iteration" << m_iteration_num << std::endl;
		//std::cout << "Multiplier" << m_multiplier << std::endl;

		// squential implementation
		evaluateGradientSpectralSDP(x, gradient, d_psi, m_multiplier);
		evaluateHessianSpectralSDP(x, H, d_psi, d2_psi, m_multiplier);
#ifdef PARDISO_SUPPORT
		if (m_penalty_hessian_symbolic_factorization_flag)
		{
			//factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Spectral SDP");
			factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver); // no warning mode
		}
		else
		{
			//factorizePardisoLLT(H, m_penalty_hessian_llt_solver, "Spectral SDP");
			factorizePardisoLLT(H, m_penalty_hessian_llt_solver); // no warning mode
			m_penalty_hessian_symbolic_factorization_flag = true;
		}
#else
		factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Spectral SDP");
#endif
		descent_dir = -m_penalty_hessian_llt_solver.solve(gradient);
		step_size = lineSearchSpectralSDP(x, gradient, descent_dir, psi, m_multiplier);

		x = x+descent_dir*step_size;

		if (true == m_spectral_sdp_is_first_iteration)
		{
			m_spectral_sdp_last_x = x;
			m_spectral_sdp_is_first_iteration = false;
		}

		ScalarType newton_decrement_square = -descent_dir.dot(gradient);
		//std::cout << "Newton decrement = " << newton_decrement_square << std::endl;
		if (newton_decrement_square < gradient.size()*EVEN_LARGER_EPSILON/*std::pow((gradient.size()*EVEN_LARGER_EPSILON), 2)*/)
		{
			if (m_enable_non_inversion && m_auto_multiplier)
			{
				// update penalty and multiplier
				updateSpectralSDPLagrangian(m_spectral_sdp_last_x, x, d_psi, m_multiplier);
				m_multiplier *= m_multiplier_mu;
				// update last state
				m_spectral_sdp_last_x = x;

				if (m_multiplier > 1e10)
				{
					m_multiplier = 1e10;
					convergence = true;
					break;
				}
			}
			else
			{
				convergence = true;
				break;
			}
		}
	}

	return convergence;
}
bool NISSolverBase::updatePennon(VectorX& x)
{
	bool convergence = false;
	VectorX gradient;
	EigenSparseMatrix H;
	VectorX descent_dir;
	ScalarType step_size;

	ScalarType(*psi)(ScalarType, ScalarType);
	ScalarType(*d_psi)(ScalarType, ScalarType);
	ScalarType(*d2_psi)(ScalarType, ScalarType);

	psi = pennon_function;
	d_psi = pennon_function_d_psi;
	d2_psi = pennon_function_d2_psi;

	m_iteration_num--;
	bool all_element_zero_penalty = false;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num++;

		// squential implementation
		evaluateGradientPennon(x, gradient, d_psi, m_multiplier);
		evaluateHessianPennon(x, H, d_psi, d2_psi, m_multiplier);
#ifdef PARDISO_SUPPORT
		if (m_penalty_hessian_symbolic_factorization_flag)
		{
			factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Pennon");
		}
		else
		{
			factorizePardisoLLT(H, m_penalty_hessian_llt_solver, "Pennon");
			m_penalty_hessian_symbolic_factorization_flag = true;
		}
#else
		factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Spectral SDP");
#endif
		descent_dir = -m_penalty_hessian_llt_solver.solve(gradient);
		step_size = lineSearchPennon(x, gradient, descent_dir, psi, m_multiplier);

		x = x + descent_dir*step_size;

		if (true == m_spectral_sdp_is_first_iteration)
		{
			m_spectral_sdp_last_x = x;
			m_spectral_sdp_is_first_iteration = false;
		}

		ScalarType newton_decrement_square = -descent_dir.dot(gradient);
		//std::cout << "Newton decrement = " << newton_decrement_square << std::endl;
		if (newton_decrement_square < gradient.size()*EVEN_LARGER_EPSILON/*std::pow((gradient.size()*EVEN_LARGER_EPSILON), 2)*/)
		{
			if (m_enable_non_inversion && m_auto_multiplier)
			{
				std::cout << "Update outer iteration. Current iteration: " << m_iteration_num << std::endl;
				// update penalty and multiplier
				updatePennonLagrangian(m_spectral_sdp_last_x, x, d_psi, m_multiplier);
				m_multiplier *= m_multiplier_mu;
				// update last state
				m_spectral_sdp_last_x = x;

				//all_element_zero_penalty = allElementsHasZeroPenaltyEnergy(x, penalty_function_quadratic_on_negative_psi);
				//if (all_element_zero_penalty)
				//{
				//	convergence = true;
				//	break;
				//}
				if (m_multiplier > 1e10)
				{
					m_multiplier = 1e10;
					convergence = true;
					break;
				}
			}
			else
			{
				convergence = true;
				break;
			}
		}
	}

	return convergence;
}

bool NISSolverBase::updatePenaltyMethodLocalGlobalBlend(VectorX& x)
{
	// prefactorization, will be done only once.
	bool convergence = false;
	localGlobalPrefactorization(x);

	VectorX gradient;
	VectorX descent_dir;
	ScalarType step_size;

	ScalarType multiplier_threshold = 1e5;
	if (m_multiplier < multiplier_threshold)
	{
		localGlobalPrefactorization(x);
		m_iteration_num --;
		for (unsigned int i = 0; i != m_inner_loop_iterations && m_multiplier < multiplier_threshold; i++)
		{
			m_iteration_num++;
			ScalarType old_multipiler = m_multiplier;
			unsigned int multiplier_ind = localGlobalChooseMultiplier(m_multiplier);
			m_multiplier = m_local_global_multipliers[multiplier_ind];

			// gradient descent
			localGlobalLocalStep(x);

			evaluateGradientLocalGlobal(x, gradient);

			// solve for descent direction
			descent_dir = -m_local_global_llt_solvers[multiplier_ind].solve(gradient);

			// update x
			x = x + descent_dir;

			m_multiplier = old_multipiler;

			if (-descent_dir.dot(gradient) < gradient.size()*EVEN_LARGER_EPSILON && m_auto_multiplier)
			{
				m_multiplier *= m_multiplier_mu;
			}
		}
	}
	else
	{
		EigenSparseMatrix H;
		//Eigen::SimplicialLDLT<EigenSparseMatrix, Eigen::Upper> H_ldlt;
		Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> H_llt;

		//for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
		//{
		switch (m_penalty_type)
		{
		case PENALTY_TYPE_QUADRATIC:
			// quadratic penalty
			evaluateGradientPenaltyMethod(x, gradient, penalty_function_quadratic_on_negative_d_psi, m_multiplier);
			evaluateHessianPenaltyMethod(x, H, penalty_function_quadratic_on_negative_d_psi, penalty_function_quadratic_on_negative_d2_psi, m_multiplier);
			//factorizeDirectSolverLDLT(H, H_ldlt, "Penalty Method");
			factorizeDirectSolverLLT(H, H_llt, "Penalty Method");
			descent_dir = -H_llt.solve(gradient);
			step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, penalty_function_quadratic_on_negative_psi, m_multiplier);
			break;
		case PENALTY_TYPE_CUBIC:
			// cubic penalty
			evaluateGradientPenaltyMethod(x, gradient, penalty_function_cubic_on_negative_d_psi, m_multiplier);
			evaluateHessianPenaltyMethod(x, H, penalty_function_cubic_on_negative_d_psi, penalty_function_cubic_on_negative_d2_psi, m_multiplier);
			//factorizeDirectSolverLDLT(H, H_ldlt, "Penalty Method");
			factorizeDirectSolverLLT(H, H_llt, "Penalty Method");
			descent_dir = -H_llt.solve(gradient);
			step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, penalty_function_cubic_on_negative_psi, m_multiplier);
			break;
		case PENALTY_TYPE_SPLINE:
			// spline penalty
			evaluateGradientPenaltyMethod(x, gradient, penalty_function_spline_d_psi, m_multiplier);
			evaluateHessianPenaltyMethod(x, H, penalty_function_spline_d_psi, penalty_function_spline_d2_psi, m_multiplier);
			//factorizeDirectSolverLDLT(H, H_ldlt, "Penalty Method");
			factorizeDirectSolverLLT(H, H_llt, "Penalty Method");
			descent_dir = -H_llt.solve(gradient);
			step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, penalty_function_spline_psi, m_multiplier);
			break;
		}

		x = x+descent_dir*step_size;

		if (-descent_dir.dot(gradient) < gradient.size()*EVEN_LARGER_EPSILON && m_auto_multiplier)
		{
			m_multiplier *= m_multiplier_mu;
			if (m_multiplier > 1e15)
			{
				m_multiplier = 1e15;
				convergence = true;
			}
		}
	}
	//for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	//{
	//	ScalarType old_multipiler = m_multiplier;
	//	unsigned int multiplier_ind = LocalGlobalChooseMultiplier(m_multiplier);
	//	m_multiplier = m_local_global_multipliers[multiplier_ind];

	//	// gradient descent
	//	localGlobalLocalStep(x);

	//	VectorX gradient;
	//	EigenSparseMatrix H;
	//	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> H_llt;
	//	VectorX descent_dir;
	//	ScalarType step_size;

	//	if (m_penalty_method_using_local_global)
	//	{
	//		evaluateGradientLocalGlobal(x, gradient);

	//		descent_dir = -m_local_global_llt_solvers[multiplier_ind].solve(gradient);
	//		step_size = 1;

	//		local_global_iterations++;
	//	}
	//	else
	//	{
	//		evaluateGradientPenaltyMethod(x, gradient, penalty_function_quadratic_on_negative_d_psi, m_multiplier);
	//		evaluateHessianPenaltyMethod(x, H, penalty_function_quadratic_on_negative_d_psi, penalty_function_quadratic_on_negative_d2_psi, m_multiplier);
	//		//factorizeDirectSolverLDLT(H, H_ldlt, "Penalty Method");
	//		factorizeDirectSolverLLT(H, H_llt, "Penalty Method");

	//		descent_dir = -H_llt.solve(gradient);
	//		step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, penalty_function_quadratic_on_negative_psi, m_multiplier);

	//		newton_iterations ++;
	//	}
	//	x = x + descent_dir*step_size;

	//	m_multiplier = old_multipiler;

	//	decrement = -descent_dir.dot(gradient);

	//	//std::cout << "old decrement = " << m_local_global_last_decrement << std::endl;
	//	//std::cout << "decrement = " << decrement << std::endl;

	//	// blend bewteen newton and local global
	//	if (decrement/m_local_global_last_decrement > 0.1 || m_multiplier > 1e6)
	//	{
	//		m_penalty_method_using_local_global = false;
	//	}
	//	else
	//	{
	//		m_penalty_method_using_local_global = true;
	//	}
	//	m_local_global_last_decrement = decrement;

	//	if (decrement < gradient.size()*1e-3 && m_auto_multiplier)
	//	{
	//		m_multiplier *= m_multiplier_mu;
	//		if (m_multiplier > 1e15)
	//		{
	//			m_multiplier = 1e15;
	//			convergence = true;
	//			break;
	//		}
	//	}
	//}

	//// log
	//std::cout << "#local_global_iterations: " << local_global_iterations << std::endl;
	//std::cout << "#newton_iterations: " << newton_iterations << std::endl;

	return convergence;
}

bool NISSolverBase::updatePenaltyMethodOnDetF(VectorX& x)
{
	SetNoninversionCriteria();
	SetRotationFieldType();
	setBarrierFunction();

	bool convergence = false;
	VectorX gradient;
	EigenSparseMatrix H;
	VectorX descent_dir;
	ScalarType step_size;

	m_iteration_num --;
	bool all_element_zero_penalty = false;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		ScalarType old_multplier = m_multiplier;
		m_multiplier = 1.0/m_multiplier;

		m_iteration_num ++;

		if (m_energy_type == ENERGY_TYPE_COROT)
		{
			UpdateRotation(x);
		}

		// squential implementation
		evaluateHessian(x, H, m_d_psi, m_d2_psi);

		// evaluate gradient
		evaluateGradient(x, gradient, m_d_psi);

		//// debug
		//VectorX g2;
		//EigenSparseMatrix H2;
		//evaluateGradientNoConstraint(x, g2);
		//evaluateHessianNoConstraint(x, H2);
		//std::cout << "gradient_difference = " << (gradient-g2).norm() << std::endl;
		//std::cout << "hessian difference =" << (H-H2).norm() << std::endl;

		Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> lltSolver;
		factorizeDirectSolverLLT(H, lltSolver, "Newton Descent");

		descent_dir = -lltSolver.solve(gradient);
		step_size = lineSearch(x, gradient, descent_dir, false);

		x = x+descent_dir*step_size;

		m_multiplier = old_multplier;
		//std::cout << "step size = " << step_size << std::endl;
		//std::cout << "newton decrement = " << -descent_dir.dot(gradient) << std::endl;
		ScalarType newton_decrement_square = -descent_dir.dot(gradient);
		if (newton_decrement_square < gradient.size()*EVEN_LARGER_EPSILON/*std::pow((gradient.size()*EVEN_LARGER_EPSILON), 2)*/)
		{
			if (m_enable_non_inversion && m_auto_multiplier)
			{
				m_multiplier *= m_multiplier_mu;
				if (m_multiplier > 1e15)
				{
					m_multiplier = 1e15;
					convergence = true;
					break;
				}
			}
			else
			{
				convergence = true;
				break;
			}
		}
	}

	return convergence;
}

bool NISSolverBase::updateLIM(VectorX& x)
{
	SetNoninversionCriteria();
	SetRotationFieldType();
	setBarrierFunction();
	if (m_energy_type == ENERGY_TYPE_COROT)
	{
		UpdateRotation(x);
	}

	// choose target stiffness:
	ScalarType old_stiffness_attachement = m_stiffness_attachment;
	m_stiffness_attachment = limChooseAlpha(x);
	ScalarType old_multiplier = m_multiplier;
	m_multiplier = 1;

	// this step also determines the regularizer
	// evaluate hessian and factorize solver
	EigenSparseMatrix hessian;
	evaluateHessian(x, hessian, m_d_psi, m_d2_psi);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> lltSolver;
	factorizeDirectSolverLLT(hessian, lltSolver, "Newton Descent");

	// evaluate gradient
	VectorX gradient;
	evaluateGradient(x, gradient, m_d_psi);
	//evaluateGradientConstrainEveryOtherOne(x, gradient);

	// solve for descent direction
	VectorX descent_dir = -lltSolver.solve(gradient);
	if (m_enable_substepping && m_regularization_mu > 1e8)
	{
		descent_dir = - gradient;
	}

	// linesearch
	ScalarType step_size = lineSearch(x, gradient, descent_dir);
	//ScalarType step_size = lineSearchConstrainEveryOtherOne(x, gradient, descent_dir);

	// update x
	x = x + descent_dir * step_size;

	std::cout << "LIM ||gradient|| = " << gradient.norm() << std::endl;
	std::cout << "LIM attachment stiffness after alpha_update: " << m_stiffness_attachment << std::endl;

	m_lim_alpha_update_last_alpha = m_stiffness_attachment;
	m_stiffness_attachment = old_stiffness_attachement;
	m_multiplier = old_multiplier;

	//ScalarType b = evaluateEnergy(x, m_psi);
	//ScalarType a = evaluateEnergy(x);
	if (gradient.norm() < gradient.size() * EPSILON)
		return true;

	return false;
}

bool NISSolverBase::updateBarrierMethod(VectorX& x, VectorX& lambda)
{
	SetNoninversionCriteria();
	SetRotationFieldType();
	setBarrierFunction();

	bool convergence = false;
	VectorX gradient;
	EigenSparseMatrix H;
	//Eigen::SimplicialLDLT<EigenSparseMatrix, Eigen::Upper> H_ldlt;
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> H_llt;
	VectorX descent_dir;
	ScalarType step_size;

	m_iteration_num --;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		UpdateRotation(x);
		m_iteration_num ++;

		evaluateGradient(x, gradient, m_d_psi);
		evaluateHessian(x, H, m_d_psi, m_d2_psi);
		factorizeDirectSolverLLT(H, H_llt, "Newton Descent");
		descent_dir = -H_llt.solve(gradient);

		// linesearch
		ScalarType step_size = lineSearch(x, gradient, descent_dir);

		x = x+descent_dir*step_size;

		if (-descent_dir.dot(gradient) < gradient.size()*EVEN_LARGER_EPSILON && m_auto_multiplier)
		{
			m_multiplier *= m_multiplier_mu;
			if (m_multiplier > 1e15)
			{
				m_multiplier = 1e15;
				convergence = true;
				break;
			}
		}
	}

	return convergence;

	return false;
}

bool NISSolverBase::updatePrimalDual(VectorX& x, VectorX& lambda)
{
	SetNoninversionCriteria();
	SetRotationFieldType();
	setBarrierFunction();

	UpdateRotation(x);
	// update multiplier
	setRescalingParameter(x, lambda);

	VectorX x_lambda;
	EigenVectorConcat(x_lambda, x, lambda);

	VectorX residual;
	//evaluatePrimalDualResidual(x, lambda, residual);
	evaluateGradient(x, lambda, residual, m_d_psi);
	if (residual.norm() < EPSILON)
		return true;

	EigenSparseMatrix KKT;
	evaluateHessian(x, lambda, KKT, m_d_psi, m_d2_psi);
	//evaluatePrimalDualKKT(x, lambda, KKT);

	EigenSparseMatrix IL(KKT.rows(), KKT.cols());
	std::vector<SparseMatrixTriplet> il_triplet;
	for (unsigned int i = 0; i != x.size(); i++)
	{
		il_triplet.push_back(SparseMatrixTriplet(i,i,1));
	}
	unsigned ind = 0;
	TriangleElementConstraint* tri;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			tri = dynamic_cast<TriangleElementConstraint*>(*it);
			il_triplet.push_back(SparseMatrixTriplet(x.size()+ind, x.size()+ind, -lambda(ind)/tri->Area()));
			ind++;
		}
	}
	assert(ind == lambda.size());
	IL.setFromTriplets(il_triplet.begin(), il_triplet.end());

	KKT = KKT*IL;

	Eigen::SimplicialLDLT<EigenSparseMatrix, Eigen::Upper> ldltSolver;
	factorizeDirectSolverLDLT(KKT, ldltSolver, "Primal Dual KKT");
	VectorX primal_dual_descent_dir = -IL*ldltSolver.solve(residual);

	// linesearch
	ScalarType step_size = lineSearchPrimalDual(x_lambda, residual, primal_dual_descent_dir, x.size(), lambda.size());
	//ScalarType step_size = lineSearchConstrainEveryOtherOne(x, gradient, descent_dir);

	// update x
	x_lambda += primal_dual_descent_dir * step_size;
	//x = x + descent_dir * step_size;
	EigenVectorDissect(x, lambda, x_lambda, x.size(), lambda.size());

	ScalarType newton_decrement = -primal_dual_descent_dir.dot(residual);

	if (newton_decrement < residual.size()*EVEN_LARGER_EPSILON && m_multiplier > 1e15)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool NISSolverBase::updateCorotNewton(VectorX& x)
{
	VectorX gradient;
	evaluateGradientCorot(x, gradient);

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;
	std::cout << "Energy = " << evaluateEnergyCorot(x) << std::endl;
	//VectorX descent_dir = - gradient;

	EigenSparseMatrix H;
	evaluateHessianCorot(x, H);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> llt_solver;
	factorizeDirectSolverLLT(H, llt_solver, "Corot Local Global Newton's method");
	VectorX descent_dir = -llt_solver.solve(gradient);

	ScalarType step_size = lineSearchCorot(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;
}
bool NISSolverBase::updateCorotLocalGlobal(VectorX& x)
{
	VectorX gradient;
	evaluateGradientCorot(x, gradient);

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;
	std::cout << "Energy = " << evaluateEnergyCorot(x) << std::endl;

	EigenSparseMatrix H;
	evaluateHessianLocalGlobalStyle(x, H);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> llt_solver;
	factorizeDirectSolverLLT(H, llt_solver, "Corot Local Global Style Hessian");

	VectorX descent_dir = -llt_solver.solve(gradient);

	ScalarType step_size = lineSearchCorot(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;
}
bool NISSolverBase::updateCorotConstantHessian1(VectorX& x)
{
	VectorX gradient;
	evaluateGradientCorot(x, gradient);
	std::cout << "Energy = " << evaluateEnergyCorot(x) << std::endl;

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;

	EigenSparseMatrix H;
	evaluateHessianConst1(x, H);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> llt_solver;
	factorizeDirectSolverLLT(H, llt_solver, "Corot Constant Hessian 1");

	VectorX descent_dir = -llt_solver.solve(gradient);

	ScalarType step_size = lineSearchCorot(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;
}
bool NISSolverBase::updateCorotConstantHessian2(VectorX& x)
{
	VectorX gradient;
	evaluateGradientCorot(x, gradient);
	std::cout << "Energy = " << evaluateEnergyCorot(x) << std::endl;

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;

	EigenSparseMatrix H;
	evaluateHessianConst2(x, H);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> llt_solver;
	factorizeDirectSolverLLT(H, llt_solver, "Corot Constant Hessian 2");

	VectorX descent_dir = -llt_solver.solve(gradient);

	ScalarType step_size = lineSearchCorot(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;
}

bool NISSolverBase::updateAlternatingProjection(VectorX& x)
{
	bool result = false;

	m_iteration_num--;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num++;
		
		// local
		prefactorizeGTG();
		VectorX original_f = m_G*x;
		m_f = original_f;

		result = CorrectFeasibilityProjectionLocal(m_f);
		if (result == true)
		{
			return result;
		}
		// global
		CorrectFeasibilityProjectionGlobal(m_f, x);

		m_last_x = x;
	}

	return result;
}

bool NISSolverBase::updateOneTangentPlane(VectorX& x)
{
	bool result = false;

	m_iteration_num--;
	for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
	{
		m_iteration_num++;

		// find projection
		prefactorizeGTG();
		VectorX original_f = m_G*x;
		m_f = original_f;

		result = CorrectFeasibilityProjectionLocal(m_f);
		if (result == true)
		{
			return result;
		}
		unsigned int dim = (m_mode == MODE_2D) ? 2 : 3;
		unsigned int primal_size = m_element_num * dim * dim;
		unsigned int dual_size = m_f.size() - primal_size;

		VectorX pi_d = m_f.block(0, 0, primal_size, 1);
		VectorX p = m_T.transpose()*pi_d;
		//VectorX p = m_T.transpose()*m_T*x;
		VectorX b = m_f.block(primal_size, 0, dual_size, 1);
		VectorX n0 = (original_f - m_f).block(0, 0, primal_size, 1);

		VectorX pb(p.size() + dual_size);
		pb.block(0, 0, p.size(), 1) = p;
		pb.block(p.size(), 0, dual_size, 1) = b;

		// alternating projection
		//VectorX x_lambda = m_M_solver.solve(pb);
		//x = x_lambda.block(0, 0, p.size(), 1);

		VectorX ita0(m_M.rows());
		ita0.setZero();
		ita0.block(0, 0, m_T.cols(), 1) = m_T.transpose() * n0;
		VectorX c0 = pb;
		ScalarType d0 = n0.dot(pi_d);
		VectorX yc = m_M_solver.solve(c0);
		VectorX yn = m_M_solver.solve(ita0);
		VectorX x_lambda = yc - yn*(ita0.dot(yc) - d0) / (ita0.dot(yn));
		x = x_lambda.block(0, 0, p.size(), 1);

		m_f = m_G*x;

		m_last_x = x;
	}

	return result;
}

void NISSolverBase::prefactorizeGTG()
{
	if (m_need_prefactorization_flag)
	{
		//evaluateGMatrix(m_G);
		//EigenSparseMatrix GTG = m_G.transpose()*m_G;
		//factorizeDirectSolverLDLT(GTG, m_GTG_solver, "pre-factorization");

		TimerWrapper t;
		t.Tic();
		evaluateTMatrix(m_T);
		evaluateAMatrix(m_A);
		EigenSparseMatrix At = m_A.transpose();
		EigenSparseMatrix TtT = m_T.transpose() * m_T;
		EigenConstructKKT(m_M, TtT, At);
		t.TocAndReport("Produce Matrix");

		t.Tic();
		m_M_solver.analyzePattern(m_M);
		m_M_solver.factorize(m_M);
		if (m_M_solver.info() != Eigen::Success)
		{
			std::cout << "Prefactorization failed. M matrix." << std::endl;
		}
		t.TocAndReport("Pre-factorize Matrix");

		m_need_prefactorization_flag = false;
	}
}

bool NISSolverBase::updateNeoHookeanGradientDescent(VectorX& x)
{
	VectorX gradient;
	evaluateGradientNeoHookean(x, gradient);

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;
	std::cout << "Energy = " << evaluateEnergyNeoHookean(x) << std::endl;

	VectorX descent_dir = - gradient;

	ScalarType step_size = lineSearchNeoHookean(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;
}
bool NISSolverBase::updateNeoHookeanConstantHessian(VectorX& x)
{
	VectorX gradient;
	evaluateGradientNeoHookean(x, gradient);

	std::cout << "||Gradient||^2 = " << gradient.squaredNorm() << std::endl;
	std::cout << "Energy = " << evaluateEnergyNeoHookean(x) << std::endl;

	EigenSparseMatrix H;
	evaluateHessianConst1(x, H);
	Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper> llt_solver;
	factorizeDirectSolverLLT(H, llt_solver, "Corot Constant Hessian 1");

	VectorX descent_dir = -llt_solver.solve(gradient);

	ScalarType step_size = lineSearchNeoHookean(x, gradient, descent_dir);

	x += step_size*descent_dir;

	return false;

}

bool NISSolverBase::updateNoninversionConstantHessian(VectorX& x)
{
		bool convergence = false;
		VectorX gradient;
		EigenSparseMatrix H;
		VectorX descent_dir;
		ScalarType step_size;

		ScalarType (*psi)(ScalarType, ScalarType);
		ScalarType (*d_psi)(ScalarType, ScalarType);
		ScalarType (*d2_psi)(ScalarType, ScalarType);

		switch(m_penalty_type)
		{
		case PENALTY_TYPE_QUADRATIC:
			psi = penalty_function_quadratic_on_negative_psi;
			d_psi = penalty_function_quadratic_on_negative_d_psi;
			d2_psi = penalty_function_quadratic_on_negative_d2_psi;
			break;
		case PENALTY_TYPE_CUBIC:
			psi = penalty_function_cubic_on_negative_psi;
			d_psi = penalty_function_cubic_on_negative_d_psi;
			d2_psi = penalty_function_cubic_on_negative_d2_psi;
			break;
		case PENALTY_TYPE_SPLINE:
			psi = penalty_function_spline_psi;
			d_psi = penalty_function_spline_d_psi;
			d2_psi = penalty_function_spline_d2_psi;
			break;
		}

		m_iteration_num --;
		bool all_element_zero_penalty = false;
		for (unsigned int i = 0; i != m_inner_loop_iterations; i++)
		{
			m_iteration_num ++;

			//std::cout << "Iteration" << m_iteration_num << std::endl;
			//std::cout << "Multiplier" << m_multiplier << std::endl;

			// squential implementation
			evaluateGradientPenaltyMethod(x, gradient, d_psi, m_multiplier);
			evaluateHessianConst1(x, H);
			//evaluateHessianPenaltyMethod(x, H, d_psi, d2_psi, m_multiplier);
#ifdef PARDISO_SUPPORT
			if (m_penalty_hessian_symbolic_factorization_flag)
			{
				factorizePardisoLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
			}
			else
			{
				factorizePardisoLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
				m_penalty_hessian_symbolic_factorization_flag = true;
			}
#else
			//// symbolic factorization
			//if (m_penalty_hessian_symbolic_factorization_flag)
			//{
			//	factorizeDirectSolverLLTWithoutSymbolicFactorization(H, m_penalty_hessian_llt_solver, "Penalty Method");
			//}
			//else
			//{
			//	factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
			//	m_penalty_hessian_symbolic_factorization_flag = true;
			//}
			// no symbolic factorization
			factorizeDirectSolverLLT(H, m_penalty_hessian_llt_solver, "Penalty Method");
#endif
			descent_dir = -m_penalty_hessian_llt_solver.solve(gradient);
			step_size = lineSearchPenaltyMethod(x, gradient, descent_dir, psi, m_multiplier);

			x = x+descent_dir*step_size;

			//std::cout << "step size = " << step_size << std::endl;
			//std::cout << "newton decrement = " << -descent_dir.dot(gradient) << std::endl;
			//std::cout << "Newton Energy = " << evaluateEnergyPenaltyMethod(x, penalty_function_quadratic_on_negative_psi) << std::endl;
			ScalarType newton_decrement_square = -descent_dir.dot(gradient);
			if (newton_decrement_square < gradient.size()*EVEN_LARGER_EPSILON/*std::pow((gradient.size()*EVEN_LARGER_EPSILON), 2)*/)
			{
				if (m_enable_non_inversion && m_auto_multiplier)
				{
					m_multiplier *= m_multiplier_mu;
					all_element_zero_penalty = allElementsHasZeroPenaltyEnergy(x, psi);
					if (all_element_zero_penalty)
					{
						convergence = true;
						break;
					}
					if (m_multiplier > 1e15)
					{
						m_multiplier = 1e15;
						convergence = true;
						break;
					}
				}
				else
				{
					convergence = true;
					break;
				}
			}
		}

		return convergence;
	
}
ScalarType NISSolverBase::evaluateEnergy(const VectorX& x)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if (m_enable_substepping)
		{
			energy += (*it)->EvaluateEnergy(x, m_multiplier, m_regularization_mu);
		}
		else
		{
			energy += (*it)->EvaluateEnergy(x, m_multiplier);
		}
	}
	return energy;
}
void NISSolverBase::evaluateGradient(const VectorX& x, VectorX& gradient)
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if (m_enable_substepping)
		{
			(*it)->EvaluateGradient(x, gradient, m_multiplier, m_regularization_mu);
		}
		else
		{
			(*it)->EvaluateGradient(x, gradient, m_multiplier);
		}
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif
}
void NISSolverBase::evaluateHessian(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if (m_enable_substepping)
		{
			(*it)->EvaluateHessian(x, h_triplets, m_multiplier, m_regularization_mu);
		}
		else
		{
			(*it)->EvaluateHessian(x, h_triplets, m_multiplier);
		}
	}

	//ScalarType tik = 1e-15;
	//for (unsigned int i = 0; i != x.size(); ++i)
	//{
	//	h_triplets.push_back(SparseMatrixTriplet(i, i, tik));
	//}

	// regularization
	if (m_enable_substepping == true && m_regularization_mu > 0)
	{
		for (unsigned int i = 0; i != x.size(); ++i)
		{
			h_triplets.push_back(SparseMatrixTriplet(i, i, m_regularization_mu));
		}
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif
}
ScalarType NISSolverBase::lineSearch(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir, bool throwable)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergy(x, m_psi, throwable);

		bool force_loop;
		do
		{
			force_loop = false;
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			try
			{
				lhs = evaluateEnergy(x_plus_tdx, m_psi, throwable);
			}
			catch(MyExceptions e)
			{
				assert(e == EXCEPTION_LOG_OF_NEGATIVE);
				force_loop = true;
				continue;
			}

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while ((force_loop || lhs >= rhs) && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}
		else
		{
			m_ls_stepsize = t;
		}

		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}

void NISSolverBase::localGlobalPrefactorization(const VectorX& x)
{
	if (m_local_global_factorization_flag == false)
	{
		ScalarType old_multiplier = m_multiplier;
		for (unsigned int i = 0; i != 16; i++)
		{
			m_local_global_multipliers[i] = std::pow(10.0, ScalarType(i));
			m_multiplier = m_local_global_multipliers[i];
			EigenSparseMatrix H;
			evaluateHessianLocalGlobal(x, H);
			factorizeDirectSolverLLT(H, m_local_global_llt_solvers[i], "Pre-factorization.");
		}

		m_multiplier = old_multiplier;
		m_local_global_factorization_flag = true;
	}
}
unsigned int NISSolverBase::localGlobalChooseMultiplier(ScalarType multiplier)
{
	unsigned int index = 0;
	ScalarType diff_min = std::abs(multiplier - m_local_global_multipliers[0]);
	for (unsigned int i = 1; i != 16; i++)
	{
		ScalarType diff = std::abs(multiplier - m_local_global_multipliers[i]);
		if (diff < diff_min)
		{
			index = i;
			diff_min = diff;
		}
	}
	return index;
}

void NISSolverBase::localGlobalLocalStep(const VectorX& x)
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->LocalGlobalLocalStep(x);
	}
}
ScalarType NISSolverBase::evaluateEnergyLocalGlobal(const VectorX& x)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyLocalGlobal(x, m_multiplier);
	}
	return energy;
}
void NISSolverBase::evaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient)
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientLocalGlobal(x, gradient, m_multiplier);
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif
}
void NISSolverBase::evaluateHessianLocalGlobal(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianLocalGlobal(x, h_triplets, m_multiplier);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif

	//	// TODO: change it back
	//	std::vector<SparseMatrixTriplet> h_triplets2;
	//	h_triplets2.clear();
	//
	//	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	//	{
	//		(*it)->EvaluateHessianNoConstraint(x, h_triplets2);
	//	}
	//
	//#ifdef ENABLE_MATLAB_DEBUGGING
	//	g_debugger->SendSparseMatrixUsingTriplets(h_triplets2, "H2", hessian.rows(), hessian.cols());
	//#endif

}

ScalarType NISSolverBase::evaluateEnergyPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyPenaltyMethod(x, psi, multiplier);
	}
	
	// add hard coded collisions
	if (m_collision_enable)
	{
		unsigned dim = (m_mode==MODE_2D)?2:3;
		for (unsigned int i = 0; i != m_surface_indices.size(); i++)
		{
			unsigned int index = m_surface_indices[i];
			ScalarType yi = x(dim*index+1);
			ScalarType diff = yi - m_collision_y_plane;
			if (diff < 0)
			{
				energy += 0.5 * m_collision_stiffness * diff * diff;
			}
		}
	}

	return energy;
}
void NISSolverBase::evaluateGradientPenaltyMethod(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientPenaltyMethod(x, gradient, d_psi, multiplier);
	}

	// add hard coded collisions
	if (m_collision_enable)
	{
		unsigned dim = (m_mode==MODE_2D)?2:3;
		for (unsigned int i = 0; i != m_surface_indices.size(); i++)
		{
			unsigned int index = m_surface_indices[i];
			ScalarType yi = x(dim*index+1);
			ScalarType diff = yi - m_collision_y_plane;
			if (diff < 0)
			{
				gradient(dim*index+1) += m_collision_stiffness * diff;
			}
		}
	}

//#ifdef ENABLE_MATLAB_DEBUGGING
//	g_debugger->SendVector(gradient, "gradient", gradient.size());
//#endif
}
void NISSolverBase::evaluateHessianPenaltyMethod(const VectorX& x, EigenSparseMatrix& hessian, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianPenaltyMethod(x, h_triplets, d_psi, d2_psi, multiplier);
	}

	// add hard coded collisions
	if (m_collision_enable)
	{
		unsigned dim = (m_mode==MODE_2D)?2:3;
		for (unsigned int i = 0; i != m_surface_indices.size(); i++)
		{
			unsigned int index = m_surface_indices[i];
			ScalarType yi = x(dim*index+1);
			ScalarType diff = yi - m_collision_y_plane;
			if (diff < 0)
			{
				h_triplets.push_back(SparseMatrixTriplet(dim*index+1, dim*index+1, m_collision_stiffness));
			}
		}
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());
}
ScalarType NISSolverBase::lineSearchPenaltyMethod(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergyPenaltyMethod(x, psi, multiplier);

		do
		{
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			lhs = evaluateEnergyPenaltyMethod(x_plus_tdx, psi, multiplier);

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while (lhs >= rhs && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}

		m_ls_stepsize = t;
		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}
bool NISSolverBase::allElementsHasZeroPenaltyEnergy(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType))
{
	bool all_element_has_zero_penalty_energy = true;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		all_element_has_zero_penalty_energy &= (*it)->HasZeroPenaltyEnergy(x, psi);
	}
	return all_element_has_zero_penalty_energy;
}

// spectral SDP
ScalarType NISSolverBase::evaluateEnergySpectralSDP(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergySpectralSDP(x, psi, multiplier);
	}

	return energy;
}
void NISSolverBase::evaluateGradientSpectralSDP(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	gradient.resize(x.size());
	gradient.setZero();

	//VectorX g2 = gradient;

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		//(*it)->EvaluateGradientSpectralSDPFD(x, g2, sp_function_spline_psi, multiplier);
		(*it)->EvaluateGradientSpectralSDP(x, gradient, d_psi, multiplier);
	}

	//#ifdef ENABLE_MATLAB_DEBUGGING
	//	g_debugger->SendVector(gradient, "gradient", gradient.size());
	//#endif
}
void NISSolverBase::evaluateHessianSpectralSDP(const VectorX& x, EigenSparseMatrix& hessian, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianSpectralSDP(x, h_triplets, d_psi, d2_psi, multiplier);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());
}
ScalarType NISSolverBase::lineSearchSpectralSDP(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergySpectralSDP(x, psi, multiplier);

		do
		{
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			lhs = evaluateEnergySpectralSDP(x_plus_tdx, psi, multiplier);

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while (lhs >= rhs && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}

		m_ls_stepsize = t;
		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}
void NISSolverBase::initSpectralSDPLagrangian()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->InitLagrangian();
	}
}
void NISSolverBase::updateSpectralSDPLagrangian(const VectorX& x_old, const VectorX& x_new, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier)
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->UpdateLagrangian(x_old, x_new, d_psi, multiplier);
	}
}

// pennon
ScalarType NISSolverBase::evaluateEnergyPennon(const VectorX& x, ScalarType(*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyPennon(x, psi, multiplier);
	}

	return energy;
}
void NISSolverBase::evaluateGradientPennon(const VectorX& x, VectorX& gradient, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	gradient.resize(x.size());
	gradient.setZero();

	//VectorX g2 = gradient;

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		//(*it)->EvaluateGradientSpectralSDPFD(x, g2, sp_function_spline_psi, multiplier);
		(*it)->EvaluateGradientPennon(x, gradient, d_psi, multiplier);
	}

	//#ifdef ENABLE_MATLAB_DEBUGGING
	//	g_debugger->SendVector(gradient, "gradient", gradient.size());
	//#endif
}
void NISSolverBase::evaluateHessianPennon(const VectorX& x, EigenSparseMatrix& hessian, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType(*d2_psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianPennon(x, h_triplets, d_psi, d2_psi, multiplier);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());
}
ScalarType NISSolverBase::lineSearchPennon(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir, ScalarType(*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0 / m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergyPennon(x, psi, multiplier);

		do
		{
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			lhs = evaluateEnergyPennon(x_plus_tdx, psi, multiplier);

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while (lhs >= rhs && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}

		m_ls_stepsize = t;
		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}
void NISSolverBase::initPennonLagrangian()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->InitLagrangian();
	}
}
void NISSolverBase::updatePennonLagrangian(const VectorX& x_old, const VectorX& x_new, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier)
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->UpdateLagrangianPennon(x_old, x_new, d_psi, multiplier);
	}
}


void NISSolverBase::evaluateEnergyGradientHessianPenaltyMethod(\
	const VectorX& x, \
	ScalarType (*psi)(ScalarType, ScalarType), \
	ScalarType (*d_psi)(ScalarType, ScalarType), \
	ScalarType (*d2_psi)(ScalarType, ScalarType), \
	ScalarType multiplier/* = 1.0*/)
{
	int i;
	#pragma omp parallel
	{
		#pragma omp for
		for (i = 0; i < m_constraints.size(); i++)
		{
			m_constraints[i]->EvaluateEnergyGradientHessianPenaltyMethod(x, psi, d_psi, d2_psi, multiplier);
		}
		//for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
		//{
		//	(*it)->EvaluateEnergyGradientHessianPenaltyMethod(x, psi, d_psi, d2_psi, multiplier);
		//}
	}
}
ScalarType NISSolverBase::retrieveEnergyPenaltyMethod()
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->RetrieveEnergy();
	}
	return energy;
}
void NISSolverBase::retrieveGradientPenaltyMethod(VectorX& gradient, unsigned size)
{
	gradient.resize(size);
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->RetrieveGradient(gradient);
	}
}
void NISSolverBase::retrieveHessianPenaltyMethod(EigenSparseMatrix& hessian, unsigned size)
{
	hessian.resize(size, size);
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->RetrieveHessian(h_triplets);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());
}
ScalarType NISSolverBase::lineSearchPenaltyMethod(const ScalarType currentObjectiveValue, const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier/* = 1.0*/)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		do
		{
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			lhs = evaluateEnergyPenaltyMethod(x_plus_tdx, psi, multiplier);

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while (lhs >= rhs && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}

		m_ls_stepsize = t;
		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}

ScalarType NISSolverBase::evaluateEnergy(const VectorX& x, ScalarType (*psi)(ScalarType), bool throwable)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if (m_enable_substepping)
		{
			energy += (*it)->EvaluateEnergyGeneralBarrier(x, psi, m_multiplier, throwable, m_regularization_mu);
		}
		else
		{
			energy += (*it)->EvaluateEnergyGeneralBarrier(x, psi, m_multiplier, throwable);
		}
	}
	return energy;
}
void NISSolverBase::evaluateGradient(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType))
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if (m_enable_substepping)
		{
			(*it)->EvaluateGradientGeneralBarrier(x, gradient, d_psi, m_multiplier, m_regularization_mu);
		}
		else
		{
			(*it)->EvaluateGradientGeneralBarrier(x, gradient, d_psi, m_multiplier);
		}
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif
}
void NISSolverBase::evaluateHessian(const VectorX& x, EigenSparseMatrix& hessian, ScalarType (*d_psi)(ScalarType), ScalarType (*exterior_points_d2_psi)(ScalarType))
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianGeneralBarrier(x, h_triplets, d_psi, exterior_points_d2_psi, m_multiplier);
	}

	//if (m_enable_substepping)
	//{
	//	for (unsigned int i = 0; i != x.size(); ++i)
	//	{
	//		h_triplets.push_back(SparseMatrixTriplet(i, i, m_regularization_mu));
	//	}
	//}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif
}

void NISSolverBase::evaluateGradient(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType))
{
	gradient.resize(x.size()+lambda.size());
	gradient.setZero();

	unsigned int i = 0;
	ScalarType multiplier = m_multiplier;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientPrimalDualGeneralBarrier(x, lambda, gradient, d_psi, multiplier);
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif
}
void NISSolverBase::evaluateHessian(const VectorX& x, const VectorX& lambda, EigenSparseMatrix& hessian, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType))
{
	hessian.resize(x.size()+lambda.size(), x.size()+lambda.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	unsigned int i = 0;
	ScalarType multiplier = m_multiplier;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianPrimalDualGeneralBarrier(x, lambda, h_triplets, d_psi, d2_psi, multiplier);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif
}
ScalarType NISSolverBase::evaluateEnergyNoConstraint(const VectorX& x)
{
	ScalarType energy = 0;

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyNoConstraint(x, 1, m_regularization_mu);
	}

	return energy;
}
void NISSolverBase::evaluateGradientNoConstraint(const VectorX& x, VectorX& gradient)
{
	gradient.resize(x.size());
	gradient.setZero();


	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientNoConstraint(x, gradient, 1, m_regularization_mu);
	}


	//#ifdef ENABLE_MATLAB_DEBUGGING
	//	g_debugger->SendVector(gradient, "gradient", gradient.size());
	//#endif
}
void NISSolverBase::evaluateHessianNoConstraint(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianNoConstraint(x, h_triplets);
	}

	// regularization
	if (m_enable_substepping == true && m_regularization_mu > 0)
	{
		for (unsigned int i = 0; i != x.size(); ++i)
		{
			h_triplets.push_back(SparseMatrixTriplet(i, i, m_regularization_mu));
		}
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif
}

void NISSolverBase::evaluatePrimalDualResidual(const VectorX& x, const VectorX& lambda, VectorX& res)
{
	evaluateGradient(x, lambda, res, m_d_psi);
}
void NISSolverBase::evaluatePrimalDualKKT(const VectorX& x, const VectorX& lambda, EigenSparseMatrix& KKT)
{
	evaluateHessian(x, lambda, KKT, m_d_psi, m_d2_psi);
}
ScalarType NISSolverBase::lineSearchPrimalDual(const VectorX& primal_dual_x_lambda, const VectorX& current_residual, const VectorX& primal_dual_descent_dir, const unsigned int x_size, const unsigned int lambda_size)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(primal_dual_x_lambda.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentResidualLength = current_residual.norm();

		bool force_loop;
		do
		{
			force_loop = false;
			t *= m_ls_beta;
			x_plus_tdx = primal_dual_x_lambda + t*primal_dual_descent_dir;
			VectorX x_plus_tdx_x = x_plus_tdx.block(0, 0, x_size, 1);
			VectorX x_plus_tdx_lambda = x_plus_tdx.block(x_size, 0, lambda_size, 1);

			// check if x_plus_tdx crosses the domain: primal
			TriangleElementConstraint* tri;
			for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
			{
				if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
				{
					tri = dynamic_cast<TriangleElementConstraint*>(*it);
					ScalarType W11, detW;
					tri->EvaluateEnergyNoninversionConstraintCore(x_plus_tdx_x, W11, detW);
					if (detW < 0 || W11 < 0)
					{
						force_loop = true;
						break;
					}
				}
			}
			// check if x_plus_tdx crosses the domain: dual
			for (unsigned int i = 0; i != lambda_size; i++)
			{
				if (x_plus_tdx_lambda(i) < 0)
				{
					force_loop = true;
					break;
				}
			}

			// do line search on residual
			VectorX new_residual;
			evaluatePrimalDualResidual(x_plus_tdx_x, x_plus_tdx_lambda, new_residual);
			lhs = new_residual.norm();

			rhs = (1-m_ls_alpha * t) * currentResidualLength;

		} while ((force_loop || lhs >= rhs) && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}
		else
		{
			m_ls_stepsize = t;
		}

		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}

ScalarType NISSolverBase::evaluateEnergyCorot(const VectorX& x)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyCorot(x);
	}
	return energy;
}
void NISSolverBase::evaluateGradientCorot(const VectorX& x, VectorX& gradient)
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientCorot(x, gradient);
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif
}

void NISSolverBase::evaluateHessianCorot(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianCorot(x, h_triplets);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif
}

ScalarType NISSolverBase::lineSearchCorot(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergyCorot(x);

		bool force_loop;
		do
		{
			force_loop = false;
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			try
			{
				lhs = evaluateEnergyCorot(x_plus_tdx);
			}
			catch(MyExceptions e)
			{
				assert(e == EXCEPTION_LOG_OF_NEGATIVE);
				force_loop = true;
				continue;
			}

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while ((force_loop || lhs >= rhs) && t > EPSILON);

		if (t < EPSILON)
		{
			std::cout << "<descent_dir, gradient> = " << descent_dir.dot(gradient_dir) << std::endl;
			t = EPSILON;
			m_ls_stepsize = t;
		}
		else
		{
			m_ls_stepsize = t;
		}

		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}

void NISSolverBase::evaluateHessianLocalGlobalStyle(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianCorotLocalGlobalStyle(x, h_triplets);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif

}

void NISSolverBase::evaluateHessianConst1(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianCorotConstant1(x, h_triplets);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif

}

void NISSolverBase::evaluateHessianConst2(const VectorX& x, EigenSparseMatrix& hessian)
{
	hessian.resize(x.size(), x.size());
	std::vector<SparseMatrixTriplet> h_triplets;
	h_triplets.clear();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateHessianCorotConstant2(x, h_triplets);
	}

	hessian.setFromTriplets(h_triplets.begin(), h_triplets.end());

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendSparseMatrixUsingTriplets(h_triplets, "H", hessian.rows(), hessian.cols());
#endif

}

ScalarType NISSolverBase::evaluateEnergyNeoHookean(const VectorX& x)
{
	ScalarType energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		energy += (*it)->EvaluateEnergyNeoHookean(x);
	}
	return energy;

}
void NISSolverBase::evaluateGradientNeoHookean(const VectorX& x, VectorX& gradient)
{
	gradient.resize(x.size());
	gradient.setZero();

	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGradientNeoHookean(x, gradient);
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->SendVector(gradient, "gradient", gradient.size());
#endif

}
ScalarType NISSolverBase::lineSearchNeoHookean(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergyNeoHookean(x);

		bool force_loop;
		do
		{
			force_loop = false;
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			try
			{
				lhs = evaluateEnergyNeoHookean(x_plus_tdx);
			}
			catch(MyExceptions e)
			{
				assert(e == EXCEPTION_LOG_OF_NEGATIVE);
				force_loop = true;
				continue;
			}

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while ((force_loop || lhs >= rhs) && t > EPSILON);

		if (t < EPSILON)
		{
			std::cout << "<descent_dir, gradient> = " << descent_dir.dot(gradient_dir) << std::endl;
			t = EPSILON;
			m_ls_stepsize = t;
		}
		else
		{
			m_ls_stepsize = t;
		}

		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}


void NISSolverBase::setBarrierFunction()
{
	switch(m_barrier_type)
	{
	case BARRIER_TYPE_LOG:
		m_psi = &function_log_barrier;
		m_d_psi = &derivative_log_barrier;
		m_d2_psi = &derivative2_log_barrier;
		break;
	case BARRIER_TYPE_LOG_PLUS_LINEAR:
		m_psi = &function_log_plus_linear_barrier;
		m_d_psi = &derivative_log_plus_linear_barrier;
		m_d2_psi = &derivative2_log_plus_linear_barrier;
		break;
	case BARRIER_TYPE_SPLINE:
		m_psi = &function_spline_barrier;
		m_d_psi = &derivative_spline_barrier;
		m_d2_psi = &derivative2_spline_barrier;
		break;
	case BARRIER_TYPE_QUADRATIC:
		m_psi = &penalty_function_quadratic_on_negative_psi;
		m_d_psi = &penalty_function_quadratic_on_negative_d_psi;
		m_d2_psi = &penalty_function_quadratic_on_negative_d2_psi;
		break;
	case BARRIER_TYPE_MODIFIED_LOG:
		m_psi = &function_modified_log_barrier;
		m_d_psi = &derivative_modified_log_barrier;
		m_d2_psi = &derivative2_modified_log_barrier;
		break;
	case BARRIER_TYPE_NR_TRANSFORMATION:
		m_psi = &function_NR_trasformation_function;
		m_d_psi = &derivative_NR_transformation_function;
		m_d2_psi = &derivative2_NR_transformation_function;
		break;
	case BARRIER_TYPE_LSL:
		m_psi = &function_LSL;
		m_d_psi = &derivative_LSL;
		m_d2_psi = &derivative2_LSL;
		break;
	}
}

void NISSolverBase::ResetInitialRescalingParameter(ScalarType initial_rescaling_parameter)
{
	if (m_auto_multiplier)
	{
		if (initial_rescaling_parameter < 0)
			m_multiplier = test4;
		else
			m_multiplier = initial_rescaling_parameter;
	}
	initSpectralSDPLagrangian();
	m_spectral_sdp_is_first_iteration = true;
}

void NISSolverBase::SetInitialLambda(const VectorX& x, VectorX& lambda)
{
	unsigned int m = 0;
	TriangleElementConstraint* tri;
	TetElementConstraint* tet;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			tri = dynamic_cast<TriangleElementConstraint*>(*it);
			ScalarType detW = tri->EvaluateEnergyNoninversionConstraintCore(x);
			lambda(m) = 1/(m_multiplier*detW);
			m ++;
		}
		else if ((*it)->Type() == CONSTRAINT_TYPE_TET_ELEMENT)
		{
			tet = dynamic_cast<TetElementConstraint*>(*it);
			ScalarType detW = tet->EvaluateEnergyNoninversionConstraintCore(x);
			lambda(m) = 1/(m_multiplier*detW);
			m ++;
		}
	}
}

void NISSolverBase::setRescalingParameter(const VectorX& x, const VectorX& lambda)
{
	if (m_auto_multiplier)
	{
		unsigned int m = 0;
		ScalarType ita = 0;
		TriangleElementConstraint* tri;
		TetElementConstraint* tet;
		for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
		{
			if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
			{
				tri = dynamic_cast<TriangleElementConstraint*>(*it);
				ScalarType detW = tri->EvaluateEnergyNoninversionConstraintCore(x);
				ita += detW * lambda(m);
				m ++;
			}
			else if ((*it)->Type() == CONSTRAINT_TYPE_TET_ELEMENT)
			{
				tet = dynamic_cast<TetElementConstraint*>(*it);
				ScalarType detW = tet->EvaluateEnergyNoninversionConstraintCore(x);
				ita += detW * lambda(m);
				m++;
			}
		}

		//ScalarType multiplier_old = m_multiplier;
		//ScalarType theoratical_multiplier = m/ita;
		//m_multiplier = m_multiplier_mu * theoratical_multiplier * std::pow(std::min((theoratical_multiplier/multiplier_old), 1.0), 2);

		m_multiplier = m_multiplier_mu * m/ita;
	}
}

ScalarType NISSolverBase::limChooseAlpha(const VectorX& x)
{
	ScalarType r = 80;

	ScalarType t = m_stiffness_attachment;
	bool old_enable_substepping = m_enable_substepping;

	m_enable_substepping = false;
	m_stiffness_attachment = 1;
	ScalarType total_energy = evaluateEnergy(x, m_psi);
	ScalarType target_energy = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			target_energy += (*it)->EvaluateEnergyGeneralBarrier(x, m_psi, m_multiplier);
		}
	}
	if (target_energy < EPSILON)
	{
		target_energy = EPSILON;
	}

	ScalarType gamma = r * (total_energy/target_energy - 1);

	m_enable_substepping = old_enable_substepping;
	m_stiffness_attachment = t;

	return std::min(std::max(gamma, m_lim_alpha_update_last_alpha), t);
}

void NISSolverBase::evaluateGMatrix(EigenSparseMatrix& G)
{
	unsigned int dim = (m_mode == MODE_2D) ? 2 : 3;
	G.resize(m_f_size, m_vertices_num * dim);
	std::vector<SparseMatrixTriplet> g_triplets;
	g_triplets.clear();

	unsigned int index = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->EvaluateGMatrix(g_triplets, index);
	}

	G.setFromTriplets(g_triplets.begin(), g_triplets.end());

	//#ifdef ENABLE_MATLAB_DEBUGGING
	//	g_debugger->SendSparseMatrixUsingTriplets(g_triplets, "G", G.rows(), G.cols());
	//#endif
}

void NISSolverBase::evaluateTMatrix(EigenSparseMatrix& T)
{
	std::vector<SparseMatrixTriplet> g_triplets;
	g_triplets.clear();
	unsigned int index = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			(*it)->EvaluateGMatrix(g_triplets, index);
		}
	}

	unsigned int dim = (m_mode == MODE_2D) ? 2 : 3;
	T.resize(index, m_vertices_num * dim);
	T.setFromTriplets(g_triplets.begin(), g_triplets.end());
}
void NISSolverBase::evaluateAMatrix(EigenSparseMatrix& A)
{
	std::vector<SparseMatrixTriplet> g_triplets;
	g_triplets.clear();
	unsigned int index = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			(*it)->EvaluateGMatrix(g_triplets, index);
		}
	}

	unsigned int dim = (m_mode == MODE_2D) ? 2 : 3;
	A.resize(index, m_vertices_num * dim);
	A.setFromTriplets(g_triplets.begin(), g_triplets.end());
}

void NISSolverBase::SetNoninversionCriteria()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->SetNonInversionCriteria(m_noninversion_criteria);
	}
}

void NISSolverBase::SetRotationFieldType()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->SetEnergyType(m_energy_type);
	}
}

ScalarType NISSolverBase::lineSearchNoConstraint(const VectorX& x, const VectorX& gradient_dir, const VectorX& descent_dir)
{
	if (m_enable_line_search)
	{
		VectorX x_plus_tdx(x.size());
		ScalarType t = 1.0/m_ls_beta;
		ScalarType lhs, rhs;

		ScalarType currentObjectiveValue = evaluateEnergyNoConstraint(x);

		bool force_loop;
		do
		{
			force_loop = false;
			t *= m_ls_beta;
			x_plus_tdx = x + t*descent_dir;

			try
			{
				lhs = evaluateEnergyNoConstraint(x_plus_tdx);
			}
			catch(MyExceptions e)
			{
				assert(e == EXCEPTION_LOG_OF_NEGATIVE);
				std::cout << "unexpected exception in lineSearchNoConstraint()" << std::endl;
				force_loop = true;
				continue;
			}

			rhs = currentObjectiveValue + m_ls_alpha * t * gradient_dir.dot(descent_dir);

		} while ((force_loop || lhs >= rhs) && t > EPSILON);

		if (t < EPSILON)
		{
			t = 0.0;
		}
		else
		{
			m_ls_stepsize = t;
		}

		return t;
	}
	else
	{
		return m_ls_stepsize;
	}
}

// matlab solve related
void NISSolverBase::evaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& dim1, unsigned int& dim2)
{
	unsigned int index = 0;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		if ((*it)->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT || (*it)->Type() == CONSTRAINT_TYPE_TET_ELEMENT)
		{
			(*it)->EvaluateGMatrix(g_triplets, index);
		}
	}

	unsigned int dim = (m_mode==MODE_2D)? 2 : 3;
	dim1 = index;
	dim2 = m_vertices_num * dim;
}

void NISSolverBase::factorizeDirectSolverLDLT(const EigenSparseMatrix& A, Eigen::SimplicialLDLT<EigenSparseMatrix, Eigen::Upper>& ldltSolver, char* warning_msg) // factorize matrix A using LDLT decomposition
{
	EigenSparseMatrix A_prime = A;
	ldltSolver.analyzePattern(A_prime);
	ldltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (ldltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		ldltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Warning: Singular matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(ldlt solver)" << std::endl;
		}
		//m_regularization_mu = Regularization;
	}
	else
	{
		//m_regularization_mu = 0;
	}
}

void NISSolverBase::factorizeSparseLU(const EigenSparseMatrix& A, Eigen::SparseLU<EigenSparseMatrix>& luSolver, char* warning_msg /* = "" */)
{
	EigenSparseMatrix A_prime = A;
	A_prime.makeCompressed();
	luSolver.analyzePattern(A_prime);
	luSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (luSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		if (Regularization > 1e20)
		{
			//std::cout << A;
			std::cout << "Warning: LU decomposition falied after adding " << Regularization << " identities. Force Stop" << std::endl;
			break;
		}
		A_prime = A_prime + Regularization*sparse_identity;
		luSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Warning: Singular matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(sparse lu solver)" << std::endl;
		}
		//m_regularization_mu = Regularization;
	}
	else
	{
		//m_regularization_mu = 0;
	}
}

void NISSolverBase::factorizeDirectSolverLLT(const EigenSparseMatrix& A, Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper>& lltSolver, char* warning_msg)
{
	EigenSparseMatrix A_prime = A;
	lltSolver.analyzePattern(A_prime);
	lltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (lltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		lltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Warning: Indefinite matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(llt solver)" << std::endl;
		}
		if (m_enable_substepping == true)
		{
			m_regularization_mu = Regularization;
		}
	}
	else
	{
		if (m_enable_substepping == true)
		{
			m_regularization_mu = 0;
		}
	}
}

void NISSolverBase::factorizeDirectSolverLLTWithoutSymbolicFactorization(const EigenSparseMatrix& A, Eigen::SimplicialLLT<EigenSparseMatrix, Eigen::Upper>& lltSolver, char* warning_msg /* = "" */)
{
	EigenSparseMatrix A_prime = A;
	lltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (lltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		lltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Warning: Indefinite matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(llt solver)" << std::endl;
		}
		if (m_enable_substepping == true)
		{
			m_regularization_mu = Regularization;
		}
	}
	else
	{
		if (m_enable_substepping == true)
		{
			m_regularization_mu = 0;
		}
	}	
}

#ifdef PARDISO_SUPPORT
void NISSolverBase::factorizePardisoLDLT(const EigenSparseMatrix& A, Eigen::PardisoLDLT<EigenSparseMatrix, Eigen::Upper>& ldltSolver, char* warning_msg) // factorize matrix A using LDLT decomposition
{
	EigenSparseMatrix A_prime = A;
	ldltSolver.analyzePattern(A_prime);
	ldltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (ldltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (unsigned int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		ldltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Pardiso Warning: Singular matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(ldlt solver)" << std::endl;
		}
	}
}

void NISSolverBase::factorizePardisoLDLTWithoutSymbolicFactorization(const EigenSparseMatrix& A, Eigen::PardisoLDLT<EigenSparseMatrix, Eigen::Upper>& ldltSolver, char* warning_msg /* = "" */)
{
	EigenSparseMatrix A_prime = A;
	ldltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (ldltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (unsigned int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		ldltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Pardiso Warning: Singular matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(ldlt solver)" << std::endl;
		}
	}
}
void NISSolverBase::factorizePardisoLLT(const EigenSparseMatrix& A, Eigen::PardisoLLT<EigenSparseMatrix, Eigen::Upper>& lltSolver, char* warning_msg) // factorize matrix A using LDLT decomposition
{
	EigenSparseMatrix A_prime = A;
	lltSolver.analyzePattern(A_prime);
	lltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (lltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (unsigned int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		lltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Pardiso Warning: Indefinite matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(ldlt solver)" << std::endl;
		}
	}
}
void NISSolverBase::factorizePardisoLLTWithoutSymbolicFactorization(const EigenSparseMatrix& A, Eigen::PardisoLLT<EigenSparseMatrix, Eigen::Upper>& lltSolver, char* warning_msg) // factorize matrix A using LDLT decomposition
{
	EigenSparseMatrix A_prime = A;
	lltSolver.factorize(A_prime);
	ScalarType Regularization = 1e-10;
	bool success = true;
	EigenSparseMatrix sparse_identity;
	while (lltSolver.info() != Eigen::Success)
	{
		if (success == true)
		{
			sparse_identity.resize(A.rows(), A.cols());
			std::vector<SparseMatrixTriplet> identity_triplets;
			identity_triplets.clear();
			for (unsigned int i = 0; i < A.rows(); i++)
			{
				identity_triplets.push_back(SparseMatrixTriplet(i,i,1));
			}
			sparse_identity.setFromTriplets(identity_triplets.begin(), identity_triplets.end());
		}
		Regularization *= 10;
		A_prime = A_prime + Regularization*sparse_identity;
		lltSolver.factorize(A_prime);
		success = false;
	}
	if (!success)
	{
		if (strlen(warning_msg) != 0)
		{
			std::cout << "Pardiso Warning: Indefinite matrix in " << warning_msg <<  ", adding "<< Regularization <<" identites.(ldlt solver)" << std::endl;
		}
	}
}

#endif

// !protected functions and utilities


NISSolverBase::NISSolverBase()
{
	m_iteration_num = 0;
}

NISSolverBase::~NISSolverBase()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		delete (*it);
	}
	m_constraints.clear();
}

void NISSolverBase::InitParametersInPhysbam()
{
	Reset();

	m_stiffness_attachment=1e4;
	m_mu=1.0;
	m_lambda=0.;
	m_enable_non_inversion=true; // enable/disable non inversion
	m_alpha=1.0;
	m_beta=1.0;
	m_gamma=1.0;
	m_multiplier=1.e-6;
	m_enable_line_search=true;
	m_ls_alpha=0;
	m_ls_beta=0.5;
	m_ls_stepsize=1;
	m_iteration_num=0;
	m_auto_multiplier=true;
	m_multiplier_mu=10;
	m_energy_type = ENERGY_TYPE_EXROT;

	m_optimization_method = OPTIMIZATION_PENALTY;
	ResetParameterToOptimizationMethod();

	switch (m_optimization_method)
	{
	case OPTIMIZATION_PENALTY:
	case OPTIMIZATION_PENALTY_LOCAL_GLOBAL_BLEND:
	case OPTIMIZATION_LOCAL_GLOBAL:
	case OPTIMIZATION_SPECTRAL_SDP:
	case OPTIMIZATION_PENNON:
	case OPTIMIZATION_NONINVERSION_CONSTANT_HESSIAN:
		m_multiplier=1;
		break;
	case OPTIMIZATION_BARRIER_METHOD:
	case OPTIMIZATION_NEWTON_PRIMAL_DUAL:
		m_multiplier=1.e-6;
		break;
	}

	//// To Ming: TODO your stuff
	//
	//m_element_num=layout.object_volume->mesh.elements.m;
	//// init constrains 
	//for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	//{
	//	delete (*it);
	//}
	//m_constraints.clear();

	//// setup attachment constraints
	//unsigned int dim = layout.d;
	//unsigned int size = layout.particles.X.m;

	//for(int b=1;b<=2;b++)
	//{
	//	for (unsigned int i = 1; i <= layout.particles_each_bone(b).m; i++)
	//	{
	//		int p=layout.particles_each_bone(b)(i);
	//		AttachmentConstraint *ac = new AttachmentConstraint(&m_stiffness_attachment, p-1, b-1, i-1,3,&m_multiplier);
	//		// stiffness
	//		// vertex id in mesh
	//		// handle / bone id
	//		// fixed_point id in handle
	//		m_constraints.push_back(ac);
	//	}
	//}

	//if(dim==3)
	//{
	//	for(int tet=1;tet<=layout.object_volume->mesh.elements.m;tet++){
	//		int p0,p1,p2,p3;
	//		layout.object_volume->mesh.elements(tet).Get(p0,p1,p2,p3);
	//		TetElementConstraint *tetc = new TetElementConstraint(tet-1,&m_mu, &m_lambda, &m_enable_non_inversion, &m_alpha, &m_beta, &m_gamma, &m_slackness, 
	//			p0-1,p1-1,p2-1,p3-1,x,&m_multiplier);
	//		m_constraints.push_back(tetc);
	//	}
	//}
	//UpdateRotation(x);
	//SetNoninversionCriteria();
	//m_dual_lambda.resize(m_element_num);
	//for(int i=0;i<m_element_num;++i) m_dual_lambda(i)=10.;
	//SetInitialLambda(x, m_dual_lambda);
	//std::cout << m_dual_lambda << std::endl;
	//std::cout << "++++++++++++++++++++++++++++++++" << std::endl;

	//// allocate spaces
	//m_handle_vertices.resize(layout.NUMBER_BONES);
	//m_per_element_rotation_2d.resize(layout.object_volume->mesh.elements.m);
	//m_per_element_rotation_3d.resize(layout.object_volume->mesh.elements.m);

	//m_initialized = true;
}

void NISSolverBase::Reset()
{
	m_energy_type = ENERGY_TYPE_EXROT;

	// interior points
	m_enable_substepping = false;
	m_regularization_mu = 0;

	m_multiplier_mu = 10;

	ResetFactorizationFlag();
	ResetParameterToOptimizationMethod();

	// collision
	m_collision_enable = false;
	m_collision_y_plane = 0.0;
	m_collision_stiffness = 1e5;
	m_surface_indices.clear();

	//// debug for collision discSmall
	//for (unsigned int i = 0; i != 100; i++)
	//{
	//	m_surface_indices.push_back(i);
	//}
	//m_collision_enable = true;
	//m_collision_y_plane = -10;

	//// debug for collision rect
	//unsigned int surface_indices_array[12] = {0, 1, 2, 3, 4, 5, 9, 10, 11, 12, 13, 14};
	//m_surface_indices = std::vector<unsigned int>(surface_indices_array, surface_indices_array+12);
	//m_collision_enable = true;
}

void NISSolverBase::ResetParameterToOptimizationMethod()
{
	switch(m_optimization_method)
	{
	case OPTIMIZATION_LOCALLY_INJECTIVE_MAPPING:
		m_noninversion_criteria = NC_DETERMINANT_F;
		m_barrier_type = BARRIER_TYPE_SPLINE;
		m_penalty_type = PENALTY_TYPE_QUADRATIC;
		m_enable_substepping = true;
		m_auto_multiplier = false;
		m_multiplier = 1;
		break;
	case OPTIMIZATION_PENALTY_ON_DETF:
		m_noninversion_criteria = NC_DETERMINANT_F;
		m_barrier_type = BARRIER_TYPE_QUADRATIC;
		m_penalty_type = PENALTY_TYPE_QUADRATIC;
		m_enable_substepping = false;
		m_auto_multiplier = true;
		m_multiplier = 1;
		break;
	case OPTIMIZATION_BARRIER_METHOD:
	case OPTIMIZATION_NEWTON_PRIMAL_DUAL:
		m_noninversion_criteria = NC_DETERMINANT_W;
		m_barrier_type = BARRIER_TYPE_LOG;
		m_penalty_type = PENALTY_TYPE_QUADRATIC;
		m_enable_substepping = false;
		m_auto_multiplier = true;
		m_multiplier = 1e-6;
		break;
	case OPTIMIZATION_PENALTY:
	case OPTIMIZATION_PENALTY_LOCAL_GLOBAL_BLEND:
	case OPTIMIZATION_LOCAL_GLOBAL:
	case OPTIMIZATION_SPECTRAL_SDP:
	case OPTIMIZATION_PENNON:
	case OPTIMIZATION_NONINVERSION_CONSTANT_HESSIAN:
	default:
		m_noninversion_criteria = NC_DETERMINANT_W;
		m_barrier_type = BARRIER_TYPE_LOG;
		m_penalty_type = PENALTY_TYPE_QUADRATIC;
		m_enable_substepping = false;
		m_auto_multiplier = true;
		m_multiplier = 1;
		break;
	}
}

void NISSolverBase::ResetFactorizationFlag()
{
	m_local_global_factorization_flag = false;
	m_penalty_hessian_symbolic_factorization_flag = false;
}

void NISSolverBase::UpdateHandleInfo()
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->UpdateHandleInfo(m_handle_vertices, m_per_element_rotation_2d, m_per_element_rotation_3d);
	}

	m_iteration_num = 0;

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->MatlabResetSDPMosek();
#endif
}

void NISSolverBase::UpdateRotation(const VectorX& x)
{
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		(*it)->UpdateRotation(x);
	}
}

bool NISSolverBase::UpdateOneIteration(VectorX& x, VectorX& lambda, ScalarType& progress, OptimizationMethod method)
{
	if (m_iteration_num == 0)
	{
#ifdef ENABLE_MATLAB_DEBUGGING
		g_debugger->SendPositionIterationTime(x, x.size(), m_iteration_num, 0.0);
#endif
		m_main_timer.Tic();
		m_main_timer.Pause();
	}

	if (method == OPTIMIZATION_TOTAL_NUM)
		method = m_optimization_method;
	bool success = false;
	VectorX last_x = x;
	m_main_timer.Resume();
	switch(method)
	{
	case OPTIMIZATION_LOCAL_GLOBAL:
		success = updateLocalGlobal(x);
		break;
	case OPTIMIZATION_PENALTY:
		success = updatePenaltyMethod(x);
		break;
	case OPTIMIZATION_SPECTRAL_SDP:
		success = updateSpectralSDP(x);
		break;
	case OPTIMIZATION_PENNON:
		success = updatePennon(x);
		break;
	case OPTIMIZATION_PENALTY_LOCAL_GLOBAL_BLEND:
		success = updatePenaltyMethodLocalGlobalBlend(x);
		break;
	case OPTIMIZATION_LOCALLY_INJECTIVE_MAPPING:
		success = updateLIM(x);
		break;
	case OPTIMIZATION_BARRIER_METHOD:
		success = updateBarrierMethod(x, lambda);
		break;
	case OPTIMIZATION_NEWTON_PRIMAL_DUAL:
		success = updatePrimalDual(x, lambda);
		break;
	case OPTIMIZATION_MOSEK_WITH_ITERATIONS:
		success = UpdateMatlabMosekYalmip(x, true);
		break;
	case OPTIMIZATION_MOSEK_WITHOUT_ITERATIONS:
		success = UpdateMatlabMosekYalmip(x);
		break;
	case OPTIMIZATION_PENALTY_ON_DETF:
		success = updatePenaltyMethodOnDetF(x);
		break;
	case OPTIMIZATION_COROT_NEWTON:
		success = updateCorotNewton(x);
		break;
	case OPTIMIZATION_COROT_LOCAL_GLOBAL_STYLE:
		success = updateCorotLocalGlobal(x);
		break;
	case OPTIMIZATION_COROT_CONSTANT_HESSIAN_1:
		success = updateCorotConstantHessian1(x);
		break;
	case OPTIMIZATION_COROT_CONSTANT_HESSIAN_2:
		success = updateCorotConstantHessian2(x);
		break;
	case OPTIMIZATION_NEOHOOKEAN_GRADIENT_DESCENT:
		success = updateNeoHookeanGradientDescent(x);
		break;
	case OPTIMIZATION_NEWHOOKEAN_CONSTANT_HESSIAN:
		success = updateNeoHookeanConstantHessian(x);
		break;
	case OPTIMIZATION_NONINVERSION_CONSTANT_HESSIAN:
		success = updateNoninversionConstantHessian(x);
		break;
	case OPTIMIZATION_ALTERNATIVE_PROJECTION:
		success = updateAlternatingProjection(x);
		break;
	case OPTIMIZATION_ONE_TANGENT_PLANE:
		success = updateOneTangentPlane(x);
		break;
	}
	m_iteration_num++;
	//m_main_timer.Pause();
	std::cout << "Current Iteration " << m_iteration_num << std::endl;
	m_main_timer.PauseAndReport("Main Timer");

	progress = (x - last_x).norm();

	if (success == true)
	{
		std::cout << "Algorithm Converges in Iteration " << m_iteration_num << std::endl;
		m_main_timer.Resume();
		m_main_timer.Toc();
		m_main_timer.Report("Main Timer");
		//m_main_timer.TocAndReport("Main Timer");
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	m_main_timer.Toc();
	g_debugger->SendPositionIterationTime(x, x.size(), m_iteration_num, m_main_timer.Duration());
#endif

	return success;
}

bool NISSolverBase::UpdateMatlabMosekYalmip(VectorX& x, bool has_max_it)
{
	std::vector<SparseMatrixTriplet> g_triplets;
	unsigned int dim1, dim2;
	evaluateGMatrix(g_triplets, dim1, dim2);

	VectorX exR;
	VectorX consVIndices;
	VectorX exV;

	unsigned int exR_size = 0;
	unsigned int consVindices_size = 0;
	for (unsigned int i = 0; i != m_constraints.size(); i++)
	{
		if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			exR_size+=4;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TET_ELEMENT)
		{
			exR_size+=9;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			consVindices_size += 1;
		}
	}
	exR.resize(exR_size);
	consVIndices.resize(consVindices_size);
	unsigned int dim = (m_mode == MODE_2D)?2:3;
	exV.resize(consVindices_size*dim);

	TetElementConstraint* tet;
	TriangleElementConstraint* tri;
	AttachmentConstraint* at;
	unsigned exR_i = 0;
	unsigned exV_i = 0;
	for (unsigned int i = 0; i != m_constraints.size(); i++)
	{
		if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			tri = dynamic_cast<TriangleElementConstraint*>(m_constraints[i]);
			EigenMatrix2 ExR_block = tri->m_ExR;
			exR(exR_i*4 + 0) = ExR_block(0,0);
			exR(exR_i*4 + 1) = ExR_block(1,0);
			exR(exR_i*4 + 2) = ExR_block(0,1);
			exR(exR_i*4 + 3) = ExR_block(1,1);
			exR_i++;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TET_ELEMENT)
		{
			tet = dynamic_cast<TetElementConstraint*>(m_constraints[i]);
			EigenMatrix3 ExR_block = tet->m_ExR;
			exR(exR_i*9 + 0) = ExR_block(0,0);
			exR(exR_i*9 + 1) = ExR_block(1,0);
			exR(exR_i*9 + 2) = ExR_block(2,0);
			exR(exR_i*9 + 3) = ExR_block(0,1);
			exR(exR_i*9 + 4) = ExR_block(1,1);
			exR(exR_i*9 + 5) = ExR_block(2,1);
			exR(exR_i*9 + 6) = ExR_block(0,2);
			exR(exR_i*9 + 7) = ExR_block(1,2);
			exR(exR_i*9 + 8) = ExR_block(2,2);
			exR_i++;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			at = dynamic_cast<AttachmentConstraint*>(m_constraints[i]);
			consVIndices(exV_i) = at->m_p0+1;
			exV.block(dim*exV_i, 0, dim, 1) = at->m_fixed_point;
			exV_i ++;
		}
	}

	assert(exV_i == consVindices_size);
	if (dim == 2)
	{
		assert(exR_i*4 == exR_size);
	}
	else
	{
		assert(exR_i*9 == exR_size);
	}

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->MatlabInitSDPMosekYalmip();
	g_debugger->MatlabSolveSDPMosekYalmip(dim, x, g_triplets, dim1, dim2, exR, consVIndices, exV, m_stiffness_attachment, has_max_it);
#endif

	return false;
}

void NISSolverBase::UpdateMatlabMosekEpigraph(VectorX& x)
{
	std::vector<SparseMatrixTriplet> g_triplets;
	unsigned int dim1, dim2;
	evaluateGMatrix(g_triplets, dim1, dim2);

	VectorX exR;
	VectorX consVIndices;
	VectorX exV;

	unsigned int exR_size = 0;
	unsigned int consVindices_size = 0;
	for (unsigned int i = 0; i != m_constraints.size(); i++)
	{
		if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			exR_size+=4;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			consVindices_size += 1;
		}
	}
	exR.resize(exR_size);
	consVIndices.resize(consVindices_size);
	unsigned int dim = (m_mode == MODE_2D)?2:3;
	exV.resize(consVindices_size*dim);

	TriangleElementConstraint* tri;
	AttachmentConstraint* at;
	unsigned exR_i = 0;
	unsigned exV_i = 0;
	for (unsigned int i = 0; i != m_constraints.size(); i++)
	{
		if (m_constraints[i]->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
		{
			tri = dynamic_cast<TriangleElementConstraint*>(m_constraints[i]);
			EigenMatrix2 ExR_block = tri->m_ExR;
			exR(exR_i*4 + 0) = ExR_block(0,0);
			exR(exR_i*4 + 1) = ExR_block(1,0);
			exR(exR_i*4 + 2) = ExR_block(0,1);
			exR(exR_i*4 + 3) = ExR_block(1,1);
			exR_i++;
		}
		else if (m_constraints[i]->Type() == CONSTRAINT_TYPE_ATTACHMENT)
		{
			at = dynamic_cast<AttachmentConstraint*>(m_constraints[i]);
			consVIndices(exV_i) = at->m_p0+1;
			exV.block(2*exV_i, 0, dim, 1) = at->m_fixed_point;
			exV_i ++;
		}
	}

	assert(exV_i == consVindices_size);
	assert(exR_i*4 == exR_size);

#ifdef ENABLE_MATLAB_DEBUGGING
	g_debugger->MatlabInitSDPMosekEpigraph();
	g_debugger->MatlabSolveSDPMosekEpigraph(x, g_triplets, dim1, dim2, exR, consVIndices, exV, m_stiffness_attachment);
#endif
}

bool NISSolverBase::CorrectFeasibilityProjectionLocal(VectorX& f)
{
	unsigned int index = 0;
	bool result = true;
	for (std::vector<Constraint*>::iterator it = m_constraints.begin(); it != m_constraints.end(); ++it)
	{
		result &= (*it)->CorrectToNoninversion(f, index);
	}
	return result;
}
void NISSolverBase::CorrectFeasibilityProjectionGlobal(VectorX& f, VectorX& x)
{
	x = m_GTG_solver.solve(m_G.transpose() * f);
	f = m_G*x;
}