#include "stdafx.h"

static bool gDrawUnitLengthNormal	= false;
static bool gRenderWireframe		= false;

static const float gDisp = 0.01f;
static const float gCamSpeed = 0.1f;
static btVector3 Eye(3.0616338f, 1.1985892f, 2.5769043f);
static btVector3 Dir(-0.66853905,-0.14004262,-0.73037237);
static btVector3 N;
static int mx = 0;
static int my = 0;

static float gAngle = 0.0f;
static float gAngle2 = 0.0f;

#define MAX_NB_CONVEXES	14
static MyConvex gConvexes0[MAX_NB_CONVEXES];
static MyConvex gConvexes1[MAX_NB_CONVEXES];
static int gConvexID0 = 0;
static int gConvexID1 = 1;

#define RAYAABB_EPSILON 0.00001f
static bool RayAABBIntersect(const btVector3& min, const btVector3& max, const btVector3& origin, const btVector3& dir, btVector3& coord)
{
	int Inside = TRUE;
	btVector3 MaxT(-1.0f, -1.0f, -1.0f);

	// Find candidate planes.
	for(int i=0;i<3;i++)
	{
		if(origin[i] < min[i])
		{
			coord[i]	= min[i];
			Inside		= FALSE;

			// Calculate T distances to candidate planes
			if(IR(dir[i]))	MaxT[i] = (min[i] - origin[i]) / dir[i];
		}
		else if(origin[i] > max[i])
		{
			coord[i]	= max[i];
			Inside		= FALSE;

			// Calculate T distances to candidate planes
			if(IR(dir[i]))	MaxT[i] = (max[i] - origin[i]) / dir[i];
		}
	}

	// Ray origin inside bounding box
	if(Inside)
	{
		coord = origin;
		return true;
	}

	// Get largest of the maxT's for final choice of intersection
	int WhichPlane = 0;
	if(MaxT[1] > MaxT[WhichPlane])	WhichPlane = 1;
	if(MaxT[2] > MaxT[WhichPlane])	WhichPlane = 2;

	// Check final candidate actually inside box
	if(IR(MaxT[WhichPlane])&0x80000000) return false;

	for(int i=0;i<3;i++)
	{
		if(i!=WhichPlane)
		{
			coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
#ifdef RAYAABB_EPSILON
			if(coord[i] < min[i] - RAYAABB_EPSILON || coord[i] > max[i] + RAYAABB_EPSILON)	return false;
#else
			if(coord[i] < min[i] || coord[i] > max[i])	return false;
#endif
		}
	}
	return true;	// ray hits box
}

static void PrintHelp()
{
	printf("Use arrow keys and the mouse to move the camera\n");
	printf("Press 2,4,6,8 (keypad) to move the convex along X or Y\n");
	printf("Press 1,3,7,9 (keypad) to rotate the convex\n");
	printf("F1-F2 to change first object\n");
	printf("F3-F4 to change second object\n");
	printf("Use the mouse + left mouse button to drag first object\n");
	printf("F5 to enable/disable debug rendering\n");
	printf("W to enable/disable wireframe rendering\n");
	printf("I to enable/disable internal objects\n");
}

extern int gExpectedNbTests;
extern int gActualNbTests;
extern bool gUseInternalObject;
extern bool gRenderDebugHelpers;

static void KeyboardCallback(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 27:	exit(0); break;

	case 1:
		gConvexID0++;
		if(gConvexID0==MAX_NB_CONVEXES)
			gConvexID0 = 0;
		break;
	case 2:
		gConvexID0--;
		if(gConvexID0<0)
			gConvexID0 = MAX_NB_CONVEXES-1;
		break;
	case 3:
		gConvexID1++;
		if(gConvexID1==MAX_NB_CONVEXES)
			gConvexID1 = 0;
		break;
	case 4:
		gConvexID1--;
		if(gConvexID1<0)
			gConvexID1 = MAX_NB_CONVEXES-1;
		break;
	case 5:
		gRenderDebugHelpers = !gRenderDebugHelpers;
		printf("\nRender debug helpers: %d\n", gRenderDebugHelpers);
		break;

	case 'i':
	case 'I':
		gUseInternalObject = !gUseInternalObject;
		printf("\nUse internal objects: %d\n", gUseInternalObject);
		break;

	case 'U':
	case 'u':
		gDrawUnitLengthNormal = !gDrawUnitLengthNormal;
		break;

	case 'W':
	case 'w':
		gRenderWireframe = !gRenderWireframe;
		break;

	case '4':
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(-gDisp,0,0));
		break;
	case '6':
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(gDisp,0,0));
		break;
	case '8':
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(0,gDisp,0));
		break;
	case '2':
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(0,-gDisp,0));
		break;
	case 43:
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(0,0,gDisp));
		break;
	case 45:
		gConvexes0[gConvexID0].mTransform.setOrigin(gConvexes0[gConvexID0].mTransform.getOrigin() + btVector3(0,0,-gDisp));
		break;

	case '7':
		gAngle += 0.1f;
		gConvexes0[gConvexID0].mTransform.setRotation(btQuaternion(0.0f, gAngle, gAngle2));
		break;
	case '9':
		gAngle -= 0.1f;
		gConvexes0[gConvexID0].mTransform.setRotation(btQuaternion(0.0f, gAngle, gAngle2));
		break;
	case '1':
		gAngle2 += 0.1f;
		gConvexes0[gConvexID0].mTransform.setRotation(btQuaternion(0.0f, gAngle, gAngle2));
		break;
	case '3':
		gAngle2 -= 0.1f;
		gConvexes0[gConvexID0].mTransform.setRotation(btQuaternion(0.0f, gAngle, gAngle2));
		break;

	case 101:	Eye += Dir * gCamSpeed; break;
	case 103:	Eye -= Dir * gCamSpeed; break;
	case 100:	Eye -= N * gCamSpeed; break;
	case 102:	Eye += N * gCamSpeed; break;
	}
}

static void ArrowKeyCallback(int key, int x, int y)
{
	KeyboardCallback(key,x,y);
}
	
static const float NxPiF32		= 3.141592653589793f;

float degToRad(float a)
	{
	return (float)0.01745329251994329547 * a;
	}

class NxQuat
	{
	public:
	NxQuat(){}

	NxQuat(const float angle, const btVector3 & axis)
	{
	x = axis.x();
	y = axis.y();
	z = axis.z();

	const float i_length =  1.0f / sqrtf( x*x + y*y + z*z );
	x = x * i_length;
	y = y * i_length;
	z = z * i_length;

	float Half = degToRad(angle * 0.5f);

	w = cosf(Half);
	const float sin_theta_over_two = sinf(Half );
	x = x * sin_theta_over_two;
	y = y * sin_theta_over_two;
	z = z * sin_theta_over_two;
	}

void NxQuat::multiply(const NxQuat& left, const btVector3& right)
	{
	float a,b,c,d;

	a = - left.x*right.x() - left.y*right.y() - left.z *right.z();
	b =   left.w*right.x() + left.y*right.z() - right.y()*left.z;
	c =   left.w*right.y() + left.z*right.x() - right.z()*left.x;
	d =   left.w*right.z() + left.x*right.y() - right.x()*left.y;

	w = a;
	x = b;
	y = c;
	z = d;
	}

void NxQuat::rotate(btVector3 & v) const
	{
	NxQuat myInverse;
	myInverse.x = -x;
	myInverse.y = -y;
	myInverse.z = -z;
	myInverse.w =  w;

	NxQuat left;
	left.multiply(*this,v);
	float vx = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
	float vy = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
	float vz = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
	v.setValue(vx, vy, vz);
	}

    float x,y,z,w;
	};

static btVector3 ComputeWorldRay(int xs, int ys)
{
	GLint viewPort[4];
	GLdouble modelMatrix[16];
	GLdouble projMatrix[16];
	glGetIntegerv(GL_VIEWPORT, viewPort);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelMatrix);
	glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);
	ys = viewPort[3] - ys - 1;
	GLdouble wx0, wy0, wz0;
	gluUnProject((GLdouble) xs, (GLdouble) ys, 0.0, modelMatrix, projMatrix, viewPort, &wx0, &wy0, &wz0);
	GLdouble wx1, wy1, wz1;
	gluUnProject((GLdouble) xs, (GLdouble) ys, 1.0, modelMatrix, projMatrix, viewPort, &wx1, &wy1, &wz1);
	btVector3 tmp(float(wx1-wx0), float(wy1-wy0), float(wz1-wz0));
	tmp = tmp.normalize();
	return tmp;
}

static bool mValidHit = false;
static float mDist = 0.0f;
static btVector3 mLocalHit;
static btVector3 mWorldHit;
static int gRotateCamera = false;

static void MouseCallback(int button, int state, int x, int y)
{
	mx = x;
	my = y;

	if(button)
		gRotateCamera = true;
	else
		gRotateCamera = false;

	mValidHit = false;

	if(!button && !state)
	{
		btVector3 Dir = ComputeWorldRay(x, y);

		btVector3 min, max;
		gConvexes0[gConvexID0].ComputeWorldAABB(min, max);

		btVector3 hit;
		if(RayAABBIntersect(min, max, Eye, Dir, hit))
		{
			mValidHit = true;
			mDist = Eye.distance(hit);
			mLocalHit = hit - gConvexes0[gConvexID0].mTransform.getOrigin();
			mWorldHit = hit;
		}
	}
}

static void MotionCallback(int x, int y)
{
	if(mValidHit)
	{
		const btVector3 Dir = ComputeWorldRay(x, y);
		gConvexes0[gConvexID0].mTransform.setOrigin(Eye + Dir*mDist - mLocalHit);
		return;
	}

	int dx = mx - x;
	int dy = my - y;
	
	Dir = Dir.normalize();
	N = Dir.cross(btVector3(0,1,0));

	NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0));
	qx.rotate(Dir);
	NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N);
	qy.rotate(Dir);

	mx = x;
	my = y;
}

static void RenderCallback()
{
	// Clear buffers
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
	// Setup camera
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f);
	gluLookAt(Eye.x(), Eye.y(), Eye.z(), Eye.x() + Dir.x(), Eye.y() + Dir.y(), Eye.z() + Dir.z(), 0.0f, 1.0f, 0.0f);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glEnable(GL_LIGHTING);

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

	gExpectedNbTests = 0;
	gActualNbTests = 0;

	// Reference
	static ConvexResults ReferenceResults;
	bool ReferenceStatus = ReferenceAlgo(ReferenceResults, gConvexes0[gConvexID0], gConvexes1[gConvexID1]);

	// Visualize all
	btVector3 color(0,0,0);
	gConvexes0[gConvexID0].Render(gRenderWireframe, color);	// Moving
	gConvexes1[gConvexID1].Render(gRenderWireframe, color);	// Fixed

	ReferenceResults.Visualize(gConvexes0[gConvexID0], btVector3(0.0f, 0.5f, 1.0f), btVector3(0.0f, 0.0f, 0.0f), gDrawUnitLengthNormal);

	if(ReferenceStatus)
	{
		const float Ratio = 100.0f * float(gActualNbTests) / float(gExpectedNbTests);
		printf("Actual tests: %d out of %d (%.02f %%)                       \r", gActualNbTests, gExpectedNbTests, Ratio);
	}
	else
	{
		printf("(no overlap)                                                \r");
	}

	if(mValidHit)
	{
		btVector3 wp = gConvexes0[gConvexID0].mTransform.getOrigin() + mLocalHit;
		DrawPoint(wp, 1.0f);
	}

	glutSwapBuffers();
}

static void ReshapeCallback(int width, int height)
{
	glViewport(0, 0, width, height);
}

static void IdleCallback()
{
	glutPostRedisplay();
}


int main(int argc, char** argv)
{
	// Initialize Glut
	glutInit(&argc, argv);
	glutInitWindowSize(512, 512);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	int mainHandle = glutCreateWindow("ConvexSAT");
	glutSetWindow(mainHandle);
	glutDisplayFunc(RenderCallback);
	glutReshapeFunc(ReshapeCallback);
	glutIdleFunc(IdleCallback);
	glutKeyboardFunc(KeyboardCallback);
	glutSpecialFunc(ArrowKeyCallback);
	glutMouseFunc(MouseCallback);
	glutMotionFunc(MotionCallback);
	MotionCallback(0,0);

	// Setup default render states
	glClearColor(0.3f, 0.4f, 0.5f, 1.0);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_CULL_FACE);

	// Setup lighting
	glEnable(GL_LIGHTING);
	float AmbientColor[] = { 0.0f, 0.1f, 0.2f, 0.0f };		glLightfv(GL_LIGHT0, GL_AMBIENT, AmbientColor);
	float DiffuseColor[] = { 1.0f, 1.0f, 1.0f, 0.0f };		glLightfv(GL_LIGHT0, GL_DIFFUSE, DiffuseColor);
	float SpecularColor[] = { 0.0f, 0.0f, 0.0f, 0.0f };		glLightfv(GL_LIGHT0, GL_SPECULAR, SpecularColor);
	float Position[] = { -10.0f, 1000.0f, -4.0f, 1.0f };	glLightfv(GL_LIGHT0, GL_POSITION, Position);
	glEnable(GL_LIGHT0);

	PrintHelp();

	//
	for(int i=0;i<MAX_NB_CONVEXES;i++)
	{
		char Buffer[1024];
		sprintf(Buffer, "convex%d.bin", i);
		bool Status = gConvexes0[i].LoadFromFile(Buffer);
		if(!Status)
		{
			printf("Failed to load object!\n");
			exit(0);
		}
		Status = gConvexes1[i].LoadFromFile(Buffer);
		if(!Status)
		{
			printf("Failed to load object!\n");
			exit(0);
		}
//		gConvexes0[i].mTransform.setOrigin(btVector3(1.0f, 1.0f, 0.0f));
		gConvexes0[i].mTransform.setOrigin(btVector3(0.20000069f, 0.95000005f, 0.0f));
		gConvexes0[i].mTransform.setOrigin(btVector3(0.20000069f, 0.3000005f, 0.0f));
//		gConvexes0[i].mTransform.setRotation(btQuaternion(0.1f, 0.25f, 0.5f));

//		gConvexes1[i].mTransform.setRotation(btQuaternion(0.1f, 0.25f, 0.5f));
	}

	printf("\n\n");

	// Run

	glutMainLoop();

	return 0;
}
