#include "quadtree.h"
#include "vector.h"

#include <boost/format.hpp>
#include <boost/timer.hpp>

#define GL_GLEXT_PROTOTYPES
#include <GL/gl.h>

#include <cmath>
#include <queue>

using std::min;

Quadtree::Quadtree(int width, int height, float *heightmap, int levels) {
	this->width = width;
	this->height = height;
	vbo_object = 0;
	root = NULL;
	heights = heightmap;

	create_nodes(levels);
}

Quadtree::~Quadtree() {
	if(vbo_object)
		glDeleteBuffers(1, &vbo_object);
	delete root;
	delete[] heights;
}

Quadtree::QuadNode::QuadNode(Quadtree *tree, QuadNode *parent, float x, float y, float width, float height, int level, bool leaf) {
	this->tree = tree;
	this->parent = parent;
	this->x = x;
	this->y = y;
	this->width = width;
	this->height = height;
	this->level = level;
	children[0] = children[1] = children[2] = children[3] = NULL;
	if(!leaf) {
		elems = 0;
		vertex_array = NULL;
		return;
	}

	fill();
}

Quadtree::QuadNode::~QuadNode() {
	if(vertex_array)
		delete[] vertex_array;
	for(int i = 0; i < 4; i++)
		if(children[i])
			delete children[i];
}

float Quadtree::QuadNode::distance(float px, float pz) {
	if(px > x && px < x+width && pz > y && pz < y+height)
		return 0;

	Vector2 p(px, pz);

	float a = (p - Vector2(x, y)).length();
	float b = (p - Vector2(x+width, y)).length();
	float c = (p - Vector2(x, y+height)).length();
	float d = (p - Vector2(x+width, y+height)).length();

	return min(min(min(a, b), c), d);
}

void Quadtree::QuadNode::fill() {
	elems = 3*5;
	int size = sizeof(float)*elems;
	vertex_array = new float[size];
	vertex_array[0] = x + width / 2;
	vertex_array[1] = tree->heights[(int)floorf((int)(x + width/2)*tree->height + (int)(y + height/2))];
	vertex_array[2] = y + height / 2;

	vertex_array[3] = x;
	vertex_array[4] = tree->heights[(int)floorf(x*tree->height+y)];
	vertex_array[5] = y;

	vertex_array[6] = x;
	vertex_array[7] = tree->heights[(int)floorf(x*tree->height + (y + height))];
	vertex_array[8] = y + height;

	vertex_array[9] = x + width;
	vertex_array[10] = tree->heights[(int)floorf((x + width)*tree->height + (y + height))];
	vertex_array[11] = y + height;

	vertex_array[12] = x + width;
	vertex_array[13] = tree->heights[(int)floorf((x + width)*tree->height + y)];
	vertex_array[14] = y;

	/* midpoint is average of corner heights when width == 1 */
	if(width == 1) {
		vertex_array[1] = (vertex_array[4] + vertex_array[7] + vertex_array[10] + vertex_array[13])/4;
	}
}

void Quadtree::QuadNode::subdivide(bool leaf) {
	if(vertex_array) {
		elems = 0;
		delete[] vertex_array;
		vertex_array = NULL;
	}

	for(int i = 0; i < 4; i++) {
		float nx = x + ((i & 1) ^ ((i & 2) >> 1)) * width / 2;
		float ny = y + ((i & 2) >> 1) * height / 2;
		children[i] = new QuadNode(tree, this, nx, ny, width / 2, height / 2, level + 1, leaf);
	}
}

void Quadtree::QuadNode::merge() {
	for(int i = 0; i < 4; i++) {
		delete children[i];
		children[i] = NULL;
	}

	fill();
}

void Quadtree::QuadNode::draw() {
	if(!vertex_array)
		return;

	float tex_coords[4][3][2] = {
		{{.5, .5}, {0, 0}, {0, 1}},
		{{.5, .5}, {0, 1}, {1, 1}},
		{{.5, .5}, {1, 1}, {1, 0}},
		{{.5, .5}, {1, 0}, {0, 0}}
	};

	glBegin(GL_TRIANGLES);
	for(int i = 0; i < 4; i++) {
		Vector3 a(vertex_array[0], vertex_array[1], vertex_array[2]);
		Vector3 b(vertex_array[i*3+3], vertex_array[i*3+4], vertex_array[i*3+5]);
		Vector3 c(vertex_array[i == 3 ? 3 : (i*3+6)], vertex_array[i == 3 ? 4 : (i*3+7)], vertex_array[i == 3 ? 5 : (i*3+8)]);

		Vector3 U(c.x-a.x, c.y-a.y, c.z-a.z);
		Vector3 V(b.x-a.x, b.y-a.y, b.z-a.z);
		Vector3 N(V.cross(U));
		glNormal3f(N.x, N.y, N.z);

		glTexCoord2f(tex_coords[i][0][0], tex_coords[i][0][1]);
		glVertex3f(a.x, a.y, a.z);

		glTexCoord2f(tex_coords[i][1][0], tex_coords[i][1][1]);
		glVertex3f(b.x, b.y, b.z);

		glTexCoord2f(tex_coords[i][2][0], tex_coords[i][2][1]);
		glVertex3f(c.x, c.y, c.z);
	}
	glEnd();
}

void Quadtree::QuadNode::draw_grid() {
	if(!vertex_array)
		return;

	glNormal3f(0, 1, 0);
	glBegin(GL_LINES);
	for(int i = 0; i < 4; i++) {
		Vector3 a(vertex_array[0], vertex_array[1], vertex_array[2]);
		Vector3 b(vertex_array[i*3+3], vertex_array[i*3+4], vertex_array[i*3+5]);

		glVertex3f(a.x, a.y, a.z);
		glVertex3f(b.x, b.y, b.z);
	}
	glEnd();
	glBegin(GL_LINE_LOOP);
		glVertex3f(vertex_array[3], vertex_array[4], vertex_array[5]);
		glVertex3f(vertex_array[6], vertex_array[7], vertex_array[8]);
		glVertex3f(vertex_array[9], vertex_array[10], vertex_array[11]);
		glVertex3f(vertex_array[12], vertex_array[13], vertex_array[14]);
	glEnd();
}

float Quadtree::QuadNode::get_height(float px, float py) {
	bool left;
	bool top;
	top = px - x >= height - (py - y);
	left = px - x >= py - y;
	Vector3 a(vertex_array[0], vertex_array[1], vertex_array[2]);
	Vector3 b, c;
	int bi, ci;
	if(left) {
		if(top) {
			bi = 3*3;
			ci = 4*3;
		} else {
			bi = 4*3;
			ci = 1*3;
		}
	} else {
		if(top) {
			bi = 2*3;
			ci = 3*3;
		} else {
			bi = 1*3;
			ci = 2*3;
		}
	}
	b = Vector3(vertex_array[bi], vertex_array[bi+1], vertex_array[bi+2]);
	c = Vector3(vertex_array[ci], vertex_array[ci+1], vertex_array[ci+2]);
	float det1 = (b.z - c.z) * (a.x - c.x) + (c.x - b.x) * (a.z - c.z);
	float det2 = (c.z - a.z) * (b.x - c.x) + (a.x - c.x) * (b.z - c.z);
	float l1 = ((b.z - c.z) * (px - c.x) + (c.x - b.x) * (py - c.z)) / det1;
	float l2 = ((c.z - a.z) * (px - c.x) + (a.x - c.x) * (py - c.z)) / det2;
	float l3 = 1 - l1 - l2;
	return l1 * a.y + l2 * b.y + l3 * c.y;
}

Vector3 Quadtree::QuadNode::get_normal(int index) {
	Vector3 a(vertex_array[0], vertex_array[1], vertex_array[2]);
	int bi, ci;
	switch(index) {
		// right
		case 0:
			bi = 1*3;
			ci = 2*3;
			break;
		// up
		case 1:
			bi = 2*3;
			ci = 3*3;
			break;
		// left
		case 2:
			bi = 3*3;
			ci = 4*3;
			break;
		// down
		case 3:
			bi = 4*3;
			ci = 1*3;
			break;
	}
	Vector3 b = Vector3(vertex_array[bi], vertex_array[bi+1], vertex_array[bi+2]);
	Vector3 c = Vector3(vertex_array[ci], vertex_array[ci+1], vertex_array[ci+2]);
	Vector3 U = c - a;
	Vector3 V = b - a;
	Vector3 N(V.cross(U));
	return N /= N.length();
}

void Quadtree::update(float x, float z) {
	return;
	std::queue<QuadNode*> q;
	q.push(root);
	while(!q.empty()) {
		QuadNode *node = q.front();
		q.pop();
		float d = node->distance(x, z);
		std::cout << d << std::endl;
		if(d < 20 && node->vertex_array)
			node->subdivide();
		else if(d > 50 && !node->vertex_array)
			node->merge();
		if(node->vertex_array)
			break;
		for(int i = 0; i < 4; i++) {
			q.push(node->children[i]);
		}
	}

	make_vbo();
}

void Quadtree::create_nodes(int levels) {
	if(root)
		delete root;

	int l = log2f(width);
	if(levels > l) {
		levels = l;
	}
	this->levels = levels;

	boost::timer t;
	root = new QuadNode(this, NULL, 0, 0, width-1, height-1, 1, levels == 0);
	std::queue<Quadtree::QuadNode*> q;
	if(levels > 0)
		q.push(root);
	while(!q.empty()) {
		Quadtree::QuadNode *node = q.front();
		q.pop();
		node->subdivide(node->level == levels);
		if(node->level < levels) {
			for(int i = 0; i < 4; i++)
				q.push(node->children[i]);
		}
	}

	make_vbo();

	init_time = t.elapsed();
}

unsigned int Quadtree::count_nodes() {
	std::queue<Quadtree::QuadNode*> q;
	q.push(root);
	unsigned int count = 0;
	while(!q.empty()) {
		Quadtree::QuadNode *node = q.front();
		q.pop();

		if(!node->vertex_array) {
			for(int i = 0; i < 4; i++)
				q.push(node->children[i]);
		} else {
			count++;
		}
	}

	return count;
}

void Quadtree::make_vbo() {
	nodes = count_nodes();
	if(vbo_object)
		glDeleteBuffers(1, &vbo_object);
	glGenBuffers(1, &vbo_object);
	glBindBuffer(GL_ARRAY_BUFFER, vbo_object);
	const size_t vertex_chunk_size = sizeof(float)*3*12;
	const size_t vertices_size = vertex_chunk_size*nodes;
	const size_t normal_chunk_size = vertex_chunk_size;
	const size_t normals_size = normal_chunk_size*nodes;
	const size_t tex_coord_chunk_size = sizeof(float)*2*12;
	const size_t tex_coords_size = tex_coord_chunk_size*nodes;

	glBufferData(GL_ARRAY_BUFFER, vertices_size + normals_size + tex_coords_size, NULL, GL_DYNAMIC_DRAW);

	std::queue<Quadtree::QuadNode*> q;
	q.push(root);
	vertices = nodes*12;
	unsigned int index = 0;
	while(!q.empty()) {
		Quadtree::QuadNode* node = q.front();
		q.pop();
		if(!node->vertex_array) {
			for(int j = 0; j < 4; j++)
				q.push(node->children[j]);
			continue;
		}
		float v[3*12];
		float n[3*12];
		v[0] = node->vertex_array[0];
		v[1] = node->vertex_array[1];
		v[2] = node->vertex_array[2];
		float tex_coords[4][3][2] = {
			{{.5, .5}, {0, 0}, {0, 1}},
			{{.5, .5}, {0, 1}, {1, 1}},
			{{.5, .5}, {1, 1}, {1, 0}},
			{{.5, .5}, {1, 0}, {0, 0}}
		};
		for(int i = 0; i < 4; i++) {
			for(int j = 0; j < 3; j++) {
				tex_coords[i][j][0] *= node->width;
				tex_coords[i][j][1] *= node->height;
			}

			v[3] = node->vertex_array[i*3+3];
			v[4] = node->vertex_array[i*3+4];
			v[5] = node->vertex_array[i*3+5];

			v[6] = node->vertex_array[i == 3 ? 3 : (i*3+6)];
			v[7] = node->vertex_array[i == 3 ? 4 : (i*3+7)];
			v[8] = node->vertex_array[i == 3 ? 5 : (i*3+8)];
			glBufferSubData(GL_ARRAY_BUFFER, vertex_chunk_size*index + sizeof(float)*3*3*i, sizeof(float)*3*3, v);
		}

		/* interpolate normals per vertex between nodes */

		QuadNode *left = get_left(node);
		QuadNode *right = get_right(node);
		QuadNode *up = get_up(node);
		QuadNode *down = get_down(node);

		// midpoint
		{
			Vector3 v(node->get_normal(0) + node->get_normal(1) + node->get_normal(2) + node->get_normal(3));
			v /= v.length();
			n[0] = n[9] = n[18] = n[27] = v.x;
			n[1] = n[10] = n[19] = n[28] = v.y;
			n[2] = n[11] = n[20] = n[29] = v.z;
		}

		// bottom-left
		{
			Vector3 v(node->get_normal(3) + node->get_normal(2));
			if(left) {
				v += left->get_normal(0);
				v += left->get_normal(3);
			}
			if(down) {
				v += down->get_normal(1);
				v += down->get_normal(2);
			}
			if(left && down) {
				QuadNode *n = get_left(down);
				if(n) {
					v += n->get_normal(0);
					v += n->get_normal(1);
				}
			}
			v /= v.length();
			n[24] = n[30] = v.x;
			n[25] = n[31] = v.y;
			n[26] = n[32] = v.z;
		}

		// bottom-right
		{
			Vector3 v(node->get_normal(0) + node->get_normal(3));
			if(right) {
				v += right->get_normal(2);
				v += right->get_normal(3);
			}
			if(down) {
				v += down->get_normal(1);
				v += down->get_normal(0);
			}
			if(down && right) {
				QuadNode *n = get_down(right);
				if(n) {
					v += n->get_normal(1);
					v += n->get_normal(2);
				}
			}
			v /= v.length();
			n[3] = n[33] = v.x;
			n[4] = n[34] = v.y;
			n[5] = n[35] = v.z;
		}

		// top-right
		{
			Vector3 v(node->get_normal(0) + node->get_normal(1));
			if(right) {
				v += right->get_normal(1);
				v += right->get_normal(2);
			}
			if(up) {
				v += up->get_normal(0);
				v += up->get_normal(3);
			}
			if(up && right) {
				QuadNode *n = get_right(up);
				if(n) {
					v += n->get_normal(2);
					v += n->get_normal(3);
				}
			}
			v /= v.length();
			n[6] = n[12] = v.x;
			n[7] = n[13] = v.y;
			n[8] = n[14] = v.z;
		}

		// top-left
		{
			Vector3 v(node->get_normal(1) + node->get_normal(2));
			if(up) {
				v += up->get_normal(3);
				v += up->get_normal(2);
			}
			if(left) {
				v += left->get_normal(0);
				v += left->get_normal(1);
			}
			if(left && up) {
				QuadNode *n = get_left(up);
				if(n) {
					v += n->get_normal(0);
					v += n->get_normal(3);
				}
			}
			v /= v.length();
			n[15] = n[21] = v.x;
			n[16] = n[22] = v.y;
			n[17] = n[23] = v.z;
		}

		glBufferSubData(GL_ARRAY_BUFFER, vertices_size + normal_chunk_size*index, normal_chunk_size, n);

		glBufferSubData(GL_ARRAY_BUFFER, vertices_size + normals_size + tex_coord_chunk_size*index, tex_coord_chunk_size, tex_coords);
		index++;
	}
}

Quadtree::QuadNode* Quadtree::find(float x, float y, int level) {
	QuadNode *node = root;
	while(!node->vertex_array) {
		if(node->level == level)
			return node;
		float mx = node->x + node->width / 2;
		float my = node->y + node->height / 2;
		int i = 2*(y > my);
		if(i < 2 && x > mx)
			i = 1;
		else if(i == 2 && x < mx)
			i = 3;
		node = node->children[i];
	}

	return node;
}

Quadtree::QuadNode *Quadtree::get_left(Quadtree::QuadNode *node) {
	QuadNode *n = find(node->x + node->width*1.5, node->y + node->height / 2);
	if(n == node)
		return NULL;
	return n;
}

Quadtree::QuadNode *Quadtree::get_right(Quadtree::QuadNode *node) {
	QuadNode *n = find(node->x - node->width/2, node->y + node->height / 2);
	if(n == node)
		return NULL;
	return n;
}

Quadtree::QuadNode *Quadtree::get_up(Quadtree::QuadNode *node) {
	QuadNode *n = find(node->x + node->width/2, node->y + node->height*1.5);
	if(n == node)
		return NULL;
	return n;
}

Quadtree::QuadNode *Quadtree::get_down(Quadtree::QuadNode *node) {
	QuadNode *n = find(node->x + node->width/2, node->y);
	if(n == node)
		return NULL;
	return n;
}