#include "Polygoniser.h"

#include <functional>
#include <map>

#include <glm/gtc/epsilon.hpp>

#include "MarchingCube.h"

using namespace glm;

#define ISOCELL 0
static const int isocells = 50;

Polygonizer::Polygonizer(Density &density, float isolevel) :
		density(density), isolevel(isolevel), isocellIsolevel(0), isocellVersion(
				0) {
}

void Polygonizer::setIsoLevel(float isolevel) {
	this->isolevel = isolevel;
}

#if ISOCELL
void Polygonizer::updateIsocell() {
	if (density.getVersion() == isocellVersion && isocellIsolevel == isolevel)
	return;
	isocell.clear();
	isocell.resize(isocells * isocells * isocells);

	const float lowerIso = isolevel * 0.7;
	const float upperIso = isolevel * 1.3;
	const float step = 1. / isocells;
#pragma omp parallel for
	for (size_t ix = 0; ix < isocells; ix++) {
		float xmin = ix * step;
		float xmax = xmin + step;
		for (size_t iy = 0; iy < isocells; iy++) {
			float ymin = iy * step;
			float ymax = ymin + step;

			float xyz = density(vec3(xmin, ymin, 0));
			float Xyz = density(vec3(xmax, ymin, 0));
			float xYz = density(vec3(xmin, ymax, 0));
			float XYz = density(vec3(xmax, ymax, 0));
			bool zAllUnder = (xyz < lowerIso) && (Xyz < lowerIso)
			&& (xYz < lowerIso) && (XYz < lowerIso);
			bool zAllOver = (xyz > upperIso) && (Xyz > upperIso)
			&& (xYz > upperIso) && (XYz > upperIso);

			for (size_t iz = 0; iz < isocells; iz++) {
				float zmin = iz * step;
				float zmax = zmin + step;

				float xyZ = density(vec3(xmin, ymin, zmax));
				float XyZ = density(vec3(xmax, ymin, zmax));
				float xYZ = density(vec3(xmin, ymax, zmax));
				float XYZ = density(vec3(xmax, ymax, zmax));
				bool ZAllUnder = (xyZ < lowerIso) && (XyZ < lowerIso)
				&& (xYZ < lowerIso) && (XYZ < lowerIso);
				bool ZAllOver = (xyZ > upperIso) && (XyZ > upperIso)
				&& (xYZ > upperIso) && (XYZ > upperIso);

				isocell[ix * isocells * isocells + iy * isocells + iz] =
				!((zAllUnder && ZAllUnder) || (zAllOver && ZAllOver));

				xyz = xyZ;
				Xyz = XyZ;
				xYz = xYZ;
				XYz = XYZ;
				zAllUnder = ZAllUnder;
				zAllOver = ZAllOver;
			}
		}
	}

	size_t n = 0;
	for(size_t i = 0; i < isocell.size(); i++)
	if(isocell[i]) n++;
	printf("%lu / %lu", n, isocell.size());
	isocellVersion = density.getVersion();
	isocellIsolevel = isolevel;
}

void Polygonizer::polygonize(const vec3 &lower, const vec3 &upper,
		float resolution) {

	vec3 l = round(lower * vec3(1.f / resolution));
	vec3 u = round(upper * vec3(1.f / resolution));
	for (size_t ix = l.x; ix < u.x; ix++) {
		double x = (double(ix) + 0.5) * resolution;
		for (size_t iy = l.y; iy < u.y; iy++) {
			double y = (double(iy) + 0.5) * resolution;
			for (size_t iz = l.z; iz < u.z; iz++) {
				double z = (double(iz) + 0.5) * resolution;

				GRIDCELL gridCell;
				gridCell.p[0] = vec3(x, y, z);
				gridCell.p[1] = vec3(x + resolution, y, z);
				gridCell.p[2] = vec3(x + resolution, y + resolution, z);
				gridCell.p[3] = vec3(x, y + resolution, z);
				gridCell.p[4] = vec3(x, y, z + resolution);
				gridCell.p[5] = vec3(x + resolution, y, z + resolution);
				gridCell.p[6] = vec3(x + resolution, y + resolution,
						z + resolution);
				gridCell.p[7] = vec3(x, y + resolution, z + resolution);

				for (size_t iCell = 0; iCell < 8; iCell++) {
					gridCell.val[iCell] = density(gridCell.p[iCell]);
				}

				TRIANGLE tris[6];
				int nTris = Polygonise(gridCell, isolevel, tris);
				for (int iTri = 0; iTri < nTris; iTri++) {
					vec3 *ps = tris[iTri].p;

					// skip degenerate vertices
					if (all(epsilonEqual(ps[0], ps[1], 1e-8f))
							|| all(epsilonEqual(ps[0], ps[2], 1e-8f))
							|| all(epsilonEqual(ps[2], ps[1], 1e-8f)))
					continue;

					uvec3 idc;
					for (size_t k = 0; k < 3; k++) {
						vec3 &p = tris[iTri].p[k];
						std::map<vec3, int>::iterator vit = vertexLookup.find(
								p);
						if (vit != vertexLookup.end()) {
							idc[k] = vit->second;
						} else {
							idc[k] = vertices.size();
							vertices.push_back(p);
							vertexLookup[p] = idc[k];
						}
					}
					indices.push_back(idc);
				}
			}
		}
	}
}

void Polygonizer::polygonize(float resolution) {
	vertexLookup.clear();
	vertices.clear();
	indices.clear();

	const float isocellSize = 1.f / isocells;
	if (resolution < isocellSize) {
		updateIsocell();

		vec3 lower, upper;
		for (size_t ix = 0; ix < isocells - 1; ix++) {
			lower.x = double(ix) * isocellSize;
			upper.x = lower.x + isocellSize;
			for (size_t iy = 0; iy < isocells - 1; iy++) {
				lower.y = (double(iy) + 0.5) * isocellSize;
				upper.y = lower.y + isocellSize;
				for (size_t iz = 0; iz < isocells - 1; iz++) {
					if (!isocell[ix * isocells * isocells + iy * isocells + iz])
					continue;
					lower.z = (double(iz) + 0.5) * isocellSize;
					upper.z = lower.z + isocellSize;

					polygonize(lower, upper, resolution);
				}
			}
		}
	} else {
		polygonize(vec3(0.f), vec3(1.0f), resolution);
	}
}

#else
void Polygonizer::calculateLayer(std::vector<float> &layer, float resolution,
		size_t nSamples, float z) {
#pragma omp parallel for
	for (size_t iy = 0; iy < nSamples; iy++) {
		float y = iy * resolution;
		size_t offset = iy * nSamples;
		for (size_t ix = 0; ix < nSamples; ix++) {
			float x = ix * resolution;
			layer[offset + ix] = density(glm::vec3(x, y, z));
		}
	}
}

void Polygonizer::polygonize(float resolution, ProgressMonitor &progress) {
	vertexLookup.clear();
	vertices.clear();
	indices.clear();

	vertices.reserve(pow(1. / resolution, 2) * 8);
	indices.reserve(pow(1. / resolution, 2) * 8);
	size_t nSteps = 1.f / resolution;
	size_t nSamples = nSteps + 1;

	// precalculate 2 layers
	std::vector<float> layers[2];
	layers[0].resize(nSamples * nSamples);
	layers[1].resize(nSamples * nSamples);

	calculateLayer(layers[0], resolution, nSamples, 0.0);

	progress.begin("Polygonize", nSteps);

	for (size_t iz = 0; iz < nSteps; iz++) {
		std::vector<float> &zLayer = layers[iz % 2];
		std::vector<float> &ZLayer = layers[(iz + 1) % 2];
		float z = iz * resolution;
		float Z = z + resolution;
		calculateLayer(ZLayer, resolution, nSamples, Z);

		if (!progress.advance())
			break;

		for (size_t iy = 0; iy < nSteps; iy++) {
			float y = iy * resolution;
			float Y = y + resolution;
			size_t yOffset = iy * nSamples;
			size_t YOffset = yOffset + nSamples;

			GRIDCELL gridCell;
			gridCell.p[0] = vec3(0, y, z);
			gridCell.p[1] = vec3(0, y, z);
			gridCell.p[2] = vec3(0, Y, z);
			gridCell.p[3] = vec3(0, Y, z);
			gridCell.p[4] = vec3(0, y, Z);
			gridCell.p[5] = vec3(0, y, Z);
			gridCell.p[6] = vec3(0, Y, Z);
			gridCell.p[7] = vec3(0, Y, Z);

			for (size_t ix = 0; ix < nSteps; ix++) {
				float x = ix * resolution;
				float X = x + resolution;

				gridCell.p[0].x = x;
				gridCell.p[1].x = X;
				gridCell.p[2].x = X;
				gridCell.p[3].x = x;
				gridCell.p[4].x = x;
				gridCell.p[5].x = X;
				gridCell.p[6].x = X;
				gridCell.p[7].x = x;

				gridCell.val[0] = zLayer[yOffset + ix];
				gridCell.val[1] = zLayer[yOffset + ix + 1];
				gridCell.val[2] = zLayer[YOffset + ix + 1];
				gridCell.val[3] = zLayer[YOffset + ix];

				gridCell.val[4] = ZLayer[yOffset + ix];
				gridCell.val[5] = ZLayer[yOffset + ix + 1];
				gridCell.val[6] = ZLayer[YOffset + ix + 1];
				gridCell.val[7] = ZLayer[YOffset + ix];

				TRIANGLE tris[6];
				int nTris = Polygonise(gridCell, isolevel, tris);
				for (int iTri = 0; iTri < nTris; iTri++) {
					vec3 *ps = tris[iTri].p;

					// skip degenerate vertices
					if (all(epsilonEqual(ps[0], ps[1], 1e-8f))
							|| all(epsilonEqual(ps[0], ps[2], 1e-8f))
							|| all(epsilonEqual(ps[2], ps[1], 1e-8f)))
						continue;

					uvec3 idc;
					for (size_t k = 0; k < 3; k++) {
						vec3 &p = tris[iTri].p[k];
						std::map<vec3, int>::iterator vit = vertexLookup.find(
								p);
						if (vit != vertexLookup.end()) {
							idc[k] = vit->second;
						} else {
							idc[k] = vertices.size();
							vertices.push_back(p);
							vertexLookup[p] = idc[k];
						}
					}
					indices.push_back(idc);
				}
			}
		}
	}

}

//void Polygonizer::polygonize(float resolution) {
//	vertexLookup.clear();
//	vertices.clear();
//	indices.clear();
//
//	size_t nSteps = 1.f / resolution;
//	size_t nSamples = nSteps + 1;
//
//	// precalculate 2 layers
//	std::vector<float> layers[2];
//	layers[0].resize(nSamples*nSamples);
//	layers[1].resize(nSamples*nSamples);
//
//	calculateLayer(layers[0], resolution, nSamples, 0.0);
//
//	size_t nT = 0;
//	for (size_t ix = 0; ix < nSteps; ix++) {
//		double x = (double(ix) + 0.5) * resolution;
//		for (size_t iy = 0; iy < nSteps; iy++) {
//			double y = (double(iy) + 0.5) * resolution;
//			for (size_t iz = 0; iz < nSteps; iz++) {
//				double z = (double(iz) + 0.5) * resolution;
//
//				GRIDCELL gridCell;
//				gridCell.p[0] = vec3(x, y, z);
//				gridCell.p[1] = vec3(x + resolution, y, z);
//				gridCell.p[2] = vec3(x + resolution, y + resolution, z);
//				gridCell.p[3] = vec3(x, y + resolution, z);
//				gridCell.p[4] = vec3(x, y, z + resolution);
//				gridCell.p[5] = vec3(x + resolution, y, z + resolution);
//				gridCell.p[6] = vec3(x + resolution, y + resolution,
//						z + resolution);
//				gridCell.p[7] = vec3(x, y + resolution, z + resolution);
//
//				for (size_t iCell = 0; iCell < 8; iCell++) {
//					gridCell.val[iCell] = density(gridCell.p[iCell]);
//				}
//
//				TRIANGLE tris[6];
//				int nTris = Polygonise(gridCell, isolevel, tris);
//				nT += nTris;
//				for (int iTri = 0; iTri < nTris; iTri++) {
//					vec3 *ps = tris[iTri].p;
//
//					// skip degenerate vertices
//					if (all(epsilonEqual(ps[0], ps[1], 1e-8f))
//							|| all(epsilonEqual(ps[0], ps[2], 1e-8f))
//							|| all(epsilonEqual(ps[2], ps[1], 1e-8f)))
//					continue;
//
//					uvec3 idc;
//					for (size_t k = 0; k < 3; k++) {
//						vec3 &p = tris[iTri].p[k];
//						std::map<vec3, int>::iterator vit = vertexLookup.find(
//								p);
//						if (vit != vertexLookup.end()) {
//							idc[k] = vit->second;
//						} else {
//							idc[k] = vertices.size();
//							vertices.push_back(p);
//							vertexLookup[p] = idc[k];
//						}
//					}
//					indices.push_back(idc);
//				}
//			}
//		}
//	}
//
//}
#endif