#include "precompile.h"
#include "simulation.h"
#include <iostream>
#include <fstream>

#define MOSEK_BBW

#ifdef MOSEK_BBW
#include "igl/bbw/bbw.h"
#endif

extern ScalarType test1;
extern ScalarType test2;
extern ScalarType test3;
extern ScalarType test4;
extern ScalarType test5;

// protected functions (utils)
void Sim::convertVerticesToBBWFormat(EigenMatrix& V)
{
	unsigned int dim = (m_mode == MODE_2D)? 2 : 3;
	V.resize(m_x.size()/dim, 2);
	for (unsigned int i = 0; i != V.rows(); ++i)
	{
		for (unsigned int j = 0; j != dim; ++j)
		{
			V(i, j) = m_x(dim*i + j);
		}
	}
}

void Sim::convertElementListToBBWFormat(Eigen::MatrixXi& T)
{
	if (m_mode == MODE_2D)
	{
		ObjMesh* mesh = dynamic_cast<ObjMesh*>(m_mesh);
		assert(mesh != NULL);

		T.resize(mesh->m_faces.size(), 3);
		unsigned int i = 0;
		for (std::vector<Mesh::Face>::iterator it = mesh->m_faces.begin(); it != mesh->m_faces.end(); ++it, ++i)
		{
			T(i, 0) = it->id1;
			T(i, 1) = it->id2;
			T(i, 2) = it->id3;
		}
	}
	else
	{
		MeshMesh* mesh = dynamic_cast<MeshMesh*>(m_mesh);
		assert(mesh != NULL);

		T.resize(mesh->m_tets.size(), 3);
		unsigned int i = 0;
		for (std::vector<Mesh::Tet>::iterator it = mesh->m_tets.begin(); it != mesh->m_tets.end(); ++it, ++i)
		{
			T(i, 0) = it->id1;
			T(i, 1) = it->id2;
			T(i, 2) = it->id3;
			T(i, 3) = it->id4;
		}
	}
}

bool Sim::convertHandlesToBBWFormat(Eigen::VectorXi& b, EigenMatrix& bc)
{
	unsigned int handle_num = m_handles.size();
	if (handle_num == 0)
	{
		return false; // no handle
	}
	unsigned int handle_vertices_num = 0;
	for (std::vector<Handle>::iterator it = m_handles.begin(); it != m_handles.end(); ++it)
	{
		handle_vertices_num += it->Indices().size();
	}

	b.resize(handle_vertices_num);
	bc.resize(handle_vertices_num, handle_num);
	bc.setZero();

	unsigned int ind = 0;
	unsigned int handle_ind = 0;
	for (std::vector<Handle>::iterator it = m_handles.begin(); it != m_handles.end(); ++it, ++handle_ind)
	{
		for (unsigned int i = 0; i < it->Indices().size(); ++i, ++ind)
		{
			b(ind) = it->Indices()[i];
			bc(ind, handle_ind) = 1;
		}
	}

	return true;
}

// !protected functions

void Sim::Reset(Mesh* mesh, bool mode)
{
	m_mesh = mesh;
	m_mode = mode;

	m_vertices_number = mesh->m_vertices.size();
	m_system_dimension = m_vertices_number * ((mode==MODE_2D)?2:3);
	m_mesh->WriteToSimulation(m_original_x);
	m_mesh->WriteToSimulation(m_x);

	if(!m_handles.empty())
		m_handles.clear();
	m_handle_id.resize(m_vertices_number);
	for (unsigned int i = 0; i != m_handle_id.size(); ++i)
	{
		m_handle_id[i] = -1;
	}
	m_selected_handle_id = -1;

	m_weights = EigenMatrix();
	m_element_weights = EigenMatrix();
	m_need_update = true;

	m_keyframe_handle_id = -1;
	m_keyframe_handle_unit_translation = 0;
	m_keyframe_handle_unit_rotation = 0;

	m_solver.Reset();
}
void Sim::Cleanup()
{
	m_handles.clear();
	m_handle_id.clear();
}

void Sim::UpdateLBS()
{
	if (m_mode == MODE_2D)
	{
		EigenMatrix3 T;
		for (unsigned int i = 0; i < m_vertices_number; ++i)
		{
			T.setZero();
			unsigned int j = 0;
			for (std::vector<Handle>::iterator it = m_handles.begin(); it != m_handles.end(); ++it, ++j)
			{
				T += m_weights(i,j) * it->GetTransformationMatrix();
			}

			EigenVector3 v = m_original_x.block<1, 3>(i, 0);
			v = T*v;
			m_x(i*2+0) = v(0);
			m_x(i*2+1) = v(1);
		}
	}
}

void Sim::Update()
{
	if (m_weights.cols() != m_handles.size() || (!m_handles.empty() && m_weights.rows() != m_vertices_number))
	{
		std::cerr << "Compute/Load Skin Weights before Skinning." << std::endl;
		return;
	}

	try
	{
		if (m_need_update)
		{
			if (m_solver.m_optimization_method == OPTIMIZATION_LBS)
			{
				UpdateLBS();
				m_need_update = false;
			}
			else
			{
				m_need_update = !m_solver.UpdateOneIteration(m_x, m_dual_lambda, m_last_iteration_progress);
			}
		}
	}
	catch(MyExceptions)
	{
		std::cout << "infeasible starting point in ExRot method." << std::endl;
	}
}

void Sim::UpdateMatlabSDPSolveYalip(bool has_max_it)
{
	m_solver.UpdateMatlabMosekYalmip(m_x, has_max_it);
}

void Sim::UpdateMatlabSDPSolveEpigraph()
{
	m_solver.UpdateMatlabMosekEpigraph(m_x);
}

void Sim::Draw(const VBO& vbos) // draw handles
{
	for (std::vector<Handle>::iterator it = m_handles.begin(); it != m_handles.end(); ++it)
	{
		it->Draw(vbos, (it->ID()==m_selected_handle_id));
	}
}

void Sim::GetOverlayChar(char* overlay_char)
{
}

void Sim::SaveSimulation()
{
	SaveHandles();
	SaveSkinWeights();
	SaveExrotationField();
	SaveTargetPoints();
	SavePerElementWeight();

	std::ofstream outfile;
	outfile.open(DEFAULT_CONFIG_VERTICES_FILE, std::ifstream::out);
	if (outfile.is_open())
	{
		// TODO: change it to memory dump.
		outfile << m_x.size() << std::endl << std::endl;

		for (unsigned int i = 0; i != m_x.size(); ++i)
		{
			outfile << m_x(i) << std::endl;
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write vertices file. Settings not saved." << std::endl; 
	}	
}
void Sim::LoadSimulation()
{
	LoadHandles();
	LoadSkinWeights();
	m_solver.ResetFactorizationFlag();

	bool successfulRead = false;

	//read file
	std::ifstream infile;
	infile.open(DEFAULT_CONFIG_VERTICES_FILE, std::ifstream::in);
	if (successfulRead = infile.is_open())
	{
		unsigned int size;
		infile >> size;

		if (size != m_x.size())
		{
			successfulRead = false;
		}
		else
		{
			for (unsigned int i = 0; i != size; ++i)
			{
				infile >> m_x(i);
			}
		}

		infile.close();
	}
	if (!successfulRead)
	{
		std::cerr << "Waning: failed loading skin weights." << std::endl;
	}
}

void Sim::NewHandle(const std::vector<unsigned int>& indices, const glm::vec3 color)
{
	// check if any points has already been assigned as handle
	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		if (m_handle_id[indices[i]] >= 0)
		{
			std::cerr << "Some of the vertices in the selection has already been assigned as handle. Please select again." << std::endl;
			return;
		}
	}

	// new handle
	unsigned int dim = (m_mode == MODE_2D)? 2 : 3;
	VectorX vertices(dim*indices.size());
	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		vertices.block(i*dim, 0, dim, 1) = m_x.block(indices[i]*dim, 0, dim, 1);
	}

	int id = m_handles.size();
	m_handles.push_back(Handle(indices, vertices, m_mode, color, id));
	// update the handle id list
	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		m_handle_id[indices[i]] = id;
	}
	//m_configuration_changed = false;

	m_solver.SetPrefactorizationFlag();
}

void Sim::DeleteHandle()
{
	if (m_selected_handle_id >= 0)
	{
		// delete handle
		m_handles.erase(m_handles.begin() + m_selected_handle_id);

		// reconstruct the id of the handles
		for (unsigned int i = m_selected_handle_id; i<m_handles.size(); ++i)
		{
			m_handles[i].ID() --;
		}

		// reconstruct the handle id list
		for (unsigned int i = 0; i != m_handle_id.size(); ++i)
		{
			if (m_handle_id[i] == m_selected_handle_id)
			{
				m_handle_id[i] = -1; // unselected
			}
			else if (m_handle_id[i] > m_selected_handle_id)
			{
				m_handle_id[i] --;
			}
		}
		m_selected_handle_id = -1;
	}
	//m_configuration_changed = false;

	m_solver.SetPrefactorizationFlag();
}

bool Sim::SelectHandle(std::vector<glm::vec3> ray)
{
	unsigned int min_bb_handle_id = -1;
	ScalarType min_bb = 1e10;

	for (unsigned int i = 0; i != m_handles.size(); ++i)
	{
		if (m_handles[i].Select(ray))
		{
			if (min_bb > m_handles[i].BoundingBoxSize())
			{
				min_bb = m_handles[i].BoundingBoxSize();
				min_bb_handle_id = i;
			}
		}
	}
	m_selected_handle_id = min_bb_handle_id;
	return (m_selected_handle_id==-1) ? false : true;
}

void Sim::MoveHandleTemporary(const glm::vec3& trans)
{
	if (m_selected_handle_id >= 0)
	{
		m_handles[m_selected_handle_id].MoveTemporary(trans);
		//UpdateHandleInfo();
	}
}
void Sim::MoveHandleFinalize()
{
	if (m_selected_handle_id >= 0)
	{
		m_handles[m_selected_handle_id].MoveFinalize();

		UpdateHandleInfo();		
	}
}
void Sim::RotateHandleToValue()
{
	if (m_selected_handle_id >= 0)
	{
		ScalarType theta;
		std::cout << "Assign rotation(degree) for handle [" << m_selected_handle_id << "]." << std::endl;
		std::cin >> theta;
		theta = theta / 180.0 * 3.1415926;
		m_handles[m_selected_handle_id].RotateToValue(theta);
		UpdateHandleInfo();
	}
}
void Sim::TranslateHandleSetStepSize()
{
	if (m_selected_handle_id >= 0)
	{
		m_keyframe_handle_id = m_selected_handle_id;
		std::cout << "Assign translation_x for handle [" << m_selected_handle_id << "]." << std::endl;
		std::cin >> m_keyframe_handle_unit_translation;
	}
}
void Sim::RotateHandleSetStepSize()
{
	if (m_selected_handle_id >= 0)
	{
		m_keyframe_handle_id = m_selected_handle_id;
		std::cout << "Assign rotation(degree) for handle [" << m_selected_handle_id << "]." << std::endl;
		std::cin >> m_keyframe_handle_unit_rotation;
		m_keyframe_handle_unit_rotation = m_keyframe_handle_unit_rotation / 180.0 * 3.1415926;
	}
}
void Sim::AnimateHandle()
{
	m_handles[m_keyframe_handle_id].ChangeTranslationOnXAxis(m_keyframe_handle_unit_translation);
	m_handles[m_keyframe_handle_id].RotateExtra(m_keyframe_handle_unit_rotation);
	std::cout << "handle [" << m_keyframe_handle_id << "] Rotation: " << m_handles[m_keyframe_handle_id].RotationAngle2d()*180.0/3.1415926 << std::endl;
	UpdateHandleInfo();
}
void Sim::RotateHandleTemporary(const glm::vec3& axis, const float& theta)
{
	if (m_selected_handle_id >= 0)
	{
		m_handles[m_selected_handle_id].RotateTemporary(axis, theta);
		//UpdateHandleInfo();
	}
}
void Sim::RotateHandleFinalize()
{
	if (m_selected_handle_id >= 0)
	{
		m_handles[m_selected_handle_id].RotateFinalize();

		UpdateHandleInfo();		
	}
}

//void Sim::CombineVertexClassificationRegions(std::vector<RegionClassification>& rc)
//{
//	assert(m_vertices_classification.size() == rc.size());
//	for (unsigned int i = 0; i != m_vertices_classification.size(); ++i)
//	{
//		RegionClassification c1 = m_vertices_classification[i];
//		RegionClassification c2 = rc[i];
//
//		if (c1 != c2)
//		{
//			if (c1 == RC_NONLINEAR || c2 == RC_NONLINEAR)
//			{
//				m_vertices_classification[i] = RC_NONLINEAR;
//				continue;
//			}
//			else
//			{
//				m_vertices_classification[i] = RC_INTERFACE;
//				continue;
//			}
//		}
//	}
//}

glm::vec3 Sim::SelectedHandleLocalCoM()
{
	assert(m_selected_handle_id >= 0);
	assert(m_selected_handle_id < m_handles.size());

	return m_handles[m_selected_handle_id].GetLocalCoM();
}

glm::vec3 Sim::SelectedHandleCoM()
{
	assert(m_selected_handle_id >= 0);
	assert(m_selected_handle_id < m_handles.size());

	return m_handles[m_selected_handle_id].GetCoM();
}

void Sim::SaveHandles()
{
	unsigned int handle_num = m_handles.size();

	//const std::vector<unsigned int>& indices, const glm::vec3 color;
	std::ofstream outfile;
	outfile.open(DEFAULT_CONFIG_HANDLES_FILE, std::ifstream::out);
	if (outfile.is_open())
	{
		// TODO: change it to memory dump.
		outfile << "HandleNum   " << m_handles.size() << std::endl << std::endl;

		for (unsigned int i = 0; i != m_handles.size(); ++i)
		{
			outfile << "Color       "
				<< m_handles[i].Color().x << " "
				<< m_handles[i].Color().y << " "
				<< m_handles[i].Color().z << std::endl;
			outfile << "Rotation2D  " << m_handles[i].RotationAngle2d() << std::endl;
			outfile << "RotationCenter "
				<< m_handles[i].CoM()[0] << " "
				<< m_handles[i].CoM()[1];
			if (m_mode == MODE_3D)
			{
				outfile << " " << m_handles[i].CoM()[2];
			}
			outfile << std::endl;
			outfile << "Translation " 
				<< m_handles[i].Translation()[0] << " "
				<< m_handles[i].Translation()[1];
			if (m_mode == MODE_3D)
			{
				outfile << " " << m_handles[i].Translation()[2];
			}
			outfile << std::endl;
			outfile << "VerticesNum " << m_handles[i].Indices().size() << std::endl;
			for (unsigned int j = 0; j != m_handles[i].Indices().size(); ++j)
			{
				outfile << m_handles[i].Indices()[j] << " ";
			}
			outfile << std::endl << std::endl;
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write handle file. Settings not saved." << std::endl; 
	}	
}

void Sim::LoadHandles()
{
	m_handles.clear();
	m_handle_id.clear();

	bool successfulRead = false;

	//read file
	std::ifstream infile;
	infile.open(DEFAULT_CONFIG_HANDLES_FILE, std::ifstream::in);
	if (successfulRead = infile.is_open())
	{
		char ignoreToken[256];

		unsigned int handle_num;
		infile >> ignoreToken >> handle_num;

		unsigned int handle_size;
		glm::vec3 color;
		std::vector<unsigned int> ids;

		unsigned int dim = (m_mode == MODE_2D)? 2 : 3;

		VectorX translation;
		VectorX com;
		ScalarType rotation2d;
		translation.resize(dim);
		com.resize(dim);
		for (unsigned int hi = 0; hi != handle_num; ++hi)
		{
			infile >> ignoreToken >> color[0] >> color[1] >> color[2];
			infile >> ignoreToken >> rotation2d;
			infile >> ignoreToken >> com[0] >> com[1];
			if (m_mode == MODE_3D)
			{
				infile >> com[2];
			}
			infile >> ignoreToken >> translation[0] >> translation[1];
			if (m_mode == MODE_3D)
			{
				infile >> translation[2];
			}
			infile >> ignoreToken >> handle_size;
			ids.resize(handle_size);
			for (unsigned int j = 0; j != handle_size; ++j)
			{
				infile >> ids[j];
			}
			VectorX vertices(dim*ids.size());
			for (unsigned int i = 0; i != ids.size(); ++i)
			{
				vertices.block(i*dim, 0, dim, 1) = m_original_x.block(ids[i], 0, 1, dim).transpose();
			}
			Handle h(ids, vertices, m_mode, color, hi);
			h.RotationAngle2d() = rotation2d;
			h.Translation() = translation;
			h.CoM() = com;
			m_handles.push_back(h);
		}
	}
	if (!successfulRead)
	{
		std::cerr << "Waning: failed loading handles." << std::endl;
	}

	m_solver.SetPrefactorizationFlag();
}

void Sim::UpdateHandleInfo()
{
	// update handle info
	m_solver.PrepareHandleInfo(m_x, m_handles, m_element_weights);

	switch (m_solver.m_optimization_method)
	{
	case OPTIMIZATION_LOCAL_GLOBAL:
	case OPTIMIZATION_PENALTY_LOCAL_GLOBAL_BLEND:
	case OPTIMIZATION_PENALTY:
	case OPTIMIZATION_PENALTY_ON_DETF:
	case OPTIMIZATION_SPECTRAL_SDP:
	case OPTIMIZATION_PENNON:
	case OPTIMIZATION_NONINVERSION_CONSTANT_HESSIAN:
		m_solver.ResetInitialRescalingParameter(1);
		break;
	case OPTIMIZATION_BARRIER_METHOD:
	case OPTIMIZATION_NEWTON_PRIMAL_DUAL:
		m_solver.UpdateRotation(m_x);
		m_solver.SetNoninversionCriteria();
		m_solver.ResetInitialRescalingParameter();
		m_solver.SetInitialLambda(m_x, m_dual_lambda);
		//m_dual_lambda_initial_guess = test5;
		//m_dual_lambda.fill(m_dual_lambda_initial_guess);
		break;
	case OPTIMIZATION_LOCALLY_INJECTIVE_MAPPING:
		m_solver.m_lim_alpha_update_last_alpha = 10;
		break;
	case OPTIMIZATION_ALTERNATIVE_PROJECTION:
	case OPTIMIZATION_ONE_TANGENT_PLANE:
	{

		bool temp = m_solver.m_enable_non_inversion;
		m_solver.m_enable_non_inversion = false;
		ScalarType progress;
		m_solver.UpdateOneIteration(m_x, m_dual_lambda, progress, OPTIMIZATION_BARRIER_METHOD);
		m_solver.m_enable_non_inversion = temp;
		m_solver.m_last_x = m_x;
	}
	break;

	}
	m_need_update = true;
}

void Sim::ComputeSkinWeights()
{
#ifdef MOSEK_BBW
	EigenMatrix V; // nx3 matrix
	Eigen::MatrixXi T;
	Eigen::VectorXi b;
	EigenMatrix bc;

	convertVerticesToBBWFormat(V);
	convertElementListToBBWFormat(T);
	if(!convertHandlesToBBWFormat(b, bc))
	{
		std::cerr << "Please set at least one handle." << std::endl;
		return;
	}

	igl::BBWData bbw_data;
	//bbw_data.active_set_params.max_iter = 200;
	if(!igl::bbw(V,T,b,bc,bbw_data,m_weights))
	{
		std::cerr << "BBW Failed!" << std::endl;
		return;
	}

	igl::normalize_row_sums(m_weights, m_weights);

	VertexWeightToElementWeight();
#endif
}

void Sim::VertexWeightToElementWeight()
{
	m_element_weights.resize(m_mesh->GetNumebrOfElements(), m_weights.cols());
	for (unsigned int col = 0; col != m_weights.cols(); ++col)
	{
		for (unsigned int row = 0; row != m_mesh->GetNumebrOfElements(); ++row)
		{
			std::vector<unsigned int>& ids = m_mesh->GetElementId(row);
			m_element_weights(row, col) = 0;
			for (unsigned int i = 0; i != ids.size(); ++i)
			{
				m_element_weights(row, col) += m_weights(ids[i], col);
			}
			m_element_weights(row, col) /= ids.size();
		}
	}
}

void Sim::ShowSkinWeights(std::vector<glm::vec3>& color)
{
	if (m_selected_handle_id < 0 || color.size() != m_weights.rows())
	{
		for (unsigned int i = 0; i < color.size(); ++i)
		{
			color[i] = glm::vec3(0.7f, 0.7f, 0.7f);
		}
	}
	else
	{
		glm::vec3 white(1.0f, 1.0f, 1.0f);
		glm::vec3 red(1.0f, 0.0f, 0.0f);
		for (unsigned int i = 0; i < color.size(); ++i)
		{
			float t = m_weights(i, m_selected_handle_id);
			color[i] = red * t + white * (1-t);
		}
	}
}

void Sim::SaveSkinWeights()
{
	std::ofstream outfile;
	outfile.open(DEFAULT_CONFIG_WEIGHTS_FILE, std::ifstream::out);
	if (outfile.is_open())
	{
		// TODO: change it to memory dump.
		outfile << m_weights.rows() << " " << m_weights.cols() << std::endl << std::endl;

		for (unsigned int i = 0; i != m_weights.rows(); ++i)
		{
			for (unsigned int j = 0; j != m_weights.cols(); ++j)
			{
				outfile << m_weights(i, j) << " ";
			}
			outfile << std::endl;
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write skin weights file. Settings not saved." << std::endl; 
	}	
}

void Sim::LoadSkinWeights()
{
	bool successfulRead = false;

	//read file
	std::ifstream infile;
	infile.open(DEFAULT_CONFIG_WEIGHTS_FILE, std::ifstream::in);
	if (successfulRead = infile.is_open())
	{
		unsigned int rows, cols;
		infile >> rows >> cols;

		if (rows != m_vertices_number || cols != m_handles.size())
		{
			successfulRead = false;
		}
		else
		{
			m_weights.resize(rows, cols);
			for (unsigned int i = 0; i != m_weights.rows(); ++i)
			{
				for (unsigned int j = 0; j != m_weights.cols(); ++j)
				{
					infile >> m_weights(i, j);
				}
			}
		}

		VertexWeightToElementWeight();

		infile.close();
	}
	if (!successfulRead)
	{
		std::cerr << "Waning: failed loading skin weights." << std::endl;
	}
}
void Sim::SaveTargetPoints()
{
	std::ofstream outfile;
	outfile.open(DEFAULT_TARGET_CONSTRAINT, std::ifstream::out);
	if (outfile.is_open())
	{
		std::vector<Constraint*>& cs = this->m_solver.m_constraints;

		AttachmentConstraint* ac;
		for (unsigned int i = 0; i!= cs.size(); ++i)
		{
			if (cs[i]->Type() == CONSTRAINT_TYPE_ATTACHMENT)
			{
				ac = dynamic_cast<AttachmentConstraint*>(cs[i]);
				outfile << ac->m_p0 << std::endl;
				outfile << ac->m_fixed_point[0] << " " << ac->m_fixed_point[1] << std::endl << std::endl;
			}
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write target file. Settings not saved." << std::endl; 
	}	

}
void Sim::SaveExrotationField()
{
	std::ofstream outfile;
	outfile.open(DEFAULT_EXROTATION_FILE, std::ifstream::out);
	if (outfile.is_open())
	{
		std::vector<Constraint*>& cs = this->m_solver.m_constraints;

		TriangleElementConstraint* tric;
		for (unsigned int i = 0; i!= cs.size(); ++i)
		{
			if (cs[i]->Type() == CONSTRAINT_TYPE_TRIANGLE_ELEMENT)
			{
				tric = dynamic_cast<TriangleElementConstraint*>(cs[i]);
				outfile << tric->m_element_id << std::endl;
				outfile << tric->m_ExR(0,0) << " " << tric->m_ExR(0,1) << std::endl 
					<< tric->m_ExR(1,0) << " " << tric->m_ExR(1,1) << std::endl << std::endl;
			}
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write target file. Settings not saved." << std::endl; 
	}	
}
void Sim::SavePerElementWeight()
{
	std::ofstream outfile;
	outfile.open(DEFAULT_CONFIG_PER_ELEMENT_WEIGHTS_FILE, std::ifstream::out);
	if (outfile.is_open())
	{
		// TODO: change it to memory dump.
		outfile << m_element_weights.rows() << " " << m_element_weights.cols() << std::endl << std::endl;

		for (unsigned int i = 0; i != m_element_weights.rows(); ++i)
		{
			for (unsigned int j = 0; j != m_element_weights.cols(); ++j)
			{
				outfile << m_element_weights(i, j) << " ";
			}
			outfile << std::endl;
		}

		outfile.close();
	}
	else
	{
		std::cerr << "Warning: Can not write skin weights file. Settings not saved." << std::endl; 
	}	
}

void Sim::InitSolver()
{
	Eigen::MatrixXi T;
	convertElementListToBBWFormat(T);

	int dim = (m_mode == MODE_2D) ? 2 : 3; 

	VectorX x(m_x.size());
	for (unsigned int i = 0; i != m_original_x.rows(); ++i)
	{
		x.block(i*dim, 0, dim, 1) = m_original_x.block(i, 0, 1, dim).transpose();
	}

	m_solver.Init(x, T, m_handles, m_mode);
	m_dual_lambda.resize(T.rows());
	m_dual_lambda.fill(1);
}

void Sim::OutputDetFFromSolver(std::vector<float>& detf_list)
{
	m_solver.GetDetF(m_x, detf_list);
}
void Sim::OutputW11FromSolver(std::vector<float>& w11_list)
{
	m_solver.GetW11(m_x, w11_list);
}
void Sim::OutputDetWFromSolver(std::vector<float>& detw_list)
{
	m_solver.GetDetW(m_x, detw_list);
}
ScalarType Sim::OutputMinDetWFromSolver()
{
	return m_solver.GetSmallestDetW(m_x);
}