/* Collision management functions */
#include "gameinc.h"

#define DEBUG_MESHOUT 0
#define DEBUG_VECCOLLISION 0

typedef struct s_C_plane { float v[3][3];
									float equation[4];
									br_model *model;
									} t_C_plane;

static void FVector3Sub(float *v1,float *v2,float *v3)
{
	v1[0] = v2[0]-v3[0];
	v1[1] = v2[1]-v3[1];
	v1[2] = v2[2]-v3[2];
}


void FVector3Normalise(float *v1,float *v2)
{
	float scale;

	scale = (float) sqrt(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2] );

	if (scale >0.00000001 )
	{
		v1[0] = v2[0] / scale;
		v1[1] = v2[1] / scale;
		v1[2] = v2[2] / scale;
	}
	else
	{
		v1[0] = 1.0F;
		v1[1] = 0.0F;
		v1[2] = 0.0F;
	}
}


static void FVector3Cross(float *v1,float *v2,float *v3)
{
	v1[0]=(v2[1]*v3[2])-(v2[2]*v3[1]);
	v1[1]=(v2[2]*v3[0])-(v2[0]*v3[2]);
	v1[2]=(v2[0]*v3[1])-(v2[1]*v3[0]);
}


static FILE *meshout;
static int totalplanes;
static int planecount;

static t_C_plane *collision_planes = NULL;

static void dumpmatrix(br_matrix34 *m)
{
	int k, l;
	float v[12];
	char buff[1024];

 	/* Dump a matrix */
	for (k = 0; k < 4; k++)
		for (l = 0; l < 3; l++)
			v[k*3+l] = BrScalarToFloat(m->m[k][l]);
	sprintf(buff,
		"transform\n%4.3f %4.3f %4.3f\n%4.3f %4.3f %4.3f\n%4.3f %4.3f %4.3f\n%4.3f %4.3f %4.3f\n",v[0], v[1], v[2],v[3], v[4], v[5],
			v[6], v[7], v[8],v[9], v[10], v[11]);

	fprintf(meshout,buff);

	/* End of hack */
}


static void dumppolylist(br_vector3 *vecs,int num)
{
	int i;
	float v[3];

	for (i=0;i<num;i++)
	{
		v[0] = BrScalarToFloat(vecs[i].v[0]);
		v[1] = BrScalarToFloat(vecs[i].v[1]);
		v[2] = BrScalarToFloat(vecs[i].v[2]);
		fprintf(meshout,"[%4d] %8.5f %8.5f %8.5f\n",i,v[0],v[1],v[2]);
	}

}


static void C_MakeEquation(t_C_plane *plane)
{
	float x,y,z;
	float a,b,c,d;
	float vec1[3],vec2[3],norm[3];

	x = plane->v[0][0];
	y = plane->v[0][1];
	z = plane->v[0][2];

	FVector3Sub(vec1,plane->v[1],plane->v[0]);
	FVector3Sub(vec2,plane->v[2],plane->v[0]);
	FVector3Cross(norm,vec1,vec2);
	FVector3Normalise(norm,norm);

	a = norm[0];
	b = norm[1];
	c = norm[2];

	d = -(x*a+y*b+z*c);

	plane->equation[0] = a;
	plane->equation[1] = b;
	plane->equation[2] = c;
	plane->equation[3] = d;
}


static void MakeMeshCollisionForModel(br_matrix34 *thismat,br_model *this, br_actor *thisactor)
{
	int i,j,numv,numf,vertidx;
	br_face *thisface;
	br_vector3	poly_vertlist[3];
	br_vector3	tempvert;

#if DEBUG_MESHOUT
	if (this->identifier != NULL)
	{
		fprintf(meshout,"Model name '%s'\n",this->identifier);
	}
	else
	{
		fprintf(meshout,"Model name '**Unnamed**'\n");
	}
#endif

	numv = this->nvertices;
	numf = this->nfaces;

#if DEBUG_MESHOUT
	fprintf(meshout,"Num sub-planes = %d\n",numf);
#endif

	for (i=0;i<numf;i++)
	{
		thisface = &(this->faces[i]);

#if DEBUG_MESHOUT
		fprintf(meshout,"Plane [%4d]\n",planecount);
#endif
		for (j=0;j<3;j++)
		{
			vertidx = thisface->vertices[j];
			BrVector3Copy(&tempvert , &this->vertices[vertidx].p);
			BrMatrix34ApplyP(&poly_vertlist[j],&tempvert,thismat);
			collision_planes[planecount].v[j][0] = BrScalarToFloat(poly_vertlist[j].v[0]);
			collision_planes[planecount].v[j][1] = BrScalarToFloat(poly_vertlist[j].v[1]);
			collision_planes[planecount].v[j][2] = BrScalarToFloat(poly_vertlist[j].v[2]);
		}

		collision_planes[planecount].model = this;
		C_MakeEquation(&collision_planes[planecount]);

		planecount++;
#if DEBUG_MESHOUT
		dumppolylist(poly_vertlist,3);
#endif
	}
}


static void RecursiveBuildMesh(br_actor *base,br_actor *this)
{
	br_model *thismodel;
	br_matrix34 thismat;

	BrActorToActorMatrix34(&thismat,this,base);
//	dumpmatrix(&thismat);

	if (this->type == BR_ACTOR_MODEL && this->model != NULL)
	{
		thismodel = this->model;
		MakeMeshCollisionForModel(&thismat,thismodel,this);
	}

	if (this->next != NULL)
		RecursiveBuildMesh(base,this->next);
	if (this->children != NULL)
		RecursiveBuildMesh(base,this->children);
}


static void RecursiveTotalMesh(br_actor *base,br_actor *this)
{
	br_model *thismodel;

	if (this->type == BR_ACTOR_MODEL && this->model != NULL)
	{
		thismodel = this->model;
		totalplanes += thismodel->nfaces;
	}

	if (this->next != NULL)
		RecursiveTotalMesh(base,this->next);
	if (this->children != NULL)
		RecursiveTotalMesh(base,this->children);
}


void Collision_RecursiveBuildMesh(br_actor *base,br_actor *root)
{

#if DEBUG_MESHOUT
	meshout = fopen("d:\\cult\\temp\\levmesh.txt","w");
	fprintf(meshout,"Total number of planes = %d\n",totalplanes);
#endif

	planecount=0;
	RecursiveBuildMesh(base,root);

#if DEBUG_MESHOUT
	fclose(meshout);
#endif

}


void Collision_RecursiveTotalMesh(br_actor *base,br_actor *root)
{

	RecursiveTotalMesh(base,root);

}


void Collision_MeshUpBaseWorld(br_actor *base_world_actor,br_actor *levelroot)
{
	char buff[256];
	int collsize;

	totalplanes = 0;

	/* Free the previous planes */
	if (collision_planes != NULL)
	{
		free(collision_planes);
	}

	Editor_OverlayDebug("      Collision and visability database build");
	Collision_RecursiveTotalMesh(base_world_actor,levelroot);
	collsize = sizeof(t_C_plane)*totalplanes;
	sprintf(buff,"      Total memory used is %.2fK",(float) collsize/1024.0F);
	Editor_OverlayDebug(buff);
	collision_planes = calloc(1,collsize);
	Collision_RecursiveBuildMesh(base_world_actor,levelroot);
}


br_scalar Collision_SolveForYHeight(br_scalar sxpos,br_scalar sypos,br_scalar szpos,br_scalar smaxupvec)
{
	float ysolved,solve;
	int i,cnt,p1,p2,inflag;
	float a,b,c,d;
	float x,y,z;
	float part1,part2,part3;
	float xpos,ypos,zpos,maxupvec;
	int lastsolved = -1;
//	char buff[256];

	xpos = BrScalarToFloat(sxpos);
	ypos = BrScalarToFloat(sypos);
	zpos = BrScalarToFloat(szpos);
	maxupvec  = BrScalarToFloat(smaxupvec);

	ysolved = -9999.0F;

//	Editor_OverlayDebug("Testing for polys!");
	for(i=0;i<totalplanes;i++)
	{
		/* Over a poly check... */
		inflag = 0;
		a = b = c = 0.0F;
		for (cnt=0;cnt<3;cnt++)
		{
			p1 = cnt;
			p2 = cnt+1;
			if (p2 == 3)
			{
				p2 = 0;
			}

			if ( (collision_planes[i].v[p1][2] <= zpos &&
					zpos < collision_planes[i].v[p2][2]) ||
					(collision_planes[i].v[p2][2] <= zpos &&
					zpos < collision_planes[i].v[p1][2]) )
			{

				part1 = (collision_planes[i].v[p2][0] - collision_planes[i].v[p1][0]);
				part2 = (zpos - collision_planes[i].v[p1][2]);
				part3 = (collision_planes[i].v[p2][2] - collision_planes[i].v[p1][2]);

				if (xpos < ( (part1 * part2) / part3) + collision_planes[i].v[p1][0] )
				{
					inflag = !inflag;
				}
			}
		}

		if (inflag)
		{
//			sprintf(buff,"I'm inside plane %d !!!",i);
//			Editor_OverlayDebug(buff);
			/* Poly normal find and y solve bit */
			x = collision_planes[i].v[0][0];
			y = collision_planes[i].v[0][1];
			z = collision_planes[i].v[0][2];

			a = collision_planes[i].equation[0];
			b = collision_planes[i].equation[1];
			c = collision_planes[i].equation[2];
			d = collision_planes[i].equation[3];

			if (b > 0.1)
			{
				solve = -(a*BrScalarToFloat(xpos)+c*BrScalarToFloat(zpos) +d )/b;
				if (solve-BrScalarToFloat(ypos) < BrScalarToFloat(maxupvec))
				{
					if (solve > ysolved)
					{
						ysolved=solve;
						lastsolved = i;
					}
//					sprintf(buff,"Plane solve=%f ysolve=%f",solve,ysolved);
//					Editor_OverlayDebug(buff);
				}
				else
				{

					if (BrScalarToFloat(maxupvec) == 0.0F)
					{
						if (solve > ysolved)
						{
							ysolved=solve;
							lastsolved = i;
						}
					}
					else
					{
//						sprintf(buff,"Ignored plane solve=%f ysolve=%f",solve,ysolved);
//						Editor_OverlayDebug(buff);
					}
				}
			}
		}
	}

#if 0
	if (lastsolved != -1)
	{
		if (collision_planes[lastsolved].model->identifier != NULL)
		{
			char buff2[256];
			sprintf(buff2,"Standing on model '%s'",collision_planes[lastsolved].model->identifier);
			Editor_OverlayDebug(buff2);
		}
		else
		{
			Editor_OverlayDebug("Standing on model 'UNNAMED'");
		}
	}
#endif

	if (ysolved == -9999.0F)
	{
		return ypos;
	}

	return BR_SCALAR(ysolved);
}


static int C_PointInPolyCheck(float x,float y,float z,int plane)
{
	int axis1,axis2;
	float lval;
	int lidx;
	int i,cnt,p1,p2,inflag;
	float part1,part2,part3;
	float xpos,ypos;
	int lastsolved = -1;
//	char buff[256];

	i = plane;
	inflag = 0;

// FIXME Precalc most of this data?
	/* Discard the largest ordinate of the normal and collide in 2D with this point */
	lval = (float) fabs(collision_planes[plane].equation[0]);
	lidx = 0;

	if (fabs(collision_planes[plane].equation[1]) > lval)
	{
		lval =  (float) fabs(collision_planes[plane].equation[0]);
		lidx = 1;
	}

	if (fabs(collision_planes[plane].equation[2]) > lval)
	{
		lidx = 2;
	}

	switch(lidx)
	{
		case 0:
			axis1 = 1;
			axis2 = 2;
			xpos = y;
			ypos = z;
			break;
		case 1:
			axis1 = 0;
			axis2 = 2;
			xpos = x;
			ypos = z;
			break;
		case 2:
			axis1 = 0;
			axis2 = 1;
			xpos = x;
			ypos = y;
			break;
	}

		for (cnt=0;cnt<3;cnt++)
		{
			p1 = cnt;
			p2 = cnt+1;
			if (p2 == 3)
			{
				p2 = 0;
			}

			if ( (collision_planes[i].v[p1][axis2] <= ypos &&
					ypos <= collision_planes[i].v[p2][axis2]) ||
					(collision_planes[i].v[p2][axis2] <= ypos &&
					ypos <= collision_planes[i].v[p1][axis2]) )
			{

				part1 = (collision_planes[i].v[p2][axis1] - collision_planes[i].v[p1][axis1]);
				part2 = (ypos - collision_planes[i].v[p1][axis2]);
				part3 = (collision_planes[i].v[p2][axis2] - collision_planes[i].v[p1][axis2]);

				if (xpos <= ( (part1 * part2) / part3) + collision_planes[i].v[p1][axis1] )
				{
					inflag = !inflag;
				}
			}
		}

	return inflag;
}


float C_Point2Plane(float x,float y,float z,float a,float b,float c,float d)
{

	return (x*a + y*b +z*c) + d;
}


int Collision_SolveForVector(br_vector3 *solve,br_vector3 *start, br_vector3 *end, br_scalar radius)
{
	int cnt;
	float a,b,c,d;
	float x,y,z, x1,y1,z1, x2,y2,z2;
	float i,j,k;
	float t;
	float bottest;
#if DEBUG_VECCOLLISION
	char buff[256];
#endif
	int inflag;
	int retflag=0;
	float dist;
	int reptcount=0;
	float vlen;

	/* Set the solve just incase nothing is found */
	solve->v[0] = end->v[0];
	solve->v[1] = end->v[1];
	solve->v[2] = end->v[2];

	x1 = BrScalarToFloat(start->v[0]);
	y1 = BrScalarToFloat(start->v[1]);
	z1 = BrScalarToFloat(start->v[2]);

	x2 = BrScalarToFloat(end->v[0]);
	y2 = BrScalarToFloat(end->v[1]);
	z2 = BrScalarToFloat(end->v[2]);

#if DEBUG_VECCOLLISION
	sprintf(buff,"from %f,%f,%f",x1,y1,z1);
	Editor_OverlayDebug(buff);
	sprintf(buff,"to   %f,%f,%f",x2,y2,z2);
	Editor_OverlayDebug(buff);
#endif


	i = x2 - x1;
	j = y2 - y1;
	k = z2 - z1;
	vlen = (float) sqrt(i*i + j*j + k*k);

	for(cnt=0;cnt<totalplanes;cnt++)
	{
		x = collision_planes[cnt].v[0][0];
		y = collision_planes[cnt].v[0][1];
		z = collision_planes[cnt].v[0][2];

		a = collision_planes[cnt].equation[0];
		b = collision_planes[cnt].equation[1];
		c = collision_planes[cnt].equation[2];
		d = collision_planes[cnt].equation[3];


		/* Discard planes that are too far away or not needed quickly */
		/* FIXME Really need the second check? */
		if ((dist=C_Point2Plane(x1,y1,z1,a,b,c,d)) < 0.0F)
		{
			continue;
		}

		if (dist > vlen*2)
		{
			continue;
		}

// From the equation t = - (a*x1 + b*y1 + c*z1 + d)/(a*i + b*j + c*k)

		bottest = a*i + b*j + c*k;
		if (bottest != 0.0F)
		{
			t =  -(a*x1 + b*y1 + c*z1 + d) / bottest;
			if ( t <= 1.0F && t >= 0.0F)
			{

				x = x1 + i*t;
				y = y1 + j*t;
				z = z1 + k*t;

#if DEBUG_VECCOLLISION
				sprintf(buff,"cp   %f,%f,%f",x,y,z);
				Editor_OverlayDebug(buff);
#endif

				inflag = C_PointInPolyCheck(x,y,z,cnt);

#if DEBUG_VECCOLLISION
				sprintf(buff,"Collision with plane %d true %d",cnt,inflag);
				Editor_OverlayDebug(buff);
#endif

				if (inflag)
				{
					retflag = 1;
					dist = -(C_Point2Plane(x2,y2,z2,a,b,c,d)-0.1F);
					x2 = x2 + (dist * a);
					y2 = y2 + (dist * b);
					z2 = z2 + (dist * c);

					i = x2 - x1;
					j = y2 - y1;
					k = z2 - z1;
					vlen = (float) sqrt(i*i + j*j + k*k);

#if DEBUG_VECCOLLISION
					sprintf(buff,"nto  %f,%f,%f with dist %f",x2,y2,z2,dist);
					Editor_OverlayDebug(buff);
#endif
					cnt = 0;
					reptcount++;
				}
			}
		}
		if (reptcount > 20)
		{
			break;
		}
	}

	solve->v[0] = BR_SCALAR(x2);
	solve->v[1] = BR_SCALAR(y2);
	solve->v[2] = BR_SCALAR(z2);

	return retflag;
}
