#include "std.hpp"
#include <queue>
#include "world.hpp"

//0=tris compared for collision
//1=max proj err of terrain
float stats3d[10];

extern gxScene *gx_scene;
extern gxRuntime *gx_runtime;

static std::list<Object*> s_objectsEnabled, s_objectsVisible;

static void StaticEnumerateEnabled() {
	s_objectsEnabled.clear();
	for (Entity *e = Entity::GetEntityOrphans(); e; e = e->GetSuccessor()) {
		e->EnumerateEnabled(s_objectsEnabled);
	}
}

static void StaticEnumerateVisible() {
	s_objectsVisible.clear();
	for (Entity *e = Entity::GetEntityOrphans(); e; e = e->GetSuccessor()) {
		e->EnumerateVisible(s_objectsVisible);
	}
}

/******************************* Update *******************************/

static vector<Object*> _objsByType[1000];

static vector<ObjCollision*> free_colls, used_colls;

static ObjCollision *allocObjColl(Object *with, const Vector &coords, const Collision &coll) {
	ObjCollision *c;
	if (free_colls.size()) {
		c = free_colls.back();
		free_colls.pop_back();
	} else {
		c = new ObjCollision();
	}
	used_colls.push_back(c);
	c->with = with;
	c->coords = coords;
	c->collision = coll;
	return c;
}

static void collided(Object *src, Object *dest, const Line &line, const Collision &coll, float y_scale) {

	ObjCollision *c;
	const Vector &coords = line*coll.time - coll.normal*src->getCollisionRadii().x;

	c = allocObjColl(dest, coords, coll);
	c->coords.y *= y_scale;
	src->addCollision(c);

	c = allocObjColl(src, coords, coll);
	c->coords.y *= y_scale;
	dest->addCollision(c);
}

void World::clearCollisions() {
	for (int k = 0; k < 1000; ++k) {
		_collInfo[k].clear();
	}
}

void World::addCollision(int src_type, int dst_type, int method, int response) {

	vector<CollInfo> &info = _collInfo[src_type];
	for (size_t k = 0; k < info.size(); ++k) {
		const CollInfo &t = info[k];
		if (dst_type == t.dst_type) return;
	}

	CollInfo co = { dst_type,method,response };
	_collInfo[src_type].push_back(co);
}

bool World::hitTest(const Line &line, float radius, Object *obj, const Transform &tf, int method, Collision *curr_coll) {
	switch (method) {
		case COLLISION_METHOD_SPHERE:
			return curr_coll->sphereCollide(line, radius, tf.v, obj->getCollisionRadii().x);
		case COLLISION_METHOD_POLYGON:
			return obj->collide(line, radius, curr_coll, tf);
		case COLLISION_METHOD_BOX:
			Transform t = tf;
			t.m.i.normalize(); t.m.j.normalize(); t.m.k.normalize();
			if (curr_coll->boxCollide(~t*line, radius, obj->getCollisionBox())) {
				curr_coll->normal = t.m*curr_coll->normal;
				return true;
			}
	}
	return false;
}

bool World::CheckLineOfSight(Object *src, Object *dest) {

	StaticEnumerateEnabled();

	Object *coll_obj = 0;
	Collision curr_coll;

	Line line(src->GetWorldPosition(), dest->GetWorldPosition() - src->GetWorldPosition());

	for (Object* obj : s_objectsEnabled) {
		if (obj == src || obj == dest || !obj->getPickGeometry() || !obj->getObscurer()) continue;

		if (hitTest(line, 0, obj, obj->GetWorldTransform(), obj->getPickGeometry(), &curr_coll)) {
			return false;
		}
	}
	return true;
}

Object *World::traceRay(const Line &line, float radius, ObjCollision *curr_coll) {

	StaticEnumerateEnabled();

	Object *coll_obj = 0;

	for (Object* obj : s_objectsEnabled) {

		if (!obj->getPickGeometry()) continue;

		if (hitTest(line, radius, obj, obj->GetWorldTransform(), obj->getPickGeometry(), &curr_coll->collision)) {
			coll_obj = obj;
		}
	}
	if (curr_coll->with = coll_obj) {
		curr_coll->coords = line*curr_coll->collision.time - curr_coll->collision.normal*radius;
	}
	return coll_obj;
}

//
// NEW VERSION
//
void World::collide(Object *src) {
	Vector dv = src->GetWorldTransform().v;
	Vector sv = src->getPrevWorldTform().v;

	if (sv == dv) {
		if (dv.x != sv.x || dv.y != sv.y || dv.z != sv.z) {
			src->SetWorldPosition(sv);
		}
		return;
	}

	Vector panic = sv;

	static Transform y_tform;

	const Vector &radii = src->getCollisionRadii();

	float radius = radii.x, inv_y_scale;
	float y_scale = inv_y_scale = y_tform.m.j.y = 1;

	if (radii.x != radii.y) {
		y_scale = y_tform.m.j.y = radius / radii.y;
		inv_y_scale = 1 / y_scale;
		sv.y *= y_scale;
		dv.y *= y_scale;
	}

	int n_hit = 0;
	Plane planes[2];
	Line coll_line(sv, dv - sv);
	Vector dir = coll_line.d;

	float td = coll_line.d.length();
	float td_xz = Vector(coll_line.d.x, 0, coll_line.d.z).length();

	const vector<CollInfo> &collinfos = _collInfo[src->getCollisionType()];

	int hits = 0;
	for (;;) {
		Collision coll;
		Object *coll_obj = 0;
		vector<CollInfo>::const_iterator coll_it, coll_info;

		for (coll_it = collinfos.begin(); coll_it != collinfos.end(); ++coll_it) {
//			const std::list<Object*> &dst_objs = _objsByType[coll_it->dst_type];
			for (Object* dst : /*dst_objs*/_objsByType[coll_it->dst_type]) {

				if (src == dst) continue;

				const Transform &dst_tform = dst->getPrevWorldTform();

				if (y_scale == 1) {
					if (hitTest(
						coll_line, radius, dst, dst_tform,
						coll_it->method, &coll)) {
						coll_obj = dst;
						coll_info = coll_it;
					}
				} else {
					if (hitTest(
						coll_line, radius, dst, y_tform * dst_tform,
						coll_it->method, &coll)) {
						coll_obj = dst;
						coll_info = coll_it;
					}
				}
			}
		}
		if (!coll_obj) break;

		//register collision
		if (++hits == WORLD_COLLISION_HITS) {
//			exit(0);
			break;
		}

		collided(src, coll_obj, coll_line, coll, inv_y_scale);

		Plane coll_plane(coll_line*coll.time, coll.normal);

		coll_plane.d -= COLLISION_FLT_EPSILON;
		coll.time = coll_plane.t_intersect(coll_line);

		if (coll.time > 0) {// && fabs(coll.normal.dot( coll_line.d ))>FLT_EPSILON ){
			//update source position - ONLY IF AHEAD!
			sv = coll_line*coll.time;
			td *= 1 - coll.time;
			td_xz *= 1 - coll.time;
		}

		if (coll_info->response == COLLISION_RESPONSE_STOP) {
			dv = sv;
			break;
		}

		//find nearest point on plane to dest
		Vector nv = coll_plane.nearest(dv);

		if (n_hit == 0) {
			dv = nv;
		} else if (n_hit == 1) {
			if (planes[0].distance(nv) >= 0) {
				dv = nv; n_hit = 0;
			} else if (fabs(planes[0].n.dot(coll_plane.n)) < 1 - FLT_EPSILON) {
				dv = coll_plane.intersect(planes[0]).nearest(dv);
			} else {
				//SQUISHED!
				//exit(0);
				hits = WORLD_COLLISION_HITS; break;
			}
		} else if (planes[0].distance(nv) >= 0 && planes[1].distance(nv) >= 0) {
			dv = nv; n_hit = 0;
		} else {
			dv = sv; break;
		}

		Vector dd(dv - sv);

		//going behind initial direction? really necessary?
		if (dd.dot(dir) <= 0) { dv = sv; break; }

		if (coll_info->response == COLLISION_RESPONSE_SLIDE) {
			float d = dd.length();
			if (d <= FLT_EPSILON) { dv = sv; break; }
			if (d > td) dd *= td / d;
		} else if (coll_info->response == COLLISION_RESPONSE_SLIDEXZ) {
			float d = Vector(dd.x, 0, dd.z).length();
			if (d <= FLT_EPSILON) { dv = sv; break; }
			if (d > td_xz) dd *= td_xz / d;
		}

		coll_line.o = sv;
		coll_line.d = dd; dv = sv + dd;
		planes[n_hit++] = coll_plane;
	}

	if (hits) {
		if (hits < WORLD_COLLISION_HITS) {
			dv.y *= inv_y_scale;
			src->SetWorldPosition(dv);
		} else {
			src->SetWorldPosition(panic);
		}
	}
}

/*
//
// OLD VERSION
//
void World::collide( Object *src ){

	static const int MAX_HITS=100;

	Vector dv=src->getWorldTform().v;
	Vector sv=src->getPrevWorldTform().v;

	if( sv==dv ){
		if( dv.x!=sv.x || dv.y!=sv.y || dv.z!=sv.z ){
			src->setWorldPosition( sv );
		}
		return;
	}

	Vector panic=sv;

	static Transform y_tform;

	const Vector &radii=src->getCollisionRadii();

	float radius=radii.x,inv_y_scale;
	float y_scale=inv_y_scale=y_tform.m.j.y=1;

	if( radii.x!=radii.y ){
		y_scale=y_tform.m.j.y=radius/radii.y;
		inv_y_scale=1/y_scale;
		sv.y*=y_scale;
		dv.y*=y_scale;
	}

	int n_hit=0;
	Plane planes[2];
	Line coll_line( sv,dv-sv );
	Vector dir=coll_line.d;

	float td=coll_line.d.length();
	float td_xz=Vector( coll_line.d.x,0,coll_line.d.z ).length();

	const vector<CollInfo> &collinfos=_collInfo[src->getCollisionType()];

	int hits=0;
	while( hits<MAX_HITS ){

		Collision coll;
		Object *coll_obj=0;
		vector<CollInfo>::const_iterator coll_it,coll_info;

		for( coll_it=collinfos.begin();coll_it!=collinfos.end();++coll_it ){

			vector<Object*>::const_iterator dst_it;

			const vector<Object*> &dst_objs=_objsByType[coll_it->dst_type];

			for( dst_it=dst_objs.begin();dst_it!=dst_objs.end();++dst_it ){

				Object *dst=*dst_it;

				if( src==dst ) continue;

				const Transform &dst_tform=dst->getPrevWorldTform();

				if( y_scale==1 ){

					if( hitTest(
					coll_line,radius,dst,dst_tform,
					coll_it->method,&coll ) ){
						coll_obj=dst;
						coll_info=coll_it;
					}
				}else{

					if( hitTest(
					coll_line,radius,dst,y_tform * dst_tform,
					coll_it->method,&coll ) ){
						coll_obj=dst;
						coll_info=coll_it;
					}
				}
			}
		}
		if( !coll_obj ) break;

		//register collision
		++hits;
		collided( src,coll_obj,coll_line,coll,inv_y_scale );

		//create collision plane
		Plane coll_plane( coll_line*coll.time,coll.normal );

		//move plane out a bit (cough)
		coll_plane.d-=.001f;

		if( fabs(coll.normal.dot( coll_line.d ))>FLT_EPSILON ){
			float t=coll_plane.t_intersect( coll_line );
			//update source position - ONLY IF AHEAD!
			if( t>0 ){
				sv=coll_line*t;
				td*=1-coll.time;
				td_xz*=1-coll.time;
			}
		}

		//STOP?
		if( coll_info->response==COLLISION_RESPONSE_STOP ){
			dv=sv;
			break;
		}

		//find nearest point on plane to dest
		Vector nv=coll_plane.nearest( dv );

		//SLIDE!
		if( n_hit==0 ){
			dv=nv;
		}else if( n_hit==1 ){
			if( planes[0].distance(nv)>=0 ){
				dv=nv;n_hit=0;
			}else if( fabs( planes[0].n.dot( coll_plane.n ) )<1-FLT_EPSILON ){
				dv=coll_plane.intersect( planes[0] ).nearest( dv );
			}else{
				hits=MAX_HITS;break;
			}
		}else if( planes[0].distance(nv)>=0 && planes[1].distance(nv)>=0 ){
			dv=nv;n_hit=0;
		}else{
			dv=sv;break;
		}

		Vector dd( dv-sv );

		//going behind initial direction? really necessary?
		if( dd.dot( dir )<=0 ){ dv=sv;break; }

		if( coll_info->response==COLLISION_RESPONSE_SLIDE ){
			float d=dd.length();
			if( d<=FLT_EPSILON ){ dv=sv;break; }
			if( d>td ) dd*=td/d;
		}else if( coll_info->response==COLLISION_RESPONSE_SLIDEXZ ){
			float d=Vector( dd.x,0,dd.z ).length();
			if( d<=FLT_EPSILON ){ dv=sv;break; }
			if( d>td_xz ) dd*=td_xz/d;
		}

		coll_line.o=sv;
		coll_line.d=dd;dv=sv+dd;
		planes[n_hit++]=coll_plane;
	}

	if( hits ){
		if( hits<MAX_HITS ){
			dv.y*=inv_y_scale;
			src->setWorldPosition( dv );
		}else{
			src->setWorldPosition( panic );
		}
	}
}
*/

void World::update(float elapsed) {

	stats3d[0] = 0;

	for (; used_colls.size(); used_colls.pop_back()) {
		free_colls.push_back(used_colls.back());
	}

	StaticEnumerateEnabled();

	for (Object* o : s_objectsEnabled) {
		if (int n = o->getCollisionType()) {
			_objsByTypeSwappable[n].push_back(o);
		}

		o->beginUpdate(elapsed);
		if (o->getCollisionType()) collide(o);
		o->endUpdate();
	}

	for (int k = 0; k < WORLD_COLLISION_TYPES; ++k) {
		_objsByTypeSwappable[k].swap(_objsByType[k]);
		_objsByTypeSwappable[k].clear();
	}
}

/****************************** Render *********************************/

static Transform cam_tform;		//current camera transform

static vector<gxLight*> _lights;
static vector<Mirror*> _mirrors;
static vector<Listener*> _listeners;

struct OrderComp {
	bool operator()(Object *a, Object *b) {
		return a->getOrder() < b->getOrder();
	}
};

struct TransComp {
	bool operator()(Model *a, Model *b)const {
		return
			cam_tform.v.distance(a->getRenderTform().v) <
			cam_tform.v.distance(b->getRenderTform().v);
	}
};

static vector<Model*> ord_mods, unord_mods;

static priority_queue<Model*, vector<Model*>, OrderComp> ord_que;

static priority_queue<Camera*, vector<Camera*>, OrderComp> cam_que;

static priority_queue<Model*, vector<Model*>, TransComp> transparents;

void World::capture() {

	StaticEnumerateVisible();

	for (Object* o : s_objectsVisible) {
		o->capture();
	}
}

void World::render(float tween) {

	//set render tweens, and build ordered and unordered model lists...
	ord_mods.clear();
	unord_mods.clear();

	s_objectsVisible.clear();
	_lights.clear();
	_mirrors.clear();
	_listeners.clear();

	StaticEnumerateVisible();

	for (Object* o : s_objectsVisible) {
		if (!o->beginRender(tween)) continue;

		if (Light *t = o->getLight()) _lights.push_back(t->getGxLight());
		else if (Camera *t = o->getCamera()) cam_que.push(t);
		else if (Mirror *t = o->getMirror()) _mirrors.push_back(t);
		else if (Listener *t = o->getListener()) _listeners.push_back(t);
		else if (Model *t = o->getModel()) {
			if (t->getOrder()) ord_que.push(t);
			else unord_mods.push_back(t);
		}
	}

	for (; ord_que.size(); ord_que.pop()) ord_mods.push_back(ord_que.top());

//	gx_runtime->debugLog( "RenderWorld" );

	if (!gx_scene->begin(_lights)) return;

	for (; cam_que.size(); cam_que.pop()) {

		Camera *cam = cam_que.top();

		if (!cam->beginRenderFrame()) continue;

		vector<Mirror*>::const_iterator mir_it;
		for (mir_it = _mirrors.begin(); mir_it != _mirrors.end(); ++mir_it) {
			render(cam, *mir_it);
		}

		render(cam, 0);
	}

	gx_scene->end();

//	gx_runtime->debugLog( "End RenderWorld" );

	vector<Listener*>::const_iterator lis_it;
	for (lis_it = _listeners.begin(); lis_it != _listeners.end(); ++lis_it) {
		(*lis_it)->renderListener();
	}
}

void World::render(Camera *cam, Mirror *mirror) {

	if (mirror) {
		const Transform &t = mirror->getRenderTform();
		cam_tform = t * Transform(scaleMatrix(1, -1, 1)) * -t * cam->getRenderTform();
		gx_scene->setFlippedTris(true);
	} else {
		cam_tform = cam->getRenderTform();
		gx_scene->setFlippedTris(false);
	}

	//set camera matrix
	gx_scene->setViewMatrix((gxScene::Matrix*)&(-cam_tform));

	//initialize render context
	RenderContext rc(cam_tform, cam->getFrustum(), mirror != 0);

	//draw everything in order
	size_t ord = 0;
	gx_scene->setZMode(gxScene::ZMODE_DISABLE);
	while (ord < ord_mods.size() && ord_mods[ord]->getOrder()>0) {
		Model *mod = ord_mods[ord++];
		if (!mod->doAutoFade(cam_tform.v)) continue;
		render(mod, rc);
		flushTransparent();
	}

	gx_scene->setZMode(gxScene::ZMODE_NORMAL);
	for (size_t k = 0; k < unord_mods.size(); ++k) {
		Model *mod = unord_mods[k];
		if (!mod->doAutoFade(cam_tform.v)) continue;
		render(mod, rc);
	}
	gx_scene->setZMode(gxScene::ZMODE_CMPONLY);
	flushTransparent();

	gx_scene->setZMode(gxScene::ZMODE_DISABLE);
	while (ord < ord_mods.size()) {
		Model *mod = ord_mods[ord++];
		if (!mod->doAutoFade(cam_tform.v)) continue;
		render(mod, rc);
		flushTransparent();
	}
}

void World::render(Model *mod, const RenderContext &rc) {

	bool trans = mod->render(rc);

	if (mod->queueSize(Model::QUEUE_OPAQUE)) {
		if (mod->getRenderSpace() == Model::RENDER_SPACE_LOCAL) {
			gx_scene->setWorldMatrix((gxScene::Matrix*)&mod->getRenderTform());
		} else {
			gx_scene->setWorldMatrix(0);
		}
		mod->renderQueue(Model::QUEUE_OPAQUE);
	}

	if (trans || mod->queueSize(Model::QUEUE_TRANSPARENT)) {
		transparents.push(mod);
	}
}

void World::flushTransparent() {

	bool local = true;

	for (; transparents.size(); transparents.pop()) {
		Model *mod = transparents.top();
		if (mod->getRenderSpace() == Model::RENDER_SPACE_LOCAL) {
			gx_scene->setWorldMatrix((gxScene::Matrix*)&mod->getRenderTform());
			local = true;
		} else if (local) {
			gx_scene->setWorldMatrix(0);
			local = false;
		}
		mod->renderQueue(Model::QUEUE_TRANSPARENT);
	}
}