#include "world.hpp"
#include <queue>
#include "gxruntime.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 std::vector<Object*> _objsByType[1000];

static std::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)
{
	std::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 std::vector<CollInfo>& collinfos = _collInfo[src->getCollisionType()];

	int hits = 0;
	for (;;) {
		Collision                             coll;
		Object*                               coll_obj = 0;
		std::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 std::vector<gxLight*>  _lights;
static std::vector<Mirror*>   _mirrors;
static std::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 std::vector<Model*> ord_mods, unord_mods;

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

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

static std::priority_queue<Model*, std::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;

		std::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" );

	std::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);
	}
}
