#include "IrrFunctions.h"
#include <newton.h>
#include <irrlicht.h>
#include <iostream>
#include "IrrDefines.h"
#include "IrrPhysics.h"
#include <dMatrix.h>

using namespace std;

using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace io;
using namespace gui;

int CheckForCollision(Irr2DObj& obj_A, Irr2DObj& obj_B)
{
const int nContacts = 4;
float contacts[3 * nContacts];
float normals[3 * nContacts];
float penetration[3 * nContacts ];

//dMatrix t1, t2;
const float NewtonToIrr = 32.0f;
const float IrrToNewton = (1.0f / NewtonToIrr);

matrix4 mat_A,mat_B;
mat_A.makeIdentity();
mat_B.makeIdentity();
mat_A.setTranslation(obj_A->getPosition());
mat_B.setTranslation(obj_B->getPosition());
core::aabbox3d<f32> box1(vector3d<f32>(obj_A->getLength(), obj_A->getHeight(), 0));
core::aabbox3d<f32> box2(vector3d<f32>(obj_B->getLength(), obj_B->getHeight(), 0));
NewtonCollision* obj1_col = NewtonCreateBox(IrrPhysics::nworld, (obj_A->getLength()), obj_A->getHeight(), 1, NULL);
NewtonCollision* obj2_col = NewtonCreateBox(IrrPhysics::nworld, (obj_B->getLength()), obj_B->getHeight(), 1, NULL);
//NewtonCollision* obj2_col = NewtonCreateBox(IrrPhysics::nworld, 2, 10, 1, NULL);
//NewtonBody* obj1 = NewtonCreateBody(IrrPhysics::nworld, obj1_col);
//NewtonBody* obj2 = NewtonCreateBody(IrrPhysics::nworld, obj2_col);

//NewtonBodyGetMatrix( obj1, &t1[0][0] );
//NewtonBodyGetMatrix( obj2, &t2[0][0] );

//Check for collision between collision meshes,
// returns number of contact points
int nHits = NewtonCollisionCollide( IrrPhysics::nworld,nContacts,
                    obj1_col, (float*)&mat_A,
                    obj2_col, (float*)&mat_B,
                    contacts,
                    normals,
                    penetration);
//Collision detected if nHits > 0
/*if( nHits > 0)
return true;
return false;*/
NewtonReleaseCollision(IrrPhysics::nworld, obj1_col);
NewtonReleaseCollision(IrrPhysics::nworld, obj2_col);
//NewtonDestroyBody(IrrPhysics::nworld, obj1);
//NewtonDestroyBody(IrrPhysics::nworld, obj2);
return nHits;

}

int CheckForCollision(Irr3DObj& obj_A, Irr3DObj& obj_B)
{
//Matrix to store irr_node position
matrix4 mat_A,mat_B;

//Copy position
mat_A.makeIdentity();
mat_B.makeIdentity();

mat_A.setTranslation(obj_A->getSceneNode()->getPosition());
mat_B.setTranslation(obj_B->getSceneNode()->getPosition());

const int nContacts = 2;
float contacts[3 * nContacts];
float normals[3 * nContacts];
float penetration[ nContacts ];

//Check for collision between collision meshes,
// returns number of contact points
int nHits = NewtonCollisionCollide( IrrPhysics::nworld,nContacts,

                    obj_A->getNewtonCollision(), (float*)&mat_A[0],
                    obj_B->getNewtonCollision(), (float*)&mat_B[0],
                    contacts,
                    normals,
                    penetration);
//Collision detected if nHits > 0
/*if( nHits > 0)
return true;
return false;*/
return nHits;

}

//Function to create a NewtonCollision from irrlicht mesh with tree optimization
NewtonCollision * CreateTreeCollisionFromMesh(IMesh *irr_mesh )
{
//Create new (tree optimized) collision mesh
NewtonCollision *collision_obj = NewtonCreateTreeCollision(IrrPhysics::nworld ,NULL);

//Begin collision mesh construction
NewtonTreeCollisionBeginBuild(collision_obj);

int nMeshBuffer = 0; //Mesh Buffer count
int v_index[3] = {0,0,0}; //vertex indices
IMeshBuffer *mesh_buffer = NULL;
float array[9]; //Array to store 3 vertices

//Get (irr_)mesh buffers and copy face by face to collision mesh
for( nMeshBuffer=0 ; nMeshBuffer < irr_mesh->getMeshBufferCount() ; nMeshBuffer++ )
{
mesh_buffer = irr_mesh->getMeshBuffer(nMeshBuffer);

//Get pointer to vertices and indices
S3DVertex *vertices = (S3DVertex*)mesh_buffer->getVertices();
u16 *indices = mesh_buffer->getIndices();

//Fill collision mesh
for(int i=0; i<mesh_buffer->getIndexCount(); i+=3)
{
v_index[0] = indices[ i ];
v_index[1] = indices[i+1];
v_index[2] = indices[i+2];

// 1st position vertex
array[0] = vertices[ v_index[0] ].Pos.X;
array[1] = vertices[ v_index[0] ].Pos.Y;
array[2] = vertices[ v_index[0] ].Pos.Z;

// 2nd position vertex
array[3] = vertices[ v_index[1] ].Pos.X;
array[4] = vertices[ v_index[1] ].Pos.Y;
array[5] = vertices[ v_index[1] ].Pos.Z;

// 3rd position vertex
array[6] = vertices[ v_index[2] ].Pos.X;
array[7] = vertices[ v_index[2] ].Pos.Y;
array[8] = vertices[ v_index[2] ].Pos.Z;

//Add new face to collision mesh
NewtonTreeCollisionAddFace(collision_obj, //collision mesh to add face to

                    3, //number of vertices in array
                    (float*)array, //pointer to vertex array
                    3*sizeof(float),//size of each vertex
                    1); //ID of the face

}
}
//End collision contruction , set 1 as 2dn param for optimization
NewtonTreeCollisionEndBuild(collision_obj,0);

return collision_obj;

}

//Function to create a NewtonCollision from irrlicht mesh
NewtonCollision * CreateCollisionFromMesh(IMesh *irr_mesh )
{
int nMeshBuffer = 0; //Mesh Buffer count
IMeshBuffer *mesh_buffer = NULL;
float *vertices; //Array to store vertices

u32 nVertices = 0;

//Get number of vertices
for( nMeshBuffer=0 ; nMeshBuffer < irr_mesh->getMeshBufferCount() ; nMeshBuffer++ )
{
nVertices += irr_mesh->getMeshBuffer(nMeshBuffer)->getVertexCount();
}


//create buffer for vertices
vertices = new float[nVertices * 3];
u32 tmpCounter = 0;

//Get (irr_)mesh buffers and copy face vertices
for( nMeshBuffer=0 ; nMeshBuffer < irr_mesh->getMeshBufferCount() ; nMeshBuffer++ )
{

    mesh_buffer = irr_mesh->getMeshBuffer(nMeshBuffer);

    //Get pointer to vertices and indices
    S3DVertex *S3vertices = (S3DVertex*)mesh_buffer->getVertices();


//copy vertices from mesh to buffer
for(int i=0; i<mesh_buffer->getVertexCount(); i++)
{

        vertices[tmpCounter++] = S3vertices[i].Pos.X;
        vertices[tmpCounter++] = S3vertices[i].Pos.Y;
        vertices[tmpCounter++] = S3vertices[i].Pos.Z;

    }

}

//Create Newton collision object
NewtonCollision *collision_obj = NewtonCreateConvexHull(IrrPhysics::nworld,nVertices,vertices,sizeof(float)*3,NULL);

//delete vertices
delete [] vertices;

return collision_obj;

}