#include <queue>
#include <algorithm>
#include <cassert>
#include <iostream>
#include <set>
#include "world_objects.h"
#include "color.h"

static double infinity = -log(0);

IntersectInfo world_objects::CalcRayIntersect(Ray ray)
{
	IntersectInfo ii;
	ii.param_t = infinity;
	ii.E = 0;
	RayIntersect ri, best_ri;
	for(unsigned i=0; i<objects.size(); i++)
	{
		ri = objects[i]->CalcRayIntersect(ray);
		if(ri.E && ri.param_t < ii.param_t)
		{
			ii.E = ri.E;
			ii.param_t = ri.param_t;
			best_ri = ri;
		}
	}

	ii.col.rgb = vec_zero;
	if(ii.E)
	{
		ii.col.rgb = Ambient_Background.rgb * ii.E->ka;
		ii.norm = ii.E->CalcNormal(ray, best_ri);
		Ray lr;
		lr.o = ray.o + ray.d * ii.param_t;
		for(size_t i=0; i<lights.size(); i++)
		{
			lr.d = (lights[i].pt - lr.o).make_unit();
			bool visible = true;
			for(size_t j=0; j<objects.size() && visible; j++)
			{
				visible = !objects[j]->IntersectionTest(lr);
			}
			if(visible)
			{
				// Diffuse
				double l_n = ii.norm * lr.d;
				if(l_n > 0) // light is in front of me
				{
					double a = l_n * ii.E->kd;

					// Specular
					double q = -ray.d * (ii.norm * 2 * l_n - lr.d);
					double b = pow(q, ii.E->n) * ii.E->ks;
					if(q < 0) b = 0;
///					printf("%g %g\n", q, b);
					
					// A2dd contributions
					ii.col.rgb += lights[i].c.rgb * (a + b);
				}
			}
		}
		ii.col.rgb.x *= ii.E->col.rgb.x;
		ii.col.rgb.y *= ii.E->col.rgb.y;
		ii.col.rgb.z *= ii.E->col.rgb.z;
	}
	return ii;
}

int world_objects::IntersectionTest(Ray ray)
{
	for(size_t i=0; i<objects.size(); i++)
		if(objects[i]->IntersectionTest(ray))
			return 1;
	return 0;
}

void world_objects::PathCompress()
{
	std::queue<pair<Entity*, Transform> > que;

	Transform tf;
	tf.rot = mat_ident;
	tf.trans = vec_zero;
	for(size_t i=0; i<objects.size(); i++)
		que.push(pair<Entity*, Transform>(objects[i], tf));
	objects.clear();

	while(!que.empty())
	{
		Entity * E = que.front().first;
		Transform T = que.front().second;
		que.pop();

		E->PathCompress(T, que, objects);
	}
}

void world_objects::SetupBuckets()
{
	grid = new std::list<EntityWrap>[GRID_SIZEX][GRID_SIZEY][GRID_SIZEZ];
	assert(grid);
	vector<EntityWrapBound> blocks;
	for(size_t i=0; i<objects.size(); i++)
		objects[i]->GridCollect(blocks);

	if(blocks.empty()) return;
	vector3d hi = blocks[0].hi, lo = blocks[0].lo;

	for(size_t i=0; i<blocks.size(); i++)
	{
		update_min(lo, blocks[i].lo);
		update_max(hi, blocks[i].hi);
	}

	top = hi;
	hi += vec_one * 1e-8; // Ensure max is in the grid.

	base = lo;
 	diff.x = (hi.x - lo.x) / GRID_SIZEX;
 	diff.y = (hi.y - lo.y) / GRID_SIZEY;
 	diff.z = (hi.z - lo.z) / GRID_SIZEZ;
//	std::cout << "BASE: " << base << std::endl;
//	std::cout << "TOP: " << hi << std::endl;

	for(size_t i=0; i<blocks.size(); i++)
	{
		vector3d hi = blocks[i].hi - base;
		vector3d lo = blocks[i].lo - base;
		int lox = (int)(lo.x / diff.x);
		int loy = (int)(lo.y / diff.y);
		int loz = (int)(lo.z / diff.z);
		int hix = (int)(hi.x / diff.x);
		int hiy = (int)(hi.y / diff.y);
		int hiz = (int)(hi.z / diff.z);
		EntityWrap ew;
		ew.E = blocks[i].E;
		ew.special = blocks[i].special;
		int cnt=0;
		for(int X = lox; X <= hix; X++)
			for(int Y = loy; Y <= hiy; Y++)
				for(int Z = loz; Z <= hiz; Z++)
				{
					assert(X >= 0 && X < GRID_SIZEX);
					assert(Y >= 0 && Y < GRID_SIZEY);
					assert(Z >= 0 && Z < GRID_SIZEZ);
					grid[X][Y][Z].push_back(ew);
					cnt++;
				}
		assert(cnt);
	}
}

double find_entrance_t(const Ray& ray, const vector3d& base,
					   const vector3d& top)
{
	double x0 = -(ray.o.x - base.x) / ray.d.x;
	double y0 = -(ray.o.y - base.y) / ray.d.y;
	double z0 = -(ray.o.z - base.z) / ray.d.z;
	double x1 = -(ray.o.x - top.x) / ray.d.x;
	double y1 = -(ray.o.y - top.y) / ray.d.y;
	double z1 = -(ray.o.z - top.z) / ray.d.z;
	if(fabs(ray.d.x) < 1e-10) x0 = -infinity, x1 = infinity;
	if(fabs(ray.d.y) < 1e-10) y0 = -infinity, y1 = infinity;
	if(fabs(ray.d.z) < 1e-10) z0 = -infinity, z1 = infinity;

	double mn = std::min(x0, x1);
	mn = std::max(mn, std::min(y0, y1));
	mn = std::max(mn, std::min(z0, z1));
	return mn;
}

int world_objects::IntersectionTestBucket(Ray ray)
{
	double toler = 1e-10;
	double enter_t = find_entrance_t(ray, base, top);
	if(enter_t<0) enter_t = 0;
	vector3d v = ray.o + enter_t * ray.d - base;
	int sx = (int)(v.x / diff.x);
	int sy = (int)(v.y / diff.y);
	int sz = (int)(v.z / diff.z);
	v = vector3d(sx*diff.x, sy*diff.y, sz*diff.z) + base;

	int dx = fabs(ray.d.x) < toler ? 0 : (ray.d.x>0)*2-1;
	int dy = fabs(ray.d.y) < toler ? 0 : (ray.d.y>0)*2-1;
	int dz = fabs(ray.d.z) < toler ? 0 : (ray.d.z>0)*2-1;

 	double stepx = infinity;
 	double stepy = infinity;
 	double stepz = infinity;
	if(dx) stepx = dx * diff.x / ray.d.x;
	if(dy) stepy = dy * diff.y / ray.d.y;
	if(dz) stepz = dz * diff.z / ray.d.z;

	double curx = infinity;
	double cury = infinity;
	double curz = infinity;
	if(dx) curx = (v.x - ray.o.x) / ray.d.x;
	if(dy) cury = (v.y - ray.o.y) / ray.d.y;
	if(dz) curz = (v.z - ray.o.z) / ray.d.z;

//	puts("A");

	std::set<EntityWrap> done;
	while(sx>=0 && sx<GRID_SIZEX && sy>=0 &&
		  sy<GRID_SIZEY && sz>=0 && sz<GRID_SIZEZ)
	{
//		puts(" B");
		std::list<EntityWrap>::iterator it =
			grid[sx][sy][sz].begin();
		for(; it != grid[sx][sy][sz].end(); it++)
		{
			if(done.find(*it) != done.end())
				continue;
			done.insert(*it);
//			puts("  C");
			if(it->E->IntersectionTest(ray, it->special))
				return 1;
		}
		if(curx+stepx < cury+stepy && curx+stepx < curz+stepz)
			sx+=dx, curx += stepx;
		else if(cury+stepy < curz+stepz)
			sy+=dy, cury += stepy;
		else
			sz+=dz, curz += stepz;
	}
	return 0;
}

IntersectInfo world_objects::CalcRayIntersectBucket(Ray ray)
{
//	puts("HERE");
	IntersectInfo ii;
	ii.param_t = infinity;
	ii.E = 0;
	RayIntersect ri, best_ri;

	double toler = 1e-10;
	double enter_t = find_entrance_t(ray, base, top);
	if(enter_t<0) enter_t = 0;

	vector3d v = ray.o + enter_t * ray.d - base;
	int sx = (int)(v.x / diff.x);
	int sy = (int)(v.y / diff.y);
	int sz = (int)(v.z / diff.z);
	v = vector3d(sx*diff.x, sy*diff.y, sz*diff.z) + base;

	int dx = fabs(ray.d.x) < toler ? 0 : (ray.d.x>0)*2-1;
	int dy = fabs(ray.d.y) < toler ? 0 : (ray.d.y>0)*2-1;
	int dz = fabs(ray.d.z) < toler ? 0 : (ray.d.z>0)*2-1;

 	double stepx = infinity;
 	double stepy = infinity;
 	double stepz = infinity;
	if(dx) stepx = dx * diff.x / ray.d.x;
	if(dy) stepy = dy * diff.y / ray.d.y;
	if(dz) stepz = dz * diff.z / ray.d.z;

	double curx = infinity;
	double cury = infinity;
	double curz = infinity;
	if(dx) curx = (v.x - ray.o.x) / ray.d.x;
	if(dy) cury = (v.y - ray.o.y) / ray.d.y;
	if(dz) curz = (v.z - ray.o.z) / ray.d.z;

//	printf("%i %g\n", dx, stepx);

//	std::cout << ray.o << std::endl;
//	printf("%i %i %i\n", sx, sy, sz);
	std::set<EntityWrap> done;
	int sumbuck =0;
	while(sx>=0 && sx<GRID_SIZEX && sy>=0 &&
		  sy<GRID_SIZEY && sz>=0 && sz<GRID_SIZEZ)
	{
//		sumbuck += grid[sx][sy][sz].size();
		std::list<EntityWrap>::iterator it =
			grid[sx][sy][sz].begin();
		for(; it != grid[sx][sy][sz].end(); it++)
		{
			if(done.find(*it) != done.end())
				continue;

			done.insert(*it);

			ri = it->E->CalcRayIntersect(ray, it->special);
			
//			sumbuck += !!ri.E;
			if(ri.E && ri.param_t < ii.param_t)
			{
				ii.E = ri.E;
				ii.param_t = ri.param_t;
				best_ri = ri;
			}
		}
		if(curx+stepx < cury+stepy && curx+stepx < curz+stepz)
			sx += dx, curx += stepx;
		else if(cury+stepy < curz+stepz)
			sy += dy, cury += stepy;
		else
			sz += dz, curz += stepz;

		if(curx > ii.param_t &&
		   cury > ii.param_t &&
		   curz > ii.param_t)
			break;
	}



//	if(ii.E) putchar('.');
//	if(!ii.E && sumbuck) putchar('a' + sumbuck/10);
//	if(!ii.E && !sumbuck) putchar(' ');



	ii.col.rgb = vec_zero;
	if(ii.E)
	{
		ii.col.rgb = Ambient_Background.rgb * ii.E->ka;
		ii.norm = ii.E->CalcNormal(ray, best_ri);
		Ray lr;
		lr.o = ray.o + ray.d * ii.param_t;
		for(size_t i=0; i<lights.size(); i++)
		{
			lr.d = (lights[i].pt - lr.o).make_unit();
			bool visible = true;

			visible = !IntersectionTestBucket(lr);

			if(visible)
			{
				// Diffuse
				double l_n = ii.norm * lr.d;
				if(l_n > 0) // light is in front of me
				{
					double a = l_n * ii.E->kd;

					// Specular
					double q = -ray.d * (ii.norm * 2 * l_n - lr.d);
					double b = pow(q, ii.E->n) * ii.E->ks;
					if(q < 0) b = 0;
///					printf("%g %g\n", q, b);
					
					// A2dd contributions
					ii.col.rgb += lights[i].c.rgb * (a + b);
				}
			}
		}
		ii.col.rgb.x *= ii.E->col.rgb.x;
		ii.col.rgb.y *= ii.E->col.rgb.y;
		ii.col.rgb.z *= ii.E->col.rgb.z;
	}
	return ii;
}