#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, int flags, float contrib)
{
	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, flags);

		//  If we are only after an intersection test, we can quit now.
		if(ri.E && (flags & INTERSECT_ONLY))
		{
			ii.E = ri.E;
			return ii;
		}
		
		if(ri.E && ri.param_t < ii.param_t)
		{
			ii.E = ri.E;
			ii.param_t = ri.param_t;
			best_ri = ri;
		}
	}

	// We know enough right now for a simple intersection.
	if(flags & INTERSECT_ONLY)
	{
		ii.E = ri.E;
		return ii;
	}
		
	ii.col.rgb = vec_zero;
	if(ii.E)
	{
		ii.col.rgb = Ambient_Background.rgb * ii.E->kd;
		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]->CalcRayIntersect(lr, INTERSECT_ONLY).E;
			}
//			visible = true;
			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;
					ii.col.rgb += a * lights[i].cd.rgb.comp_mul(ii.E->cd.rgb);

					// Specular
					double q = -ray.d * (ii.norm * 2 * l_n - lr.d);

					// Make sure the light is visible.
					if(q >= 0)
					{
						double b = pow(q, ii.E->n) * ii.E->ks;
						ii.col.rgb += b * lights[i].cs.rgb.comp_mul(ii.E->cs.rgb);
					}
				}
			}
		}
	}
	return ii;
}

int world_objects::IntersectionTest(Ray ray, int flags)
{
	for(size_t i=0; i<objects.size(); i++)
		if(objects[i]->IntersectionTest(ray, flags))
			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, int flags)
{
	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;

	if(dx<0) curx -= stepx;
	if(dy<0) cury -= stepy;
	if(dz<0) curz -= stepz;

//	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, 0, 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, int flags, float contrib)
{
//	puts("HERE");
	IntersectInfo ii;
	ii.param_t = infinity;
	ii.E = 0;
	double contrib_tol = .02;
	int depth_tol = 20;
	// Not worth the effort to compute, or too deep.
	if(!(flags & INTERSECT_ONLY) &&
	   (contrib < contrib_tol || (flags & 0xFFFF) > depth_tol))
	{
//		printf("(%g %i)", contrib, flags & 0xFFFF);
		return ii;
	}
//	if(flags & 0xFFFF) putchar('.');

	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;

	if(dx<0) curx -= stepx;
	if(dy<0) cury -= stepy;
	if(dz<0) curz -= stepz;

	std::set<EntityWrap> done;
	while(sx>=0 && sx<GRID_SIZEX && sy>=0 &&
		  sy<GRID_SIZEY && sz>=0 && sz<GRID_SIZEZ)
	{
		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);

			if(flags & INTERSECT_ONLY)
			{
				if(it->E->IntersectionTest(ray, 0, it->special))
				{
					ii.E = (Entity*) 0xDEADBEEF;
					return ii;
				}
			}
			else
			{
				ri = it->E->CalcRayIntersect(ray, flags, it->special);
				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 we only need to know intersection, we are done now.
	if(flags & INTERSECT_ONLY)
	{
		ii.E = 0;
		return ii;
	}

	assert(!(flags & INTERSECT_ONLY));

	int num_visible = -1;
	ii.col.rgb = vec_zero;
	if(ii.E)
	{
		num_visible++;
		ii.col.rgb = Ambient_Background.rgb * ii.E->kd;
		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 = !CalcRayIntersectBucket(lr, INTERSECT_ONLY, contrib).E;
//			visible = !IntersectionTestBucket(lr, INTERSECT_ONLY);

			if(visible) num_visible++;

			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;
					ii.col.rgb += a * lights[i].cd.rgb.comp_mul(ii.E->cd.rgb);

					// Specular
					double q = -ray.d * (ii.norm * 2 * l_n - lr.d);

					// Make sure the light is visible.
					if(q >= 0)
					{
						double b = pow(q, ii.E->n) * ii.E->ks;
						ii.col.rgb += b * lights[i].cs.rgb.comp_mul(ii.E->cs.rgb);
					}
				}
			}
		}
		if(ii.E->ref * contrib > contrib_tol)
		{
			Ray rr;
			IntersectInfo ii2;
			double n_i = ray.d * ii.norm;
			rr.o = ray.o + ray.d * ii.param_t;
			rr.d = - ii.norm * 2 * n_i + ray.d;
// 			std::cout << "N " << ii.norm << "   ";
// 			std::cout << "R " << ray.d << "   ";
// 			std::cout << "o " << rr.o << "   ";
// 			std::cout << "d " << rr.d << std::endl;
			ii2 = CalcRayIntersectBucket(rr, flags+1, contrib * ii.E->ref);
			ii.col.rgb += ii2.col.rgb.comp_mul(ii.E->cs.rgb) * ii.E->ref;
//			printf("%8x ", ii2.E);
		}
	}
#if 0
	if(num_visible < 0)
		printf("  ");
	if(num_visible == 0)
		printf(". ");
	if(num_visible > 0)
		printf("%i ", num_visible);
#endif
	return ii;
}