#include <math.h>
#include "../headers/main.h"
#include "../headers/Character.h"
#include "../headers/PhysXWrapper.h"


Character :: Character (GeActor* mesh) : GeActor(mesh, TYPENO_CHARACTER) {
	mesh->actor = NULL;
	actor->userData = this;
	updateOrientation = false;
	autoCamera = true;
	initActor();
}

Character :: ~Character() {
	GeActor::~GeActor();
	physx.scene->releaseMaterial(*material);
	physx.scene->releaseMaterial(*wsm);
}

void Character::initActor() {
	NxMaterialDesc m;
	m.restitution = 1.0f;
	material = physx.scene->createMaterial(m);
	m.flags |= NX_MF_DISABLE_FRICTION;
	wsm = physx.scene->createMaterial(m);

	NxReal radius = actor->getShapes()[0]->isSphere()->getRadius();

	actor->setMaxAngularVelocity(0.0f);
	colShape = actor->getShapes()[1];
	colShape->setMaterial(material->getMaterialIndex());
	shapeLocalOrientation = colShape->getLocalOrientation();

	actor->setGlobalPosition(NxVec3(0,0,0));
	actor->raiseBodyFlag(NX_BF_FROZEN_ROT);

	actor->setAngularDamping(0.5);
	
	NxWheelShapeDesc wheelShapeDesc;

	wheelShapeDesc.materialIndex = wsm->getMaterialIndex();

	wheelShapeDesc.localPose.t = NxVec3(0,0,0);
	NxQuat q;
	q.fromAngleAxis(90, NxVec3(1,0,0));
	wheelShapeDesc.localPose.M.fromQuat(q);
	
	wheelInitOrientation = wheelShapeDesc.localPose.M;
	
	//wheelShapeDesc.suspension.spring = ;
	//wheelShapeDesc.suspension.damper = ;
	//wheelShapeDesc.suspension.targetValue = ;

	// determines the velocity the wheel torques can achieve
	wheelShapeDesc.inverseWheelMass = 0.1f;

	//wheelShapeDesc.lateralTireForceFunction = ;
	//wheelShapeDesc.longitudalTireForceFunction = ;

	wheelShapeDesc.radius = radius;
	wheelShapeDesc.suspensionTravel = 0.0; 
	
	
	wheelShapeRollAngle = 0.0f;
	wheelShape = static_cast<NxWheelShape *>(actor->createShape(wheelShapeDesc));	
}


void Character::jump(void) {
	NxWheelContactData data;
	if ((!jumping) && (wheelShape->getContact(data) != NULL)) {
		jumping = true;
		actor->addForce(physx.curGravity*-4.0f+actor->getLinearVelocity()*0.2f,NX_IMPULSE);
	}
}


void Character::update() {
	jumping = false;
	if (updateOrientation) {
		NxQuat q(actor->getGlobalOrientation());
		NxVec3 localForward = q.rot(NxVec3(0.0f, 1.0f, 0.0f));

		NxVec3 rightVec = contactNormal ^ localForward; //physx.curGravity ^ contactNormal;
		rightVec.normalize();
		NxVec3 forwardVec = rightVec ^ contactNormal;

		NxMat33 rotMat; //(NxQuat(angle/NxPi*180.0f, vec));
		rotMat.setColumn(0, rightVec);
		rotMat.setColumn(1, forwardVec);
		rotMat.setColumn(2, -contactNormal);

		actor->setGlobalOrientation(rotMat); //*mat);
/*
		NxMat33 globalOrient = rotMat * wheelInitOrientation;
		wheelShape->setLocalOrientation(globalOrient);
		colShape->setLocalOrientation(globalOrient * shapeLocalOrientation);
*/
		NxVec3 r = contactNormal ^ physx.curGravity;
		r.normalize();
		q.fromAngleAxisFast(acos(contactNormal | physx.curGravity), r);
		cam->changeOrientation(q);

		physx.changeGravity(contactNormal);	
		updateOrientation = false;
	}



	UpdateWheelShapePos();

	//NxVec3 p = this->actor->getGlobalPosition();
	//cam->actor->setGlobalPosition(actor->getGlobalPosition());
	cam->actor->setGlobalPosition(actor->getGlobalPosition());
	/*
	cam->t.x = -p[0];
	cam->t.y = -p[1];
	cam->t.z = -p[2];
	*/

	NxReal curMotorTorque = wheelShape->getMotorTorque();

	/*if (glfwGetKey(' ')) {
		jump();
	}*/

	if (glfwGetKey('W')) {
		actor->wakeUp();

		if(curMotorTorque<=0) {
			if(curMotorTorque>-MAX_WHEEL_SPEED) {
				wheelShape->setMotorTorque(curMotorTorque-WHEEL_ACCELERATION);
			}
			wheelShape->setBrakeTorque(0);
		} else {
			wheelShape->setMotorTorque(0);
			wheelShape->setBrakeTorque(MAX_WHEEL_SPEED);
		}
		if(autoCamera) { //!glfwGetKey(GLFW_KEY_LSHIFT)) {
			cam->rotateTo(wheelShape->getSteerAngle());
		}
	} else if (glfwGetKey('S')) {
		actor->wakeUp();
		if(curMotorTorque>=0) {
			if(curMotorTorque<MAX_WHEEL_SPEED) {
				wheelShape->setMotorTorque(curMotorTorque+WHEEL_ACCELERATION);
			}
			wheelShape->setBrakeTorque(0);
		} else {
			wheelShape->setMotorTorque(0);
			wheelShape->setBrakeTorque(MAX_WHEEL_SPEED);
		}
		if(autoCamera) { //!glfwGetKey(GLFW_KEY_LSHIFT)) {
			cam->rotateTo(wheelShape->getSteerAngle());
		}
	} else {		
		wheelShape->setMotorTorque(0);
	}
	
	if (glfwGetKey('A')) {
		wheelShape->setSteerAngle(wheelShape->getSteerAngle()+NxPi*0.018f);

	} else if (glfwGetKey('D')) {
		wheelShape->setSteerAngle(wheelShape->getSteerAngle()-NxPi*0.018f);
	} 
}
	
void Character::collide(GeActor* other, NxU32 events, NxContactPair* pair) {
	NxActorGroup group = other->actor->getGroup();
	if (group == TYPENO_ITEM) {
		// Item* item = (Item*) other;
		/*if (events & NX_NOTIFY_ON_START_TOUCH) {
			other->material.ambient.b = 0.0f;
			other->material.ambient.g = 0.0f;
		} else if (events & NX_NOTIFY_ON_END_TOUCH) {
			other->material.ambient.b = 1.0f;
			other->material.ambient.g = 1.0f;
		}*/
	}
	else if (group == TYPENO_WALL) {
		NxContactStreamIterator i(pair->stream);
		if(i.goNextPair()) {
			if(i.goNextPatch()) {
				contactNormal = i.getPatchNormal();
				contactNormal.normalize();
				if(((physx.curGravity | contactNormal) >= COS45) && !physx.curGravity.equals(contactNormal, 0.001f)) {
					updateOrientation = true;	
					//done = true;
				}				
			}
		}
	}
}

void Character::render() {
	if (actor != NULL) {
		glPushMatrix();

		//actor->getGlobalPose().getColumnMajor44(glMat);
		//glMultMatrixf(glMat);
		glMultMatrixf(wheelShapePoseMat);

		renderOnly();

		glPopMatrix();
	}
	else renderOnly();
}


void Character::shadowPass(GeLight* light) {
	if (actor != NULL) {
		setOutOfDate();
		meshRenderer->shadowPass(light, wheelShapePoseMat);
	}
	else 
		GeActor::shadowPass(light);
}

void Character::UpdateWheelShapePos() {
		
	// Need to save away roll angle in wheel shape user data
	NxReal rollAngle = wheelShapeRollAngle; //sud->wheelShapeRollAngle;
	rollAngle += wheelShape->getAxleSpeed() * 1.0f/60.0f;
	while (rollAngle > NxTwoPi)	//normally just 1x
		rollAngle -= NxTwoPi;
	while (rollAngle < -NxTwoPi) {	//normally just 1x
		rollAngle += NxTwoPi + 2.0f;
	}

	// We have the roll angle for the wheel now
	wheelShapeRollAngle = rollAngle;

	NxMat34 pose;
	pose = wheelShape->getGlobalPose();

	NxReal steerAngle = wheelShape->getSteerAngle();

	NxMat33 rot(NxQuat(steerAngle/NxPi*180.0f, wheelShape->getGlobalOrientation() * NxVec3(0.0f, 1.0f, 0.0f)));

	NxMat33 rollRot;
	rollRot.rotX(rollAngle);

	pose.M = rot * pose.M * rollRot;

	pose.M.getColumnMajorStride4(&(wheelShapePoseMat[0]));
	pose.t.get(&(wheelShapePoseMat[12]));
	
	NxMat33 rotBox;
	rotBox.rotY(steerAngle);
	colShape->setGlobalOrientation(wheelShape->getGlobalOrientation()*rotBox*rollRot*shapeLocalOrientation);

	//clear the elements we don't need:
	wheelShapePoseMat[3] = wheelShapePoseMat[7] = wheelShapePoseMat[11] = 0.0f;
	wheelShapePoseMat[15] = 1.0f;
	
}