#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <malloc.h>

#include <pspgu.h>
#include <pspgum.h>

#include "vert.h"

#define ABS(x) (x)<0?-(x):(x)
#define PLANESTOCLIP 4

#define CLIP_VFPU 1

/* inline */ void normalizePlane(ScePspFVector4 *plane)
{
	//printf("plane: %.3f,%.3f,%.3f,%.3f\n",plane.x,plane.y,plane.z,plane.w);
#ifdef CLIP_VFPU
	__asm__ volatile (
	"lv.q	C000, %0\n"	// 16 byte aligned assumed.
	"vdot.t	S010, C000, C000\n"	// squared magnitude of x,y,z 
	"vrsq.s S010, S010\n"	// 1.0f/sqrtf(x*x+y*y+z*z)
	"vscl.q	C000, C000, S010\n"	// vector/=mag
	"sv.q	C000, %0\n"	// save it again
	: "+m" (*plane)
	);
#else
	float mag=sqrtf(plane->x*plane->x+plane->y*plane->y+plane->z*plane->z);
	
	if(mag==0.0f) {
		plane->w=0;
		return;
	}
	plane->x/=mag;
	plane->y/=mag;
	plane->z/=mag;
	plane->w/=mag;
#endif
}

int CLIPPLANE=1;
extern float from[3],to[3],up[3];
ScePspFVector4 __attribute__((aligned(16))) frustumPlane[PLANESTOCLIP];	// right, left, bottom, top, back, front
void frustumFromView()
{
	ScePspFMatrix4 view,model,projection,viewmodel,clip;

	sceGumUpdateMatrix();
	sceGumMatrixMode(GU_PROJECTION);
	sceGumStoreMatrix(&projection);
	sceGumMatrixMode(GU_VIEW);
	sceGumStoreMatrix(&view);
	sceGumMatrixMode(GU_MODEL);
	sceGumStoreMatrix(&model);

	gumMultMatrix(&viewmodel,&projection,&view);
	gumMultMatrix(&clip,&viewmodel,&model);
	
	float *t=(float *)&clip;
	int i;
	for(i=0;i<PLANESTOCLIP/2;i++) {	// front and back don't have clipping issues
		// Right (Bottom, not Back)
		frustumPlane[i*2+0].x=t[3]-t[0+i];
		frustumPlane[i*2+0].y=t[7]-t[4+i];
		frustumPlane[i*2+0].z=t[11]-t[8+i];
		frustumPlane[i*2+0].w=t[15]-t[12+i];
		normalizePlane(&frustumPlane[i*2+0]);
		// Left (Top, not Front)
		frustumPlane[i*2+1].x=t[3]+t[0+i];
		frustumPlane[i*2+1].y=t[7]+t[4+i];
		frustumPlane[i*2+1].z=t[11]+t[8+i];
		frustumPlane[i*2+1].w=t[15]+t[12+i];
		normalizePlane(&frustumPlane[i*2+1]);
	}
#ifdef CLIP_VFPU
	__asm__ volatile (
	"lv.q	C700, %0\n"	// Load plane into register (16byte align)
	"lv.q	C710, %1\n"	// Load plane into register (16byte align)
	"lv.q	C720, %2\n"	// Load plane into register (16byte align)
	"lv.q	C730, %3\n"	// Load plane into register (16byte align)
	//"lv.q	C420, %4\n"	// Load plane into register (16byte align)
	//"lv.q	C430, %5\n"	// Load plane into register (16byte align)
	:: "m"(frustumPlane[0]),"m"(frustumPlane[1]),"m"(frustumPlane[2]),"m"(frustumPlane[3]) //,"m"(frustumPlane[4]),"m"(frustumPlane[5])
	);
#endif
}

inline int pointInside(ScePspFVector3 *point,ScePspFVector4 *plane)
{
	if(plane->x*point->x+plane->y*point->y+plane->z*point->z+plane->w>=0) return 1;
	return 0;
}

/* inline */ int pointInsideFrustum(float x,float y,float z)
{
#ifdef CLIP_VFPU
	float dist[4];
	__asm__ volatile (
		"mtv	%4, S630\n"		// Load vertex into register
		"mtv	%5, S631\n"		// Load vertex into register
		"mtv	%6, S632\n"		// Load vertex into register
		"vone.s	S633\n"			// w=1
		"vdot.q	S620, C700, C630\n"	// s620 = vect . left plane
		"vdot.q	S621, C710, C630\n"	// s621 = vect . right plane
		"vdot.q	S622, C720, C630\n"	// s622 = vect . top plane
		"vdot.q	S623, C730, C630\n"	// s623 = vect . bottom plane
		// should use vmin.p, vmin.s here.
		"mfv	%0, S620\n"		// out1 = s620
		"mfv	%1, S621\n"		// out2 = s621
		"mfv	%2, S622\n"		// out3 = s622
		"mfv	%3, S623\n"		// out4 = s623
		: "=r"(dist[0]), "=r"(dist[1]), "=r"(dist[2]), "=r"(dist[3]) : "r"(x),"r"(y),"r"(z)
	);

	// Should be returning a bit array of which planes to clip to
	if(dist[0]<=0 || dist[1]<=0 || dist[2]<=0 || dist[3]<=0.0f)
	{
		return 0;
	}

#else
	int i;
	for(i = 0; i < PLANESTOCLIP; i++ ) {
		// Calculate the plane equation and check if the point is behind a side of the frustum
		if(frustumPlane[i].x*x+frustumPlane[i].y*y+frustumPlane[i].z*z+frustumPlane[i].w<=0) return 0;
	}
#endif
	// The point was inside of the frustum (In front of ALL the sides of the frustum)
	return 1;
}

int pointInsideGuardBand(float x,float y,float z)
{
	float band=2048.0f/480.0f;
#ifdef CLIP_VFPU
	float dist[4];
	__asm__ volatile (
		"vone.q	C600\n"			// 1,1,1,1
		"mtv	%7, S603\n"		// Band scale factor
		"mtv	%4, S630\n"		// Load vertex into register
		"mtv	%5, S631\n"		// Load vertex into register
		"mtv	%6, S632\n"		// Load vertex into register
		"vone.s	S633\n"			// w=1
		"vmul.q	C610, C700, C600\n"	// scale for the band 
		"vdot.q	S620, C610, C630\n"	// s620 = vect . left plane
		"vmul.q	C610, C710, C600\n"	// scale for the band 
		"vdot.q	S621, C610, C630\n"	// s621 = vect . right plane
		"vmul.q	C610, C720, C600\n"	// scale for the band 
		"vdot.q	S622, C610, C630\n"	// s622 = vect . top plane
		"vmul.q	C610, C730, C600\n"	// scale for the band 
		"vdot.q	S623, C610, C630\n"	// s623 = vect . bottom plane
		// should use vmin.p, vmin.s here.
		"mfv	%0, S620\n"		// out1 = s620
		"mfv	%1, S621\n"		// out2 = s621
		"mfv	%2, S622\n"		// out3 = s622
		"mfv	%3, S623\n"		// out4 = s623
		: "=r"(dist[0]), "=r"(dist[1]), "=r"(dist[2]), "=r"(dist[3]) : "r"(x),"r"(y),"r"(z),"r"(band)
	);

	// Should be returning a bit array of which planes to clip to
	if(dist[0]<=0 || dist[1]<=0 || dist[2]<=0 || dist[3]<=0.0f)
	{
		return 0;
	}

#else
	int i;
	for(i = 0; i < PLANESTOCLIP; i++ ) {
		if(i==4) band=1.0f;
		// Calculate the plane equation and check if the point is behind a side of the frustum
		if(frustumPlane[i].x*x+frustumPlane[i].y*y+frustumPlane[i].z*z+frustumPlane[i].w*band<=0) return 0;
	}
#endif
	// The point was inside of the frustum (In front of ALL the sides of the frustum)
	return 1;
}

#if 0
int sphereInside( float x, float y, float z, float radius )
{
	int planesOut=0;
#ifdef CLIP_VFPU
	float dist[4];
	__asm__ volatile (
		"mtv	%4, S630\n"		// Load vertex into register
		"mtv	%5, S631\n"		// Load vertex into register
		"mtv	%6, S632\n"		// Load vertex into register
		"vone.s	S633\n"			// w=1
		"vdot.q	S620, C700, C630\n"	// s620 = vect . left plane
		"vdot.q	S621, C710, C630\n"	// s621 = vect . right plane
		"vdot.q	S622, C720, C630\n"	// s622 = vect . top plane
		"vdot.q	S623, C730, C630\n"	// s623 = vect . bottom plane
		"mfv	%0, S620\n"		// out1 = s620
		"mfv	%1, S621\n"		// out2 = s621
		"mfv	%2, S622\n"		// out3 = s622
		"mfv	%3, S623\n"		// out4 = s623
		: "=r"(dist[0]), "=r"(dist[1]), "=r"(dist[2]), "=r"(dist[3]) : "r"(x),"r"(y),"r"(z)
	);

	if(dist[0]<=-radius) planesOut++;
	if(dist[1]<=-radius) planesOut++;
	if(dist[2]<=-radius) planesOut++;
	if(dist[3]<=-radius) planesOut++;
#else
	int i;
	for(i = 0; i < 4; i++ )	{
		// If the center of the sphere is farther away from the plane than the radius
		if( frustumPlane[i].x*x+frustumPlane[i].y*y+frustumPlane[i].z*z+frustumPlane[i].w<=-radius ) planesOut++;
	}
#endif
	// The sphere was inside of the frustum!
	if(planesOut==4) return 0;	// Completely outside
	if(planesOut>0) return 1;	// Partly outside
	return 2;	// Completely inside
}
int cubeInside( float *minmax )
{
	int planesOut=0;
	float dist[2]={ (minmax[3]-minmax[0])*0.5f, (minmax[4]-minmax[1])*0.5f };
	float x=(minmax[3]+minmax[0])*0.5f;
	float y=(minmax[4]+minmax[1])*0.5f;
	float z=(minmax[5]+minmax[2])*0.5f;
#ifdef CLIP_VFPU
	float distRes[4];
	__asm__ volatile (
		"mtv	%4, S630\n"		// Load vertex into register
		"mtv	%5, S631\n"		// Load vertex into register
		"mtv	%6, S632\n"		// Load vertex into register
		"vone.s	S633\n"			// w=1
		"vdot.q	S620, C700, C630\n"	// s620 = vect . left plane
		"vdot.q	S621, C710, C630\n"	// s621 = vect . right plane
		"vdot.q	S622, C720, C630\n"	// s622 = vect . top plane
		"vdot.q	S623, C730, C630\n"	// s623 = vect . bottom plane
		"mfv	%0, S620\n"		// out1 = s620
		"mfv	%1, S621\n"		// out2 = s621
		"mfv	%2, S622\n"		// out3 = s622
		"mfv	%3, S623\n"		// out4 = s623
		: "=r"(distRes[0]), "=r"(distRes[1]), "=r"(distRes[2]), "=r"(distRes[3]) : "r"(x),"r"(y),"r"(z)
	);

	if(distRes[0]<=-dist[0]) planesOut++;
	if(distRes[1]<=-dist[0]) planesOut++;
	if(distRes[2]<=-dist[1]) planesOut++;
	if(distRes[3]<=-dist[1]) planesOut++;
#else
	int i;
	for(i = 0; i < 4; i++ )	{
		// If the center of the sphere is farther away from the plane than the radius
		if( frustumPlane[i].x*x+frustumPlane[i].y*y+frustumPlane[i].z*z+frustumPlane[i].w<=-dist[i/2] ) planesOut++;
	}
#endif
	// The sphere was inside of the frustum!
	if(planesOut==4) return 0;	// Completely outside
	if(planesOut>0) return 1;	// Partly outside
	return 2;	// Completely inside
}
#endif

int clipPolygonToPlane( Vertex3DCNP *src, int n, Vertex3DCNP *dest, int *outCount, ScePspFVector4 *plane)
{
    int o = 0;
    int clipped=0;
    int i;
    for( i=0; i < n; ++i) {
	    //dest[o++] = src[i];
#if 1
	int prev = i?i-1:n-1;
	if(prev<0) return clipped;

	float t0 = plane->x*src[prev].x + plane->y*src[prev].y +
	    plane->z*src[prev].z + plane->w;
	float t1 = plane->x*src[i].x + plane->y*src[i].y +
	    plane->z*src[i].z + plane->w;

	if( t0 >= 0 && t1 >= 0) {
	    dest[o++] = src[i];
	} else if( (t0 >= 0 || t1 >= 0) ) {
	    float tt = t0/(t0-t1);
	    dest[o].color = src[prev].color;
	    dest[o].nx = src[prev].nx;
	    dest[o].ny = src[prev].ny;
	    dest[o].nz = src[prev].nz;
	    dest[o].x = src[prev].x + (src[i].x-src[prev].x) * tt;
	    dest[o].y = src[prev].y + (src[i].y-src[prev].y) * tt;
	    dest[o].z = src[prev].z + (src[i].z-src[prev].z) * tt;
	    //dest[o].u = src[prev].u + (src[i].u-src[prev].u) * tt;
	    //dest[o].v = src[prev].v + (src[i].v-src[prev].v) * tt;
	    ++o;
	    if( t1 >= 0) dest[o++] = src[i];
	    clipped++;
	}
#endif
    }
    *outCount=o;
    return clipped;
}

int clipPolygon( Vertex3DCNP *src, int n, Vertex3DCNP *dest, int *o)
{
	Vertex3DCNP *tmp;
	tmp=malloc(sizeof(Vertex3DCNP)*(n+6));
	Vertex3DCNP *freeable;
	freeable=tmp;
	int i;
	int clipped=0;
	*o=0;
	for(i=0;i<PLANESTOCLIP;i++) {
		clipped+=clipPolygonToPlane(src,n,tmp,o,&frustumPlane[i]);
		n=*o;
		if(n==0) break;
		src=tmp;
		tmp=dest;
		dest=src;
	}
	free(freeable);
	return clipped;
}

void drawMesh(Octtree *mesh)
{
	sceGumDrawArray(GU_TRIANGLES,GU_COLOR_8888|GU_NORMAL_32BITF|GU_VERTEX_32BITF|GU_TRANSFORM_3D,mesh->vertCount,0,mesh->vert);

	int i;
	for(i=0;i<8;i++) {
		drawMesh(mesh->child[i]);
	}
}

void drawMeshClipped(Octtree *mesh,float clipLen)
{
	// Speedup by doing simple clip culling.
	if(mesh==0) return;
	if(mesh->width<clipLen) {
		// Polygons are small enough not that clipping is no problem
		drawMesh(mesh);
		return;
	}
#if 0
	int res=frustum.sphereInside(mesh->boundingSphere[0],mesh->boundingSphere[1],mesh->boundingSphere[2],mesh->boundingSphere[3]);
	if(res==0) return;	// Totally not visible
	if(res==2) {
		drawMesh(mesh);	// No clipping needed
		return;
	}
#endif
#if 0
	// Border case.  Double check with cube
	res=frustum.cubeInside(mesh->boundingRect);
	if(res==0) return;
	if(res==2) {
		drawMesh(mesh);	// No clipping needed
		return;
	}
#endif
	// Clipping of some polygons probably needed.
	sceGumDrawArray(GU_TRIANGLES,GU_COLOR_8888|GU_NORMAL_32BITF|GU_VERTEX_32BITF|GU_TRANSFORM_3D,mesh->vertCount,0,mesh->vert);

	int inCount=0,outCount=0;
	int i;
	Vertex3DCNP *outvert=0;
	Vertex3DCNP *invert=0;
	int getCount=0;
	for(i=0;i<mesh->vertCount;i+=3) {
		int j;
		int n;
		n=0;
#if 1
		for(j=0;j<3;j++) {
			if( pointInsideFrustum(
			  mesh->vert[i+j].x,
			  mesh->vert[i+j].y,
			  mesh->vert[i+j].z) ) n++;
		}
		if(n==3) continue;	// False alarm, it drew okay anyway.
#endif
		invert=mesh->vert+(i+j);
		//n=3;
		inCount+=3;

		if(getCount>3000) {
			sceGuFinish();
			sceGuSync(0,0);
			guStart();
			getCount=0;
		}

		if(outvert==0) {
			outvert=(Vertex3DCNP *) sceGuGetMemory(9 * sizeof(Vertex3DCNP));
			getCount++;
		}
		int clipped=clipPolygon(invert,3,outvert,&n);
		if(n<=2) continue;
		if(clipped==0) continue;	// We didn't alter any points
		outCount+=n;
		sceGumDrawArray(GU_TRIANGLE_FAN,GU_COLOR_8888|GU_NORMAL_32BITF|GU_VERTEX_32BITF|GU_TRANSFORM_3D,n,0,outvert);
		outvert=0;
	}
//printf("Mesh %08x In: %d pts; Out: %d pts\n",(int)mesh,inCount,outCount);
	sceGuFinish();
	sceGuSync(0,0);
	guStart();
	
	for(i=0;i<8;i++) {
		drawMeshClipped(mesh->child[i],clipLen);
	}
}

void drawTreeClipped(Octtree *mesh,float cliplen)
{
	if(!mesh) return;
	frustumFromView();
	drawMeshClipped(mesh,cliplen);
}
