#include "soft_body.hpp"
#include "mesh.hpp"
#include "constraints.hpp"
#include "tetgen.h"

SoftBody::SoftBody(Mesh* mesh, float compliance) : compliance(compliance) {
	edges.push_back(Edge(0, 1, 10));
	triangles.push_back(Triangle(Face(0, 1, 2), 40));
	tetrahedra.push_back(Tetrahedron(0, 1, 2, 3, 50));

	
	tetgenbehavior behavior;
	behavior.parse_commandline(std::string("pYa0.01Qfez").data());

	tetgenio in;
	in.numberofpoints = static_cast<int>(mesh->vertices.size());
	in.pointlist = new REAL[mesh->vertices.size() * 3];
	in.numberoffacets = static_cast<int>(mesh->faces.size());
	in.facetlist = new tetgenio::facet[mesh->faces.size()];
	in.numberofpointattributes = 3;
	in.pointattributelist = new REAL[mesh->vertices.size() * 3];

	for (size_t i = 0; i < mesh->vertices.size(); i++){
		in.pointlist[i * 3 + 0] = mesh->vertices[i].position.x;
		in.pointlist[i * 3 + 1] = mesh->vertices[i].position.y;
		in.pointlist[i * 3 + 2] = mesh->vertices[i].position.z;
		in.pointattributelist[i * 3 + 0] = (mesh->vertices[i].position.x + 1) / 2;
		in.pointattributelist[i * 3 + 1] = (mesh->vertices[i].position.y + 1) / 2;
		in.pointattributelist[i * 3 + 2] = (mesh->vertices[i].position.z + 1) / 2;
	}

	for (size_t i = 0; i < mesh->faces.size(); i++){
		tetgenio::facet &f = in.facetlist[i];
		tetgenio::polygon p;

		p.numberofvertices = 3;
		p.vertexlist = new int[3];
		p.vertexlist[0] = static_cast<int>(mesh->faces[i].a);
		p.vertexlist[1] = static_cast<int>(mesh->faces[i].b);
		p.vertexlist[2] = static_cast<int>(mesh->faces[i].c);

		f.numberofholes = 0;
		f.holelist = nullptr;
		f.numberofpolygons = 1;
		f.polygonlist = new tetgenio::polygon[1];
		f.polygonlist[0] = p;
	}

	tetgenio out;
	tetrahedralize(&behavior, &in, &out);

	vertices.reserve(out.numberofpoints / 3);
	for (size_t i = 0; i < out.numberofpoints; i++){
		float x = static_cast<float>(out.pointlist[i * 3 + 0]);
		float y = static_cast<float>(out.pointlist[i * 3 + 1]);
		float z = static_cast<float>(out.pointlist[i * 3 + 2]);
		float r = static_cast<float>(out.pointattributelist[i * 3 + 0]);
		float g = static_cast<float>(out.pointattributelist[i * 3 + 1]);
		float b = static_cast<float>(out.pointattributelist[i * 3 + 2]);
		vertices.emplace_back(Vertex({x, y, z}, {r, g, b}));
	}

	faces.reserve(out.numberoftrifaces);
	for (size_t i = 0; i < out.numberoftrifaces; i++){
		uint32_t a = out.trifacelist[i * 3 + 0];
		uint32_t b = out.trifacelist[i * 3 + 1];
		uint32_t c = out.trifacelist[i * 3 + 2];
		if (out.trifacemarkerlist[i] != 0)
			faces.emplace_back(Face(a, b, c));
	}
	faces.shrink_to_fit();

}

void SoftBody::applyOffset(const glm::vec3 &offset) {
	for (Vertex& vertex : vertices){
		vertex.position += offset;
	}
}