#include <stdio.h>
#include <math.h>
#include "../headers/PhysXWrapper.h"
#include "../headers/GeCamera.h"

#define max(a,b) ((a > b) ? (a) : (b))
#define min(a,b) ((a < b) ? (a) : (b))

NxVec3 intersectPlanes(NxVec3 n1, NxReal o1, NxVec3 n2, NxReal o2, NxVec3 n3, NxReal o3) {
	return -(o1 * (n2 ^ n3) + o2 * (n3 ^ n1) + o3 * (n1 ^ n2))/(n1 | (n2 ^ n3));
}

GeCamera::GeCamera(float viewAngle, float viewRatio, float distance, float sight) {
	glfwSetMouseWheel(0);
	this->distance = distance;
	this->sight = sight;
	zoom = 0.0f;
	totalDistance = distance+zoom;
	pitch = 0.3f;
	yaw = 0.0f;
	orientation.zero();
	for (int i=0; i<16; i++) {
		orimat[i] = ((i % 5) == 0) ? 1.0f : 0.0f;
	}
	interZoom = new MInterpolate<float>(&zoom);
	interYaw = new MInterpolate<float>(&yaw);
	interOrientation = new MInterpolate<NxQuat>(&orientation);

	NxActorDesc actorDesc;
	actorDesc.flags |= NX_AF_DISABLE_RESPONSE; // NX_AF_DISABLE_COLLISION | 
	NxBodyDesc* body = new NxBodyDesc();
	body->flags |= NX_BF_DISABLE_GRAVITY; // NX_BF_KINEMATIC | 
	body->mass = 1;
	actorDesc.body = body;
	//actorDesc.density = 1.0f;

	setupProjection(viewAngle, viewRatio);


	NxShapeDesc* viewingVolumeDesc = getViewFrustum();
	actorDesc.shapes.pushBack(viewingVolumeDesc);

	actorDesc.group = TYPENO_CAMERA;
	actorDesc.userData = this;
	actor = physx.scene->createActor(actorDesc);
	viewingVolume = actor->getShapes()[0];

	updateSingle();
}

GeCamera::~GeCamera(void) {
}

void GeCamera::setupProjection(GLfloat viewAngle, GLfloat viewRatio) {
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(viewAngle, viewRatio, 1, 1+sight);
	glMatrixMode(GL_MODELVIEW);
	//calcFrustum();
}

NxShapeDesc* GeCamera::getViewFrustum(void) {
	NxConvexMeshDesc meshDesc;
	NxConvexShapeDesc* shapeDesc = NULL;
	NxVec3 vertexbuffer[8];
	float proj[16];
	NxVec3 nTop, nBottom, nLeft, nRight, nNear, nFar; // normal vector
	float oTop, oBottom, oLeft, oRight, oNear, oFar; // offset
	float t;

	glGetFloatv( GL_PROJECTION_MATRIX, proj );

	nRight.x = proj[ 3] - proj[ 0];
	nRight.y = proj[ 7] - proj[ 4];
	nRight.z = proj[11] - proj[ 8];
	oRight = proj[15] - proj[12];

	/* Normalize the result */
	t = nRight.magnitude(); //sqrt((nRight | nRight) + oRight * oRight);
	nRight /= t;
	oRight /= t;

	/* Extract the numbers for the LEFT plane */
	nLeft.x = proj[ 3] + proj[ 0];
	nLeft.y = proj[ 7] + proj[ 4];
	nLeft.z = proj[11] + proj[ 8];
	oLeft = proj[15] + proj[12];

	/* Normalize the result */
	t = nLeft.magnitude(); //sqrt((nLeft | nLeft) + oLeft * oLeft);
	nLeft /= t;
	oLeft /= t;

	/* Extract the BOTTOM plane */
	nBottom.x = proj[ 3] + proj[ 1];
	nBottom.y = proj[ 7] + proj[ 5];
	nBottom.z = proj[11] + proj[ 9];
	oBottom = proj[15] + proj[13];

	/* Normalize the result */
	t = nBottom.magnitude(); //sqrt((nBottom | nBottom) + oBottom * oBottom);
	nBottom /= t;
	oBottom /= t;

	/* Extract the TOP plane */
	nTop.x = proj[ 3] - proj[ 1];
	nTop.y = proj[ 7] - proj[ 5];
	nTop.z = proj[11] - proj[ 9];
	oTop = proj[15] - proj[13];

	/* Normalize the result */
	t = sqrt((nTop | nTop) + oTop * oTop);
	nTop /= t;
	oTop /= t;

	/* Extract the FAR plane */
	nFar.x = proj[ 3] - proj[ 2];
	nFar.y = proj[ 7] - proj[ 6];
	nFar.z = proj[11] - proj[10];
	oFar = proj[15] - proj[14];

	/* Normalize the result */
	t = nFar.magnitude(); //sqrt((nFar | nFar) + oFar * oFar);
	nFar /= t;
	oFar /= t;

	/* Extract the NEAR plane */
	nNear.x = proj[ 3] + proj[ 2];
	nNear.y = proj[ 7] + proj[ 6];
	nNear.z = proj[11] + proj[10];
	oNear = proj[15] + proj[14];

	/* Normalize the result */
	t = nNear.magnitude(); //sqrt((nNear | nNear) + oNear * oNear);
	nNear /= t;
	oNear /= t;

	vertexbuffer[0] = intersectPlanes(nNear, oNear, nTop, oTop, nLeft, oLeft);
	vertexbuffer[1] = intersectPlanes(nNear, oNear, nTop, oTop, nRight, oRight);
	vertexbuffer[2] = intersectPlanes(nNear, oNear, nBottom, oBottom, nRight, oRight);
	vertexbuffer[3] = intersectPlanes(nNear, oNear, nBottom, oBottom, nLeft, oLeft);
	vertexbuffer[4] = intersectPlanes(nFar, oFar, nTop, oTop, nLeft, oLeft);
	vertexbuffer[5] = intersectPlanes(nFar, oFar, nTop, oTop, nRight, oRight);
	vertexbuffer[6] = intersectPlanes(nFar, oFar, nBottom, oBottom, nRight, oRight);
	vertexbuffer[7] = intersectPlanes(nFar, oFar, nBottom, oBottom, nLeft, oLeft);

	int trianglebuffer[] = {
		0, 1, 2,  0, 2, 3,  1, 5, 2,  2, 5, 6,  0, 5, 1,  0, 4, 5,
		0, 3, 4,  3, 7, 4,  2, 6, 3,  3, 6, 7,  5, 4, 6,  4, 7, 6};

	meshDesc.numVertices = 8;
	meshDesc.points = vertexbuffer;
	meshDesc.pointStrideBytes = sizeof(NxVec3);
	meshDesc.numTriangles = 12;
	meshDesc.triangles = trianglebuffer;
	meshDesc.triangleStrideBytes = 3*sizeof(int);

	shapeDesc = physx.cookConvexMesh(meshDesc);
	shapeDesc->shapeFlags |= NX_SF_DISABLE_RESPONSE | NX_SF_DISABLE_RAYCASTING;
	shapeDesc->shapeFlags &= ~NX_SF_VISUALIZATION;
	shapeDesc->group = SHAPE_VIEWVOL;
	shapeDesc->localPose.M.rotX(NxHalfPi);
	shapeDesc->localPose.t = NxVec3(0.0f, -distance, 0.0f);

	return shapeDesc;
}

void GeCamera::renderSingle(void) {
	glLoadIdentity();
	gluLookAt(0.0f,-totalDistance,0.0f, 0.0f,0.0f,0.0f, 0.0f, 0.0f, 1.0f);

	glRotatef(pitch*180/NxPi, 1.0f, 0.0f, 0.0f);
	glRotatef(yaw*180/NxPi, 0.0f, 0.0f, -1.0f);
	glMultMatrixf(orimat);

	glTranslatef(t.x, t.y, t.z);
}

void GeCamera::updateSingle(void) {
	NxQuat qYaw, qPitch, qTarget;
	NxRay worldRay;
    NxRaycastHit hit;

	int mousewheel = glfwGetMouseWheel();
	if (mousewheel > 50) {
		glfwSetMouseWheel(50);
		mousewheel = 50;
	}
	if (mousewheel < -50) {
		glfwSetMouseWheel(-50);
		mousewheel = -50;
	}
	interZoom->setTarget(mousewheel*0.06f);

	interOrientation->update();
	interYaw->update();
	interZoom->update();

	qYaw.fromAngleAxisFast(yaw, NxVec3(0.0f, 0.0f, -1.0f));
	qPitch.fromAngleAxisFast(pitch, NxVec3(1.0f, 0.0f, 0.0f));
	qTarget = qPitch*qYaw*orientation;


	NxMat33(orientation).getColumnMajorStride4(orimat);
	qTarget.invert();
	if (actor != NULL) {
		t = -actor->getGlobalPosition();
		actor->setGlobalOrientationQuat(qTarget);
		//actor->setGlobalOrientationQuat(qTarget);
	}
	

	worldRay.orig = -t; //NxVec3(1.6, 3.0, -1.5);
	worldRay.dir = qTarget.rot(NxVec3(0.0, -1.0, 0.0)); //NxVec3(0.0, -1.0, 0.0);
	//printf("orig: %f, %f, %f\n", worldRay.orig.x, worldRay.orig.y, worldRay.orig.z);
	//printf("dir: %f, %f, %f\n", worldRay.dir.x, worldRay.dir.y, worldRay.dir.z);
	worldRay.dir.normalize();

	totalDistance = distance;
	totalDistance = distance-zoom;
	if ((physx.scene->raycastClosestShape(worldRay, NX_STATIC_SHAPES, hit, 1 << SHAPE_OTHER) != NULL) &&
		(hit.flags & NX_RAYCAST_DISTANCE) && (totalDistance > hit.distance)) {
		totalDistance = hit.distance;
	}
	totalDistance = max(totalDistance, 2.0f);
	viewingVolume->setLocalPosition(NxVec3(0.0f, -totalDistance, 0.0f));
	pos = actor->getGlobalPosition() + totalDistance*worldRay.dir;
}

void GeCamera::rotateStep(float dYaw, float dPitch) {
	yaw = fmod(yaw+dYaw, NxTwoPi);
	pitch = min(max(pitch+dPitch,-NxPi*0.40),NxPi*0.30);
	interYaw->setTarget(yaw);
}

void GeCamera::rotateTo(float newYaw) {
	newYaw = fmod(newYaw, NxTwoPi);
	if (newYaw-yaw > NxPi) yaw += NxTwoPi;
	else if (yaw-newYaw > NxPi) yaw -= NxTwoPi;

	interYaw->setTarget(newYaw);
}

void GeCamera::changeOrientation(NxQuat q) {
	interOrientation->setTarget(interOrientation->getTarget()*q);
}
