#include "TriangleMeshVoxelizer.h"
#include <algorithm>
#include <unordered_set>

// Singletons
#include "../../PropertyLoader.h"
#include "../../shaders/ShaderLoader.h"
// Imports
#include "../../scene/PNG.h"
#include "../../scene/Scene.h"
#include "../../scene/ObjLoader.h"
#include "../Util/Stopwatch.h"
#include "../IntersectTests.h"
#include "../BitHelper.h"

bool TriangleMeshVoxelizer::Initialize() {
	if (verbose) printf("Initializing BaseOctreeBuilder... ");
	Stopwatch watch;
	watch.Reset();

	if (!InitializeGLFW())
		return false;

	glewExperimental = GL_TRUE;
	if (glewInit() != GLEW_OK) {
		fprintf(stderr, "Failed to initialize GLEW\n");
		return false;
	}
	if (verbose) printf("Initialization took %d ms \n", (int)(watch.GetTime() * 1000));
	return true;
}

bool TriangleMeshVoxelizer::LoadScene(const std::string& sceneFileName)
{
	if (verbose) printf("Reading scene \"%s\"... ", sceneFileName.c_str());
	Stopwatch watch; watch.Reset();
	if (!mObjLoader->Load(sceneFileName.c_str(), mScene)) return false;
	if (verbose) printf("Done in %d ms\n", (int)(watch.GetTime() * 1000));

	// Prepare OpenGL for rendering
	LoadScene(mScene);
	LoadShaders(mScene);

	// Calculate scene details
	CalculateBoundingBox(mScene);
	CalculateRootNode();
	return true;
}


bool TriangleMeshVoxelizer::UnloadScene()
{
	// Clear the texture from memory
	for (Mesh mesh : mScene.meshes)
		glDeleteTextures(1, &mTextures[mesh.texture]);
	// Clear the scene
	mScene = Scene();
	return true;
}

std::vector<glm::uvec3> TriangleMeshVoxelizer::GetValidCoords(unsigned8 scale)
{
	if (scale == 0) return std::vector<glm::uvec3>(1, glm::uvec3(0));
	else return CalculateValidSubtrees(scale);
}

void TriangleMeshVoxelizer::Voxelize(unsigned8 scale, unsigned8 partScale, glm::uvec3 partCoord, const std::function<void(const VoxelInfo&, bool best)>& nodeAdder, bool colors, bool normals, bool reflectivity)
{
	mReadColors = colors; mReadNormals = normals; mReadReflectivity = reflectivity;

	float stepSize = 1.f / (float)BitHelper::Exp2(scale - partScale);
	mCurPassRootSize = mRootSize * stepSize;
	mCurPassGridSize = (unsigned)(BitHelper::Exp2(partScale));

	InitDepthPeel();
	DepthPeel(partCoord, nodeAdder);
	TerminateDepthPeel();
}

void TriangleMeshVoxelizer::CalculateBoundingBox(Scene& scene)
{
	// Left = minX
	sceneLeft = (*std::min_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.x < b.x; })).x;
	// Right = maxX
	sceneRight = (*std::max_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.x < b.x; })).x;
	// Bottom = minY
	sceneBottom = (*std::min_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.y < b.y; })).y;
	// Top = maxY
	sceneTop = (*std::max_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.y < b.y; })).y;
	// Near = maxZ (OpenGL is weird ;) )
	sceneNear = (*std::max_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.z < b.z; })).z;
	// Far = minZ (OpenGL is weird ;) )
	sceneFar = (*std::min_element(scene.vertices.begin(), scene.vertices.end(), [](glm::vec3 a, glm::vec3 b) { return a.z < b.z; })).z;

	float width = sceneRight - sceneLeft;
	float height = sceneTop - sceneBottom;
	float depth = sceneNear - sceneFar;
	float cubesize = std::max(std::max(width, height), depth);
	float boundingBoxOffset = cubesize * 0.01f;
	sceneLeft -= boundingBoxOffset;
	sceneRight += boundingBoxOffset;
	sceneBottom -= boundingBoxOffset;
	sceneTop += boundingBoxOffset;
	sceneNear += boundingBoxOffset;
	sceneFar -= boundingBoxOffset;
}

void TriangleMeshVoxelizer::CalculateRootNode()
{
	float width = sceneRight - sceneLeft;
	float height = sceneTop - sceneBottom;
	float depth = sceneNear - sceneFar;

	mRootSize = std::max(std::max(width, height), depth);

	// Line below centers the scene (but puts it on the bottom)
	mRootCenter = glm::vec3(sceneLeft + (width * 0.5f), sceneBottom + (0.5f * mRootSize), sceneFar + (0.5f * depth));

	// Line below places the scene on the left bottom far position (optimal for compression).
	//mRootCenter = glm::vec3(sceneLeft, sceneBottom, sceneFar) + 0.5f * mRootSize;
	//mRootSize *= 1.01f;
}

std::vector<glm::uvec3> TriangleMeshVoxelizer::CalculateValidSubtrees(unsigned8 level)
{
	Stopwatch watch; watch.Reset();
	if (verbose) printf("Calculating valid subtrees... ");
	unsigned steps = 1 << level;
	glm::vec3 minPosition = mRootCenter - (mRootSize * 0.5f);
	glm::vec3 maxPosition = mRootCenter + (mRootSize * 0.5f);
	glm::vec3 cellSize = glm::vec3(mRootSize) / (float)steps;

	auto uvec3hasher = [&](glm::uvec3 v) -> std::size_t { return (size_t)(v.x + v.y * steps + v.z * steps * steps); };
	std::unordered_set < glm::uvec3, decltype(uvec3hasher)> validCoords(10, uvec3hasher);
	// For all triangles in the scene
	for (size_t i = 0; i + 2 < mScene.indices.size(); i += 3)
	{
		unsigned a = mScene.indices[i];
		unsigned b = mScene.indices[i + 1];
		unsigned c = mScene.indices[i + 2];
		glm::vec3 triangle[3] = { mScene.vertices[a], mScene.vertices[b], mScene.vertices[c] };
		// Build AABB bounding box around triangle:
		glm::vec3 triangleMin = glm::min(glm::min(triangle[0], triangle[1]), triangle[2]);
		glm::vec3 triangleMax = glm::max(glm::max(triangle[0], triangle[1]), triangle[2]);
		// Process only the cells within this bounding box
		glm::vec3 triangleMinCoordsF = glm::floor((triangleMin - minPosition) / cellSize);
		glm::uvec3 triangleMinCoords(triangleMinCoordsF.x, triangleMinCoordsF.y, triangleMinCoordsF.z);
		glm::vec3 triangleMaxCoordsF = glm::ceil((triangleMax - minPosition) / cellSize);
		glm::uvec3 triangleMaxCoords(triangleMaxCoordsF.x, triangleMaxCoordsF.y, triangleMaxCoordsF.z);
		// If there is an intersection, add the coord to validCoords
		for (unsigned x = triangleMinCoords.x; x < triangleMaxCoords.x; x++)
			for (unsigned y = triangleMinCoords.y; y < triangleMaxCoords.y; y++)
				for (unsigned z = triangleMinCoords.z; z < triangleMaxCoords.z; z++)
				{
					glm::vec3 coordF(x, y, z);
					glm::vec3 cellMinPosition = minPosition + cellSize * coordF;
					glm::vec3 cellMaxPosition = cellMinPosition + cellSize;
					if (IntersectTests::BoxTriangleIntersection(cellMinPosition, cellMaxPosition, triangle))
					{
						validCoords.insert(glm::uvec3(x, y, z));
					}
				}
	}


	std::vector<glm::uvec3> res;
	for (glm::uvec3 v : validCoords)
		res.push_back(v);
	if (verbose) printf("Found %llu / %u valid subtrees in %u ms.\n", (unsigned64)validCoords.size(), steps * steps * steps, (unsigned)(watch.GetTime() * 1000));
	return res;
}

void TriangleMeshVoxelizer::InitDepthPeel()
{
	Stopwatch watch; watch.Reset();
	if (verbose) printf("Initializing Depth peeling... ");

	depthData = new std::vector<GLfloat>(mCurPassGridSize * mCurPassGridSize);
	colorData = new std::vector<GLubyte>(mReadColors ? mCurPassGridSize * mCurPassGridSize * 3 : 0);
	normalData = new std::vector<GLfloat>(mReadNormals ? mCurPassGridSize * mCurPassGridSize * 3 : 0);
	angleData = new std::vector<GLfloat>(mReadColors && interpolateColors ? mCurPassGridSize * mCurPassGridSize : 0);
	reflectivityData = new std::vector<GLfloat>(mReadReflectivity ? mCurPassGridSize * mCurPassGridSize : 0);

	// The framebuffer, which regroups 0, 1, or more textures, and 0 or 1 depth buffer.
	glGenFramebuffers(1, &mFramebufferName);
	glBindFramebuffer(GL_FRAMEBUFFER, mFramebufferName);

	// Create a framebuffer texture containing the colors of the current frame
	//if (readColors)
	//{
		glGenTextures(1, &mColorTexture);
		glBindTexture(GL_TEXTURE_2D, mColorTexture);
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, mCurPassGridSize, mCurPassGridSize, 0, GL_RGB, GL_UNSIGNED_BYTE, 0);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	//}

	//if (readNormals)
	//{
		glGenTextures(1, &mNormalTexture);
		glBindTexture(GL_TEXTURE_2D, mNormalTexture);
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, mCurPassGridSize, mCurPassGridSize, 0, GL_RGB, GL_FLOAT, 0);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	//}

	// Create a framebuffer texture containing the angle of the vector pointing to the camera and the normal of the triangle
	//if (readColors & interpolateColors)
	//{
		glGenTextures(1, &mAngleTexture);
		glBindTexture(GL_TEXTURE_2D, mAngleTexture);
		glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, mCurPassGridSize, mCurPassGridSize, 0, GL_RED, GL_FLOAT, 0);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	//}

	glGenTextures(1, &mReflectivityTexture);
	glBindTexture(GL_TEXTURE_2D, mReflectivityTexture);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, mCurPassGridSize, mCurPassGridSize, 0, GL_RED, GL_FLOAT, 0);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

	// Create a first texture to contain a depth map
	glGenTextures(1, &mDepthTexture1);
	glBindTexture(GL_TEXTURE_2D, mDepthTexture1);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32, mCurPassGridSize, mCurPassGridSize, 0, GL_DEPTH_COMPONENT, GL_FLOAT, 0);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

	// Create a second texture to contain another depth map
	glGenTextures(1, &mDepthTexture2);
	glBindTexture(GL_TEXTURE_2D, mDepthTexture2);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT32, mCurPassGridSize, mCurPassGridSize, 0, GL_DEPTH_COMPONENT, GL_FLOAT, 0);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);

	// Build a depthbuffer for correct depth testing in the framebuffer.
	glGenRenderbuffers(1, &mDepthRenderbuffer);
	glBindRenderbuffer(GL_RENDERBUFFER, mDepthRenderbuffer);
	glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, mCurPassGridSize, mCurPassGridSize);
	glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, mDepthRenderbuffer);

	// Set "depthTexture" as our GL_DEPTH_ATTACHMENT
	glFramebufferTexture(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, mDepthTexture1, 0);
	//if (readColors)
	glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, mColorTexture, 0);
	//if (readColors && interpolateColors)
	glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT1, mAngleTexture, 0);
	//if (readNormals)
	glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT2, mNormalTexture, 0);
		
	glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT3, mReflectivityTexture, 0);

	// Set the list of draw buffers.
	GLenum DrawBuffers[4] = { GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1, GL_COLOR_ATTACHMENT2, GL_COLOR_ATTACHMENT3 };
	glDrawBuffers(4, DrawBuffers); // "3" is the size of DrawBuffers

	// Always check that our framebuffer is ok
	if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) {
		std::cout << "Framebuffer broke. Building the octree failed" << std::endl;
		return;
	}

	// The fullscreen quad's FBO
	static const GLfloat g_quad_vertex_buffer_data[] = {
		-1.0f, -1.0f, 0.0f,
		1.0f, -1.0f, 0.0f,
		-1.0f, 1.0f, 0.0f,
		-1.0f, 1.0f, 0.0f,
		1.0f, -1.0f, 0.0f,
		1.0f, 1.0f, 0.0f,
	};

	glGenBuffers(1, &mQuadVertexbuffer);
	glBindBuffer(GL_ARRAY_BUFFER, mQuadVertexbuffer);
	glBufferData(GL_ARRAY_BUFFER, sizeof(g_quad_vertex_buffer_data), g_quad_vertex_buffer_data, GL_STATIC_DRAW);


	mQuadProgram = mShaderLoader->LoadShader("RenderTexture.vert", "RenderTexture.frag");
	mImageRenderDepthTexID = glGetUniformLocation(mQuadProgram, "depthTexture");
	mImageRenderColorTexID = glGetUniformLocation(mQuadProgram, "renderTexture");
	mImageRenderUseDepthID = glGetUniformLocation(mQuadProgram, "showDepth");

	if (verbose) printf("Depth peeling initialized in %d ms\n", (int)(watch.GetTime() * 1000));
}

void TriangleMeshVoxelizer::DepthPeel(glm::uvec3 coord, const std::function<void(const VoxelInfo&, bool best)>& nodeAdder) {
	mMissingNodes.clear();
	glm::vec3 coordF(coord.x, coord.y, coord.z);
	mCurPassRootCenter = (mRootCenter - (mRootSize * 0.5f)) + coordF * mCurPassRootSize + mCurPassRootSize * 0.5f;

	unsigned frames = 0;
	bool done = false;
	Direction startDir = Direction::Top;

	Direction curDir = startDir;
	unsigned lastFrames = frames;

	GLuint oldDepthTexture = mDepthTexture2;
	GLuint curDepthTexture = mDepthTexture1;

	SetDirection(curDir, oldDepthTexture);

	Stopwatch mainWatch; mainWatch.Reset();
	Stopwatch watch; watch.Reset();
	Stopwatch frameWatch; frameWatch.Reset();
	do {
		if (!pixelsLeft) {
			// Print details about the last direction
			if (verbose) printf("Processed in %6d ms, %5.2f FPS\n", (int)(watch.GetTime() * 1000), (double)(frames - lastFrames) / watch.GetTime());
			watch.Reset();  lastFrames = frames;

			// Move to the next direction
			unsigned curDirInt = static_cast<unsigned>(curDir);
			curDir = static_cast<Direction>(++curDirInt % 3);
			// We're done if we processed all directions
			if (startDir == curDir)
				break;
			SetDirection(curDir, oldDepthTexture);
		}
		frames++;

		// ************************************************************************************************************************************ //
		// Render to our framebuffer																											//
		// ************************************************************************************************************************************ //
		glBindFramebuffer(GL_FRAMEBUFFER, mFramebufferName);
		glViewport(0, 0, mCurPassGridSize, mCurPassGridSize); // Render on the whole framebuffer, complete from the lower left corner to the upper right
		glFramebufferTexture(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, curDepthTexture, 0);

		// Clear the screen
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		RenderScene(curDir, oldDepthTexture, (float)mCurPassGridSize, (float)mCurPassGridSize);

		// ************************************************************************************************************************************ //
		// Render to what the user sees (before processing to improve performance)																//
		// ************************************************************************************************************************************ //
		do
		{
			// Only render the debug image if the last debug image was drawn more than 1/60th of a second ago
			if (manual || frameWatch.GetTime() > 1.0 / 60.0 || frames == 1)
			{
				frameWatch.Reset();
				glBindFramebuffer(GL_FRAMEBUFFER, 0);
				glViewport(0, 0, mWidth, mHeight); // Render on the whole framebuffer, complete from the lower left corner to the upper right


				// Clear the screen
				glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

				if (renderScene)
				{
					RenderScene(curDir, oldDepthTexture, (float)mWidth, (float)mHeight);
				}
				else
				{
					RenderDebugImage(curDepthTexture);
				}

				// Swap buffers
				glfwSwapBuffers(mWindow);
				glfwPollEvents();
			}
		} while (manual && glfwGetKey(mWindow, GLFW_KEY_SPACE) != GLFW_PRESS);

		// Wait until the user releases the space bar
		do { glfwPollEvents(); } while (glfwGetKey(mWindow, GLFW_KEY_SPACE) == GLFW_PRESS);

		// ************************************************************************************************************************************ //
		// Process the current layer, add all voxels to the tree																				//
		// ************************************************************************************************************************************ //
		glBindFramebuffer(GL_FRAMEBUFFER, mFramebufferName);
		glReadPixels(startTexX, startTexY, texWidth, texHeight, GL_DEPTH_COMPONENT, GL_FLOAT, &depthData->at(0));
		colorTexWidth = BitHelper::CeilToNearestPowerOfTwo(texWidth);
		if (mReadColors)
		{
			glReadBuffer(GL_COLOR_ATTACHMENT0);
			// When reading the colors, read the whole image, since something goes wrong with indices that aren't a power of two it seems...
			glReadPixels(startTexX, startTexY, colorTexWidth, texHeight, GL_RGB, GL_UNSIGNED_BYTE, &colorData->at(0));
		}
		if (mReadColors && interpolateColors)
		{
			glReadBuffer(GL_COLOR_ATTACHMENT1);
			glReadPixels(startTexX, startTexY, colorTexWidth, texHeight, GL_RED, GL_FLOAT, &angleData->at(0));
		}
		if (mReadNormals)
		{
			glReadBuffer(GL_COLOR_ATTACHMENT2);
			glReadPixels(startTexX, startTexY, colorTexWidth, texHeight, GL_RGB, GL_FLOAT, &normalData->at(0));
		}
		if (mReadReflectivity)
		{
			glReadBuffer(GL_COLOR_ATTACHMENT3);
			glReadPixels(startTexX, startTexY, colorTexWidth, texHeight, GL_RED, GL_FLOAT, &reflectivityData->at(0));
		}
		SetGridData(curDir, nodeAdder);

		// Swap the old and new depthtexture, thus advancing to the next layer
		oldDepthTexture = oldDepthTexture == mDepthTexture1 ? mDepthTexture2 : mDepthTexture1;
		curDepthTexture = curDepthTexture == mDepthTexture1 ? mDepthTexture2 : mDepthTexture1;
		firstPass = false;
	} while (glfwGetKey(mWindow, GLFW_KEY_ESCAPE) != GLFW_PRESS && !done);

	if (verbose) printf("Adding missing nodes...");
	if (mReadColors && interpolateColors)
	{
		// Add the missing nodes in the current subtree
		for (auto node : mMissingNodes)
			nodeAdder(node.second, false);
	}

	if (verbose) printf("Depth peeling took %d ms.\n", (int)(mainWatch.GetTime() * 1000));

}

void TriangleMeshVoxelizer::TerminateDepthPeel()
{
	// Cleanup the used resources from GPU
	glDeleteFramebuffers(1, &mFramebufferName);
	glDeleteRenderbuffers(1, &mDepthRenderbuffer);
	glDeleteTextures(1, &mDepthTexture1);
	glDeleteTextures(1, &mDepthTexture2);
	/*if (readColors)*/ glDeleteTextures(1, &mColorTexture);
	/*if (readNormals)*/ glDeleteTextures(1, &mNormalTexture);
	glDeleteTextures(1, &mAngleTexture);
	glDeleteTextures(1, &mReflectivityTexture);
	glDeleteBuffers(1, &mQuadVertexbuffer);
	glDeleteProgram(mQuadProgram);

	delete depthData;
	delete colorData;
	delete normalData;
	delete angleData;
	delete reflectivityData;
}

glm::ivec3 TriangleMeshVoxelizer::WorldToGrid(glm::vec3 world)
{
	float halfRootSize = mCurPassRootSize * 0.5f;
	float left = mCurPassRootCenter.x - halfRootSize;
	float bottom = mCurPassRootCenter.y - halfRootSize;
	float far = mCurPassRootCenter.z - halfRootSize;
	return glm::ivec3(
		((world.x - left) / mCurPassRootSize) * mCurPassGridSize,
		((world.y - bottom) / mCurPassRootSize) * mCurPassGridSize,
		((world.z - far) / mCurPassRootSize) * mCurPassGridSize);
}

void TriangleMeshVoxelizer::SetDirection(Direction dir, unsigned oldDepthTexture)
{
	if (verbose) printf("Processing %s view... ", dir == Top ? "Top  " : (dir == Side ? "Side " : "Front"));
	float one = 1;
	firstPass = true; // First pass in this direction
	pixelsLeft = true; // Assume there are at least some pixels in this direction
	glClearTexImage(oldDepthTexture, 0, GL_DEPTH_COMPONENT32, GL_FLOAT, &one); // Clear the old depth texture, makes sure we don't use old depth peeling values for a new direction
	// Reset AABB for texture read optimalization (based on scene size within octree)
	glm::ivec3 lbf = WorldToGrid(glm::vec3(sceneLeft, sceneBottom, sceneFar));
	glm::ivec3 rtn = WorldToGrid(glm::vec3(sceneRight, sceneTop, sceneNear));
	// Make sure the coordinates are actually on the grid
	lbf = glm::clamp(lbf, glm::ivec3(0), glm::ivec3(mCurPassGridSize));
	rtn = glm::clamp(rtn, glm::ivec3(0), glm::ivec3(mCurPassGridSize));
	texWidth = dir == Side ? rtn.z - lbf.z : rtn.x - lbf.x;
	texHeight = dir == Top ? rtn.z - lbf.z : rtn.y - lbf.y;
	startTexX = dir == Side ? lbf.z : lbf.x;
	startTexY = dir == Top ? mCurPassGridSize - lbf.z - texHeight : lbf.y;


	// Make sure we're looking in the right direction
	Transform(dir);
}

void TriangleMeshVoxelizer::Transform(Direction dir) {

	float halfRootSize = mCurPassRootSize * 0.5f;
	// Calculate the edges of the cube in world space
	float left = mCurPassRootCenter.x - halfRootSize;
//	float right = mCurPassRootCenter.x + halfRootSize;
//	float bottom = mCurPassRootCenter.y - halfRootSize;
	float top = mCurPassRootCenter.y + halfRootSize;
	float near = mCurPassRootCenter.z + halfRootSize;
//	float far = mCurPassRootCenter.z - halfRootSize;

	glm::vec3 viewPoint = mCurPassRootCenter;
	switch (dir)
	{
	case Side: viewPoint.x = left; break;
	case Top: viewPoint.y = top; break;
	case Front: viewPoint.z = near; break;
	}

	glm::mat4 mViewMatrix = glm::lookAt(viewPoint, mCurPassRootCenter, dir == Top ? glm::vec3(0, 0, -1) : glm::vec3(0, 1, 0));
	mProjectionMatrix = glm::ortho(-halfRootSize, +halfRootSize, -halfRootSize, +halfRootSize, 0.f, mCurPassRootSize);
	mMVP = mProjectionMatrix * mViewMatrix;
}

void TriangleMeshVoxelizer::SetGridData(Direction dir, const std::function<void(const VoxelInfo&, bool best)>& nodeAdder) {
	// Check if there are any pixels left
	pixelsLeft = false;
	unsigned minX = mCurPassGridSize, minY = mCurPassGridSize, maxX = 0, maxY = 0;

	glm::u8vec3 color(0);
	glm::vec3 normal(0);
	float reflectivity = 0.f;
	float depth;
	float minAngle = 0.25f * sqrtf(2);

	// From OpenGL Documentation:
	// The data for the ith pixel in the jth row is placed in location j * width + i
	for (unsigned j = 0; j < texHeight; j++) // row
		for (unsigned i = 0; i < texWidth; i++) // col
		{
			depth = depthData->at(i + j * texWidth);
			if (depth == 1.f)
				continue;

			pixelsLeft |= true;

			unsigned x = i + startTexX;
			unsigned y = j + startTexY;

			// Find min and max for both directions in the image to determine bounding box for next run.
			minX = std::min(minX, x);
			minY = std::min(minY, y);
			maxX = std::max(maxX, x);
			maxY = std::max(maxY, y);

			// Get the color of the texel
			unsigned texCoord = i + (j * colorTexWidth);
			if (mReadColors)
			{
				unsigned colorTexCoord = (3 * texCoord);
				color = glm::u8vec3(colorData->at(colorTexCoord), colorData->at(colorTexCoord + 1), colorData->at(colorTexCoord + 2));
			}
			if (mReadNormals)
			{
				unsigned normalTexCoord = (3 * texCoord);
				normal = (glm::vec3(normalData->at(normalTexCoord), normalData->at(normalTexCoord + 1), normalData->at(normalTexCoord + 2)) * 2.0f) - 1.0f;
			}
			if (mReadReflectivity)
			{
				reflectivity = reflectivityData->at(texCoord);
			}
			//color = glm::u8vec3(glm::abs(normal) * 255.0f);

			// Based on the direction, current location and value of the pixel, mark one of the voxels.
			// We're always looking from zero to higher values in the grid
			unsigned gridDepth = (unsigned)(depth * (float)mCurPassGridSize);
			glm::uvec3 coord;
			switch (dir)
			{
			case Side: coord = glm::uvec3(gridDepth, y, x); break;
			case Top: coord = glm::uvec3(x, mCurPassGridSize - gridDepth - 1, mCurPassGridSize - y - 1); break;
			case Front: coord = glm::uvec3(x, y, mCurPassGridSize - gridDepth - 1); break;
			}
			
			VoxelInfo info(coord, color, normal, 0.f, reflectivity);
			if (mReadColors && interpolateColors)
			{
				info.angle = angleData->at(texCoord);
				if (info.angle < minAngle)
				{
					auto missingNode = mMissingNodes.find(coord);
					if (missingNode == mMissingNodes.end())
						mMissingNodes.insert(std::make_pair(coord, info));
					else if (missingNode->second.angle < info.angle)
						missingNode->second = info;
					continue;
				}
			}

			nodeAdder(info, true);
		}
	startTexX = minX;
	startTexY = minY;
	texWidth = maxX -minX + 1;
	texHeight = maxY - minY + 1;
}

void TriangleMeshVoxelizer::LoadScene(Scene& scene)
{
	Stopwatch watch;
	watch.Reset();
	if (verbose) printf("Loading scene... ");

	TriangleMeshVoxelizer::Load2DTexture(mMissingTexture, interpolateColors);

	for (Mesh mesh : scene.meshes)
	{
		TriangleMeshVoxelizer::Load2DTexture(mesh.texture, interpolateColors);
	}

	//glDisable(GL_DEPTH_TEST);
	glEnable(GL_DEPTH_TEST);
	glCullFace(GL_FRONT_AND_BACK);

	// Generate and bind vertex array object
	glGenVertexArrays(1, &mVertexArrayID);
	glBindVertexArray(mVertexArrayID);

	// Generate vertex buffer
	glGenBuffers(1, &mVertexBuffer);
	glm::vec3 zeroVec(0);
	glBindBuffer(GL_ARRAY_BUFFER, mVertexBuffer);
	if (!scene.vertices.empty())
		glBufferData(GL_ARRAY_BUFFER, scene.vertices.size() * sizeof(glm::vec3), &scene.vertices[0], GL_STATIC_DRAW);
	else
		glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3), &zeroVec, GL_STATIC_DRAW);

	// Generate texture coordinate buffer
	glGenBuffers(1, &mTextureBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, mTextureBuffer);
	if (!scene.uvs.empty())
		glBufferData(GL_ARRAY_BUFFER, scene.uvs.size() * sizeof(glm::vec2), &scene.uvs[0], GL_STATIC_DRAW);
	else
		glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec2), &zeroVec, GL_STATIC_DRAW);

	// Generate the vertex color buffer
	glGenBuffers(1, &mVertexColorBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, mVertexColorBuffer);
	if (!scene.colors.empty())
		glBufferData(GL_ARRAY_BUFFER, scene.colors.size() * sizeof(glm::vec3), &scene.colors[0], GL_STATIC_DRAW);
	else
		glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3), &zeroVec, GL_STATIC_DRAW);

	// Generate normal buffer
	glGenBuffers(1, &mNormalBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, mNormalBuffer);
	if (!scene.normals.empty())
		glBufferData(GL_ARRAY_BUFFER, scene.normals.size() * sizeof(glm::vec3), &scene.normals[0], GL_STATIC_DRAW);
	else
		glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3), &zeroVec, GL_STATIC_DRAW);

	// Generate element buffer
	glGenBuffers(1, &mElementBuffer);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mElementBuffer);
	if (!scene.indices.empty())
		glBufferData(GL_ELEMENT_ARRAY_BUFFER, scene.indices.size() * sizeof(unsigned), &scene.indices[0], GL_STATIC_DRAW);

	if (verbose) printf("Scene loaded in %d ms\n", (int)(watch.GetTime() * 1000));
}

void TriangleMeshVoxelizer::RenderScene(Direction dir, unsigned oldDepthTexture, float viewPortWidth, float viewPortHeight)
{
	// Enable the shaders
	glUseProgram(mDefaultProgramID);

	// Bind old depth texture to texture unit 0
	glActiveTexture(GL_TEXTURE0);
	glBindTexture(GL_TEXTURE_2D, oldDepthTexture);
	// Tell the fragment shader to use texture unit 0 for the old depth texture
	glUniform1i(mOldDepthBufferSampler, 0);
	glUniform1i(mFirstPass, firstPass);
	glUniform1f(mDepthmargin, 1.0f / (float)mCurPassGridSize); // Half a cell margin
	glm::vec3 cameraDir((dir == Side) ? -1.f : 0, (dir == Top) ? -1.f : 0, (dir == Front) ? 1.f : 0);
	glUniform3f(mCameraDir, cameraDir.x, cameraDir.y, cameraDir.z);
	glProgramUniformMatrix4fv(mDefaultProgramID, mPropertyLoader->GetIntProperty("shader_MVP"), 1, GL_FALSE, &mMVP[0][0]);

	// Enable vertex attributes array (positions)
	glEnableVertexAttribArray(mPropertyLoader->GetIntProperty("shader_vertexPosition"));
	glBindBuffer(GL_ARRAY_BUFFER, mVertexBuffer);
	glVertexAttribPointer(
		mPropertyLoader->GetIntProperty("shader_vertexPosition"),		// Attribute positions
		3,																// Size
		GL_FLOAT,														// Type
		GL_FALSE,														// Normalized
		0,																// Stride
		(void*)0														// Array buffer offset
		);

	// Enable vertex attributes array (texture coordinates)
	glEnableVertexAttribArray(mPropertyLoader->GetIntProperty("shader_vertexUV"));
	glBindBuffer(GL_ARRAY_BUFFER, mTextureBuffer);
	glVertexAttribPointer(
		mPropertyLoader->GetIntProperty("shader_vertexUV"),			// Attribute texture coordinates
		2,																// Size
		GL_FLOAT,														// Type
		GL_FALSE,														// Normalized
		0,																// Stride
		(void*)0														// Array buffer offset
		);

	// Enable vertex attributes array (vertex colors)
	glEnableVertexAttribArray(stoi(mPropertyLoader->GetProperty("shader_vertexColor")));
	glBindBuffer(GL_ARRAY_BUFFER, mVertexColorBuffer);
	glVertexAttribPointer(
		stoi(mPropertyLoader->GetProperty("shader_vertexColor")),		// Attribute texture coordinates
		3,																// Size
		GL_FLOAT,														// Type
		GL_FALSE,														// Normalized
		0,																// Stride
		(void*)0														// Array buffer offset
		);
	// Enable vertex attributes array (normals)
	glEnableVertexAttribArray(stoi(mPropertyLoader->GetProperty("shader_vertexNormal")));
	glBindBuffer(GL_ARRAY_BUFFER, mNormalBuffer);
	glVertexAttribPointer(
		stoi(mPropertyLoader->GetProperty("shader_vertexNormal")),		// Attribute normals
		3,																// Size
		GL_FLOAT,														// Type
		GL_FALSE,														// Normalized
		0,																// Stride
		(void*)0														// Array buffer offset
		);

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Render meshes
	for (Mesh mesh : mScene.meshes) {
		// Bind texture to texture unit 1
		glActiveTexture(GL_TEXTURE1);
		auto loadedTexture = mTextures.find(mesh.texture);
		if (loadedTexture == mTextures.end() || mesh.texture == "")
			glBindTexture(GL_TEXTURE_2D, mTextures[mMissingTexture]);
		else
			glBindTexture(GL_TEXTURE_2D, mTextures[mesh.texture]);
		// Set texture sampler to texture unit 1
		glUniform1i(mTextureSampler, 1);
		glUniform1f(mReflectivity, mesh.reflectivity);


		// Draw the element arrays
		glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, mElementBuffer);
		glDrawElements(
			GL_TRIANGLES,								// Drawing mode
			mesh.size,									// Element count
			GL_UNSIGNED_INT,							// Type
			(void*)(mesh.offset * sizeof(unsigned))		// Element array buffer offset
			);
	}

	// Disable vertex attributes arrays
	glDisableVertexAttribArray(stoi(mPropertyLoader->GetProperty("shader_vertexPosition")));
	glDisableVertexAttribArray(stoi(mPropertyLoader->GetProperty("shader_vertexUV")));
	glDisableVertexAttribArray(stoi(mPropertyLoader->GetProperty("shader_vertexNormal")));
}

void TriangleMeshVoxelizer::RenderDebugImage(GLuint depthTexture)
{
	// Use our shader
	glUseProgram(mQuadProgram);

	// Bind our depth texture in Texture Unit 0
	glActiveTexture(GL_TEXTURE0);
	glBindTexture(GL_TEXTURE_2D, depthTexture);
	// Set our sampler to user Texture Unit 0 for depth texture
	glUniform1i(mImageRenderDepthTexID, 0);

	// Bind our color texture in Texture Unit 1
	glActiveTexture(GL_TEXTURE1);
	glBindTexture(GL_TEXTURE_2D, (!mReadColors && mReadNormals) ? mNormalTexture : mColorTexture);
	glUniform1i(mImageRenderColorTexID, 1);

	glUniform1i(mImageRenderUseDepthID, !mReadColors && !mReadNormals ? 1 : 0);

	// 1rst attribute buffer : vertices
	glEnableVertexAttribArray(0);
	glBindBuffer(GL_ARRAY_BUFFER, mQuadVertexbuffer);
	glVertexAttribPointer(
		0,                  // attribute 0. No particular reason for 0, but must match the layout in the shader.
		3,                  // size
		GL_FLOAT,           // type
		GL_FALSE,           // normalized?
		0,                  // stride
		(void*)0            // array buffer offset
		);

	// Draw the triangles !
	glDrawArrays(GL_TRIANGLES, 0, 6); // 2*3 indices starting at 0 -> 2 triangles

	glDisableVertexAttribArray(0);
}

//************************************
// Returns the index of the texture to access it in the texture vector
//************************************
size_t TriangleMeshVoxelizer::Load2DTexture(std::string filename, bool interpolation) {
	if (textureLoaded(filename))
		return mTextures[filename];

	unsigned textureID;
	glGenTextures(1, &textureID);
	glBindTexture(GL_TEXTURE_2D, textureID);

	PNG* png = new PNG(filename.c_str());

	if (png->Initialized())
		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, png->W(), png->H(), 0, GL_RGBA, GL_UNSIGNED_BYTE, png->Data());
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR_MIPMAP_LINEAR);
	if (interpolation)
	{
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
		glGenerateMipmap(GL_TEXTURE_2D);
	}
	else
	{
		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	}

	delete png;

	glBindTexture(GL_TEXTURE_2D, 0);

	mTextures.insert(std::pair<std::string, unsigned>(filename, textureID));

	return mTextures.size() - 1;
}

bool TriangleMeshVoxelizer::textureLoaded(std::string filename) {
	auto it = mTextures.find(filename);
	return it != mTextures.end();
}

void TriangleMeshVoxelizer::ReloadShaders(const Scene& scene) {
	glDeleteProgram(mDefaultProgramID);
	LoadShaders(scene);
}

void TriangleMeshVoxelizer::LoadShaders(const Scene& scene)
{
	Stopwatch watch; watch.Reset();
	if (verbose) printf("Loading shaders... ");
	std::map<std::string, std::string> additionalProperties;
	// TODO: support scenes that contain texture-based and vertex-color based meshes.
	if (scene.meshes[0].hasUVs && scene.meshes[0].hasVertexColors)
		additionalProperties.insert(std::pair<std::string, std::string>("colorType", "TEXTURE_AND_VERTEX_COLORS"));
	else if (scene.meshes[0].hasVertexColors)
		additionalProperties.insert(std::pair<std::string, std::string>("colorType", "VERTEX_COLORS"));
	else if (scene.meshes[0].hasUVs)
		additionalProperties.insert(std::pair<std::string, std::string>("colorType", "TEXTURE_COLORS"));
	else
		additionalProperties.insert(std::pair<std::string, std::string>("colorType", "NO_COLORS"));

	// Initialize and use shader program
	mDefaultProgramID = mShaderLoader->LoadShader("DepthPeel.vert", "DepthPeel.frag", additionalProperties);

	// Get texture ID
	mTextureSampler = glGetUniformLocation(mDefaultProgramID, "textureSampler");
	mReflectivity = glGetUniformLocation(mDefaultProgramID, "reflectivity");
	mOldDepthBufferSampler = glGetUniformLocation(mDefaultProgramID, "lastDepthMap");
	mFirstPass = glGetUniformLocation(mDefaultProgramID, "firstPass");
	mDepthmargin = glGetUniformLocation(mDefaultProgramID, "depthMargin");
	mCameraDir = glGetUniformLocation(mDefaultProgramID, "cameraDir");
	if (verbose) printf("Shaders loaded in %d ms\n", (int)(watch.GetTime() * 1000));
}


TriangleMeshVoxelizer::TriangleMeshVoxelizer() :
	depthData(NULL),
	colorData(NULL),
	normalData(NULL),
	angleData(NULL),
	mWindow(NULL)
{
	ObjLoader::Create();
	mObjLoader = ObjLoader::Instance();
	ShaderLoader::Create();
	mShaderLoader = ShaderLoader::Instance();
	//mShaderLoader->SetShaderPath("../Research/shaders/");
	PropertyLoader::Create();
	mPropertyLoader = PropertyLoader::Instance();
	mPropertyLoader->Clear();
	mPropertyLoader->AddPropertyFile(std::string("properties.txt"));
	mPropertyLoader->AddPropertyFile(std::string("shaders/shader_properties.txt"));
	mPropertyLoader->Update();

	mWidth = mPropertyLoader->GetIntProperty("octreebuilder_debug_width");
	mHeight = mPropertyLoader->GetIntProperty("octreebuilder_debug_height");
	manual = mPropertyLoader->GetProperty("octreebuilder_manual") != "0";
	renderScene = mPropertyLoader->GetProperty("octreebuilder_renderscene") != "0";
	verbose = mPropertyLoader->GetProperty("octreebuilder_verbose") != "0";
	interpolateColors = mPropertyLoader->GetProperty("octreebuilder_interpolate_colors") != "0";
	useCache = mPropertyLoader->GetProperty("octreebuilder_usecache") != "0";
	mMissingTexture = mPropertyLoader->GetProperty("octreebuilder_missing_material");
}

TriangleMeshVoxelizer::~TriangleMeshVoxelizer() {

	glDeleteBuffers(1, &mVertexBuffer);
	glDeleteBuffers(1, &mTextureBuffer);
	glDeleteBuffers(1, &mNormalBuffer);
	glDeleteBuffers(1, &mVertexColorBuffer);
	glDeleteBuffers(1, &mElementBuffer);

	glDeleteProgram(mDefaultProgramID);

	std::vector<unsigned> textureIDs;
	for (std::map<std::string, unsigned>::iterator it = mTextures.begin(); it != mTextures.end(); ++it) {
		textureIDs.push_back(it->second);
	}

	if (!textureIDs.empty())
		glDeleteTextures((GLsizei)textureIDs.size(), &textureIDs[0]);
	mTextures.clear();
	glDeleteTextures(1, &mTextureSampler);

	glDeleteVertexArrays(1, &mVertexArrayID);

	glfwTerminate();

	ObjLoader::Destroy();
	ShaderLoader::Destroy();
	PropertyLoader::Destroy();
}


bool TriangleMeshVoxelizer::Reinitialize(bool fullscreen) {
	glDeleteBuffers(1, &mVertexBuffer);
	glDeleteBuffers(1, &mTextureBuffer);
	glDeleteBuffers(1, &mNormalBuffer);
	glDeleteBuffers(1, &mElementBuffer);

	glDeleteProgram(mDefaultProgramID);

	std::vector<unsigned> textureIDs;
	for (std::map<std::string, unsigned>::iterator it = mTextures.begin(); it != mTextures.end(); ++it) {
		textureIDs.push_back(it->second);
	}

	glDeleteTextures((GLsizei)textureIDs.size(), &textureIDs[0]);
	mTextures.clear();
	glDeleteTextures(1, &mTextureSampler);

	glDeleteVertexArrays(1, &mVertexArrayID);

	glfwTerminate();

	return Initialize();
}

bool TriangleMeshVoxelizer::InitializeGLFW() {
	// Initialize GLFW
	if (!glfwInit()) {
		fprintf(stderr, "Failed to initialize GLFW\n");
		return false;
	}
	glfwWindowHint(GLFW_SAMPLES, std::stoi(mPropertyLoader->GetProperty("anti_aliasing")));
	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, std::stoi(mPropertyLoader->GetProperty("opengl_version_major")));
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, std::stoi(mPropertyLoader->GetProperty("opengl_version_minor")));
	glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
	glfwWindowHint(GLFW_OPENGL_DEBUG_CONTEXT, 1);
	glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);
	glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);

	// Open a window and create OpenGL context
	mWindow = glfwCreateWindow(mWidth, mHeight, mPropertyLoader->GetProperty("window_name").c_str(), NULL, NULL);
	if (mWindow == NULL) {
		fprintf(stderr, "Failed to open GLFW window.\n");
		glfwTerminate();
		return false;
	}
	glfwMakeContextCurrent(mWindow);

	glfwSetInputMode(mWindow, GLFW_STICKY_KEYS, GL_TRUE);

	return true;
}