#ifndef _CONSTRAINT_H_
#define _CONSTRAINT_H_

#include "..\headers\global_headers.h"
#include "..\headers\math_headers.h"

typedef enum
{
	CONSTRAINT_TYPE_ATTACHMENT,
	CONSTRAINT_TYPE_GLUE,
	CONSTRAINT_TYPE_TRIANGLE_ELEMENT,
	CONSTRAINT_TYPE_TET_ELEMENT,
	CONSTRAINT_TYPE_TOTAL_NUM

} ConstraintType;

typedef enum
{
	NC_DETERMINANT_W,
	NC_DETERMINANT_F,
	NC_DETERMINANT_F_UPDATE_ROTATION,
	NC_TOTAL_NUM
} NoninversionCriteria;

typedef enum
{
	ENERGY_TYPE_COROT,
	ENERGY_TYPE_EXROT,
	ENERGY_TYPE_TOTAL_NUM,
}EnergyType;

// elements only have linear/non-linear classifications, vertices have interface option in addition.
typedef enum
{
	RC_DEFAULT = 0x00,
	RC_LINEAR = 0x01,
	RC_NONLINEAR = 0x10,
	RC_INTERFACE = 0x11
}RegionClassification;

class Constraint
{
public:
	Constraint();
	Constraint(ConstraintType type);
	Constraint(const Constraint& other);
	virtual ~Constraint();

	// pure virtual functions
	virtual ScalarType  EvaluateEnergy(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0) = 0;
	virtual void  EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0) = 0;
	virtual void  EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0) = 0;

	// local global
	virtual void LocalGlobalLocalStep(const VectorX& x) {}
	virtual ScalarType EvaluateEnergyLocalGlobal(const VectorX& x, ScalarType multiplier = 1) {return EvaluateEnergy(x);}
	virtual void EvaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianLocalGlobal(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}

	// penalty method
	virtual ScalarType EvaluateEnergyPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {return EvaluateEnergy(x);}
	virtual void EvaluateGradientPenaltyMethod(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianPenaltyMethod(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}
	virtual void EvaluateHessianPenaltyMethodFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}
	virtual bool HasZeroPenaltyEnergy(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType)) {return true;}
	// penalty method for parallelization
	virtual void EvaluateEnergyGradientHessianPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType),  ScalarType (*d_psi)(ScalarType, ScalarType),  ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {}
	virtual ScalarType RetrieveEnergy() {return 0;}
	virtual void RetrieveGradient(VectorX& gradient) {}
	virtual void RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets) {}

	// Spectral SDP
	virtual ScalarType EvaluateEnergySpectralSDP(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {return EvaluateEnergy(x);}
	virtual void EvaluateGradientSpectralSDP(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianSpectralSDP(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}
	virtual void EvaluateGradientSpectralSDPFD(const VectorX& x, VectorX& gradient, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianSpectralSDPFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}
	virtual void InitLagrangian() {}
	virtual void UpdateLagrangian(const VectorX& x_old, const VectorX& x_new, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {}

	// Pennon
	virtual ScalarType EvaluateEnergyPennon(const VectorX& x, ScalarType(*psi)(ScalarType, ScalarType), ScalarType multiplier = 1) { return EvaluateEnergy(x); }
	virtual void EvaluateGradientPennon(const VectorX& x, VectorX& gradient, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) { EvaluateGradient(x, gradient); }
	virtual void EvaluateHessianPennon(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType(*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) { EvaluateHessian(x, hessian_triplets); }
	virtual void UpdateLagrangianPennon(const VectorX& x_old, const VectorX& x_new, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1) {}

	// no constraint
	virtual ScalarType EvaluateEnergyNoConstraint(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0) {return EvaluateEnergy(x, multiplier, regularization_mu);}
	virtual void  EvaluateGradientNoConstraint(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0) {EvaluateGradient(x, gradient, multiplier, regularization_mu);}
	virtual void  EvaluateHessianNoConstraint(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0) {EvaluateHessian(x, hessian_triplets, multiplier, regularization_mu);}

	// energy gradient hessian for a gerneral barrier function (for LIM)
	virtual ScalarType EvaluateEnergyGeneralBarrier(const VectorX& x, ScalarType (*psi)(ScalarType), ScalarType multiplier = 1, bool throwable = true, ScalarType regularization_mu = 0) {return EvaluateEnergy(x, 1, regularization_mu);}
	virtual void EvaluateGradientGeneralBarrier(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0) {EvaluateGradient(x, gradient, 1, regularization_mu);}
	virtual void EvaluateHessianGeneralBarrier(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0) {EvaluateHessian(x, hessian_triplets, 1, regularization_mu);}

	// energy gradient hessian for primal-dual interior-point method
	virtual void EvaluateGradientPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1) {EvaluateHessian(x, hessian_triplets);}

	// energy gradient hessian for corot model
	virtual ScalarType EvaluateEnergyCorot(const VectorX& x) {return EvaluateEnergy(x);}
	virtual void EvaluateGradientCorot(const VectorX& x, VectorX& gradient) {EvaluateGradient(x, gradient);}
	virtual void EvaluateHessianCorot(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets){EvaluateHessian(x, hessian_triplets);}
	virtual void EvaluateHessianCorotLocalGlobalStyle(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets){EvaluateHessian(x, hessian_triplets);}
	virtual void EvaluateHessianCorotConstant1(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets){EvaluateHessian(x, hessian_triplets);}
	virtual void EvaluateHessianCorotConstant2(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets){EvaluateHessian(x, hessian_triplets);}

	// energy gradient hessian for neo-hookean model
	virtual ScalarType EvaluateEnergyNeoHookean(const VectorX& x) {return EvaluateEnergy(x);}
	virtual void EvaluateGradientNeoHookean(const VectorX& x, VectorX& gradient) {EvaluateGradient(x, gradient);}

	// update handle info
	virtual void UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotation_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d) {}
	virtual void UpdateRotation(const VectorX& x) {}

	virtual void SetNonInversionCriteria(const NoninversionCriteria nc) {}
	virtual void SetEnergyType(const EnergyType et) {}

	// data accessor 
	inline const ConstraintType& Type() {return m_constraint_type;}  //read only

	// feasibility correction
	// local step
	virtual bool  CorrectToNoninversion(VectorX& f, unsigned int& index) { return true; }
	// global step
	virtual void  EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index) {} // state independent
	virtual void  GetVectorizedExrotationField(VectorX& r, unsigned int& index) {}
	virtual void  EvaluateVectorizedDeformationGradient(const VectorX& x, VectorX& f) {}


protected:
	ConstraintType m_constraint_type;
	ScalarType * p_multiplier;

	// for parallelization
	ScalarType m_penalty_energy;
	VectorX m_penalty_gradient;
	std::vector<SparseMatrixTriplet> m_penalty_hessian_triplets;
};

class AttachmentConstraint : public Constraint
{
public:
	AttachmentConstraint(ScalarType *stiffness, unsigned int p0, unsigned int handle_id, unsigned int handle_vertex_id, unsigned int dim);
	AttachmentConstraint(const AttachmentConstraint& other);
	virtual ~AttachmentConstraint();

	virtual void UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotation_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d);
	virtual ScalarType  EvaluateEnergy(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// penalty method for parallelization
	virtual void EvaluateEnergyGradientHessianPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType),  ScalarType (*d_psi)(ScalarType, ScalarType),  ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual ScalarType RetrieveEnergy();
	virtual void RetrieveGradient(VectorX& gradient);
	virtual void RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets);

	// hard constraints
	void EvaluateHardConstraintTargetPoints(VectorX& x, VectorX& target_points, int& index);
	void EvaluateHardConstraintAttachmentMatrix(std::vector<SparseMatrixTriplet>& target_triplets, int& index);

	// local step
	virtual bool  CorrectToNoninversion(VectorX& f, unsigned int& index);
	// global step
	virtual void  EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index); // state independent
	virtual void  GetVectorizedExrotationField(VectorX& r, unsigned int& index);

public:
	
	ScalarType * p_k; // stiffness
	unsigned int m_p0;  // constrained vertex id
	unsigned int m_handle_id; // constrained with id-th handle
	unsigned int m_handle_vertex_id; // constrainted with id-th vertex in a handle

	VectorX m_fixed_point;
	unsigned int m_dim;
};

class GlueConstraint : public Constraint // special case for zero length springs
{
public:
	GlueConstraint(ScalarType *stiffness, unsigned int p0, unsigned int p1, unsigned int dim);
	GlueConstraint(const GlueConstraint& other);
	virtual ~GlueConstraint();

	virtual ScalarType  EvaluateEnergy(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// data accessor
	inline const ScalarType& Stiffness() {return *p_k;}

protected:
	
	ScalarType * p_k; // stiffness
	unsigned int m_p0;  // constrained vertex id
	unsigned int m_p1; // constrained vertex id

	unsigned int m_dim;
};

class TriangleElementConstraint : public Constraint
{
public:
	TriangleElementConstraint(unsigned int element_id, ScalarType *mu, ScalarType *lambda, bool *enable_non_inversion, ScalarType *alpha, ScalarType *beta, ScalarType *slackness, unsigned int p1, unsigned int p2, unsigned int p3, const VectorX& x);
	TriangleElementConstraint(const TriangleElementConstraint& other);
	virtual ~TriangleElementConstraint();

	// accessor
	ScalarType Area() {return m_area;}

	virtual void UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotation_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d);
	virtual void UpdateRotation(const VectorX& x);

	virtual void SetNonInversionCriteria(const NoninversionCriteria nc) {m_noninversion_criteria = nc;}
	virtual void SetEnergyType(const EnergyType et) {m_energy_type = et;}

	ScalarType EvaluateEnergyNoninversionConstraintCore(const VectorX& x);
	void EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_block);
	void EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_block);

	void EvaluateEnergyNoninversionConstraintCore(const VectorX& x, ScalarType& W11, ScalarType& detW);
	void EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_W11_block, VectorX& gradient_detW_block);
	void EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_W11_block, EigenMatrix& Hessian_detW_block);

	ScalarType EvaluateEnergyNoConstraintCore(const VectorX& x, ScalarType multiplier = 1);
	void EvaluateGradientNoConstraintCore(const VectorX& x, VectorX& gradient_block, ScalarType multiplier = 1);
	void EvaluateHessianNoConstraintCore(const VectorX& x, EigenMatrix& hessian_block, ScalarType multiplier = 1);

	virtual ScalarType  EvaluateEnergy(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// local global
	virtual void LocalGlobalLocalStep(const VectorX& x);
	virtual ScalarType EvaluateEnergyLocalGlobal(const VectorX& x, ScalarType multiplier = 1);
	virtual void EvaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1);
	virtual void EvaluateHessianLocalGlobal(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1);

	// penalty method newton
	virtual ScalarType EvaluateEnergyPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientPenaltyMethod(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPenaltyMethod(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPenaltyMethodFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual bool HasZeroPenaltyEnergy(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType));
	// penalty method for parallelization
	virtual void EvaluateEnergyGradientHessianPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType),  ScalarType (*d_psi)(ScalarType, ScalarType),  ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual ScalarType RetrieveEnergy();
	virtual void RetrieveGradient(VectorX& gradient);
	virtual void RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets);

	// Spectral SDP
	virtual ScalarType EvaluateEnergySpectralSDP(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientSpectralSDP(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianSpectralSDP(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientSpectralSDPFD(const VectorX& x, VectorX& gradient, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianSpectralSDPFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	
	virtual void InitLagrangian();
	virtual void UpdateLagrangian(const VectorX& x_old, const VectorX& x_new, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);

	// Pennon
	virtual ScalarType EvaluateEnergyPennon(const VectorX& x, ScalarType(*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientPennon(const VectorX& x, VectorX& gradient, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPennon(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType(*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void UpdateLagrangianPennon(const VectorX& x_old, const VectorX& x_new, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientPennonFD(const VectorX& x, VectorX& gradient, ScalarType(*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPennonFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType(*d_psi)(ScalarType, ScalarType), ScalarType(*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);

	// energy gradient hessian no constraint
	virtual ScalarType EvaluateEnergyNoConstraint(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradientNoConstraint(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessianNoConstraint(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// energy gradient hessian for a gerneral barrier function
	virtual ScalarType EvaluateEnergyGeneralBarrier(const VectorX& x, ScalarType (*psi)(ScalarType), ScalarType multiplier = 1, bool throwable = true, ScalarType regularization_mu = 0);
	virtual void EvaluateGradientGeneralBarrier(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void EvaluateHessianGeneralBarrier(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// primal dual general barrier
	virtual void EvaluateGradientPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPrimalDualGeneralBarrierFD(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1);
		
	// energy gradient hessian for corot model
	virtual ScalarType EvaluateEnergyCorot(const VectorX& x);
	virtual void EvaluateGradientCorot(const VectorX& x, VectorX& gradient);
	virtual void EvaluateHessianCorot(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets);
	virtual void EvaluateHessianCorotLocalGlobalStyle(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets);
	virtual void EvaluateHessianCorotConstant1(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets);
	virtual void EvaluateHessianCorotConstant2(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets);

	// energy gradient hessian for neo-hookean model
	virtual ScalarType EvaluateEnergyNeoHookean(const VectorX& x);
	virtual void EvaluateGradientNeoHookean(const VectorX& x, VectorX& gradient);


	// local step
	virtual bool  CorrectToNoninversion(VectorX& f, unsigned int& index);
	// global step
	virtual void  EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index); // state independent
	virtual void  GetVectorizedExrotationField(VectorX& r, unsigned int& index);
	virtual void  EvaluateVectorizedDeformationGradient(const VectorX& x, VectorX& f);

	// debugging, should not be in the final release mode
	void  EvaluateGradientFiniteDifference(const VectorX& x, VectorX& gradient); // for debugging
	void  EvaluateHessianFiniteDifference(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets); // for debugging
	void  EvaluateHessianLocalGlobalFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1); // for debugging
	void  EvaluateGradientFiniteDifferencePenalty(const VectorX& x, VectorX& gradient); // for debugging
	void  EvaluateHessianFiniteDifferencePenalty(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets); // for debugging
	EigenMatrix2 GetW(const VectorX& x);
	EigenMatrix2 GetF(const VectorX& x);
	unsigned int * VID() {return m_p;}
	// !debugging

protected:
	void getEdgeMatrix(const VectorX& x, EigenMatrix2& D);
	void getDeformationGradient(const VectorX& x, EigenMatrix2& F);
	void getRotationMatrix(const EigenMatrix2& F, EigenMatrix2& R);

public:
	unsigned int m_element_id;

	ScalarType * p_mu; // stiffness
	ScalarType * p_lambda; // stiffness
	bool * p_enable_non_inversion; // stiffness
	ScalarType * p_alpha;
	ScalarType * p_beta;
	unsigned int m_p[3];  // constrained vertex id

	EigenMatrix2 m_Dm; // [x1-x3|x2-x3]
	EigenMatrix2 m_Dm_inv; 
	ScalarType m_area; // 1/2 det(Dm);

	EigenMatrix2 m_ExR; // exrot
	EigenMatrix2 m_R; // corot

	// for Spectral SDP
	EigenMatrix2 m_V; // Lagrangian in Spectral SDP
	// for Pennon
	EigenMatrix2 m_U; // Lagrangian in Pennon

	// local global
	bool m_W_is_SPD;
	EigenMatrix2 m_W_projection;

	// control
	NoninversionCriteria m_noninversion_criteria;
	EnergyType m_energy_type;

	// depricated
	bool * p_enable_modified_barrier; // depricated
	ScalarType * p_slackness; // depricated
};

class TetElementConstraint : public Constraint
{
public:
	TetElementConstraint(unsigned int element_id, ScalarType *mu, ScalarType *lambda, bool *enable_non_inversion, ScalarType *alpha, ScalarType *beta, ScalarType *gamma, ScalarType *slackness, unsigned int p1, unsigned int p2, unsigned int p3, unsigned int p4, const VectorX& x);
	TetElementConstraint(const TetElementConstraint& other);
	virtual ~TetElementConstraint();

	virtual void UpdateHandleInfo(const std::vector<VectorX>& handle_vertices, const std::vector<ScalarType>& per_element_rotation_2d, const std::vector<EigenQuaternion>& per_element_rotation_3d);
	virtual void UpdateRotation(const VectorX& x);

	virtual void SetNonInversionCriteria(const NoninversionCriteria nc) {m_noninversion_criteria = nc;}
	virtual void SetEnergyType(const EnergyType et) {m_energy_type = et;}

	virtual ScalarType  EvaluateEnergy(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradient(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessian(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// local global
	virtual void LocalGlobalLocalStep(const VectorX& x);
	virtual ScalarType EvaluateEnergyLocalGlobal(const VectorX& x, ScalarType multiplier = 1);
	virtual void EvaluateGradientLocalGlobal(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1);
	virtual void EvaluateHessianLocalGlobal(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1);

	// penalty method newton
	virtual ScalarType EvaluateEnergyPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateGradientPenaltyMethod(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPenaltyMethod(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPenaltyMethodFD(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType, ScalarType), ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual bool HasZeroPenaltyEnergy(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType));
	// penalty method for parallelization
	virtual void EvaluateEnergyGradientHessianPenaltyMethod(const VectorX& x, ScalarType (*psi)(ScalarType, ScalarType),  ScalarType (*d_psi)(ScalarType, ScalarType),  ScalarType (*d2_psi)(ScalarType, ScalarType), ScalarType multiplier = 1);
	virtual ScalarType RetrieveEnergy();
	virtual void RetrieveGradient(VectorX& gradient);
	virtual void RetrieveHessian(std::vector<SparseMatrixTriplet>& hessian_triplets);

	// energy gradient hessian no constraint
	virtual ScalarType EvaluateEnergyNoConstraint(const VectorX& x, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateGradientNoConstraint(const VectorX& x, VectorX& gradient, ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void  EvaluateHessianNoConstraint(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	virtual void  EvaluateGMatrix(std::vector<SparseMatrixTriplet>& g_triplets, unsigned int& index); // state independent

	// general barrier methods
	ScalarType EvaluateEnergyNoninversionConstraintCore(const VectorX& x);
	void EvaluateGradientNoninversionConstraintCore(const VectorX& x, VectorX& gradient_block);
	void EvaluateHessianNoninversionConstraintCore(const VectorX& x, EigenMatrix& hessian_block);
	void EvaluateEnergyNoninversionConstraintCore(const VectorX& x, ScalarType& W11, ScalarType& detW22, ScalarType& detW);

	ScalarType EvaluateEnergyNoConstraintCore(const VectorX& x, ScalarType multiplier = 1);
	void EvaluateGradientNoConstraintCore(const VectorX& x, VectorX& gradient_block, ScalarType multiplier = 1);
	void EvaluateHessianNoConstraintCore(const VectorX& x, EigenMatrix& hessian_block, ScalarType multiplier = 1);

	// energy gradient hessian for a gerneral barrier function
	virtual ScalarType EvaluateEnergyGeneralBarrier(const VectorX& x, ScalarType (*psi)(ScalarType), ScalarType multiplier = 1, bool throwable = true, ScalarType regularization_mu = 0);
	virtual void EvaluateGradientGeneralBarrier(const VectorX& x, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0);
	virtual void EvaluateHessianGeneralBarrier(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1, ScalarType regularization_mu = 0);

	// primal dual general barrier
	virtual void EvaluateGradientPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, VectorX& gradient, ScalarType (*d_psi)(ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPrimalDualGeneralBarrier(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1);
	virtual void EvaluateHessianPrimalDualGeneralBarrierFD(const VectorX& x, const VectorX& lambda, std::vector<SparseMatrixTriplet>& hessian_triplets, ScalarType (*d_psi)(ScalarType), ScalarType (*d2_psi)(ScalarType), ScalarType multiplier = 1);

	// debugging, should not be in the final release mode
	void  EvaluateGradientFiniteDifference(const VectorX& x, VectorX& gradient); // for debugging
	void  EvaluateHessianFiniteDifference(const VectorX& x, std::vector<SparseMatrixTriplet>& hessian_triplets); // for debugging
	EigenMatrix3 GetW(const VectorX& x);
	EigenMatrix3 GetF(const VectorX& x);
	unsigned int * VID() {return m_p;}
	// !debugging

protected:
	void getEdgeMatrix(const VectorX& x, EigenMatrix3& D);
	void getDeformationGradient(const VectorX& x, EigenMatrix3& F);

public:
	unsigned int m_element_id;

	ScalarType * p_mu; // stiffness
	ScalarType * p_lambda; // stiffness
	bool * p_enable_non_inversion; // stiffness
	ScalarType * p_alpha;
	ScalarType * p_beta;
	ScalarType * p_gamma;
	unsigned int m_p[4];  // constrained vertex id

	EigenMatrix3 m_Dm; // [x1-x4|x2-x4|x3-x4]
	EigenMatrix3 m_Dm_inv; 
	ScalarType m_volume; // 1/6 det(Dm);

	EigenMatrix3 m_ExR;
	EigenMatrix3 m_R;
	
	// local global
	bool m_W_is_SPD;
	EigenMatrix3 m_W_projection;

	// control
	NoninversionCriteria m_noninversion_criteria;
	EnergyType m_energy_type;
	
	// depricated
	ScalarType * p_slackness; // depricated
};

#endif