#define PHYSXWRAPPER_CPP
#include <stdio.h>
#include "../headers/GeActor.h"
#include "../headers/main.h"
#include "../headers/PhysXWrapper.h"

/****************/
/* PhysXWrapper */
/****************/
PhysXWrapper :: PhysXWrapper(void) {
	physXSDK = NULL;
	cooking = NULL;
	scene = NULL;
}

bool PhysXWrapper :: init(void) {
	bool ret = false;
	NxSDKCreateError errorCode;

	physXSDK = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION, NULL, this, NxPhysicsSDKDesc(), &errorCode);
	if(physXSDK) {
		cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);
		if ((cooking != NULL) && (cooking->NxInitCooking(NULL, this))) {
			physXSDK->setParameter(NX_SKIN_WIDTH, 0.002f);

			physXSDK->setParameter(NX_VISUALIZATION_SCALE, 1.0);
			physXSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES, 1);
			physXSDK->setParameter(NX_VISUALIZE_ACTOR_AXES, 1);

			physXSDK->setParameter(NX_VISUALIZE_CONTACT_POINT, 1);
			physXSDK->setParameter(NX_VISUALIZE_CONTACT_NORMAL, 1);

			scene = physXSDK->createScene(NxSceneDesc());
			if(scene != NULL) {
				// Create the default material
				NxMaterial* defaultMaterial = scene->getMaterialFromIndex(0); 
				defaultMaterial->setRestitution(0.5);
				defaultMaterial->setStaticFriction(0.5);
				defaultMaterial->setDynamicFriction(0.5);

				scene->setUserContactReport(this);
				scene->setUserTriggerReport(this);

				scene->setTiming(UPDATE_INTERVAL);
				changeGravity(NxVec3(0.0,0.0,-1.0));
				//scene->setGravity(curGravity);
				scene->setActorGroupPairFlags(TYPENO_CHARACTER, TYPENO_WALL,
					NX_NOTIFY_ON_START_TOUCH | NX_NOTIFY_ON_TOUCH); // NX_NOTIFY_ON_TOUCH | NX_NOTIFY_ON_END_TOUCH
				scene->setActorGroupPairFlags(TYPENO_CHARACTER, TYPENO_ITEM,
					NX_NOTIFY_ON_START_TOUCH | NX_NOTIFY_ON_END_TOUCH); // NX_NOTIFY_ON_TOUCH
				scene->setActorGroupPairFlags(TYPENO_CAMERA, TYPENO_ITEM,
					NX_NOTIFY_ON_START_TOUCH | NX_NOTIFY_ON_END_TOUCH);
				scene->setActorGroupPairFlags(TYPENO_CAMERA, TYPENO_WALL,
					NX_NOTIFY_ON_START_TOUCH | NX_NOTIFY_ON_END_TOUCH);
				scene->setActorGroupPairFlags(TYPENO_CAMERA, TYPENO_LIGHT,
					NX_NOTIFY_ON_START_TOUCH | NX_NOTIFY_ON_END_TOUCH);

				
				scene->setGroupCollisionFlag(SHAPE_VIEWVOL, SHAPE_VIEWVOL, false);

				scene->setGroupCollisionFlag(SHAPE_SPHERE,  SHAPE_SPHERE,  false);
				scene->setGroupCollisionFlag(SHAPE_SPHERE,  SHAPE_VIEWVOL, true);

				scene->setGroupCollisionFlag(SHAPE_OTHER,   SHAPE_SPHERE,  false);
				scene->setGroupCollisionFlag(SHAPE_OTHER,   SHAPE_VIEWVOL, false);
				scene->setGroupCollisionFlag(SHAPE_OTHER,   SHAPE_OTHER,   true);

				scene->setGroupCollisionFlag(SHAPE_DUMMY,	SHAPE_DUMMY,   false);
				scene->setGroupCollisionFlag(SHAPE_DUMMY,	SHAPE_SPHERE,  false);
				scene->setGroupCollisionFlag(SHAPE_DUMMY,	SHAPE_OTHER,   false);
				scene->setGroupCollisionFlag(SHAPE_DUMMY,	SHAPE_VIEWVOL, false);
				
				scene->setGroupCollisionFlag(SHAPE_LIGHT_SPHERE, SHAPE_LIGHT_SPHERE, false);
				scene->setGroupCollisionFlag(SHAPE_LIGHT_SPHERE, SHAPE_OTHER, false);
				scene->setGroupCollisionFlag(SHAPE_LIGHT_SPHERE, SHAPE_SPHERE, true);
				scene->setGroupCollisionFlag(SHAPE_LIGHT_SPHERE, SHAPE_VIEWVOL, true);
				scene->setGroupCollisionFlag(SHAPE_LIGHT_SPHERE, SHAPE_DUMMY, false);
				
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_LIGHT_SPHERE, false);
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_OTHER, true);
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_SPHERE, false);
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_VIEWVOL, false);
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_DUMMY, false); // ??
				scene->setGroupCollisionFlag(SHAPE_TRIGGER, SHAPE_TRIGGER, false);

				ret = true;
			} else {
				printf("Error: Unable to create a PhysX scene\n");
			}
		} else {
			printf("Error: Unable to init the Cooking Interface\n");
		}
	} else {
		printf("Error: Unable to init PhysX SDK\n");
	}
	return ret;
}

void PhysXWrapper :: fetch(void) {
	scene->fetchResults(NX_RIGID_BODY_FINISHED, true);
}

void PhysXWrapper :: advance(void) {
	scene->simulate((NxReal)UPDATE_INTERVAL);
	scene->flushStream();
}

void PhysXWrapper :: release(void) {
	triggerList.clear();
	if(physXSDK != NULL) {
		if(cooking != NULL) cooking->NxCloseCooking();
		if(scene != NULL) physXSDK->releaseScene(*scene);
		physXSDK->release();
	}
}

MVector3 PhysXWrapper :: quat2euler(NxQuat q) {
	MVector3 ret;
	ret.x = atan2(2*q.x*q.w-2*q.y*q.z, 1 - 2*(q.x*q.x) - 2*(q.z*q.z))/NxPi*180;
	ret.y = atan2(2*q.y*q.w-2*q.x*q.z, 1 - 2*(q.y*q.y) - 2*(q.z*q.z))/NxPi*180;
	ret.z = asin(2*q.x*q.y + 2*q.z*q.w)/NxPi*180;
	return ret;
}

/* report handling */
void PhysXWrapper :: onContactNotify(NxContactPair& pair, NxU32 events) {
	if (pair.actors[0]->getGroup() == TYPENO_CAMERA) {
		if (pair.actors[1]->getGroup() == TYPENO_LIGHT) {
			if(events & NX_NOTIFY_ON_START_TOUCH) {
				((GeLight*)(pair.actors[1]->userData))->activate();
			} else if(events & NX_NOTIFY_ON_END_TOUCH) {
				((GeLight*)(pair.actors[1]->userData))->deactivate();
			}
		}
		else {
			if(events & NX_NOTIFY_ON_START_TOUCH) {
				((GeActor*)(pair.actors[1]->userData))->cull = false;
			}
			else if(events & NX_NOTIFY_ON_END_TOUCH) {
				((GeActor*)(pair.actors[1]->userData))->cull = true;
			}
		}
	}
	else if (pair.actors[1]->getGroup() == TYPENO_CAMERA) {
		if (pair.actors[0]->getGroup() == TYPENO_LIGHT) {
			if(events & NX_NOTIFY_ON_START_TOUCH) {
				((GeLight*)(pair.actors[0]->userData))->activate();
			} else if(events & NX_NOTIFY_ON_END_TOUCH) {
				((GeLight*)(pair.actors[0]->userData))->deactivate();
			}
		}
		else {
			if(events & NX_NOTIFY_ON_START_TOUCH) {
				((GeActor*)(pair.actors[0]->userData))->cull = false;
			}
			else if(events & NX_NOTIFY_ON_END_TOUCH) {
				((GeActor*)(pair.actors[0]->userData))->cull = true;
			}
		}
	}
	else {
		GeActor* obj1 = (GeActor*)pair.actors[0]->userData;
		GeActor* obj2 = (GeActor*)pair.actors[1]->userData;
		if ((obj1 != NULL) && (obj2 != NULL)) {
			obj1->collide(obj2, events, &pair);
			obj2->collide(obj1, events, &pair);
		}
	}
}

void PhysXWrapper :: onTrigger(NxShape& triggerShape, NxShape& otherShape, NxTriggerFlag status) {
	if (triggerShape.getActor().getGroup() == TYPENO_TRIGGER) {
		void (*actionCallback)(NxActor*, NxTriggerFlag);
		Trigger* t = (Trigger*)(triggerShape.getActor().userData);
		(*t->actionCallback)(&(otherShape.getActor()), t, status);
	}
	/*if (triggerShape.getActor().getGroup() == TYPENO_CAMERA) {
		GeCamera* trigger = (GeCamera*)(triggerShape.getActor().userData);
		if(otherShape.getActor().getGroup() == TYPENO_LIGHT) {
			if(status & NX_TRIGGER_ON_ENTER) {
				printf("drinnen!%d\n", ((GeLight*)(otherShape.getActor().userData))->getIndex() );
				((GeLight*)(otherShape.getActor().userData))->activate();
			} else if(status & NX_TRIGGER_ON_LEAVE) {
				printf("draussen!%d\n", ((GeLight*)(otherShape.getActor().userData))->getIndex() );
				((GeLight*)(otherShape.getActor().userData))->deactivate();
			}
			
		} else {
			GeActor* mesh = (GeActor*)(otherShape.getActor().userData);
			if ((trigger != NULL) && (mesh != NULL)) {
				trigger->trigger(mesh, status);
			}
		}
	}*/
}

/* cooking */
NxConvexShapeDesc* PhysXWrapper :: cookConvexMesh(const NxConvexMeshDesc& desc) {
	NxConvexShapeDesc* convexShapeDesc = NULL;
	MemoryWriteBuffer buf;
	if(cooking->NxCookConvexMesh(desc, buf)) {
		convexShapeDesc = new NxConvexShapeDesc();
		convexShapeDesc->meshData = physXSDK->createConvexMesh(MemoryReadBuffer(buf.data));
	}
	return convexShapeDesc;
}

NxTriangleMeshShapeDesc* PhysXWrapper :: cookTriangleMesh(const NxTriangleMeshDesc& desc) {
	NxTriangleMeshShapeDesc* meshDesc = NULL;
	MemoryWriteBuffer buf;
	if(cooking->NxCookTriangleMesh(desc, buf)) {
		meshDesc = new NxTriangleMeshShapeDesc();
		meshDesc->meshData = physXSDK->createTriangleMesh(MemoryReadBuffer(buf.data));
	}
	return meshDesc;
}

/* output handling */
void PhysXWrapper :: reportError (NxErrorCode code, const char *message, const char* file, int line) {
    //this should be routed to the application
    //specific error handling. If this gets hit
    //then you are in most cases using the SDK
    //wrong and you need to debug your code!
    //however, code may  just be a warning or
    //information.

    if (code < NXE_DB_INFO)
    {
		printf("PhysX OutputStream: %s", message);
		closeGame();
    }
}
        
NxAssertResponse PhysXWrapper :: reportAssertViolation (const char *message, const char *file,int line) {
    //this should not get hit by
    // a properly debugged SDK!
    assert(0);
    return NX_AR_CONTINUE;
}
        
void PhysXWrapper :: print (const char *message) {
    printf("SDK says: %s\n", message);
}

/* other */
void PhysXWrapper :: changeGravity(NxVec3 vec) {
	curGravity = vec;
	scene->setGravity(vec*20.0f); //9.8f);
}

/**********************/
/* PhysXDebugRenderer */
/**********************/
void PhysXDebugRenderer::render() {
	glDisable(GL_LIGHTING);
	glLineWidth(1.0f);
	// Render points
	{
		NxU32 NbPoints = data->getNbPoints();
		const NxDebugPoint* Points = data->getPoints();
		glBegin(GL_POINTS);

		while(NbPoints--)
		{
			setupColor(Points->color);
			glVertex3fv(&Points->p.x);
			Points++;
		}
		glEnd();
	}
    
	// Render lines
	{
		NxU32 NbLines = data->getNbLines();
		const NxDebugLine* Lines = data->getLines();
        
		glBegin(GL_LINES);

		while(NbLines--)
		{
			setupColor(Lines->color);
			glVertex3fv(&Lines->p0.x);
			glVertex3fv(&Lines->p1.x);
			Lines++;
		}
		glEnd(); 
	}
    
	// Render triangles
	{
		NxU32 NbTris = data->getNbTriangles();
		const NxDebugTriangle* Triangles = data->getTriangles();
		glBegin(GL_TRIANGLES);

		while(NbTris--)
		{
			setupColor(Triangles->color);
			glVertex3fv(&Triangles->p0.x);
			glVertex3fv(&Triangles->p1.x);
			glVertex3fv(&Triangles->p2.x);
			Triangles++;
		}
		glEnd();
	}
	glEnable(GL_LIGHTING);
}

void PhysXDebugRenderer::update() {
	data = physx.scene->getDebugRenderable();
}

void PhysXDebugRenderer::setupColor(NxU32 color)
{
	NxF32 Blue= NxF32((color)&0xff)/255.0f;
	NxF32 Green= NxF32((color>>8)&0xff)/255.0f;
	NxF32 Red= NxF32((color>>16)&0xff)/255.0f;
	glColor3f(Red, Green, Blue);
}


/*********************/
/* MemoryWriteBuffer */
/*********************/
MemoryWriteBuffer::MemoryWriteBuffer() : currentSize(0), maxSize(0), data(NULL)
	{
	}

MemoryWriteBuffer::~MemoryWriteBuffer()
	{
	NX_DELETE_ARRAY(data);
	}

void MemoryWriteBuffer::clear()
	{
	currentSize = 0;
	}

NxStream& MemoryWriteBuffer::storeByte(NxU8 b)
	{
	storeBuffer(&b, sizeof(NxU8));
	return *this;
	}
NxStream& MemoryWriteBuffer::storeWord(NxU16 w)
	{
	storeBuffer(&w, sizeof(NxU16));
	return *this;
	}
NxStream& MemoryWriteBuffer::storeDword(NxU32 d)
	{
	storeBuffer(&d, sizeof(NxU32));
	return *this;
	}
NxStream& MemoryWriteBuffer::storeFloat(NxReal f)
	{
	storeBuffer(&f, sizeof(NxReal));
	return *this;
	}
NxStream& MemoryWriteBuffer::storeDouble(NxF64 f)
	{
	storeBuffer(&f, sizeof(NxF64));
	return *this;
	}
NxStream& MemoryWriteBuffer::storeBuffer(const void* buffer, NxU32 size)
	{
	NxU32 expectedSize = currentSize + size;
	if(expectedSize > maxSize)
		{
		maxSize = expectedSize + 4096;

		NxU8* newData = new NxU8[maxSize];
		NX_ASSERT(newData!=NULL);

		if(data)
			{
			memcpy(newData, data, currentSize);
			delete[] data;
			}
		data = newData;
		}
	memcpy(data+currentSize, buffer, size);
	currentSize += size;
	return *this;
	}


/********************/
/* MemoryReadBuffer */
/********************/
MemoryReadBuffer::MemoryReadBuffer(const NxU8* data) : buffer(data)
	{
	}

MemoryReadBuffer::~MemoryReadBuffer()
	{
	// We don't own the data => no delete
	}

NxU8 MemoryReadBuffer::readByte() const
	{
	NxU8 b;
	memcpy(&b, buffer, sizeof(NxU8));
	buffer += sizeof(NxU8);
	return b;
	}

NxU16 MemoryReadBuffer::readWord() const
	{
	NxU16 w;
	memcpy(&w, buffer, sizeof(NxU16));
	buffer += sizeof(NxU16);
	return w;
	}

NxU32 MemoryReadBuffer::readDword() const
	{
	NxU32 d;
	memcpy(&d, buffer, sizeof(NxU32));
	buffer += sizeof(NxU32);
	return d;
	}

float MemoryReadBuffer::readFloat() const
	{
	float f;
	memcpy(&f, buffer, sizeof(float));
	buffer += sizeof(float);
	return f;
	}

double MemoryReadBuffer::readDouble() const
	{
	double f;
	memcpy(&f, buffer, sizeof(double));
	buffer += sizeof(double);
	return f;
	}

void MemoryReadBuffer::readBuffer(void* dest, NxU32 size) const
	{
	memcpy(dest, buffer, size);
	buffer += size;
	}