#include "FF.h"
#define _USE_MATH_DEFINES
#include <cmath>

#include <sstream>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <algorithm>


/*
* Triangle_cmp and Triangle_cmp are needed only to detect tet faces that participate to the mesh boundary
*/
struct Triangle {
	Triangle(int v0, int v1, int v2) { v[0] = v0; v[1] = v1;v[2] = v2; }
	void reorder() {
		if (v[0] > v[2]) std::swap(v[0], v[2]);
		if (v[0] > v[1]) std::swap(v[0], v[1]);
		if (v[1] > v[2]) std::swap(v[1], v[2]);
	}
	int v[3]; 
};
struct Triangle_cmp {
	bool operator()(const Triangle& t0, const Triangle& t1) {
		if (t0.v[0] > t1.v[0]) return true;
		else if (t0.v[0] < t1.v[0]) return false;
		else if (t0.v[1] > t1.v[1]) return true;
		else if (t0.v[1] < t1.v[1]) return false;
		return t0.v[2] < t1.v[2] ;
	}
};





int main(int argc, char* argv[])
{
	// mesh representation
	int nbv;						// number of vertices
	int nbt;						// number of tets
	std::vector<double> X;			// X[3*v+d] is the d^{th} coordinate of vertex v
	std::vector<double> n1;			// n1[3*v+d] is the d^{th} coordinate of the first locked normal of vertex v
	std::vector<double> n2;			// n2[3*v+d] is the d^{th} coordinate of the second locked normal of vertex v
	std::vector<int>	tet;		// tet[4*t+] is the i^{th} vertex d of tet t



	/*
	* Load the mesh
	*/                 
	if (argc < 2) {
		std::cerr << "Usage: " << argv[0] << " inputfile.tet" << std::endl;
		return 1;
	}
	std::string line;
	std::ifstream input_file(argv[1]);
	if (!input_file.is_open()) {
		std::cout << "Unable to open file";
		return 1;
	}
	std::cerr << " Load file " << std::endl;

	int num = 0;
	while (std::getline(input_file, line)) {
		std::istringstream iss(line);
		if (num == 0) {
			iss >> nbv;
			X.resize(3 * nbv,0);
			n1.resize(3 * nbv, 0);
			n2.resize(3 * nbv, 0);
		} else if (num == 1) {
			iss >> nbt;
			tet.resize(4 * nbt, 0);
		} else if (num < nbv + 2) {
			iss >> X[3 * (num - 2)] >> X[3 * (num - 2) + 1] >> X[3 * (num - 2) + 2];
		} else  {
			int trash;
			iss >> trash >> tet[4 * (num - 2 - nbv)] >> tet[4 * (num - 2 - nbv)+1] >> tet[4 * (num - 2 - nbv)+2] >> tet[4 * (num - 2 - nbv)+3];
		}
		num++;
	}
	input_file.close();
	



	/*
	* Compute normal constraints
	*/
	int tr_in_tet[4][3] = {
		{0,1,3},{0,3,2},{0,2,1},{1,2,3}
	};
	std::vector<Triangle> tr;
	for (int t = 0; t < nbt; t++) for (int f=0;f<4;f++){
		tr.push_back(Triangle(tet[4 * t + tr_in_tet[f][0]], tet[4 * t + tr_in_tet[f][1]], tet[4 * t + tr_in_tet[f][2]]));
	}
	for ( int t = 0; t < int(tr.size()); t++) tr[t].reorder();
	std::sort(tr.begin(), tr.end(), Triangle_cmp());
	for (int t = 1; t < int(tr.size()-1); t++) {
		if (tr[t].v[0] == tr[t + 1].v[0] && tr[t].v[1] == tr[t + 1].v[1] && tr[t].v[2] == tr[t + 1].v[2]) continue;
		if (tr[t].v[0] == tr[t - 1].v[0] && tr[t].v[1] == tr[t - 1].v[1] && tr[t].v[2] == tr[t - 1].v[2]) continue;
		double* x[3] = { X.data()+3 * tr[t].v[0],X.data() + 3 * tr[t].v[1],X.data() + 3 * tr[t].v[2] };
		double n[3] = {
			(x[1][1] - x[0][1])*(x[2][2] - x[0][2]) - (x[1][2] - x[0][2])*(x[2][1] - x[0][1]),
			(x[1][2] - x[0][2])*(x[2][0] - x[0][0]) - (x[1][0] - x[0][0])*(x[2][2] - x[0][2]),
			(x[1][0] - x[0][0])*(x[2][1] - x[0][1]) - (x[1][1] - x[0][1])*(x[2][0] - x[0][0])
		};
		for(int e=0;e<3;e++)for (int d = 0; d < 3; d++) n1[3 * tr[t].v[e] + d] = n[d];
	}



	/*
	* Call our algorithm
	*/
	std::vector<double>	rot(9 * nbv);
	computeFF(nbv,nbt, n1.data(), tet.data(), rot.data());




	/*
	* save the field
	*/
	std::ofstream outfield;
	outfield.open("out.txt");
	for (int v = 0; v < nbv; v++) {
		for (int d = 0; d < 9; d++) outfield << rot[9 * v + d] << "  ";
		outfield << "\n";
	}
	outfield.close();


	/*
	* save the mesh (little cubes .obj)
	*/
	double min[3] = { 1e20, 1e20, 1e20 };
	double max[3] = { -1e20, -1e20, -1e20 };
	for (int v = 0; v < nbv; v++) for (int d = 0; d < 3; d++) {
		if (min[d] > X[3 * v + d]) min[d] = X[3 * v + d];
		if (max[d] < X[3 * v + d]) max[d] = X[3 * v + d];
	}
	double scale = .03 / sqrt(pow(max[0] - min[0], 2) + pow(max[1] - min[1], 2) + pow(max[2] - min[2], 2));

	std::ofstream outfile;
	outfile.open("out.obj");
	for (int v = 0; v < nbv; v++) {
		double *G = X.data() + 3 * v;
		for (int dz = -1; dz < 3; dz += 2)
			for (int dy = -1; dy < 3; dy += 2)
				for (int dx = -1; dx < 3; dx += 2) {
					outfile << "v  ";
					for (int d = 0; d < 3; d++) 		
						outfile << G[d] + scale*(dx*rot[9 * v + 3 * 0 + d] + dy*rot[9 * v + 3 * 1 + d] + dz*rot[9 * v + 3 * 2 + d]) << "  " ;
					outfile << std::endl;
				}
	}
	for (int v = 0; v < nbv; v++) {
		int off = 8 * v + 1;
		outfile << "f  " << off + 0 << " " << off + 2 << " " << off + 3 << " " << off + 1 << std::endl;
		outfile << "f  " << off + 4 << " " << off + 5 << " " << off + 7 << " " << off + 6 << std::endl;
		outfile << "f  " << off + 0<< " " << off + 1<< " " << off + 5<< " " << off + 4<< std::endl;
		outfile << "f  " << off + 1<< " " << off + 3<< " " << off + 7<< " " << off + 5<< std::endl;
		outfile << "f  " << off + 3<< " " << off + 2<< " " << off +6 << " " << off + 7<< std::endl;
		outfile << "f  " << off + 0<< " " << off + 4<< " " << off + 6<< " " << off + 2<< std::endl;
	}
	outfile.close();
	return 0;
}
