#pragma once

#include <stdlib.h>
#include <fstream>
#include <vector>
#include <string>
#include <iomanip>
#include "mesh_struct.h"
#include "mesh_utils.h"

//----------------------------FILE-IO------------------------------//
//----------------------------FILE-IO------------------------------//
//write and load functions

/*
 * method which loads a mesh from a .msh file
 * Creates a mesh with all informations
*/
Mesh* loadFile_msh(char* filename)
{
	Mesh *m = (Mesh*)malloc(sizeof(Mesh));
	Node *nodes = (Node*)malloc(sizeof(Node));
	Face *faces = (Face*)malloc(sizeof(Face));
	Cell *cells = (Cell*)malloc(sizeof(Cell));

	int MINUS_ONE_DIM = 536870912;
	//int MINUS_TWO_DIM = 1073741824;
	//int MINUS_THREE_DIM = 2147483648;

	std::ifstream _if(filename);

	if (!_if.is_open())
	{
		printf("error opening file: %s", filename);
		return nullptr;
	}
	std::printf("\nThe file is indeed open\n");

	int _dim = 0, filesize, pos = 0, num_nodes, num_elements, e_code;
	char *ptr, *file_string;

	//initialize file reading
	_if.seekg(-1, std::ios_base::end);
	filesize = (int)_if.tellg();
	_if.seekg(0);
	file_string = (char *)malloc(filesize + 2);
	//read file -> string
	while (!_if.eof())
	{
		file_string[pos] = (char)_if.get();
		pos++;
	}
	// the file is now in the string
	std::string aux_str(file_string);
	ptr = file_string;
	// get the info on the nodes
	//ptr = strstr( ptr,"$Nodes")+6;
	size_t str_pos = aux_str.find("$Nodes");
	ptr = file_string + 6 + str_pos;
	num_nodes = (int)strtod(ptr, &ptr);
	//store the info in a vector of nodes
	int v_act = 0;
	std::vector<int> breast_node_ids;
	std::vector<int> breast_nodes_code;
	std::vector< std::vector<int> > breast_nodes_faces;
	std::vector< std::vector<int> > breast_nodes_cells;
	std::vector< std::vector<double> > breast_node_coords;
	breast_node_coords.resize(num_nodes);
	breast_nodes_code.resize(num_nodes);
	for (int i = 0; i < num_nodes; i++) { breast_node_coords[i].resize(3); }
	for (int i = 0; i<num_nodes; i++)
	{
		v_act = (int)strtod(ptr, &ptr);
		breast_node_ids.push_back(v_act);
		breast_node_coords[i][0] = strtod(ptr, &ptr);
		breast_node_coords[i][1] = strtod(ptr, &ptr);
		breast_node_coords[i][2] = strtod(ptr, &ptr);
		//printf("\ni:%d vertex %d  v_act:%d -> aux_vec:%d,%d,%d", i, breast_node_ids[i], v_act, breast_node_coords[i][0], breast_node_coords[i][1], breast_node_coords[i][2]);
	}
	//read info on the elements
	//ptr = file_string;
	str_pos = aux_str.find("$Elements");
	ptr = file_string + 9 + str_pos;
	num_elements = (int)strtod(ptr, &ptr);
	std::vector<int> e_label;
	std::vector<int> e_type_element, e_nb_node, e_code_physical, e_code_elementary, e_node, e_dim;
	std::vector< std::vector<int> > e_nodes;

	e_label.resize(num_elements);
	e_type_element.resize(num_elements);
	e_nb_node.resize(num_elements);
	e_code_physical.resize(num_elements);
	e_code_elementary.resize(num_elements);
	e_dim.resize(num_elements);
	e_nodes.resize(num_elements);
	for (int i = 0; i<num_elements; i++)
	{
		e_label[i] = (int)strtod(ptr, &ptr);
		e_type_element[i] = (int)strtod(ptr, &ptr);
		e_code = (int)strtod(ptr, &ptr);
		if ((e_code < 1)) { printf("invalid number of code in msh file"); return nullptr; }
		if (e_code == 1) e_code_physical[i] = 0;
		else e_code_physical[i] = (int)strtod(ptr, &ptr);
		e_code_elementary[i] = (int)strtod(ptr, &ptr);
		if (e_code >2) // eliminate the other tag if any
		{
			for (int j = 0; j< (e_code - 2); j++)  pos = (int)strtod(ptr, &ptr);
		}
		switch (e_type_element[i])
		{
		case 15:   // node
			e_nb_node[i] = 1;
			e_dim[i] = 0;
			break;
		case 1:  // line
				 //case 8:
			e_nb_node[i] = 2;
			e_dim[i] = 1;
			break;
		case 2:  //triangle
				 //case 9:    
				 //printf("\nYEY a face\n");
			e_nb_node[i] = 3;
			e_dim[i] = 2;
			break;
		case 4:  //tetrahedron
				 //case 11:
			e_nb_node[i] = 4;
			e_dim[i] = 3;
			break;
		default:
			printf("error code element in file msh");
			return nullptr;
			break;
		}
		if (_dim < e_dim[i]) _dim = e_dim[i];
		e_nodes[i].resize(e_nb_node[i]);
		//printf("\ne_nodes\n");
		for (int j = 0; j<e_nb_node[i]; j++) {
			e_nodes[i][j] = (int)strtod(ptr, &ptr);
			//printf("%d -", e_nodes[i][j]);
		}
		//printf("\n\n");
	}
	//printf("\nReading process complete\n");
	free(file_string);

	//convert the elements into a temporary structure
	//elements with e_dim=1 -> nodes neighbours
	//elements with e_dim=2 -> faces
	//elements with e_dim=3 -> cells
	//organizational stage

	int pos_dim3 = 0, id_edge = 0, id_face = 0, id_cell = 0;
	std::vector<int> edge_act; edge_act.resize(2);
	std::vector<int> face_act, face_edge_act; face_act.resize(3); face_edge_act.resize(3);
	std::vector<int> cell_act; cell_act.resize(4);
	std::vector<int> cell_face_act; cell_face_act.resize(4);
	bool exist;
	//temporary store structures
	std::vector<int> edge_label;
	std::vector< std::vector<int> > edge_vertexes; //2 vertexes
	std::vector<int> edge_code;

	std::vector<int> face_label;
	//vector< vector<int> > face_edges; //3 vertexes
	std::vector< std::vector<int> > face_vertexes; //3 vertexes
	std::vector< std::vector<int> > face_edges; //3 edges
	std::vector< std::vector<int> > face_cells; //3 vertexes
	std::vector<int> face_code;

	std::vector<int> cell_label;
	std::vector< std::vector<int> > cell_vertexes; //4 vertexes
	std::vector< std::vector<int> > cell_faces;    //4 faces
	std::vector<int> cell_code;

	pos_dim3 = 0;
	for (int i = 0; i<num_elements; i++)
	{
		//point
		if (e_type_element[i] == 15)
		{
			//printf("\npoint\n");
			pos_dim3++;
			e_label[i] = breast_node_ids[e_nodes[i][0] - 1];// label is no longer GMElement label but  give 
															// the label of the vertex in the _vertex table
		}
		//edges
		if (e_type_element[i] == 1)
		{
			//printf("\nedge\n");  
			pos_dim3++;
			edge_act.resize(2);
			//edge_act[0] = (breast_node_ids[e_nodes[i][0] - 1]);
			//edge_act[1] = (breast_node_ids[e_nodes[i][1] - 1]);
			edge_act[0] = e_nodes[i][0];
			edge_act[1] = e_nodes[i][1];
			exist = false;
			for (int j = 0; j<id_edge; j++)// look if the edge still exist
			{
				bool is_equal1 = false, is_equal2 = false;
				if (edge_act[0] == edge_vertexes[j][0]) is_equal1 = true;
				if (edge_act[0] == edge_vertexes[j][1]) is_equal1 = true;
				if (edge_act[1] == edge_vertexes[j][0]) is_equal2 = true;
				if (edge_act[1] == edge_vertexes[j][1]) is_equal2 = true;
				if (is_equal1 && is_equal2) {
					exist = true; e_label[i] = j + 1;
				}
			}
			if (!exist)
			{
				e_label[i] = id_edge + 1;
				edge_label.push_back(id_edge + 1);
				edge_vertexes.push_back(edge_act);
				edge_code.push_back(0);
				id_edge++;
			}
		}
		//faces        
		if ((e_type_element[i] == 2))  // treat   a triangle
		{
			//printf("\nface\n");
			pos_dim3++;
			face_act.resize(3);
			//face_act[0] = (breast_node_ids[e_nodes[i][0] - 1]);
			//face_act[1] = (breast_node_ids[e_nodes[i][1] - 1]);
			//face_act[2] = (breast_node_ids[e_nodes[i][2] - 1]);
			face_act[0] = e_nodes[i][0];
			face_act[1] = e_nodes[i][1];
			face_act[2] = e_nodes[i][2];
			for (int j = 0; j <= 2; j++)
			{
				edge_act.resize(2);
				//edge_act[0] = (breast_node_ids[face_act[j] - 1]);
				//edge_act[1] = (breast_node_ids[face_act[(j+1)%3] - 1]);
				edge_act[0] = face_act[j];
				edge_act[1] = face_act[(j + 1) % 3];
				exist = false;
				int tam = (int)edge_label.size();
				for (int k = 0; k < tam; k++)
				{
					bool is_equal1 = false, is_equal2 = false;
					if (edge_act[0] == edge_vertexes[k][0]) is_equal1 = true;
					if (edge_act[0] == edge_vertexes[k][1]) is_equal1 = true;
					if (edge_act[1] == edge_vertexes[k][0]) is_equal2 = true;
					if (edge_act[1] == edge_vertexes[k][1]) is_equal2 = true;
					if (is_equal1 && is_equal2) { exist = true; face_edge_act[j] = k + 1; }
				}
				if (!exist)
				{
					edge_label.push_back(id_edge + 1);
					edge_vertexes.push_back(edge_act);
					edge_code.push_back(0);
					face_edge_act[j] = id_edge + 1;
					id_edge++;
				}
			}
			//bool is_equal1 = false, is_equal2 = false, is_equal3 = false;
			// at that stage we have all the edge 
			// we creat the face if necessary and set the code by dimension reduction
			//face.nb_edge=ptr_el->type_element+1;  
			exist = false;
			for (int j = 0; j<id_face; j++)// look if the face still exist
			{
				int test_equal = 0;
				if (face_act[0] == (face_vertexes[j][0])) test_equal++;
				if (face_act[0] == (face_vertexes[j][1])) test_equal++;
				if (face_act[0] == (face_vertexes[j][2])) test_equal++;
				if (face_act[1] == (face_vertexes[j][0])) test_equal++;
				if (face_act[1] == (face_vertexes[j][1])) test_equal++;
				if (face_act[1] == (face_vertexes[j][2])) test_equal++;
				if (face_act[2] == (face_vertexes[j][0])) test_equal++;
				if (face_act[2] == (face_vertexes[j][1])) test_equal++;
				if (face_act[2] == (face_vertexes[j][2])) test_equal++;
				//printf("\n face_act:%d,%d,%d -- face_vertexes:%d,%d,%d \n", face_act[0], face_act[1], face_act[2], face_vertexes[j][0], face_vertexes[j][1], face_vertexes[j][2]);
				if (test_equal == 3) { exist = true; }// //the face still exist,  
			}
			if (!exist)
			{
				e_label[i] = id_face + 1;
				face_label.push_back(id_face + 1);
				face_code.push_back(0);
				face_vertexes.push_back(face_act);
				face_edges.push_back(face_edge_act);
				id_face++;
			}
		}
		//cells
		if (e_type_element[i] == 4)
		{
			//printf("\ncell\n");
			//cout<<"creating tetrahedron element"<<endl;fflush(NULL); 
			//pos_dim3++;
			cell_act.resize(4);
			//cell_act[0] = (breast_node_ids[e_nodes[i][0] - 1]);
			//cell_act[1] = (breast_node_ids[e_nodes[i][1] - 1]);
			//cell_act[2] = (breast_node_ids[e_nodes[i][2] - 1]);
			//cell_act[3] = (breast_node_ids[e_nodes[i][3] - 1]);
			cell_act[0] = e_nodes[i][0];
			cell_act[1] = e_nodes[i][1];
			cell_act[2] = e_nodes[i][2];
			cell_act[3] = e_nodes[i][3];

			for (int j = 0; j < 4; j++)
			{
				face_act.resize(3);
				//face_act[0] = (breast_node_ids[cell_act[j] - 1]);
				//face_act[1] = (breast_node_ids[cell_act[(j + 1) % 4] - 1]);
				//face_act[2] = (breast_node_ids[cell_act[(j + 2) % 4] - 1]);
				face_act[0] = cell_act[j];
				face_act[1] = cell_act[(j + 1) % 4];
				face_act[2] = cell_act[(j + 2) % 4];
				for (int k = 0; k < 3; k++)
				{
					edge_act.resize(2);
					//edge_act[0] = (breast_node_ids[face_act[k] - 1]);
					//edge_act[1] = (breast_node_ids[face_act[(k + 1) % 3] - 1]);
					edge_act[0] = face_act[k];
					edge_act[1] = face_act[(k + 1) % 3];
					exist = false;
					int tam = (int)edge_label.size();
					for (int ki = 0; ki < tam; ki++)
					{
						bool is_equal1 = false, is_equal2 = false;
						if (edge_act[0] == edge_vertexes[ki][0]) is_equal1 = true;
						if (edge_act[0] == edge_vertexes[ki][1]) is_equal1 = true;
						if (edge_act[1] == edge_vertexes[ki][0]) is_equal2 = true;
						if (edge_act[1] == edge_vertexes[ki][1]) is_equal2 = true;
						if (is_equal1 && is_equal2) { exist = true; face_edge_act[k] = ki + 1; }
					}
					if (!exist)
					{
						edge_label.push_back(id_edge + 1);
						edge_vertexes.push_back(edge_act);
						edge_code.push_back(0);
						face_edge_act[k] = id_edge + 1;
						id_edge++;
					}
				}
				//bool is_equal1 = false, is_equal2 = false, is_equal3 = false;
				bool exist = false;
				int tam = (int)face_label.size();
				for (int k = 0; k<tam; k++)// look if the face still exist
				{
					int test_equal = 0;
					if (face_act[0] == (face_vertexes[k][0])) test_equal++;
					if (face_act[0] == (face_vertexes[k][1])) test_equal++;
					if (face_act[0] == (face_vertexes[k][2])) test_equal++;
					if (face_act[1] == (face_vertexes[k][0])) test_equal++;
					if (face_act[1] == (face_vertexes[k][1])) test_equal++;
					if (face_act[1] == (face_vertexes[k][2])) test_equal++;
					if (face_act[2] == (face_vertexes[k][0])) test_equal++;
					if (face_act[2] == (face_vertexes[k][1])) test_equal++;
					if (face_act[2] == (face_vertexes[k][2])) test_equal++;
					//printf("\n face_act:%d,%d,%d -- face_vertexes:%d,%d,%d \n", face_act[0], face_act[1], face_act[2], face_vertexes[j][0], face_vertexes[j][1], face_vertexes[j][2]);
					if (test_equal == 3) { cell_face_act[j] = k + 1; exist = true; }// //the face already exist,  
				}
				if (!exist)
				{
					face_label.push_back(id_face + 1);
					face_code.push_back(0);
					face_vertexes.push_back(face_act);
					face_edges.push_back(face_edge_act);
					cell_face_act[j] = id_face + 1;
					id_face++;
				}

			}

			// at that stage we have all the face and edge now check the code
			exist = false;
			//bool is_equal1 = false, is_equal2 = false, is_equal3 = false, is_equal4 = false;
			for (int j = 0; j<id_cell; j++)// look if the cell still exist
			{
				int test_equal = 0;
				if (cell_act[0] == cell_vertexes[j][0]) test_equal++;
				if (cell_act[0] == cell_vertexes[j][1]) test_equal++;
				if (cell_act[0] == cell_vertexes[j][2]) test_equal++;
				if (cell_act[0] == cell_vertexes[j][3]) test_equal++;
				if (cell_act[1] == cell_vertexes[j][0]) test_equal++;
				if (cell_act[1] == cell_vertexes[j][1]) test_equal++;
				if (cell_act[1] == cell_vertexes[j][2]) test_equal++;
				if (cell_act[1] == cell_vertexes[j][3]) test_equal++;
				if (cell_act[2] == cell_vertexes[j][0]) test_equal++;
				if (cell_act[2] == cell_vertexes[j][1]) test_equal++;
				if (cell_act[2] == cell_vertexes[j][2]) test_equal++;
				if (cell_act[2] == cell_vertexes[j][3]) test_equal++;
				if (cell_act[3] == cell_vertexes[j][0]) test_equal++;
				if (cell_act[3] == cell_vertexes[j][1]) test_equal++;
				if (cell_act[3] == cell_vertexes[j][2]) test_equal++;
				if (cell_act[3] == cell_vertexes[j][3]) test_equal++;
				//printf("\n cell_act:%d,%d,%d,%d -- cell_vertexes:%d,%d,%d,%d \n", cell_act[0], cell_act[1], cell_act[2], cell_act[3], cell_vertexes[j][0], cell_vertexes[j][1], cell_vertexes[j][2], cell_vertexes[j][3]);
				if (test_equal == 4) { exist = true; }
			}
			if (!exist)
			{
				cell_code.push_back(e_code_physical[i]);
				e_label[i] = id_cell + 1;
				cell_label.push_back(id_cell + 1);
				cell_vertexes.push_back(cell_act); // it is a real new cell so save it
				cell_faces.push_back(cell_face_act);
				id_cell++;
			}
		}
	}

	for (int cont = pos_dim3 + 1; cont > 0; cont--)
	{
		int i = cont - 1;
		if (e_type_element[i] == 2)
		{
			if ((e_code_physical[i] / MINUS_ONE_DIM) == 2)
			{
				breast_nodes_code[e_nodes[i][0] - 1] = e_code_physical[i] % MINUS_ONE_DIM;
				breast_nodes_code[e_nodes[i][1] - 1] = e_code_physical[i] % MINUS_ONE_DIM;
				breast_nodes_code[e_nodes[i][2] - 1] = e_code_physical[i] % MINUS_ONE_DIM;
			}
			else if ((e_code_physical[i] / MINUS_ONE_DIM) == 0)
			{
				face_code[e_label[i] - 1] = e_code_physical[i];
			}
		}
		if (e_type_element[i] == 1)
		{
			if ((e_code_physical[i] / MINUS_ONE_DIM) == 1)
			{
				breast_nodes_code[e_nodes[i][0] - 1] = e_code_physical[i] % MINUS_ONE_DIM;
				breast_nodes_code[e_nodes[i][1] - 1] = e_code_physical[i] % MINUS_ONE_DIM;
			}
			else if ((e_code_physical[i] / MINUS_ONE_DIM) == 0) { edge_code[e_label[i] - 1] = e_code_physical[i]; }
		}
		if (e_type_element[i] == 15)
		{
			breast_nodes_code[e_label[i] - 1] = e_code_physical[i];
		}
	}

	//int num_edges = (int)edge_label.size();
	int num_faces = (int)face_label.size();
	int num_cells = (int)cell_label.size();

	//std::printf("\n num_nodes:%d :: num_edges:%d :: num_faces:%d :: num_cells:%d \n", (int)num_nodes, (int)num_edges, (int)num_faces, (int)num_cells);


	//printf("\n num_edges: %d <-> %d :: num_faces: %d <-> %d :: num_cells: %d <-> %d\n", num_edges, id_edge, num_faces, id_face, num_cells, id_cell);
	//nodes
	std::vector<int> new_act_faces;
	std::vector< std::vector<int> > new_node_faces;
	std::vector< std::vector<int> > new_node_cells;
	new_node_faces.resize(num_nodes);
	new_node_cells.resize(num_nodes);
	for (int i = 0; i < num_nodes; i++)
	{
		//cells
		new_act_faces.resize(0);
		new_node_faces[i].resize(0);
		new_node_cells[i].resize(0);
		for (int i_c = 0; i_c < num_cells; i_c++)
		{
			if ((breast_node_ids[i] == cell_vertexes[i_c][0]) || (breast_node_ids[i] == cell_vertexes[i_c][1]) || (breast_node_ids[i] == cell_vertexes[i_c][2]) || (breast_node_ids[i] == cell_vertexes[i_c][3]))
			{
				new_node_cells[i].push_back(cell_label[i_c]);

				//faces
				for (int i_f = 0; i_f < 4; i_f++)
				{
					int f_id = cell_faces[i_c][i_f];
					int newsize = (int)new_act_faces.size();
					bool new_exist = false;
					for (int iaux = 0; iaux < newsize; iaux++)
					{
						if (f_id == new_act_faces[iaux]) new_exist = true;
					}
					if (!new_exist) new_act_faces.push_back(f_id);
				}
			}
		}
		int newsize = (int)new_act_faces.size();
		for (int i_f = 0; i_f < newsize; i_f++) 
		{ 
			new_node_faces[i].push_back(new_act_faces[i_f]);
		}
	}
	//faces
	std::vector<int> new_face_nodes, new_label_chass, new_label_skin;
	std::vector<double> new_face_nx, new_face_ny, new_face_nz;
	std::vector<double> new_face_centroid_x, new_face_centroid_y, new_face_centroid_z;
	new_face_nodes.resize(0), new_label_chass.resize(0), new_label_skin.resize(0);
	new_face_nx.resize(0); new_face_ny.resize(0); new_face_nz.resize(0);
	new_face_centroid_x.resize(0); new_face_centroid_y.resize(0); new_face_centroid_z.resize(0);
	for (int i = 0; i < num_faces; i++)
	{
		//face_code
		//Face3 new_face = Face3();
		//new_face.id = face_label[i];
		//new_face.code = face_code[i];
		if (face_code[i] == 2) { new_label_chass.push_back(i); }
		if (face_code[i] == 3) { new_label_skin.push_back(i); }
		int pos_v = 0;
		for (int j = 0; j < 3; j++)
		{
			bool exist = false;
			for (int k = 0; k < pos_v; k++) { if (edge_vertexes[face_edges[i][j] - 1][0] == face_vertexes[i][k]) exist = true; }
			if (!exist) { face_vertexes[i][pos_v] = edge_vertexes[face_edges[i][j] - 1][0]; pos_v++; }
			exist = false;
			for (int k = 0; k < pos_v; k++) { if (edge_vertexes[face_edges[i][j] - 1][1] == face_vertexes[i][k]) exist = true; }
			if (!exist) { face_vertexes[i][pos_v] = edge_vertexes[face_edges[i][j] - 1][1]; pos_v++; }
		}

		new_face_nx.push_back(breast_node_coords[face_vertexes[i][0] - 1][0]); new_face_ny.push_back(breast_node_coords[face_vertexes[i][0] - 1][1]); new_face_nz.push_back(breast_node_coords[face_vertexes[i][0] - 1][2]);
		new_face_nx.push_back(breast_node_coords[face_vertexes[i][1] - 1][0]); new_face_ny.push_back(breast_node_coords[face_vertexes[i][1] - 1][1]); new_face_nz.push_back(breast_node_coords[face_vertexes[i][1] - 1][2]);
		new_face_nx.push_back(breast_node_coords[face_vertexes[i][2] - 1][0]); new_face_ny.push_back(breast_node_coords[face_vertexes[i][2] - 1][1]); new_face_nz.push_back(breast_node_coords[face_vertexes[i][2] - 1][2]);
		new_face_centroid_x.push_back((breast_node_coords[face_vertexes[i][0] - 1][0] + breast_node_coords[face_vertexes[i][1] - 1][0] + breast_node_coords[face_vertexes[i][2] - 1][0]) / 3.0);
		new_face_centroid_y.push_back((breast_node_coords[face_vertexes[i][0] - 1][1] + breast_node_coords[face_vertexes[i][1] - 1][1] + breast_node_coords[face_vertexes[i][2] - 1][1]) / 3.0);
		new_face_centroid_z.push_back((breast_node_coords[face_vertexes[i][0] - 1][2] + breast_node_coords[face_vertexes[i][1] - 1][2] + breast_node_coords[face_vertexes[i][2] - 1][2]) / 3.0);

		new_face_nodes.push_back(face_vertexes[i][0]);
		new_face_nodes.push_back(face_vertexes[i][1]);
		new_face_nodes.push_back(face_vertexes[i][2]);
	}
	//cells
	std::vector<int> new_cell_ids;
	new_cell_ids.resize(0);
	for (int i = 0; i < num_cells; i++)
	{
		//Cell3 new_cell = Cell3();
		//new_cell.id = cell_label[i];
		//new_cell.code = cell_code[i];

		int pos_v = 0;

		for (int j = 0; j < 4; j++)
		{
			for (int k = 0; k < 3; k++)
			{
				bool exist = false;
				for (int m = 0; m < pos_v; m++) { if (edge_vertexes[face_edges[cell_faces[i][j] - 1][k] - 1][0] == cell_vertexes[i][m]) exist = true; }
				if (!exist) { cell_vertexes[i][pos_v] = edge_vertexes[face_edges[cell_faces[i][j] - 1][k] - 1][0]; pos_v++; }
				exist = false;
				for (int m = 0; m < pos_v; m++) { if (edge_vertexes[face_edges[cell_faces[i][j] - 1][k] - 1][1] == cell_vertexes[i][m]) exist = true; }
				if (!exist) { cell_vertexes[i][pos_v] = edge_vertexes[face_edges[cell_faces[i][j] - 1][k] - 1][1]; pos_v++; }
			}
		}

		new_cell_ids.push_back(cell_vertexes[i][1]);
		new_cell_ids.push_back(cell_vertexes[i][2]);
		new_cell_ids.push_back(cell_vertexes[i][3]);
		new_cell_ids.push_back(cell_vertexes[i][0]);
	}
	//processing step
	//faces pre-process
	std::vector<int> new_fchass, new_fskin;
	std::vector<double> fchass_nx, fchass_ny, fchass_nz;
	std::vector<double> fchass_cx, fchass_cy, fchass_cz;
	std::vector<double> fskin_nx, fskin_ny, fskin_nz;
	std::vector<double> fskin_cx, fskin_cy, fskin_cz;
	new_fchass.resize(0); new_fskin.resize(0);
	int num_fchass = 0, num_fskin = 0;
	for (int i = 0; i < num_faces; i++)
	{
		if (face_code[i] == 2)
		{
			num_fchass++;
			new_fchass.push_back(new_face_nodes[i * 3]);
			new_fchass.push_back(new_face_nodes[i * 3 + 1]);
			new_fchass.push_back(new_face_nodes[i * 3 + 2]);
			fchass_nx.push_back(new_face_nx[i * 3]);
			fchass_nx.push_back(new_face_nx[i * 3 + 1]);
			fchass_nx.push_back(new_face_nx[i * 3 + 2]);
			fchass_ny.push_back(new_face_ny[i * 3]);
			fchass_ny.push_back(new_face_ny[i * 3 + 1]);
			fchass_ny.push_back(new_face_ny[i * 3 + 2]);
			fchass_nz.push_back(new_face_nz[i * 3]);
			fchass_nz.push_back(new_face_nz[i * 3 + 1]);
			fchass_nz.push_back(new_face_nz[i * 3 + 2]);
			fchass_cx.push_back(new_face_centroid_x[i]);
			fchass_cy.push_back(new_face_centroid_y[i]);
			fchass_cz.push_back(new_face_centroid_z[i]);
		}
		if (face_code[i] == 3)
		{
			num_fskin++;
			new_fskin.push_back(new_face_nodes[i * 3]);
			new_fskin.push_back(new_face_nodes[i * 3 + 1]);
			new_fskin.push_back(new_face_nodes[i * 3 + 2]);
			fskin_nx.push_back(new_face_nx[i * 3]);
			fskin_nx.push_back(new_face_nx[i * 3 + 1]);
			fskin_nx.push_back(new_face_nx[i * 3 + 2]);
			fskin_ny.push_back(new_face_ny[i * 3]);
			fskin_ny.push_back(new_face_ny[i * 3 + 1]);
			fskin_ny.push_back(new_face_ny[i * 3 + 2]);
			fskin_nz.push_back(new_face_nz[i * 3]);
			fskin_nz.push_back(new_face_nz[i * 3 + 1]);
			fskin_nz.push_back(new_face_nz[i * 3 + 2]);
			fskin_cx.push_back(new_face_centroid_x[i]);
			fskin_cy.push_back(new_face_centroid_y[i]);
			fskin_cz.push_back(new_face_centroid_z[i]);
		}
	}
	//nodes pre-process
	std::vector< std::vector<int> > new_face_chass, new_face_skin;
	std::vector<int> act_chass, act_skin;
	new_face_chass.resize(0); new_face_skin.resize(0);
	//int nchass, nskin;
	for (int i = 0; i < num_nodes; i++)
	{
		act_chass.resize(0); act_skin.resize(0);
		//nchass = 0; nskin = 0;
		for (int k = 0; k < (int)new_node_faces[i].size(); k++)
		{
			if (face_code[new_node_faces[i][k] - 1] == 2)
			{
				int chass_id = -666;
				for (int ki = 0; ki < num_fchass; ki++)
				{
					if ((new_node_faces[i][k] - 1) == (new_label_chass[ki]))
					{
						chass_id = ki;
					}
				}
				if (chass_id == -666) printf("\nUPSC\n");
				act_chass.push_back(chass_id);
				//nchass++;
			}
			if (face_code[new_node_faces[i][k] - 1] == 3)
			{
				int skin_id = -666;
				for (int ki = 0; ki < num_fskin; ki++)
				{
					if ((new_node_faces[i][k] - 1) == (new_label_skin[ki]))
					{
						skin_id = ki;
					}
				}
				if (skin_id == -666) printf("\nUPSS\n");
				act_skin.push_back(skin_id);
				//nskin++;
			}
		}
		//act_chass[0] = nchass;
		//act_skin[0] = nskin;
		new_face_chass.push_back(act_chass);
		new_face_skin.push_back(act_skin);
	}

	//NODES
	nodes->size = num_nodes;
	nodes->code = (int*)malloc(sizeof(int)*num_nodes);
	nodes->x = (double*)malloc(sizeof(double)*num_nodes);
	nodes->y = (double*)malloc(sizeof(double)*num_nodes);
	nodes->z = (double*)malloc(sizeof(double)*num_nodes);
	nodes->cells = (int**)malloc(sizeof(int*)*num_nodes);
	nodes->chass = (int**)malloc(sizeof(int*)*num_nodes);
	nodes->skin  = (int**)malloc(sizeof(int*)*num_nodes);
	for (int i = 0; i < num_nodes; i++)
	{
		nodes->code[i] = breast_nodes_code[i];
		nodes->x[i] = breast_node_coords[i][0];
		nodes->y[i] = breast_node_coords[i][1];
		nodes->z[i] = breast_node_coords[i][2];
		
		nodes->cells[i] = (int*)malloc(sizeof(int)*(new_node_cells[i].size() + 1));
		nodes->cells[i][0] = (int)new_node_cells[i].size();
		for (int k = 0; k < nodes->cells[i][0]; k++) { nodes->cells[i][k+1] = new_node_cells[i][k] - 1; }
		
		nodes->chass[i] = (int*)malloc(sizeof(int)*(new_face_chass[i].size() + 1));
		nodes->chass[i][0] = (int)new_face_chass[i].size();
		for (int k = 0; k < nodes->chass[i][0]; k++) { nodes->chass[i][k+1] = new_face_chass[i][k]; }
		
		nodes->skin[i] = (int*)malloc(sizeof(int)*(new_face_skin[i].size() + 1));
		nodes->skin[i][0] = (int)new_face_skin[i].size();
		for (int k = 0; k < nodes->skin[i][0]; k++) { nodes->skin[i][k+1] = new_face_skin[i][k]; }
	}

	//FACES
	//printf("\nNum faces : %d  :: chass:%d  :: skin:%d\n", (int)num_faces, (int)num_fchass, (int)num_fskin);
	faces->size_all = (int)num_faces;
	faces->size_chass = (int)num_fchass;
	faces->size_skin = (int)num_fskin;
	faces->nodes_all = (int*)malloc(sizeof(int) * num_faces * 3);
	faces->face_codes = (int*)malloc(sizeof(int) * num_faces);
	faces->nodes_chass = (int*)malloc(sizeof(int) * num_fchass * 3);
	faces->nodes_skin = (int*)malloc(sizeof(int) * num_fskin * 3);
	faces->cx_chass = (double*)malloc(sizeof(double) * num_fchass);
	faces->cy_chass = (double*)malloc(sizeof(double) * num_fchass);
	faces->cz_chass = (double*)malloc(sizeof(double) * num_fchass);
	faces->oa_skin = (double*)malloc(sizeof(double) * num_fskin * 3);
	faces->ob_skin = (double*)malloc(sizeof(double) * num_fskin * 3);
	faces->energy_chass = (double*)malloc(sizeof(double) * num_fchass);
	faces->surface_chass = (double*)malloc(sizeof(double) * num_fchass);
	faces->energy_skin = (double*)malloc(sizeof(double) * num_fskin);
	faces->surface_skin = (double*)malloc(sizeof(double) * num_fskin);
	//aux	
	double *nx_skin = (double*)malloc(sizeof(double) * num_fskin * 3);
	double *ny_skin = (double*)malloc(sizeof(double) * num_fskin * 3);
	double *nz_skin = (double*)malloc(sizeof(double) * num_fskin * 3);
	for (int i = 0; i < num_fchass; i++)
	{
		faces->nodes_chass[i * 3] = new_fchass[i * 3] - 1;
		faces->nodes_chass[i * 3 + 1] = new_fchass[i * 3 + 1] - 1;
		faces->nodes_chass[i * 3 + 2] = new_fchass[i * 3 + 2] - 1;
		faces->cx_chass[i] = fchass_cx[i];
		faces->cy_chass[i] = fchass_cy[i];
		faces->cz_chass[i] = fchass_cz[i];
		faces->energy_chass[i] = 0.0;
		faces->surface_chass[i] = 0.0;
	}
	for (int i = 0; i < num_fskin; i++)
	{
		faces->nodes_skin[i * 3] = new_fskin[i * 3] - 1;
		faces->nodes_skin[i * 3 + 1] = new_fskin[i * 3 + 1] - 1;
		faces->nodes_skin[i * 3 + 2] = new_fskin[i * 3 + 2] - 1;
		nx_skin[i * 3    ] = fskin_nx[i * 3];
		nx_skin[i * 3 + 1] = fskin_nx[i * 3 + 1];
		nx_skin[i * 3 + 2] = fskin_nx[i * 3 + 2];
		ny_skin[i * 3    ] = fskin_ny[i * 3];
		ny_skin[i * 3 + 1] = fskin_ny[i * 3 + 1];
		ny_skin[i * 3 + 2] = fskin_ny[i * 3 + 2];
		nz_skin[i * 3    ] = fskin_nz[i * 3];
		nz_skin[i * 3 + 1] = fskin_nz[i * 3 + 1];
		nz_skin[i * 3 + 2] = fskin_nz[i * 3 + 2];
		faces->energy_skin[i] = 0.0;
		faces->surface_skin[i] = 0.0;
	}
	
	for (int i = 0; i < num_fskin; i++)
	{
		faces->oa_skin[i * 3    ] = nx_skin[i * 3 + 1] - nx_skin[i * 3];
		faces->oa_skin[i * 3 + 1] = ny_skin[i * 3 + 1] - ny_skin[i * 3];
		faces->oa_skin[i * 3 + 2] = nz_skin[i * 3 + 1] - nz_skin[i * 3];
		faces->ob_skin[i * 3    ] = nx_skin[i * 3 + 2] - nx_skin[i * 3];
		faces->ob_skin[i * 3 + 1] = ny_skin[i * 3 + 2] - ny_skin[i * 3];
		faces->ob_skin[i * 3 + 2] = nz_skin[i * 3 + 2] - nz_skin[i * 3];
	}
	free(nx_skin); free(ny_skin); free(nz_skin);
	for (int i = 0; i < num_faces; i++)
	{
		faces->nodes_all[i * 3] = new_face_nodes[i * 3] - 1;
		faces->nodes_all[i * 3 + 1] = new_face_nodes[i * 3 + 1] - 1;
		faces->nodes_all[i * 3 + 2] = new_face_nodes[i * 3 + 2] - 1;
		faces->face_codes[i] = face_code[i];
	}

	//CELLS
	cells->size = (int)num_cells;
	cells->ids = (int*)malloc(sizeof(int) * num_cells * 4);
	cells->code = (int*)malloc(sizeof(int) * num_cells);
	cells->energy = (double*)malloc(sizeof(double) * num_cells);
	cells->volume = (double*)malloc(sizeof(double) * num_cells);
	cells->jMat = (double*)malloc(sizeof(double) * num_cells * 9);
	for (int i = 0; i < num_cells; i++)
	{
		cells->ids[i * 4] = new_cell_ids[i * 4] - 1;
		cells->ids[i * 4 + 1] = new_cell_ids[i * 4 + 1] - 1;
		cells->ids[i * 4 + 2] = new_cell_ids[i * 4 + 2] - 1;
		cells->ids[i * 4 + 3] = new_cell_ids[i * 4 + 3] - 1;
		cells->code[i] = cell_code[i];
		cells->energy[i] = 0.0;
		cells->volume[i] = 0.0;
		for (int k = 0; k < 9; k++) { cells->jMat[i * 9 + k] = 0.0; }
	}
	m->energy = 0.0;

	//gather all
	m->nodes = nodes;
	m->faces = faces;
	m->cells = cells;

	return m;
}

/*
* method which writes a mesh to a .msh file
*/
void writeFile_msh(char* filename, Mesh *m, Position *p)
{
	std::ofstream _of;
	_of.open(filename);

	if (!_of.is_open()) 
	{
		printf("error opening file: %s", filename);
		return;
	}
	_of << "$MeshFormat\n";
	_of << "2.2 0 8\n";
	_of << "$EndMeshFormat\n";
	_of << "$Nodes\n";
	_of << m->nodes->size << "\n";
	_of << std::fixed;
	for (int i = 0; i < m->nodes->size; i++)
	{
		_of << (i+1) << " ";
		_of << std::setprecision(17) << p->px[i] << " ";
		_of << std::setprecision(17) << p->py[i] << " ";
		_of << std::setprecision(17) << p->pz[i] << "\n";
	}
	_of << "$EndNodes\n";
	_of << "$Elements\n";
	int num_elems = m->faces->size_all + m->cells->size + m->nodes->size;
	int k = 1;
	_of << num_elems << "\n";
	for(int i = 0; i < m->nodes->size; i++)
	{
		_of << k++ << " ";
		_of << 15 << " ";
		_of << 2 << " ";
		_of << m->nodes->code[i] << " ";
		_of << m->nodes->code[i] << " ";
		_of << (k-1) << "\n";
	}
	for (int i = 0; i < m->faces->size_all; i++)
	{
		_of << k++ << " ";
		_of << 2 << " ";
		_of << 2 << " ";
		_of << m->faces->face_codes[i] << " ";
		_of << m->faces->face_codes[i] << " ";
		_of << (m->faces->nodes_all[i * 3] + 1) << " ";
		_of << (m->faces->nodes_all[i * 3 + 1] + 1) << " ";
		_of << (m->faces->nodes_all[i * 3 + 2] + 1) << "\n";
	}
	for (int i = 0; i < m->cells->size; i++)
	{
		_of << k++ << " ";
		_of << 4 << " ";
		_of << 2 << " ";
		_of << m->cells->code[i] << " ";
		_of << m->cells->code[i] << " ";
		_of << (m->cells->ids[i * 4] + 1) << " ";
		_of << (m->cells->ids[i * 4 + 1] + 1) << " ";
		_of << (m->cells->ids[i * 4 + 2] + 1) << " ";
		_of << (m->cells->ids[i * 4 + 3] + 1) << "\n";
	}
	_of << "$EndElements";
	_of.close();
}

/*
* method which writes a mesh to a .txt file
*/
void writeMesh_txt(char* filename, Mesh *m) 
{
	std::ofstream _of(filename);
	_of << std::fixed;
	if (_of.is_open()) 
	{
		//write mesh data
		_of << "Mesh\n";
		_of << "NODES\n";
		_of << m->nodes->size << "\n";
		_of << "N::Codes\n";
		for (int i = 0; i < m->nodes->size; i++) 
		{
			_of << m->nodes->code[i] << "\n";
		}
		_of << "N::Coords\n";
		for (int i = 0; i < m->nodes->size; i++)
		{
			_of << std::setprecision(20) << m->nodes->x[i] << "\n";
			_of << std::setprecision(20) << m->nodes->y[i] << "\n";
			_of << std::setprecision(20) << m->nodes->z[i] << "\n";
		}
		_of << "N::Cells\n";
		for (int i = 0; i < m->nodes->size; i++)
		{
			_of << m->nodes->cells[i][0] << "\n";
			for (int k = 0; k < m->nodes->cells[i][0] ; k++)
			{
				_of << m->nodes->cells[i][k+1] << "\n";
			}
		}
		_of << "N::Chass\n";
		for (int i = 0; i < m->nodes->size; i++)
		{
			_of << m->nodes->chass[i][0] << "\n";
			for (int k = 0; k < m->nodes->chass[i][0]; k++)
			{
				_of << m->nodes->chass[i][k + 1] << "\n";
			}
		}
		_of << "N::Skin\n";
		for (int i = 0; i < m->nodes->size; i++)
		{
			_of << m->nodes->skin[i][0] << "\n";
			for (int k = 0; k < m->nodes->skin[i][0]; k++)
			{
				_of << m->nodes->skin[i][k + 1] << "\n";
			}
		}
		_of << "FACES\n";
		_of << m->faces->size_chass << "\n";
		_of << m->faces->size_skin << "\n";
		_of << "F::All\n";
		_of << m->faces->size_all << "\n";
		for (int i = 0; i < m->faces->size_all; i++)
		{
			_of << m->faces->nodes_all[i * 3] << "\n";
			_of << m->faces->nodes_all[i * 3 + 1] << "\n";
			_of << m->faces->nodes_all[i * 3 + 2] << "\n";
			_of << m->faces->face_codes[i] << "\n";
		}
		_of << "F::Chass\n";
		_of << "Chass::nodes\n";
		for (int i = 0; i < m->faces->size_chass; i++)
		{
			_of << m->faces->nodes_chass[i * 3] << "\n";
			_of << m->faces->nodes_chass[i * 3 + 1] << "\n";
			_of << m->faces->nodes_chass[i * 3 + 2] << "\n";
		}
		_of << "Chass::coords\n";
		for (int i = 0; i < m->faces->size_chass; i++)
		{ 
			_of << std::setprecision(20) << m->faces->cx_chass[i] << "\n";
			_of << std::setprecision(20) << m->faces->cy_chass[i] << "\n";
			_of << std::setprecision(20) << m->faces->cz_chass[i] << "\n";
		}
		_of << "Chass::energy\n";
		for (int i = 0; i < m->faces->size_chass; i++)
		{
			_of << std::setprecision(20) << m->faces->energy_chass[i] << "\n";
			_of << std::setprecision(20) << m->faces->surface_chass[i] << "\n";
		}
		_of << "F::Skin\n";
		_of << "Skin::nodes\n";
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			_of << m->faces->nodes_skin[i * 3] << "\n";
			_of << m->faces->nodes_skin[i * 3 + 1] << "\n";
			_of << m->faces->nodes_skin[i * 3 + 2] << "\n";
		}
		_of << "Skin::coords\n";
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			_of << std::setprecision(20) << m->faces->oa_skin[i * 3] << "\n";
			_of << std::setprecision(20) << m->faces->oa_skin[i * 3 + 1] << "\n";
			_of << std::setprecision(20) << m->faces->oa_skin[i * 3 + 2] << "\n";
			_of << std::setprecision(20) << m->faces->ob_skin[i * 3] << "\n";
			_of << std::setprecision(20) << m->faces->ob_skin[i * 3 + 1] << "\n";
			_of << std::setprecision(20) << m->faces->ob_skin[i * 3 + 2] << "\n";
		}
		_of << "Skin::energy\n";
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			_of << std::setprecision(20) << m->faces->energy_skin[i] << "\n";
			_of << std::setprecision(20) << m->faces->surface_skin[i] << "\n";
		}
		_of << "CELLS\n";
		_of << m->cells->size << "\n";
		_of << "C::nodes\n";
		for (int i = 0; i < m->cells->size; i++)
		{
			_of << m->cells->ids[i * 4] << "\n";
			_of << m->cells->ids[i * 4 + 1] << "\n";
			_of << m->cells->ids[i * 4 + 2] << "\n";
			_of << m->cells->ids[i * 4 + 3] << "\n";
			_of << m->cells->code[i] << "\n";
		}
		_of << "C::energy\n";
		for (int i = 0; i < m->cells->size; i++)
		{
			_of << std::setprecision(20) << m->cells->energy[i] << "\n";
			_of << std::setprecision(20) << m->cells->volume[i] << "\n";
			for (int k = 0; k < 9; k++) 
			{
				_of << std::setprecision(20) << m->cells->jMat[i * 9 + k] << "\n";
			}
		}
	}
	else
	{
		printf("\nError opening mesh file txt\n");
	}
	_of.close();
}

/*
* method which writes a position to a .txt file
*/
void writePosition_txt(char* filename, Position *p)
{
	std::ofstream _of(filename);
	_of << std::fixed;
	if (_of.is_open())
	{
		_of << "Position\n";
		_of << p->size << "\n";
		for (int i = 0; i < p->size; i++)
		{
			_of << std::setprecision(20) << p->px[i] << "\n";
			_of << std::setprecision(20) << p->py[i] << "\n";
			_of << std::setprecision(20) << p->pz[i] << "\n";
			_of << p->code[i] << "\n";
		}
	}
	else
	{
		printf("\nError opening pos file txt\n");
	}
	_of.close();
}

/*
* method which writes a DOF to a .txt file
*/
void writeDOF_txt(char* filename, DOF *d)
{
	std::ofstream _of(filename);
	_of << std::fixed;
	if (_of.is_open())
	{
		_of << "DOF\n";
		_of << d->size << "\n";
		for (int i = 0; i < d->size; i++)
		{
			_of << std::setprecision(20) << d->x[i] << "\n";
			_of << d->map[i * 2] << "\n";
			_of << d->map[i * 2 + 1] << "\n";
		}
	}
	else
	{
		printf("\nError opening dof file txt\n");
	}
	_of.close();
}

/*
* method which loads a mesh from a .txt file
*/
Mesh* loadMesh_txt(char* filename)
{
	Mesh *m;
	std::ifstream _if(filename);
	std::string line;
	if (_if.is_open())
	{
		m = (Mesh*)malloc(sizeof(Mesh));
		m->nodes = (Node*)malloc(sizeof(Node));
		m->faces = (Face*)malloc(sizeof(Face));
		m->cells = (Cell*)malloc(sizeof(Cell));
		std::getline(_if, line); // "Mesh\n"
		std::getline(_if, line); // "NODES\n"
		std::getline(_if, line);
		m->nodes->size = std::stoi(line);
		m->nodes->code = (int*)malloc(sizeof(int)*m->nodes->size);
		m->nodes->x = (double*)malloc(sizeof(double)*m->nodes->size);
		m->nodes->y = (double*)malloc(sizeof(double)*m->nodes->size);
		m->nodes->z = (double*)malloc(sizeof(double)*m->nodes->size);
		m->nodes->cells = (int**)malloc(sizeof(int*)*m->nodes->size);
		m->nodes->chass = (int**)malloc(sizeof(int*)*m->nodes->size);
		m->nodes->skin = (int**)malloc(sizeof(int*)*m->nodes->size);
		std::getline(_if, line); // "N::Codes\n"
		for (int i = 0; i < m->nodes->size; i++)
		{
			std::getline(_if, line);
			m->nodes->code[i] = std::stoi(line);
		}
		std::getline(_if, line); // "N::Coords\n"
		for (int i = 0; i < m->nodes->size; i++)
		{
			std::getline(_if, line);
			m->nodes->x[i] = std::stod(line);
			std::getline(_if, line);
			m->nodes->y[i] = std::stod(line);
			std::getline(_if, line);
			m->nodes->z[i] = std::stod(line);
		}
		std::getline(_if, line); // "N::Cells\n"
		for (int i = 0; i < m->nodes->size; i++)
		{
			std::getline(_if, line);
			int aux = std::stoi(line);
			m->nodes->cells[i] = (int*)malloc(sizeof(int)*(aux+1));
			m->nodes->cells[i][0] = aux;
			for (int k = 0; k < m->nodes->cells[i][0]; k++)
			{
				std::getline(_if, line);
				m->nodes->cells[i][k + 1] = std::stoi(line);
			}
		}
		std::getline(_if, line); // "N::Chass\n"
		for (int i = 0; i < m->nodes->size; i++)
		{
			std::getline(_if, line);
			int aux = std::stoi(line);
			m->nodes->chass[i] = (int*)malloc(sizeof(int)*(aux + 1));
			m->nodes->chass[i][0] = aux;
			for (int k = 0; k < m->nodes->chass[i][0]; k++)
			{
				std::getline(_if, line);
				m->nodes->chass[i][k + 1] = std::stoi(line);
			}
		}
		std::getline(_if, line); // "N::Skin\n"
		for (int i = 0; i < m->nodes->size; i++)
		{
			std::getline(_if, line);
			int aux = std::stoi(line);
			m->nodes->skin[i] = (int*)malloc(sizeof(int)*(aux + 1));
			m->nodes->skin[i][0] = aux;
			for (int k = 0; k < m->nodes->skin[i][0]; k++)
			{
				std::getline(_if, line);
				m->nodes->skin[i][k + 1] = std::stoi(line);
			}
		}
		std::getline(_if, line); // "FACES\n"
		std::getline(_if, line);
		m->faces->size_chass = std::stoi(line);
		std::getline(_if, line);
		m->faces->size_skin = std::stoi(line);
		std::getline(_if, line); // "F::All\n"
		std::getline(_if, line);
		m->faces->size_all = std::stoi(line);
		m->faces->nodes_all = (int*)malloc(sizeof(int) * m->faces->size_all * 3);
		m->faces->face_codes = (int*)malloc(sizeof(int) * m->faces->size_all);
		for (int i = 0; i < m->faces->size_all; i++)
		{
			std::getline(_if, line);
			m->faces->nodes_all[i * 3] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_all[i * 3 + 1] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_all[i * 3 + 2] = std::stoi(line);
			std::getline(_if, line);
			m->faces->face_codes[i] = std::stoi(line);
		}
		std::getline(_if, line); // "F::Chass\n"
		std::getline(_if, line); // "Chass::nodes\n"
		m->faces->nodes_chass = (int*)malloc(sizeof(int)*m->faces->size_chass*3);
		for (int i = 0; i < m->faces->size_chass; i++)
		{
			std::getline(_if, line);
			m->faces->nodes_chass[i * 3] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_chass[i * 3 + 1] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_chass[i * 3 + 2] = std::stoi(line);
		}
		std::getline(_if, line); // "Chass::coords\n"
		m->faces->cx_chass = (double*)malloc(sizeof(double)*m->faces->size_chass);
		m->faces->cy_chass = (double*)malloc(sizeof(double)*m->faces->size_chass);
		m->faces->cz_chass = (double*)malloc(sizeof(double)*m->faces->size_chass);
		for (int i = 0; i < m->faces->size_chass; i++)
		{
			std::getline(_if, line);
			m->faces->cx_chass[i] = std::stod(line);
			std::getline(_if, line);
			m->faces->cy_chass[i] = std::stod(line);
			std::getline(_if, line);
			m->faces->cz_chass[i] = std::stod(line);
		}
		std::getline(_if, line); // "Chass::energy\n"
		m->faces->energy_chass = (double*)malloc(sizeof(double)*m->faces->size_chass);
		m->faces->surface_chass = (double*)malloc(sizeof(double)*m->faces->size_chass);
		for (int i = 0; i < m->faces->size_chass; i++)
		{
			std::getline(_if, line);
			m->faces->energy_chass[i] = std::stod(line);
			std::getline(_if, line);
			m->faces->surface_chass[i] = std::stod(line);
		}
		std::getline(_if, line); // "F::Skin\n"
		std::getline(_if, line); // "Skin::nodes\n"
		m->faces->nodes_skin = (int*)malloc(sizeof(int)*m->faces->size_skin * 3);
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			std::getline(_if, line);
			m->faces->nodes_skin[i * 3] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_skin[i * 3 + 1] = std::stoi(line);
			std::getline(_if, line);
			m->faces->nodes_skin[i * 3 + 2] = std::stoi(line);
		}
		std::getline(_if, line); // "Skin::coords\n"
		m->faces->oa_skin = (double*)malloc(sizeof(double)*m->faces->size_skin * 3);
		m->faces->ob_skin = (double*)malloc(sizeof(double)*m->faces->size_skin * 3);
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			std::getline(_if, line);
			m->faces->oa_skin[i * 3    ] = std::stod(line);
			std::getline(_if, line);
			m->faces->oa_skin[i * 3 + 1] = std::stod(line);
			std::getline(_if, line);
			m->faces->oa_skin[i * 3 + 2] = std::stod(line);
			std::getline(_if, line);
			m->faces->ob_skin[i * 3    ] = std::stod(line);
			std::getline(_if, line);
			m->faces->ob_skin[i * 3 + 1] = std::stod(line);
			std::getline(_if, line);
			m->faces->ob_skin[i * 3 + 2] = std::stod(line);
		}
		std::getline(_if, line); // "Skin::energy\n"
		m->faces->energy_skin = (double*)malloc(sizeof(double)*m->faces->size_skin);
		m->faces->surface_skin = (double*)malloc(sizeof(double)*m->faces->size_skin);
		for (int i = 0; i < m->faces->size_skin; i++)
		{
			std::getline(_if, line);
			m->faces->energy_skin[i] = std::stod(line);
			std::getline(_if, line);
			m->faces->surface_skin[i] = std::stod(line);
		}
		std::getline(_if, line); // "CELLS\n"
		std::getline(_if, line);
		m->cells->size = std::stoi(line);
		std::getline(_if, line); // "C::nodes\n"
		m->cells->ids = (int*)malloc(sizeof(int)*m->cells->size * 4);
		m->cells->code = (int*)malloc(sizeof(int)*m->cells->size);
		for (int i = 0; i < m->cells->size; i++)
		{
			std::getline(_if, line);
			m->cells->ids[i * 4] = std::stoi(line);
			std::getline(_if, line);
			m->cells->ids[i * 4 + 1] = std::stoi(line);
			std::getline(_if, line);
			m->cells->ids[i * 4 + 2] = std::stoi(line);
			std::getline(_if, line);
			m->cells->ids[i * 4 + 3] = std::stoi(line);
			std::getline(_if, line);
			m->cells->code[i] = std::stoi(line);
		}
		std::getline(_if, line); // "C::energy\n"
		m->cells->energy = (double*)malloc(sizeof(double)*m->cells->size);
		m->cells->volume = (double*)malloc(sizeof(double)*m->cells->size);
		m->cells->jMat = (double*)malloc(sizeof(double)*m->cells->size * 9);
		for (int i = 0; i < m->cells->size; i++)
		{
			std::getline(_if, line);
			m->cells->energy[i] = std::stod(line);
			std::getline(_if, line);
			m->cells->volume[i] = std::stod(line);
			for (int k = 0; k < 9; k++)
			{
				std::getline(_if, line);
				m->cells->jMat[i * 9 + k] = std::stod(line);
			}
		}
		m->energy = 0.0;
		_if.close();
		return m;
	}
	else
	{
		printf("\nError loading File\n");
		return nullptr;
	}
}

/*
* method which loads a position from a .txt file
*/
Position* loadPosition_txt(char* filename)
{
	Position *p;
	std::ifstream _if(filename);
	std::string line;
	if (_if.is_open())
	{
		p = (Position*)malloc(sizeof(Position));
		std::getline(_if, line); // "Position\n"
		std::getline(_if, line);
		p->size = std::stoi(line);
		p->px = (double*)malloc(sizeof(double)*p->size);
		p->py = (double*)malloc(sizeof(double)*p->size);
		p->pz = (double*)malloc(sizeof(double)*p->size);
		p->code = (int*)malloc(sizeof(int)*p->size);
		for (int i = 0; i < p->size; i++)
		{
			std::getline(_if, line);
			p->px[i] = std::stod(line);
			std::getline(_if, line);
			p->py[i] = std::stod(line);
			std::getline(_if, line);
			p->pz[i] = std::stod(line);
			std::getline(_if, line);
			p->code[i] = std::stoi(line);
		}
		_if.close();
		return p;
	}
	else 
	{
		printf("\nError loading File\n");
		return nullptr;
	}
}

/*
* method which loads a DOF from a .txt file
*/
DOF* loadDOF_txt(char* filename)
{
	DOF *d; 
	std::ifstream _if(filename);
	std::string line;
	if (_if.is_open())
	{
		d = (DOF*)malloc(sizeof(DOF));
		std::getline(_if, line); // "DOF\n"
		std::getline(_if, line);
		d->size = std::stoi(line);
		d->x = (double*)malloc(sizeof(double)*d->size);
		d->map = (int*)malloc(sizeof(int)*d->size*2);
		for (int i = 0; i < d->size; i++)
		{
			std::getline(_if, line);
			d->x[i] = std::stod(line);
			std::getline(_if, line);
			d->map[i * 2] = std::stoi(line);
			std::getline(_if, line);
			d->map[i * 2 + 1] = std::stoi(line);
		}
		_if.close();
		return d;
	}
	else
	{
		printf("\nError loading File\n");
		return nullptr;
	}
}


//----------------------------INIT------------------------------//
//----------------------------INIT------------------------------//
//initialisation and cleaning functions
/*
* method which initialises and orders the cells nodes 
*/
void initMesh(Mesh *m) 
{
	int ncells = m->cells->size, ids[4];
	double n0[3], n1[3], n2[3], n3[3], v0[3], v1[3], v2[3], vx[3], det;
	for (int i = 0; i < ncells; i++) 
	{
		ids[0] = m->cells->ids[i * 4]; ids[1] = m->cells->ids[i * 4 + 1]; ids[2] = m->cells->ids[i * 4 + 2]; ids[3] = m->cells->ids[i * 4 + 3];
		n0[0] = m->nodes->x[m->cells->ids[i * 4]]; n0[1] = m->nodes->y[m->cells->ids[i * 4]]; n0[2] = m->nodes->z[m->cells->ids[i * 4]];
		n1[0] = m->nodes->x[m->cells->ids[i * 4 + 1]]; n1[1] = m->nodes->y[m->cells->ids[i * 4 + 1]]; n1[2] = m->nodes->z[m->cells->ids[i * 4 + 1]];
		n2[0] = m->nodes->x[m->cells->ids[i * 4 + 2]]; n2[1] = m->nodes->y[m->cells->ids[i * 4 + 2]]; n2[2] = m->nodes->z[m->cells->ids[i * 4 + 2]];
		n3[0] = m->nodes->x[m->cells->ids[i * 4 + 3]]; n3[1] = m->nodes->y[m->cells->ids[i * 4 + 3]]; n3[2] = m->nodes->z[m->cells->ids[i * 4 + 3]];
		v0[0] = n1[0] - n0[0]; v0[1] = n1[1] - n0[1]; v0[2] = n1[2] - n0[2];
		v1[0] = n2[0] - n0[0]; v1[1] = n2[1] - n0[1]; v1[2] = n2[2] - n0[2];
		v2[0] = n3[0] - n0[0]; v2[1] = n3[1] - n0[1]; v2[2] = n3[2] - n0[2];
		sarch_cross(v0, v1, vx);
		det = sarch_dot(vx, v2);
		if(det < 0)
		{
			m->cells->ids[i * 4] = ids[2];
			m->cells->ids[i * 4 + 1] = ids[1];
			m->cells->ids[i * 4 + 2] = ids[0];
			m->cells->ids[i * 4 + 3] = ids[3];
		}
	}
}

/*
* method which initialises a Position from a mesh file
*/
void initPosition(Mesh *m, Position *p)
{
	int size = m->nodes->size;
	p->size = size;
	p->px = (double*)malloc(sizeof(double)*size);
	p->py = (double*)malloc(sizeof(double)*size);
	p->pz = (double*)malloc(sizeof(double)*size);
	p->code = (int*)malloc(sizeof(int)*size);
	for (int i = 0; i < size; i++)
	{
		p->px[i] = m->nodes->x[i];
		p->py[i] = m->nodes->y[i];
		p->pz[i] = m->nodes->z[i];
		p->code[i] = m->nodes->code[i];
	}
}

/*
* method which initialises a DOF from a mesh file
*/
void initDOF(Mesh *m, DOF *d)
{
	int count = 0, size = m->nodes->size;
	for (int i = 0; i < size; i++)
	{
		switch (m->nodes->code[i])
		{
		case(0):
		{
			count += 3;
		}break;
		case(2):
		{
			count += 2;
		}break;
		case(3):
		{
			count += 3;
		}break;
		case(4): //left suture
		{
			count += 2;
		}break;
		case(5): //right suture
		{
			count += 2;
		}break;
		case(6): //back suture
		{
			count += 2;
		}break;
		case(7): //double left suture
		{
			count += 1;
		}break;
		case(8): //double right
		{
			count += 1;
		}break;
		}
	}
	d->size = count;
	d->x = (double*)malloc(sizeof(double)*count);
	d->map = (int*)malloc(sizeof(int)*count * 2);
	count = 0;
	for (int i = 0; i < size; i++)
	{
		switch (m->nodes->code[i])
		{
		case(0):
		{
			d->x[count] = m->nodes->x[i];
			d->x[count + 1] = m->nodes->y[i];
			d->x[count + 2] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 0;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 1;
			d->map[count * 2 + 4] = i; d->map[count * 2 + 5] = 2;
			count += 3;
		}break;
		case(2):
		{
			d->x[count] = m->nodes->x[i];
			d->x[count + 1] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 0;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 2;
			count += 2;
		}break;
		case(3):
		{
			d->x[count] = m->nodes->x[i];
			d->x[count + 1] = m->nodes->y[i];
			d->x[count + 2] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 0;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 1;
			d->map[count * 2 + 4] = i; d->map[count * 2 + 5] = 2;
			count += 3;
		}break;
		case(4): //left suture
		{
			d->x[count] = m->nodes->y[i];
			d->x[count + 1] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 1;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 2;
			count += 2;
		}break;
		case(5): //right suture
		{
			d->x[count] = m->nodes->y[i];
			d->x[count + 1] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 1;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 2;
			count += 2;
		}break;
		case(6): //back suture
		{
			d->x[count] = m->nodes->x[i];
			d->x[count + 1] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 0;
			d->map[count * 2 + 2] = i; d->map[count * 2 + 3] = 2;
			count += 2;
		}break;
		case(7): //double left suture
		{
			d->x[count] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 2;
			count += 1;
		}break;
		case(8): //double right suture
		{
			d->x[count] = m->nodes->z[i];
			d->map[count * 2] = i; d->map[count * 2 + 1] = 2;
			count += 1;
		}break;
		}
	}
}

/*
 * method which initialised the face and cell energy structures
*/
void initEnergy(Mesh *m) 
{
	//faces
	int nface_chass = m->faces->size_chass;
	int nface_skin = m->faces->size_skin;
	for (int i = 0; i < nface_chass; i++) 
	{
		double ab[3], ac[3];
		
		ab[0] = m->nodes->x[m->faces->nodes_chass[i * 3 + 1]] - m->nodes->x[m->faces->nodes_chass[i * 3]];
		ab[1] = m->nodes->y[m->faces->nodes_chass[i * 3 + 1]] - m->nodes->y[m->faces->nodes_chass[i * 3]];
		ab[2] = m->nodes->z[m->faces->nodes_chass[i * 3 + 1]] - m->nodes->z[m->faces->nodes_chass[i * 3]];
		ac[0] = m->nodes->x[m->faces->nodes_chass[i * 3 + 2]] - m->nodes->x[m->faces->nodes_chass[i * 3]];
		ac[1] = m->nodes->y[m->faces->nodes_chass[i * 3 + 2]] - m->nodes->y[m->faces->nodes_chass[i * 3]];
		ac[2] = m->nodes->z[m->faces->nodes_chass[i * 3 + 2]] - m->nodes->z[m->faces->nodes_chass[i * 3]];


		m->faces->surface_chass[i] = 0.5 * sqrt(	(ab[1] * ac[2] - ab[2] * ac[1]) * (ab[1] * ac[2] - ab[2] * ac[1]) + 
													(ab[2] * ac[0] - ab[0] * ac[2]) * (ab[2] * ac[0] - ab[0] * ac[2]) +
													(ab[0] * ac[1] - ab[1] * ac[0]) * (ab[0] * ac[1] - ab[1] * ac[0])
												);
	}
	for (int i = 0; i < nface_skin; i++)
	{
		double ab[3], ac[3];

		ab[0] = m->faces->oa_skin[i * 3];
		ab[1] = m->faces->oa_skin[i * 3 + 1];
		ab[2] = m->faces->oa_skin[i * 3 + 2];
		ac[0] = m->faces->ob_skin[i * 3];
		ac[1] = m->faces->ob_skin[i * 3 + 1];
		ac[2] = m->faces->ob_skin[i * 3 + 2];

		m->faces->surface_skin[i] = 0.5 * sqrt(	(ab[1] * ac[2] - ab[2] * ac[1]) * (ab[1] * ac[2] - ab[2] * ac[1]) +
												(ab[2] * ac[0] - ab[0] * ac[2]) * (ab[2] * ac[0] - ab[0] * ac[2]) +
												(ab[0] * ac[1] - ab[1] * ac[0]) * (ab[0] * ac[1] - ab[1] * ac[0])
											  );
	}

	//cells
	int ncells = m->cells->size;
	double FX[3], FY[3], FZ[3], DX[9];
	double n0[3], n1[3], n2[3], n3[3];
	double d1[3], d2[3], d3[3], d4[3];
	for (int i = 0; i < ncells; i++)
	{
		n0[0] = m->nodes->x[m->cells->ids[i * 4]]; n0[1] = m->nodes->y[m->cells->ids[i * 4]]; n0[2] = m->nodes->z[m->cells->ids[i * 4]];
		n1[0] = m->nodes->x[m->cells->ids[i * 4 + 1]]; n1[1] = m->nodes->y[m->cells->ids[i * 4 + 1]]; n1[2] = m->nodes->z[m->cells->ids[i * 4 + 1]];
		n2[0] = m->nodes->x[m->cells->ids[i * 4 + 2]]; n2[1] = m->nodes->y[m->cells->ids[i * 4 + 2]]; n2[2] = m->nodes->z[m->cells->ids[i * 4 + 2]];
		n3[0] = m->nodes->x[m->cells->ids[i * 4 + 3]]; n3[1] = m->nodes->y[m->cells->ids[i * 4 + 3]]; n3[2] = m->nodes->z[m->cells->ids[i * 4 + 3]];
		DX[0] = n3[0] - n0[0]; DX[1] = n3[1] - n0[1]; DX[2] = n3[2] - n0[2];
		DX[3] = n3[0] - n1[0]; DX[4] = n3[1] - n1[1]; DX[5] = n3[2] - n1[2];
		DX[6] = n3[0] - n2[0]; DX[7] = n3[1] - n2[1]; DX[8] = n3[2] - n2[2];
		FX[0] = 1.0; FX[1] = 0.0; FX[2] = 0.0;
		sarch_gauss(DX, FX);
		FY[0] = 0.0; FY[1] = 1.0; FY[2] = 0.0;
		sarch_gauss(DX, FY);
		FZ[0] = 0.0; FZ[1] = 0.0; FZ[2] = 1.0;
		sarch_gauss(DX, FZ);
		m->cells->jMat[i * 9    ] = FX[0];
		m->cells->jMat[i * 9 + 1] = FY[0];
		m->cells->jMat[i * 9 + 2] = FZ[0];
		m->cells->jMat[i * 9 + 3] = FX[1];
		m->cells->jMat[i * 9 + 4] = FY[1];
		m->cells->jMat[i * 9 + 5] = FZ[1];
		m->cells->jMat[i * 9 + 6] = FX[2];
		m->cells->jMat[i * 9 + 7] = FY[2];
		m->cells->jMat[i * 9 + 8] = FZ[2];
		d1[0] = n3[0] - n0[0]; d1[1] = n3[1] - n0[1]; d1[2] = n3[2] - n0[2];
		d2[0] = n3[0] - n1[0]; d2[1] = n3[1] - n1[1]; d2[2] = n3[2] - n1[2];
		d3[0] = n3[0] - n2[0]; d3[1] = n3[1] - n2[1]; d3[2] = n3[2] - n2[2];
		sarch_cross(d2, d3, d4);
		m->cells->volume[i] = fabs(sarch_dot(d1, d4) / 6.0);
		//printf("cells->volume:%.20e :: dot(d1,d4):%.20e \n", m->cells->volume[i], sarch_dot(d1, d4) / 6.0);
	}
}

/*
* method which initialises the surgery parameters
*/
SParams* initParams() 
{
	SParams *params = (SParams*)malloc(sizeof(SParams));
	params->gravity[0] = 0.000000000000000000000000000;
	params->gravity[1] = 0.000000000000000000000000000;
	params->gravity[2] = -9.80000000000000000000000000;
	params->coef_chass = 6000000.000000000000000000000;
	params->log_parameter = 1e-10;
	params->lambda = 1000.0000000000000000000000000000;
	params->mu = 150.000000000000000000000000000000000;
	params->lambdap = 32.0;//16.00000000000000000000000000000; // 8000.0;
	params->mup = 6.4;//3.2000000000000000000000000000000000; // 1600.0;
	params->density = 1000.000000000000000000000000000;
	params->derivative_precision = 1e-9;
	params->energy_variation = 1e-9;
	params->max_iterations = 1500;
	params->rot = 0.0;
	params->rot_left = 45.0;
	params->rot_right = 45.0;
	params->ax = 0.0;
	params->az = 0.1;
	return params;
}

/*
* method which initialises the surgery parameters
*/
AuxArch* initAuxiliarStructs(DOF *d)
{
	AuxArch *aa = (AuxArch*)malloc(sizeof(AuxArch));
	aa->r = (double*)malloc(sizeof(double)*d->size);
	aa->r1 = (double*)malloc(sizeof(double)*d->size);
	aa->x1 = (double*)malloc(sizeof(double)*d->size);
	aa->x2 = (double*)malloc(sizeof(double)*d->size);
	aa->p1 = (double*)malloc(sizeof(double)*d->size);
	aa->p2 = (double*)malloc(sizeof(double)*d->size);
	return aa;
}

/*
* method which cleans all data structures (Mesh, Position, DOF)
*/
void cleanAll(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	int size = m->nodes->size;
	//mesh
	//nodes
	for (int i = 0; i < size; i++)
	{
		free(m->nodes->cells[i]);
		free(m->nodes->chass[i]);
		free(m->nodes->skin[i]);
	}
	free(m->nodes->x); free(m->nodes->y); free(m->nodes->z);
	free(m->nodes->code); free(m->nodes->cells);
	free(m->nodes->chass); free(m->nodes->skin);
	//faces
	//chass
	free(m->faces->nodes_chass); free(m->faces->energy_chass); free(m->faces->surface_chass);
	free(m->faces->cx_chass); free(m->faces->cy_chass); free(m->faces->cz_chass);
	//skin
	free(m->faces->nodes_skin); free(m->faces->energy_skin); free(m->faces->surface_skin);
	free(m->faces->oa_skin); free(m->faces->ob_skin);
	//cells
	free(m->cells->ids); free(m->cells->code);
	free(m->cells->energy); free(m->cells->volume); free(m->cells->jMat);
	//mesh inner structs
	free(m->nodes);
	free(m->faces);
	free(m->cells);
	//position
	free(p->px); free(p->py); free(p->pz);
	//DOF
	free(d->x); free(d->map);
	//Aux_Arch
	free(aa->r); free(aa->r1); free(aa->x1); free(aa->x2);
	free(aa->p1); free(aa->p2);
	//free all
	free(m);
	free(p);
	free(d);
	free(params);
	free(aa);
}


//-----------------------------VISUAL EFFECTS------------------------------//
void recalc_normals(Mesh *m, Position *p)
{
	//chass
	double a[3], b[3], cross[3], ncross[3], centroid[3];
	double n0[3], n1[3], n2[3];
	printf("\nRecalculating mesh normals\n");
	for(int i=0; i < m->faces->size_all; i++)
	{
		if(m->faces->face_codes[i] == 2)
		{
			a[0] = m->nodes->x[m->faces->nodes_all[i * 3 + 1]] - m->nodes->x[m->faces->nodes_all[i * 3]];
			a[1] = m->nodes->y[m->faces->nodes_all[i * 3 + 1]] - m->nodes->y[m->faces->nodes_all[i * 3]];
			a[2] = m->nodes->z[m->faces->nodes_all[i * 3 + 1]] - m->nodes->z[m->faces->nodes_all[i * 3]];
			b[0] = m->nodes->x[m->faces->nodes_all[i * 3 + 2]] - m->nodes->x[m->faces->nodes_all[i * 3]];
			b[1] = m->nodes->y[m->faces->nodes_all[i * 3 + 2]] - m->nodes->y[m->faces->nodes_all[i * 3]];
			b[2] = m->nodes->z[m->faces->nodes_all[i * 3 + 2]] - m->nodes->z[m->faces->nodes_all[i * 3]];
			sarch_cross(a, b, cross);
			sarch_normalize(cross, ncross);
			if(ncross[1] <= 0.0)
			{
				int aux = m->faces->nodes_all[i * 3 + 1];
				m->faces->nodes_all[i * 3 + 1] = m->faces->nodes_all[i * 3 + 2];
				m->faces->nodes_all[i * 3 + 2] = aux;
			}
		}
		if(m->faces->face_codes[i] == 3)
		{
			n0[0] = p->px[m->faces->nodes_all[i * 3]];
			n0[1] = p->py[m->faces->nodes_all[i * 3]];
			n0[2] = p->pz[m->faces->nodes_all[i * 3]];
			n1[0] = p->px[m->faces->nodes_all[i * 3 + 1]];
			n1[1] = p->py[m->faces->nodes_all[i * 3 + 1]];
			n1[2] = p->pz[m->faces->nodes_all[i * 3 + 1]];
			n2[0] = p->px[m->faces->nodes_all[i * 3 + 2]];
			n2[1] = p->py[m->faces->nodes_all[i * 3 + 2]];
			n2[2] = p->pz[m->faces->nodes_all[i * 3 + 2]];
			centroid[0] = (n0[0] + n1[0] + n2[0]) / 3.0;
			centroid[1] = (n0[1] + n1[1] + n2[1]) / 3.0;
			centroid[2] = (n0[2] + n1[2] + n2[2]) / 3.0;

			
			a[0] = p->px[m->faces->nodes_all[i * 3 + 1]] - p->px[m->faces->nodes_all[i * 3]];
			a[1] = p->py[m->faces->nodes_all[i * 3 + 1]] - p->py[m->faces->nodes_all[i * 3]];
			a[2] = p->pz[m->faces->nodes_all[i * 3 + 1]] - p->pz[m->faces->nodes_all[i * 3]];
			b[0] = p->px[m->faces->nodes_all[i * 3 + 2]] - p->px[m->faces->nodes_all[i * 3]];
			b[1] = p->py[m->faces->nodes_all[i * 3 + 2]] - p->py[m->faces->nodes_all[i * 3]];
			b[2] = p->pz[m->faces->nodes_all[i * 3 + 2]] - p->pz[m->faces->nodes_all[i * 3]];
			sarch_cross(a, b, cross);
			sarch_normalize(cross, ncross);

			if(centroid[0] > 1e-8)
			{
				if(ncross[0] < 1e-8)
				{
					int aux = m->faces->nodes_all[i * 3 + 1];
					m->faces->nodes_all[i * 3 + 1] = m->faces->nodes_all[i * 3 + 2];
					m->faces->nodes_all[i * 3 + 2] = aux;
				}	
			}
			else
			{
				if(ncross[0] > 1e-8)
				{
					int aux = m->faces->nodes_all[i * 3 + 1];
					m->faces->nodes_all[i * 3 + 1] = m->faces->nodes_all[i * 3 + 2];
					m->faces->nodes_all[i * 3 + 2] = aux;
				}	
			}
		}
	}
	for(int i=0; i < m->faces->size_all; i++)
	{
		if(m->faces->face_codes[i] == 2)
		{
			a[0] = m->nodes->x[m->faces->nodes_all[i * 3 + 1]] - m->nodes->x[m->faces->nodes_all[i * 3]];
			a[1] = m->nodes->y[m->faces->nodes_all[i * 3 + 1]] - m->nodes->y[m->faces->nodes_all[i * 3]];
			a[2] = m->nodes->z[m->faces->nodes_all[i * 3 + 1]] - m->nodes->z[m->faces->nodes_all[i * 3]];
			b[0] = m->nodes->x[m->faces->nodes_all[i * 3 + 2]] - m->nodes->x[m->faces->nodes_all[i * 3]];
			b[1] = m->nodes->y[m->faces->nodes_all[i * 3 + 2]] - m->nodes->y[m->faces->nodes_all[i * 3]];
			b[2] = m->nodes->z[m->faces->nodes_all[i * 3 + 2]] - m->nodes->z[m->faces->nodes_all[i * 3]];
			sarch_cross(a, b, cross);
			sarch_normalize(cross, ncross);
		}
		if(m->faces->face_codes[i] == 3)
		{
			n0[0] = p->px[m->faces->nodes_all[i * 3]];
			n0[1] = p->py[m->faces->nodes_all[i * 3]];
			n0[2] = p->pz[m->faces->nodes_all[i * 3]];
			n1[0] = p->px[m->faces->nodes_all[i * 3 + 1]];
			n1[1] = p->py[m->faces->nodes_all[i * 3 + 1]];
			n1[2] = p->pz[m->faces->nodes_all[i * 3 + 1]];
			n2[0] = p->px[m->faces->nodes_all[i * 3 + 2]];
			n2[1] = p->py[m->faces->nodes_all[i * 3 + 2]];
			n2[2] = p->pz[m->faces->nodes_all[i * 3 + 2]];
			centroid[0] = (n0[0] + n1[0] + n2[0]) / 3.0;
			centroid[1] = (n0[1] + n1[1] + n2[1]) / 3.0;
			centroid[2] = (n0[2] + n1[2] + n2[2]) / 3.0;

			
			a[0] = p->px[m->faces->nodes_all[i * 3 + 1]] - p->px[m->faces->nodes_all[i * 3]];
			a[1] = p->py[m->faces->nodes_all[i * 3 + 1]] - p->py[m->faces->nodes_all[i * 3]];
			a[2] = p->pz[m->faces->nodes_all[i * 3 + 1]] - p->pz[m->faces->nodes_all[i * 3]];
			b[0] = p->px[m->faces->nodes_all[i * 3 + 2]] - p->px[m->faces->nodes_all[i * 3]];
			b[1] = p->py[m->faces->nodes_all[i * 3 + 2]] - p->py[m->faces->nodes_all[i * 3]];
			b[2] = p->pz[m->faces->nodes_all[i * 3 + 2]] - p->pz[m->faces->nodes_all[i * 3]];
			sarch_cross(a, b, cross);
			sarch_normalize(cross, ncross);
		}
	}

}



//----------------------------SIMULATION------------------------------//
//----------------------------SIMULATION------------------------------//

/*
* method which passes to DOF the current mesh free coordinates from Position
*/
void pos2dof_global(Position *p, DOF *d) 
{
	int coord;
	for (int i = 0; i < d->size; i++)
	{
		coord = d->map[i * 2 + 1];
		switch (coord)
		{
		case(0):
		{
			d->x[i] = p->px[d->map[i * 2]];
		} break;
		case(1):
		{
			d->x[i] = p->py[d->map[i * 2]];
		} break;
		case(2):
		{
			d->x[i] = p->pz[d->map[i * 2]];
		} break;
		}
	}
}

/*
 * method which passes to Position the changes made in DOF 
*/
void dof2pos_global(Position *p, DOF *d)
{
	int coord, id, code;
	for (int i = 0; i < d->size; i++) 
	{
		id = d->map[i * 2];
		coord = d->map[i * 2 + 1];
		code = p->code[id];
		switch (coord)
		{
			case(0): 
			{
				p->px[id] = d->x[i];
			} break;
			case(1):
			{
				p->py[id] = d->x[i];
			} break;
			case(2):
			{
				switch(code)
				{
					case(4):
					{
						p->px[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(5):
					{
						p->px[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(2): //chassignac
					{
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(6): //back suture
					{
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(7):
					{
						p->px[id] = 0.0;
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(8):
					{
						p->px[id] = 0.0;
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					default:
					{
						p->pz[id] = d->x[i];
					} break;				
				}
			} break;
		}
	}

}

void suture(Mesh *m, Position *p, DOF *d, SParams *params)
{
	int coord, id, code;
	for (int i = 0; i < d->size; i++) 
	{
		id = d->map[i * 2];
		coord = d->map[i * 2 + 1];
		code = p->code[id];
		switch (coord)
		{
			case(0): 
			{
				p->px[id] = d->x[i];
			} break;
			case(1):
			{
				p->py[id] = d->x[i];
			} break;
			case(2):
			{
				switch(code)
				{
					case(4):
					{
						p->px[id] = 0.0;
						p->pz[id] = d->x[i];
						p->pz[id] = (m->nodes->x[id] + params->ax) * sin(params->rot_left - params->rot) + 
									(m->nodes->z[id] + params->az) * cos(params->rot_left - params->rot);
					} break;
					case(5):
					{
						p->px[id] = 0.0;
						p->pz[id] = d->x[i];
						p->pz[id] = (m->nodes->x[id] + params->ax) * sin(-params->rot_right - params->rot) + 
									(m->nodes->z[id] + params->az) * cos(-params->rot_right - params->rot);
					} break;
					case(2): //chassignac
					{
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(6): //back suture
					{
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
					} break;
					case(7):
					{
						p->px[id] = 0.0;
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
						p->pz[id] = (m->nodes->x[id] + params->ax) * sin(params->rot_left - params->rot) + 
									(m->nodes->z[id] + params->az) * cos(params->rot_left - params->rot);
					} break;
					case(8):
					{
						p->px[id] = 0.0;
						p->py[id] = 0.0;
						p->pz[id] = d->x[i];
						p->pz[id] = (m->nodes->x[id] + params->ax) * sin(-params->rot_right - params->rot) + 
									(m->nodes->z[id] + params->az) * cos(-params->rot_right - params->rot);
					} break;
					default:
					{
						p->pz[id] = d->x[i];
					} break;				
				}
			} break;
		}
	}
}

/*
 * method which passes to Position the changes made in a specific DOF
 * id is the index of the DOF that needs to be updated in Position
*/
void dof2pos_local(int id, Position *p, DOF *d)
{
	int coord, code, i;
	i = d->map[id * 2];
	coord = d->map[id * 2 + 1];
	code = p->code[i];
	switch (coord)
	{
		case(0):
		{
			p->px[i] = d->x[id];
		} break;
		case(1):
		{
			p->py[i] = d->x[id];
		} break;
		case(2):
		{
				switch(code)
				{
					case(4):
					{
						p->px[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(5):
					{
						p->px[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(7):
					{
						p->px[i] = 0.0;
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(8):
					{
						p->px[i] = 0.0;
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(2):
					{
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(6):
					{
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					default:
					{
						p->pz[i] = d->x[id];
					} break;				
				}
		} break;
	}
}

/*
 * method which passes to Position the changes made in a specific DOF
 * id is the index of the DOF that needs to be updated in Position
*/
void suture_local(int id, Mesh *m, Position *p, DOF *d, SParams *params)
{
	int coord, code, i;
	i = d->map[id * 2];
	coord = d->map[id * 2 + 1];
	code = p->code[i];
	switch (coord)
	{
		case(0):
		{
			p->px[i] = d->x[id];
		} break;
		case(1):
		{
			p->py[i] = d->x[id];
		} break;
		case(2):
		{
				switch(code)
				{
					case(4):
					{
						p->px[i] = 0.0;
						p->pz[i] = d->x[id];
						p->pz[i] =  (m->nodes->x[i] + params->ax) * sin(params->rot_left - params->rot) + 
									(m->nodes->z[i] + params->az) * cos(params->rot_left - params->rot);
					} break;
					case(5):
					{
						p->px[i] = 0.0;
						p->pz[i] = d->x[id];
						p->pz[i] =  (m->nodes->x[i] + params->ax) * sin(-params->rot_right - params->rot) + 
									(m->nodes->z[i] + params->az) * cos(-params->rot_right - params->rot);
					} break;
					case(7):
					{
						p->px[i] = 0.0;
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
						p->pz[i] =  (m->nodes->x[i] + params->ax) * sin(params->rot_left - params->rot) + 
									(m->nodes->z[i] + params->az) * cos(params->rot_left - params->rot);
					} break;
					case(8):
					{
						p->px[i] = 0.0;
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
						p->pz[i] =  (m->nodes->x[i] + params->ax) * sin(-params->rot_right - params->rot) + 
									(m->nodes->z[i] + params->az) * cos(-params->rot_right - params->rot);
					} break;
					case(2):
					{
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					case(6):
					{
						p->py[i] = 0.0;
						p->pz[i] = d->x[id];
					} break;
					default:
					{
						p->pz[i] = d->x[id];
					} break;				
				}
		} break;
	}
}

/*
* method which computes the chassaignac space faces strain-energy
*/
double calc_energy_chass(int id, Face *faces, Position *p, SParams *params)
{
	double res, op[3];
	op[0] = ((p->px[faces->nodes_chass[id * 3]] + p->px[faces->nodes_chass[id * 3 + 1]] + p->px[faces->nodes_chass[id * 3 + 2]]) / 3.0) - faces->cx_chass[id];
	op[1] = ((p->py[faces->nodes_chass[id * 3]] + p->py[faces->nodes_chass[id * 3 + 1]] + p->py[faces->nodes_chass[id * 3 + 2]]) / 3.0) - faces->cy_chass[id];
	op[2] = ((p->pz[faces->nodes_chass[id * 3]] + p->pz[faces->nodes_chass[id * 3 + 1]] + p->pz[faces->nodes_chass[id * 3 + 2]]) / 3.0) - faces->cz_chass[id];
	res = ((op[0] * op[0]) + (op[1] * op[1]) + (op[2] * op[2])) * params->coef_chass * faces->surface_chass[id];
	return res;
}

/*
* method which computes the chassaignac space faces strain-energy
*/
//double calc_energy_skin_old(int id, Face *faces, Position *p, SParams *params)
double calc_energy_skin(int id, Face *faces, Position *p, SParams *params)
{
	double res, oa[3], ob[3], oap[3], obp[3], dop[4], tri[4], crossA[3], crossB[3], cxy[4], tr, det;
	oa[0] = faces->oa_skin[id * 3];
	oa[1] = faces->oa_skin[id * 3 + 1];
	oa[2] = faces->oa_skin[id * 3 + 2];
	ob[0] = faces->ob_skin[id * 3];
	ob[1] = faces->ob_skin[id * 3 + 1];
	ob[2] = faces->ob_skin[id * 3 + 2];
	oap[0] = p->px[faces->nodes_skin[id * 3 + 1]] - p->px[faces->nodes_skin[id * 3]];
	oap[1] = p->py[faces->nodes_skin[id * 3 + 1]] - p->py[faces->nodes_skin[id * 3]];
	oap[2] = p->pz[faces->nodes_skin[id * 3 + 1]] - p->pz[faces->nodes_skin[id * 3]];
	obp[0] = p->px[faces->nodes_skin[id * 3 + 2]] - p->px[faces->nodes_skin[id * 3]];
	obp[1] = p->py[faces->nodes_skin[id * 3 + 2]] - p->py[faces->nodes_skin[id * 3]];
	obp[2] = p->pz[faces->nodes_skin[id * 3 + 2]] - p->pz[faces->nodes_skin[id * 3]];
	dop[0] = sqrt((oa[0] * oa[0]) + (oa[1] * oa[1]) + (oa[2] * oa[2]));
	dop[1] = sqrt((ob[0] * ob[0]) + (ob[1] * ob[1]) + (ob[2] * ob[2]));
	dop[2] = sqrt((oap[0] * oap[0]) + (oap[1] * oap[1]) + (oap[2] * oap[2]));
	dop[3] = sqrt((obp[0] * obp[0]) + (obp[1] * obp[1]) + (obp[2] * obp[2]));
	crossA[0] = oa[1] * ob[2] - oa[2] * ob[1];
	crossA[1] = oa[2] * ob[0] - oa[0] * ob[2];
	crossA[2] = oa[0] * ob[1] - oa[1] * ob[0];
	crossB[0] = oap[1] * obp[2] - oap[2] * obp[1];
	crossB[1] = oap[2] * obp[0] - oap[0] * obp[2];
	crossB[2] = oap[0] * obp[1] - oap[1] * obp[0];
	tri[0] = sarch_dot(oa, ob) / dop[0] / dop[1];
	tri[1] = sqrt((crossA[0] * crossA[0]) + (crossA[1] * crossA[1]) + (crossA[2] * crossA[2])) / dop[0] / dop[1];
	tri[2] = sarch_dot(oap, obp) / dop[2] / dop[3];
	tri[3] = sqrt((crossB[0] * crossB[0]) + (crossB[1] * crossB[1]) + (crossB[2] * crossB[2])) / dop[2] / dop[3];
	cxy[0] = dop[3] / dop[1];
	cxy[1] = 0.0;
	cxy[2] = (tri[2] * dop[2] - cxy[0] * tri[0] * dop[0]) / tri[1] / dop[0]; 
	cxy[3] = tri[3] * dop[2] / tri[1] / dop[0];
	tr = (cxy[0] * cxy[0]) + (cxy[1] * cxy[1]) + (cxy[2] * cxy[2]) + (cxy[3] * cxy[3]);
	//tr = (cxy[0] * cxy[0]) + (cxy[2] * cxy[2]) + (cxy[3] * cxy[3]);
	det = cxy[0] * cxy[3] - cxy[2] * cxy[1];
	//det = cxy[0] * cxy[3];
	if (det < params->log_parameter) 
	{
		det = params->log_parameter;
	}
	res = params->mup * 0.5 * (tr - 2.0 - 2.0 * log(det));
	res = res + ( params->lambdap * 0.5 * (det - 1.0) * (det - 1.0) );
	res *= faces->surface_skin[id];
	
	return res;
}

double calc_energy_skin_reduced_divs(int id, Face *faces, Position *p, SParams *params)
{
	double res, oa[3], ob[3], oap[3], obp[3], dop[4], idop[4], tri[4], crossA[3], crossB[3], cxy[4], tr, det;
	oa[0] = faces->oa_skin[id * 3];
	oa[1] = faces->oa_skin[id * 3 + 1];
	oa[2] = faces->oa_skin[id * 3 + 2];
	ob[0] = faces->ob_skin[id * 3];
	ob[1] = faces->ob_skin[id * 3 + 1];
	ob[2] = faces->ob_skin[id * 3 + 2];
	oap[0] = p->px[faces->nodes_skin[id * 3 + 1]] - p->px[faces->nodes_skin[id * 3]];
	oap[1] = p->py[faces->nodes_skin[id * 3 + 1]] - p->py[faces->nodes_skin[id * 3]];
	oap[2] = p->pz[faces->nodes_skin[id * 3 + 1]] - p->pz[faces->nodes_skin[id * 3]];
	obp[0] = p->px[faces->nodes_skin[id * 3 + 2]] - p->px[faces->nodes_skin[id * 3]];
	obp[1] = p->py[faces->nodes_skin[id * 3 + 2]] - p->py[faces->nodes_skin[id * 3]];
	obp[2] = p->pz[faces->nodes_skin[id * 3 + 2]] - p->pz[faces->nodes_skin[id * 3]];
	dop[0] = sqrt((oa[0] * oa[0]) + (oa[1] * oa[1]) + (oa[2] * oa[2]));
	dop[1] = sqrt((ob[0] * ob[0]) + (ob[1] * ob[1]) + (ob[2] * ob[2]));
	dop[2] = sqrt((oap[0] * oap[0]) + (oap[1] * oap[1]) + (oap[2] * oap[2]));
	dop[3] = sqrt((obp[0] * obp[0]) + (obp[1] * obp[1]) + (obp[2] * obp[2]));
	idop[0] = 1.0 / dop[0];
	idop[1] = 1.0 / dop[1];
	idop[2] = 1.0 / dop[2];
	idop[3] = 1.0 / dop[3];
	crossA[0] = oa[1] * ob[2] - oa[2] * ob[1];
	crossA[1] = oa[2] * ob[0] - oa[0] * ob[2];
	crossA[2] = oa[0] * ob[1] - oa[1] * ob[0];
	crossB[0] = oap[1] * obp[2] - oap[2] * obp[1];
	crossB[1] = oap[2] * obp[0] - oap[0] * obp[2];
	crossB[2] = oap[0] * obp[1] - oap[1] * obp[0];
	tri[0] = sarch_dot(oa, ob) * idop[0] * idop[1];
	tri[1] = sqrt((crossA[0] * crossA[0]) + (crossA[1] * crossA[1]) + (crossA[2] * crossA[2])) * idop[0] * idop[1];
	tri[2] = sarch_dot(oap, obp) * idop[2] * idop[3];
	tri[3] = sqrt((crossB[0] * crossB[0]) + (crossB[1] * crossB[1]) + (crossB[2] * crossB[2])) * idop[2] * idop[3];
	cxy[0] = dop[3] * idop[1];
	cxy[1] = 0.0;
	cxy[2] = (tri[2] * dop[2] - cxy[0] * tri[0] * dop[0]) / tri[1] * idop[0]; 
	cxy[3] = tri[3] * dop[2] / tri[1] * idop[0];
	tr = (cxy[0] * cxy[0]) + (cxy[1] * cxy[1]) + (cxy[2] * cxy[2]) + (cxy[3] * cxy[3]);
	//tr = (cxy[0] * cxy[0]) + (cxy[2] * cxy[2]) + (cxy[3] * cxy[3]);
	det = cxy[0] * cxy[3] - cxy[2] * cxy[1];
	//det = cxy[0] * cxy[3];
	if (det < params->log_parameter) 
	{
		det = params->log_parameter;
	}
	res = params->mup * 0.5 * (tr - 2.0 - 2.0 * log(det));
	res = res + ( params->lambdap * 0.5 * (det - 1.0) * (det - 1.0) );
	res *= faces->surface_skin[id];
	
	return res;
}

/*
* method which computes the celsl strain-energy
*/
double calc_energy_cell(int id, Cell *cells, Position *p, SParams *params)
{
	double res, F[9], E[9], tr, det;
	F[0] = p->px[cells->ids[id * 4 + 3]] - p->px[cells->ids[id * 4    ]];
	F[1] = p->px[cells->ids[id * 4 + 3]] - p->px[cells->ids[id * 4 + 1]];
	F[2] = p->px[cells->ids[id * 4 + 3]] - p->px[cells->ids[id * 4 + 2]];
	F[3] = p->py[cells->ids[id * 4 + 3]] - p->py[cells->ids[id * 4    ]];
	F[4] = p->py[cells->ids[id * 4 + 3]] - p->py[cells->ids[id * 4 + 1]];
	F[5] = p->py[cells->ids[id * 4 + 3]] - p->py[cells->ids[id * 4 + 2]];
	F[6] = p->pz[cells->ids[id * 4 + 3]] - p->pz[cells->ids[id * 4    ]];
	F[7] = p->pz[cells->ids[id * 4 + 3]] - p->pz[cells->ids[id * 4 + 1]];
	F[8] = p->pz[cells->ids[id * 4 + 3]] - p->pz[cells->ids[id * 4 + 2]];
	E[0] = cells->jMat[id * 9    ] * F[0] + cells->jMat[id * 9 + 1] * F[1] + cells->jMat[id * 9 + 2] * F[2];
	E[1] = cells->jMat[id * 9 + 3] * F[0] + cells->jMat[id * 9 + 4] * F[1] + cells->jMat[id * 9 + 5] * F[2];
	E[2] = cells->jMat[id * 9 + 6] * F[0] + cells->jMat[id * 9 + 7] * F[1] + cells->jMat[id * 9 + 8] * F[2];
	E[3] = cells->jMat[id * 9    ] * F[3] + cells->jMat[id * 9 + 1] * F[4] + cells->jMat[id * 9 + 2] * F[5];
	E[4] = cells->jMat[id * 9 + 3] * F[3] + cells->jMat[id * 9 + 4] * F[4] + cells->jMat[id * 9 + 5] * F[5];
	E[5] = cells->jMat[id * 9 + 6] * F[3] + cells->jMat[id * 9 + 7] * F[4] + cells->jMat[id * 9 + 8] * F[5];
	E[6] = cells->jMat[id * 9    ] * F[6] + cells->jMat[id * 9 + 1] * F[7] + cells->jMat[id * 9 + 2] * F[8];
	E[7] = cells->jMat[id * 9 + 3] * F[6] + cells->jMat[id * 9 + 4] * F[7] + cells->jMat[id * 9 + 5] * F[8];
	E[8] = cells->jMat[id * 9 + 6] * F[6] + cells->jMat[id * 9 + 7] * F[7] + cells->jMat[id * 9 + 8] * F[8];
	tr = (E[0] * E[0]) + (E[1] * E[1]) + (E[2] * E[2]) + (E[3] * E[3]) + (E[4] * E[4]) + (E[5] * E[5]) + (E[6] * E[6]) + (E[7] * E[7]) + (E[8] * E[8]);
	det = E[0] * (E[4] * E[8] - E[5] * E[7]) - E[1] * (E[3] * E[8] - E[5] * E[6]) + E[2] * (E[3] * E[7] - E[4] * E[6]);
	if (det < params->log_parameter)
	{
		det = params->log_parameter;
	}
	res = params->mu * 0.5 * (tr - 3.0 - 2.0 * log(det));
	res = res + ( params->lambda * 0.5 * (det - 1.0) * (det - 1.0) );
	res = res - (  
					params->density * params->gravity[0] * (p->px[cells->ids[id * 4]] + p->px[cells->ids[id * 4 + 1]] + p->px[cells->ids[id * 4 + 2]] + p->px[cells->ids[id * 4 + 3]]) * 0.25 +
					params->density * params->gravity[1] * (p->py[cells->ids[id * 4]] + p->py[cells->ids[id * 4 + 1]] + p->py[cells->ids[id * 4 + 2]] + p->py[cells->ids[id * 4 + 3]]) * 0.25 +
					params->density * params->gravity[2] * (p->pz[cells->ids[id * 4]] + p->pz[cells->ids[id * 4 + 1]] + p->pz[cells->ids[id * 4 + 2]] + p->pz[cells->ids[id * 4 + 3]]) * 0.25
				);
	res *= cells->volume[id];
	//printf("4->val:%.20e\n", res);
	return res;
}

/*
* method which computes the meshe's strain-energy 
*/
void calc_energy_global(Mesh *m, Position *p, SParams *params) 
{
	int ncells = m->cells->size, nchass = m->faces->size_chass, nskin = m->faces->size_skin;
	double sum = 0.0, val;
	//printf("\nchass\n");
	for (int i = 0; i < nchass ; i++) 
	{
		//energy_chass
		val = calc_energy_chass(i, m->faces, p, params);
		m->faces->energy_chass[i] = val;
		sum += val;
		//printf("val:%.20e\n", val);
	}
	//printf("\nskin\n");
	for (int i = 0; i < nskin; i++)
	{
		//energy_skin
		val = calc_energy_skin(i, m->faces, p, params);
		m->faces->energy_skin[i] = val;
		sum += val;
		//printf("val:%.20e\n", val);
	}
	//printf("\ncells\n");
	for (int i = 0; i < ncells; i++)
	{
		//energy_cell
		val = calc_energy_cell(i, m->cells, p, params);
		m->cells->energy[i] = val;
		sum += val;
		//printf("val:%.20e\n", val);
	}
	//printf("sum:%.20e\n", sum);
	m->energy = sum;
}

/*
* method which computes the local gradient of the mesh's strain-energy
*/
void local_gradient(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	int ndof = d->size, nchass, nskin, ncells, id;
	double act_val = 0.0, aux_energy = 0.0, local_energy = 0.0, h = params->derivative_precision;
	for (int i = 0; i < ndof; i++)
	{
		act_val = d->x[i];
		d->x[i] += h;
		//dof2pos_local
		dof2pos_local(i, p, d);
		//local_gradient
		local_energy = m->energy;
		id = d->map[i * 2];
		nchass = m->nodes->chass[id][0];
		nskin = m->nodes->skin[id][0];
		ncells = m->nodes->cells[id][0];
		//cells
		for (int k = 0; k < ncells; k++)
		{
			int idcell = m->nodes->cells[id][k + 1];
			aux_energy = calc_energy_cell(idcell, m->cells, p, params);
			local_energy = local_energy - m->cells->energy[idcell] + aux_energy;
		}
		//chass
		for (int k = 0; k < nchass; k++)
		{
			aux_energy = calc_energy_chass(m->nodes->chass[id][k + 1], m->faces, p, params);
			local_energy = local_energy - m->faces->energy_chass[m->nodes->chass[id][k + 1]] + aux_energy;
		}
		//skin
		for (int k = 0; k < nskin; k++)
		{
			aux_energy = calc_energy_skin(m->nodes->skin[id][k + 1], m->faces, p, params);
			local_energy = local_energy - m->faces->energy_skin[m->nodes->skin[id][k + 1]] + aux_energy;
		}
		aa->r[i] = (local_energy - m->energy) / h;
		d->x[i] = act_val;
		//dof2pos_local
		dof2pos_local(i, p, d);
	}
}

/*
* method which computes the local gradient of the mesh's strain-energy
*/
void local_gradient_suture(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	int ndof = d->size, nchass, nskin, ncells, id;
	double act_val = 0.0, aux_energy = 0.0, local_energy = 0.0, h = params->derivative_precision;
	for (int i = 0; i < ndof; i++)
	{
		act_val = d->x[i];
		d->x[i] += h;
		//dof2pos_local
		suture_local(i, m, p, d, params);
		//local_gradient
		local_energy = m->energy;
		id = d->map[i * 2];
		nchass = m->nodes->chass[id][0];
		nskin = m->nodes->skin[id][0];
		ncells = m->nodes->cells[id][0];
		//cells
		for (int k = 0; k < ncells; k++)
		{
			int idcell = m->nodes->cells[id][k + 1];
			aux_energy = calc_energy_cell(idcell, m->cells, p, params);
			local_energy = local_energy - m->cells->energy[idcell] + aux_energy;
		}
		//chass
		for (int k = 0; k < nchass; k++)
		{
			aux_energy = calc_energy_chass(m->nodes->chass[id][k + 1], m->faces, p, params);
			local_energy = local_energy - m->faces->energy_chass[m->nodes->chass[id][k + 1]] + aux_energy;
		}
		//skin
		for (int k = 0; k < nskin; k++)
		{
			aux_energy = calc_energy_skin(m->nodes->skin[id][k + 1], m->faces, p, params);
			local_energy = local_energy - m->faces->energy_skin[m->nodes->skin[id][k + 1]] + aux_energy;
		}
		aa->r[i] = (local_energy - m->energy) / h;
		d->x[i] = act_val;
		//dof2pos_local
		suture_local(i, m, p, d, params);
	}
}

/*
* method which computes the gradient and the strain-energy residual
*/
void calc_gradient(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	//calc_energy_global
	calc_energy_global(m, p, params);
	//loop dof
	local_gradient(m, p, d, params, aa);
}


/*
* method which computes the gradient and the strain-energy residual
*/
void calc_gradient_suture(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	//calc_energy_global
	calc_energy_global(m, p, params);
	//loop dof
	local_gradient_suture(m, p, d, params, aa);
}

/*
* method which computes the best displacement factor to apply on the current mesh configuration (new method)
*/
void line_search(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	double E = 0.0, Ep = 1000.0, Em = 0.0, eps = 1e-6, a_old = 1000.0, a_new = 0.0;
	int ndof = d->size;
	int counter = 0;
	//loop
	while (((fabs(Ep - Em) / fabs(Ep + Em)) > (eps)) && (counter < 100)) 
	{
		a_old = a_new;
		for (int i = 0; i < ndof; i++) 
		{
			d->x[i] = aa->x1[i] + a_old * aa->p2[i];
		}
		dof2pos_global(p, d);
		calc_energy_global(m, p, params);
		E = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i] + (a_old + eps) * aa->p2[i];
		}
		dof2pos_global(p, d);
		calc_energy_global(m, p, params);
		Ep = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i] + (a_old - eps) * aa->p2[i];
		}
		dof2pos_global(p, d);
		calc_energy_global(m, p, params);
		Em = m->energy;
		if ((-2.0*E + Ep + Em) == 0.0) 
		{
			a_new = eps;
		}
		else 
		{
			a_new = a_old - (((Ep - Em) * eps) / (-2.0*E + Ep + Em)) * 0.5;
		}
		counter++;
		//printf("\na_new:%.20e -> a_old:%.20e -> E,Ep,Em:(%.20e,%.20e,%.20e) \n", a_new, a_old, E, Ep, Em);
	}
	aa->alpha = a_new;
}

/*
* method which computes the best displacement factor to apply on the current mesh configuration (new method)
*/
void line_search_suture(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	double E = 0.0, Ep = 1000.0, Em = 0.0, eps = 1e-6, a_old = 1000.0, a_new = 0.0;
	int ndof = d->size;
	int counter = 0;
	//loop
	while (((fabs(Ep - Em) / fabs(Ep + Em)) > (eps)) && (counter < 100)) 
	{
		a_old = a_new;
		for (int i = 0; i < ndof; i++) 
		{
			d->x[i] = aa->x1[i] + a_old * aa->p2[i];
		}
		suture(m, p, d, params);
		calc_energy_global(m, p, params);
		E = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i] + (a_old + eps) * aa->p2[i];
		}
		suture(m, p, d, params);
		calc_energy_global(m, p, params);
		Ep = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i] + (a_old - eps) * aa->p2[i];
		}
		suture(m, p, d, params);
		calc_energy_global(m, p, params);
		Em = m->energy;
		if ((-2.0*E + Ep + Em) == 0.0) 
		{
			a_new = eps;
		}
		else 
		{
			a_new = a_old - (((Ep - Em) * eps) / (-2.0*E + Ep + Em)) * 0.5;
		}
		counter++;
		//printf("\na_new:%.20e -> a_old:%.20e -> E,Ep,Em:(%.20e,%.20e,%.20e) \n", a_new, a_old, E, Ep, Em);
	}
	aa->alpha = a_new;
}

/*
* method which computes the best displacement factor to apply on the current mesh configuration (old method)
*/
void line_search_old(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch* aa)
{
	double j1 = 0.0, j2 = 0.0, eps = 1e-6, a = 0.0, b = 100.0, c = 0.0, e = 0.0, gg = 0.1;
	int ndof = d->size;
	//loop
	while (fabs(b - a) > eps)
	{
		e = gg * (b - a) * 0.5;
		c = (a + b) * 0.5;
		//1st
		for (int i = 0; i < ndof; i++) 
		{
			d->x[i] = aa->x1[i] + (c - e) * aa->p2[i];
		}
		dof2pos_global(p, d);
		calc_energy_global(m, p, params);
		j1 = m->energy;
		//2nd
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i] + (c + e) * aa->p2[i];
		}
		dof2pos_global(p, d);
		calc_energy_global(m, p, params);
		j2 = m->energy;
		if (j1 < j2)
		{
			b = c + e;
		}
		else if (j1 > j2)
		{
			a = c - e;
		}
		else
		{
			a = c - e;
			b = c + e;
		}
		//printf("\na:%.20e -> b:%.20e -> j1,j2:(%.20e,%.20e) \n", a, b, j1, j2);
	}
	aa->alpha = a;
}


/*
 * method which simulates a surgical procedure 
 * Mesh has the information about the nodes, faces and cells
 * Position holds the data about the current node configuration
 * DOF holds the data for displacement evaluation
 * Params holds the parameters of the surgery
*/
void simulation(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch *aa) 
{
	int ndof = d->size, counter = 0;
	//params->gravity[2] = -9.80000000000000000000000000000000;
	double nor1 = 0.0, nor2 = 0.0, nor3 = 0.0, strainEnergy = 1e6;
	//pos2dof_global
	pos2dof_global(p, d);
	//calc_gradient
	calc_gradient(m, p, d, params, aa);
	//printf("\nenergy:%.20e\n", m->energy);
	//return;
	for (int i = 0; i < ndof; i++)
	{
		aa->p2[i] = -aa->r[i];
		aa->x1[i] = d->x[i];
	}
	//line_search
	line_search(m, p, d, params, aa);
	for (int i = 0; i < ndof; i++)
	{
		aa->x2[i] = aa->x1[i] + aa->alpha * aa->p2[i];
		nor1 += (aa->r[i] * aa->r[i]);
		aa->p1[i] = aa->p2[i];
		aa->x1[i] = aa->x2[i];
	}
	//loop
	//printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	while ( (fabs(strainEnergy - m->energy) > params->energy_variation) && (counter < params->max_iterations))
	//while ( (counter < params->max_iterations))
	{
		nor2 = 0.0; nor3 = 0.0;
		strainEnergy = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i];
			aa->r1[i] = aa->r[i];
		}
		//dof2pos_global
		dof2pos_global(p, d);
		//calc_gradient
		calc_gradient(m, p, d, params, aa);
		//beta calculation
		for (int i = 0; i < ndof; i++)
		{
			nor2 += aa->r[i] * aa->r[i];
			nor3 += aa->r[i] * aa->r1[i];
		}
		aa->beta = (nor2 - nor3) / nor1;
		nor1 = nor2;
		for (int i = 0; i < ndof; i++)
		{
			aa->p2[i] = -aa->r[i] + aa->beta * aa->p1[i];
			d->x[i] = aa->x1[i];
		}
		//line_search
		line_search(m, p, d, params, aa);
		for (int i = 0; i < ndof; i++)
		{
			aa->x2[i] = aa->x1[i] + aa->alpha * aa->p2[i];
			aa->p1[i] = aa->p2[i];
			aa->x1[i] = aa->x2[i];
		}
		counter++;
		//printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	}
	printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	//dof2pos_global
	dof2pos_global(p, d);
}


/*
 * method which simulates a surgical procedure 
 * Mesh has the information about the nodes, faces and cells
 * Position holds the data about the current node configuration
 * DOF holds the data for displacement evaluation
 * Params holds the parameters of the surgery
*/
void simulation_suture(Mesh *m, Position *p, DOF *d, SParams *params, AuxArch *aa) 
{
	int ndof = d->size, counter = 0;
	params->gravity[2] = 0.00000000000000000000000000000000;
	double nor1 = 0.0, nor2 = 0.0, nor3 = 0.0, strainEnergy = 1e6;
	//pos2dof_global
	//pos2dof_global(p, d);
	suture(m, p, d, params);
	//calc_gradient
	//calc_gradient(m, p, d, params, aa);
	calc_gradient_suture(m, p, d, params, aa);
	printf("\nenergy:%.20e\n", m->energy);

	for(int i=0; i < p->size; i++)
	{
		printf("%d -> (%e,%e,%e)\n", i, p->px[i], p->py[i], p->pz[i]);
	}

	for (int i = 0; i < ndof; i++)
	{
		aa->p2[i] = -aa->r[i];
		aa->x1[i] = d->x[i];
	}
	//line_search
	line_search(m, p, d, params, aa);
	for (int i = 0; i < ndof; i++)
	{
		aa->x2[i] = aa->x1[i] + aa->alpha * aa->p2[i];
		nor1 += (aa->r[i] * aa->r[i]);
		aa->p1[i] = aa->p2[i];
		aa->x1[i] = aa->x2[i];
	}
	//loop
	printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	
	//while ( (fabs(strainEnergy - m->energy) > params->energy_variation) && (counter < params->max_iterations))
	//while ( (counter < params->max_iterations))
		//suture step
	//while(counter < 500)
	//{
		nor2 = 0.0; nor3 = 0.0;
		strainEnergy = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i];
			aa->r1[i] = aa->r[i];
		}
		//dof2pos_global
		suture(m, p, d, params);
		//dof2pos_global(p, d);	
		//calc_gradient
		calc_gradient_suture(m, p, d, params, aa);
		//beta calculation
		for (int i = 0; i < ndof; i++)
		{
			nor2 += aa->r[i] * aa->r[i];
			nor3 += aa->r[i] * aa->r1[i];
		}
		aa->beta = (nor2 - nor3) / nor1;
		nor1 = nor2;
		for (int i = 0; i < ndof; i++)
		{
			aa->p2[i] = -aa->r[i] + aa->beta * aa->p1[i];
			d->x[i] = aa->x1[i];
		}
		//line_search
		line_search_suture(m, p, d, params, aa);
		for (int i = 0; i < ndof; i++)
		{
			aa->x2[i] = aa->x1[i] + aa->alpha * aa->p2[i];
			aa->p1[i] = aa->p2[i];
			aa->x1[i] = aa->x2[i];
		}
		counter++;
		printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	//}
	//minimisation_step
	
	//while(counter < 500)
	while ( (fabs(strainEnergy - m->energy) > params->energy_variation) && (counter < params->max_iterations))
	{
		nor2 = 0.0; nor3 = 0.0;
		strainEnergy = m->energy;
		for (int i = 0; i < ndof; i++)
		{
			d->x[i] = aa->x1[i];
			aa->r1[i] = aa->r[i];
		}
		//dof2pos_global
		dof2pos_global(p, d);
		//calc_gradient
		calc_gradient(m, p, d, params, aa);
		//beta calculation
		for (int i = 0; i < ndof; i++)
		{
			nor2 += aa->r[i] * aa->r[i];
			nor3 += aa->r[i] * aa->r1[i];
		}
		aa->beta = (nor2 - nor3) / nor1;
		nor1 = nor2;
		for (int i = 0; i < ndof; i++)
		{
			aa->p2[i] = -aa->r[i] + aa->beta * aa->p1[i];
			d->x[i] = aa->x1[i];
		}
		//line_search
		line_search(m, p, d, params, aa);
		for (int i = 0; i < ndof; i++)
		{
			aa->x2[i] = aa->x1[i] + aa->alpha * aa->p2[i];
			aa->p1[i] = aa->p2[i];
			aa->x1[i] = aa->x2[i];
		}
		counter++;
		printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	}
	
	printf("\nMesh energy:%e :: residual: %e :: number of iterations: %d\n", m->energy, nor1, counter);
	//dof2pos_global
	dof2pos_global(p, d);
}

//--------------------------------Evaluate cuts--------------------------------//
//--------------------------------Evaluate cuts--------------------------------//

/*
* method which evaluates the points and cuts to be performed on the breast
* find point A
* find point B
* find point Lp
* find point Rp
* find point Lp'
* find point Rp'
*/
int evalCuts(Mesh *m, Position *p, std::vector<int> &toRemove)
{
	double a_coords[3], b_coords[3], distance = 0.0;
	double min = 1e6;
	int indexA = -1, indexB = -1;
	b_coords[0] = -666;
	b_coords[1] = -666;
	b_coords[2] = -666;	
	for(int i = 0; i < p->size; i++)
	{
		if( (p->code[i] == 2 || p->code[i] == 1) && (min > p->pz[i]))
		{
			min = p->pz[i];
			indexA = i;
		}
	}
	if(indexA == -1)
	{
		printf("\nError finding point A\n");
	}
	else
	{
		min = 1e6;
		a_coords[0] = p->px[indexA];
		a_coords[1] = p->py[indexA];
		a_coords[2] = p->pz[indexA];
		printf("\nPoint A:id->%d ; coords->(%e,%e,%e) ; code->%d\n", indexA, a_coords[0], a_coords[1], a_coords[2], p->code[indexA]);
		for(int i = 0; i < p->size; i++)
		{	
			distance = sqrt( 	(a_coords[0] - p->px[i]) * (a_coords[0] - p->px[i]) +
								(a_coords[2] - p->pz[i]) * (a_coords[2] - p->pz[i])
							);
			if( (p->code[i] == 3) && (min > distance) && (p->pz[i] <= a_coords[2]) && (p->px[i] > -1e-3 && p->px[i] < 1e-3) )
			{
				min = distance;
				indexB = i;
			}
		}	
		if(indexB == -1)
		{
			printf("\nError finding point B\n");
		}
		else
		{	
			b_coords[0] = p->px[indexB];
			b_coords[1] = p->py[indexB];
			b_coords[2] = p->pz[indexB];	
			printf("\nPoint B:id->%d ; coords->(%e,%e,%e) ; code->%d\n", indexB, b_coords[0], b_coords[1], b_coords[2], p->code[indexB]);
		}
	}
	//after obtaining point B get points to cut on the right and left side on a cut of 45º
	//use the z < m.x + b 
	//m = tan(45º) = 1 and tan(-45º) = -1
	//b = p->pz[indexB]
	for(int i = 0; i < p->size; i++)
	{
		if((p->pz[i] < (-p->px[i] + b_coords[2])) && (p->pz[i] < (p->px[i] + b_coords[2])))
		{
			toRemove.push_back(i);
		}
	}		

	return indexB;
}