/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source distribution.
 */

#include "dgBody.h"
#include "dgWorld.h"
#include "hpl1/engine/libraries/newton/core/dg.h"

//#include "dgContact.h"
#include "dgCollisionBVH.h"

dgCollisionBVH::dgCollisionBVH(dgMemoryAllocator *const allocator) : dgCollisionMesh(allocator, m_boundingBoxHierachy), dgAABBPolygonSoup() {
	m_rtti |= dgCollisionBVH_RTTI;
	m_builder = NULL;
	m_userRayCastCallback = NULL;
}

dgCollisionBVH::dgCollisionBVH(dgWorld *const world,
                               dgDeserialize deserialization, void *const userData) : dgCollisionMesh(world, deserialization, userData), dgAABBPolygonSoup() {
	m_rtti |= dgCollisionBVH_RTTI;
	m_builder = NULL;
	;
	m_userRayCastCallback = NULL;

	dgAABBPolygonSoup::Deserialize(deserialization, userData);

	dgVector p0;
	dgVector p1;
	GetAABB(p0, p1);
	SetCollisionBBox(p0, p1);
}

dgCollisionBVH::~dgCollisionBVH(void) {
}

void dgCollisionBVH::Serialize(dgSerialize callback, void *const userData) const {
	SerializeLow(callback, userData);
	dgAABBPolygonSoup::Serialize((dgSerialize)callback, userData);
}

void dgCollisionBVH::BeginBuild() {
	m_builder = new (m_allocator) dgPolygonSoupDatabaseBuilder(m_allocator);
	m_builder->Begin();
}

void dgCollisionBVH::AddFace(dgInt32 vertexCount,
                             const dgFloat32 *const vertexPtr, dgInt32 strideInBytes,
                             dgInt32 faceAttribute) {
	dgInt32 faceArray;
	dgInt32 indexList[256];

	faceArray = vertexCount;
	NEWTON_ASSERT(vertexCount < dgInt32(sizeof(indexList) / sizeof(indexList[0])));
	for (dgInt32 i = 0; i < vertexCount; i++) {
		indexList[i] = i;
	}
	m_builder->AddMesh(vertexPtr, vertexCount, strideInBytes, 1, &faceArray,
	                   indexList, &faceAttribute, dgGetIdentityMatrix());
}

void dgCollisionBVH::SetCollisionRayCastCallback(
    dgCollisionBVHUserRayCastCallback rayCastCallback) {
	m_userRayCastCallback = rayCastCallback;
}

void dgCollisionBVH::EndBuild(dgInt32 optimize) {
	dgVector p0;
	dgVector p1;

	bool state = optimize ? true : false;

	m_builder->End(state);
	Create(*m_builder, state);

	GetAABB(p0, p1);
	SetCollisionBBox(p0, p1);

	delete m_builder;
	m_builder = NULL;
}

void dgCollisionBVH::GetCollisionInfo(dgCollisionInfo *info) const {
	dgCollision::GetCollisionInfo(info);

	info->m_offsetMatrix = GetOffsetMatrix();
	info->m_collisionType = m_collsionId;

	dgGetVertexListIndexList data;
	data.m_indexList = NULL;
	data.m_userDataList = NULL;
	data.m_maxIndexCount = 1000000000;
	data.m_triangleCount = 0;
	dgVector p0(-1.0e10f, -1.0e10f, -1.0e10f, 1.0f);
	dgVector p1(1.0e10f, 1.0e10f, 1.0e10f, 1.0f);
	ForAllSectors(p0, p1, GetTriangleCount, &data);

	info->m_bvhCollision.m_vertexCount = GetVertexCount();
	info->m_bvhCollision.m_indexCount = data.m_triangleCount * 3;
}

dgIntersectStatus dgCollisionBVH::CollectVertexListIndexList(void *context,
        const dgFloat32 *const polygon, dgInt32 strideInBytes,
        const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgGetVertexListIndexList &data = (*(dgGetVertexListIndexList *)context);

	if ((data.m_triangleCount + indexCount - 2) * 3 > data.m_maxIndexCount) {
		return t_StopSearh;
	}

	dgInt32 k = data.m_triangleCount;
	dgInt32 j = data.m_triangleCount * 3;
	dgInt32 index0 = indexArray[0];
	dgInt32 index1 = indexArray[1];
	dgInt32 atribute = indexArray[-1];
	for (dgInt32 i = 2; i < indexCount; i++) {
		dgInt32 index2 = indexArray[i];
		data.m_indexList[j + 0] = index0;
		data.m_indexList[j + 1] = index1;
		data.m_indexList[j + 2] = index2;
		data.m_userDataList[k] = atribute;
		index1 = index2;
		k++;
		j += 3;
	}

	NEWTON_ASSERT(j <= data.m_maxIndexCount);
	data.m_triangleCount = k;
	NEWTON_ASSERT((data.m_triangleCount * 3) <= data.m_maxIndexCount);

	return t_ContinueSearh;
}

dgIntersectStatus dgCollisionBVH::GetTriangleCount(void *context,
        const dgFloat32 *const polygon, dgInt32 strideInBytes,
        const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgGetVertexListIndexList &data = (*(dgGetVertexListIndexList *)context);

	//  if ((data.m_triangleCount + indexCount - 2) * 3 >= data.m_maxIndexCount) {
	if ((data.m_triangleCount + indexCount - 2) * 3 > data.m_maxIndexCount) {
		return t_StopSearh;
	}

	data.m_triangleCount += (indexCount - 2);
	NEWTON_ASSERT((data.m_triangleCount * 3) <= data.m_maxIndexCount);
	return t_ContinueSearh;
}

void dgCollisionBVH::GetVertexListIndexList(const dgVector &p0,
        const dgVector &p1, dgGetVertexListIndexList &data) const {
	ForAllSectors(p0, p1, CollectVertexListIndexList, &data);

	data.m_veterxArray = GetLocalVertexPool();
	data.m_vertexCount = GetVertexCount();
	data.m_vertexStrideInBytes = GetStrideInBytes();
}

dgFloat32 dgCollisionBVH::RayHitSimd(void *context,
                                     const dgFloat32 *const polygon, dgInt32 strideInBytes,
                                     const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgBVHRay &me = *((dgBVHRay *)context);
	dgVector normal(
	    &polygon[indexArray[indexCount] * (strideInBytes / sizeof(dgFloat32))]);
	dgFloat32 t = me.PolygonIntersectSimd(normal, polygon, strideInBytes,
	                                      indexArray, indexCount);
	if (t < dgFloat32(1.0f)) {
		if (t <= (me.m_t * dgFloat32(1.0001f))) {
			if ((t * dgFloat32(1.0001f)) >= me.m_t) {
				dgFloat32 dist0;
				dgFloat32 dist1;
				dist0 = me.m_diff % normal;
				dist1 = me.m_diff % me.m_normal;
				if (dist0 < dist1) {
					me.m_t = t;
					me.m_normal = normal;
					me.m_id = me.m_me->GetTagId(indexArray);
				} else {
					t = me.m_t;
				}
			} else {
				me.m_t = t;
				me.m_normal = normal;
				me.m_id = me.m_me->GetTagId(indexArray);
			}
		}
	}
	return t;
}

dgFloat32 dgCollisionBVH::RayHit(void *context, const dgFloat32 *const polygon,
                                 dgInt32 strideInBytes, const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgBVHRay &me = *((dgBVHRay *)context);
	dgVector normal(
	    &polygon[indexArray[indexCount] * (strideInBytes / sizeof(dgFloat32))]);
	dgFloat32 t = me.PolygonIntersect(normal, polygon, strideInBytes, indexArray,
	                                  indexCount);
	if (t < dgFloat32(1.0f)) {
		//      if (t <= me.m_t) {
		if (t <= (me.m_t * dgFloat32(1.0001f))) {
			if ((t * dgFloat32(1.0001f)) >= me.m_t) {
				dgFloat32 dist0;
				dgFloat32 dist1;
				dist0 = me.m_diff % normal;
				dist1 = me.m_diff % me.m_normal;
				if (dist0 < dist1) {
					me.m_t = t;
					me.m_normal = normal;
					me.m_id = me.m_me->GetTagId(indexArray);
				} else {
					t = me.m_t;
				}
			} else {
				me.m_t = t;
				me.m_normal = normal;
				me.m_id = me.m_me->GetTagId(indexArray);
			}
		}
	}
	return t;
}

dgFloat32 dgCollisionBVH::RayHitUserSimd(void *context,
        const dgFloat32 *const polygon, dgInt32 strideInBytes,
        const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgFloat32 t = dgFloat32(1.2f);
	dgBVHRay &me = *((dgBVHRay *)context);
	dgVector normal(
	    &polygon[indexArray[indexCount] * (strideInBytes / sizeof(dgFloat32))]);
	t = me.PolygonIntersectSimd(normal, polygon, strideInBytes, indexArray,
	                            indexCount);
	if (t < dgFloat32(1.0f)) {
		if (t < me.m_t) {
			me.m_t = t;
			me.m_normal = normal;
			me.m_id = me.m_me->GetTagId(indexArray);
		}
		normal = me.m_matrix.RotateVectorSimd(normal);
		t = me.m_me->GetDebugRayCastCallback()(me.m_myBody, me.m_me, t, &normal[0],
		                                       dgInt32(me.m_me->GetTagId(indexArray)), me.m_userData);
	}
	return t;
}

dgFloat32 dgCollisionBVH::RayHitUser(void *context,
                                     const dgFloat32 *const polygon, dgInt32 strideInBytes,
                                     const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgFloat32 t = dgFloat32(1.2f);
	dgBVHRay &me = *((dgBVHRay *)context);
	dgVector normal(
	    &polygon[indexArray[indexCount] * (strideInBytes / sizeof(dgFloat32))]);
	t = me.PolygonIntersect(normal, polygon, strideInBytes, indexArray,
	                        indexCount);
	if (t < dgFloat32(1.0f)) {
		if (t < me.m_t) {
			me.m_t = t;
			me.m_normal = normal;
			me.m_id = me.m_me->GetTagId(indexArray);
		}
		normal = me.m_matrix.RotateVector(normal);
		t = me.m_me->GetDebugRayCastCallback()(me.m_myBody, me.m_me, t, &normal[0],
		                                       dgInt32(me.m_me->GetTagId(indexArray)), me.m_userData);
	}
	return t;
}

dgFloat32 dgCollisionBVH::RayCastSimd(const dgVector &localP0,
                                      const dgVector &localP1, dgContactPoint &contactOut,
                                      OnRayPrecastAction preFilter, const dgBody *const body,
                                      void *const userData) const {
	if (PREFILTER_RAYCAST(preFilter, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(this), userData)) {
		return dgFloat32(1.2f);
	}

	dgFloat32 param = dgFloat32(1.2f);
	dgBVHRay ray(localP0, localP1);
	ray.m_t = 2.0f;
	ray.m_me = this;
	ray.m_userData = userData;
	if (!m_userRayCastCallback) {
		ForAllSectorsRayHitSimd(ray, RayHitSimd, &ray);
		if (ray.m_t <= 1.0f) {
			param = ray.m_t;
			contactOut.m_normal = ray.m_normal.Scale(
			                          dgRsqrt((ray.m_normal % ray.m_normal) + 1.0e-8f));
			contactOut.m_userId = ray.m_id;
		}
	} else {
		if (body) {
			ray.m_matrix = body->m_collisionWorldMatrix;
		}

		ForAllSectorsRayHitSimd(ray, RayHitUserSimd, &ray);
		if (ray.m_t <= 1.0f) {
			param = ray.m_t;
			contactOut.m_normal = ray.m_normal.Scale(
			                          dgRsqrt((ray.m_normal % ray.m_normal) + 1.0e-8f));
			contactOut.m_userId = ray.m_id;
		}
	}

	return param;
}

dgFloat32 dgCollisionBVH::RayCast(const dgVector &localP0,
                                  const dgVector &localP1, dgContactPoint &contactOut,
                                  OnRayPrecastAction preFilter, const dgBody *const body,
                                  void *const userData) const {
	if (PREFILTER_RAYCAST(preFilter, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(this), userData)) {
		return dgFloat32(1.2f);
	}

	dgFloat32 param = dgFloat32(1.2f);

	dgBVHRay ray(localP0, localP1);
	ray.m_t = 2.0f;
	ray.m_me = this;
	ray.m_userData = userData;
	if (!m_userRayCastCallback) {
		ForAllSectorsRayHit(ray, RayHit, &ray);
		if (ray.m_t <= 1.0f) {
			param = ray.m_t;
			contactOut.m_normal = ray.m_normal.Scale(
			                          dgRsqrt((ray.m_normal % ray.m_normal) + 1.0e-8f));
			contactOut.m_userId = ray.m_id;
		}
	} else {
		if (body) {
			ray.m_matrix = body->m_collisionWorldMatrix;
		}

		ForAllSectorsRayHit(ray, RayHitUser, &ray);
		if (ray.m_t <= 1.0f) {
			param = ray.m_t;
			contactOut.m_normal = ray.m_normal.Scale(
			                          dgRsqrt((ray.m_normal % ray.m_normal) + 1.0e-8f));
			contactOut.m_userId = ray.m_id;
		}
	}
	return param;
}

dgIntersectStatus dgCollisionBVH::GetPolygon(void *context,
        const dgFloat32 *const polygon, dgInt32 strideInBytes,
        const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgPolygonMeshDesc &data = (*(dgPolygonMeshDesc *)context);
	if (data.m_faceCount >= DG_MAX_COLLIDING_FACES) {
		NEWTON_ASSERT(0);
		return t_StopSearh;
	}

	if ((data.m_globalIndexCount + indexCount) >= DG_MAX_COLLIDING_VERTEX) {
		NEWTON_ASSERT(0);
		return t_StopSearh;
	}

	if (data.m_me->GetDebugCollisionCallback()) {
		dgTriplex triplex[128];
		dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat32));
		const dgMatrix &matrix = data.m_polySoupBody->GetCollisionMatrix();
		for (dgInt32 i = 0; i < indexCount; i++) {
			dgVector p(&polygon[indexArray[i] * stride]);
			p = matrix.TransformVector(p);
			triplex[i].m_x = p.m_x;
			triplex[i].m_y = p.m_y;
			triplex[i].m_z = p.m_z;
		}
		data.m_me->GetDebugCollisionCallback()(data.m_polySoupBody, data.m_objBody,
		                                       indexArray[-1], indexCount, &triplex[0].m_x, sizeof(dgTriplex));
	}

	NEWTON_ASSERT(data.m_vertex == polygon);

	data.m_userAttribute[data.m_faceCount] = indexArray[-1];
	data.m_faceIndexCount[data.m_faceCount] = indexCount;
	data.m_faceNormalIndex[data.m_faceCount] = indexArray[indexCount];
	data.m_faceMaxSize[data.m_faceCount] = dgFloat32(
	        indexArray[(indexCount << 1) + 1]);
	data.m_faceCount++;

	dgInt32 j = data.m_globalIndexCount;
	for (dgInt32 i = 0; i < indexCount; i++) {
		data.m_faceVertexIndex[j] = indexArray[i];
		data.m_faceAdjencentEdgeNormal[j] = indexArray[i + indexCount + 1];
		j++;
	}
	data.m_globalIndexCount = j;

	return t_ContinueSearh;
}

void dgCollisionBVH::GetCollidingFacesSimd(dgPolygonMeshDesc *const data) const {
	data->m_faceCount = 0;

	data->m_me = this;
	data->m_vertex = GetLocalVertexPool();
	data->m_vertexStrideInBytes = GetStrideInBytes();

	data->m_globalIndexCount = 0;
	data->m_faceMaxSize = data->m_globalFaceMaxSize;
	data->m_userAttribute = data->m_globalUserAttribute;
	data->m_faceIndexCount = data->m_globalFaceIndexCount;
	data->m_faceVertexIndex = data->m_globalFaceVertexIndex;
	data->m_faceNormalIndex = data->m_globalFaceNormalIndex;
	data->m_faceAdjencentEdgeNormal = data->m_globalAdjencentEdgeNormal;
	ForAllSectorsSimd(data->m_boxP0, data->m_boxP1, GetPolygon, data);
}

void dgCollisionBVH::GetCollidingFaces(dgPolygonMeshDesc *const data) const {
	data->m_faceCount = 0;

	data->m_me = this;
	data->m_vertex = GetLocalVertexPool();
	data->m_vertexStrideInBytes = GetStrideInBytes();

	data->m_globalIndexCount = 0;
	data->m_faceMaxSize = data->m_globalFaceMaxSize;
	data->m_userAttribute = data->m_globalUserAttribute;
	data->m_faceIndexCount = data->m_globalFaceIndexCount;
	data->m_faceVertexIndex = data->m_globalFaceVertexIndex;
	data->m_faceNormalIndex = data->m_globalFaceNormalIndex;
	data->m_faceAdjencentEdgeNormal = data->m_globalAdjencentEdgeNormal;
	ForAllSectors(data->m_boxP0, data->m_boxP1, GetPolygon, data);
}

dgVector dgCollisionBVH::SupportVertex(const dgVector &dir) const {
	return ForAllSectorsSupportVectex(dir);
}

struct dgCollisionBVHShowPolyContext {
	dgMatrix m_matrix;
	void *m_userData;
	OnDebugCollisionMeshCallback m_callback;
};

dgIntersectStatus dgCollisionBVH::ShowDebugPolygon(void *context,
        const dgFloat32 *const polygon, dgInt32 strideInBytes,
        const dgInt32 *const indexArray, dgInt32 indexCount) {
	dgTriplex triplex[128];
	dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat32));

	dgCollisionBVHShowPolyContext &data =
	    *(dgCollisionBVHShowPolyContext *)context;
	for (dgInt32 i = 0; i < indexCount; i++) {
		dgVector p(&polygon[indexArray[i] * stride]);
		p = data.m_matrix.TransformVector(p);
		triplex[i].m_x = p.m_x;
		triplex[i].m_y = p.m_y;
		triplex[i].m_z = p.m_z;
	}
	data.m_callback(data.m_userData, indexCount, &triplex[0].m_x, indexArray[-1]);

	return t_ContinueSearh;
}

void dgCollisionBVH::DebugCollision(const dgMatrix &matrixPtr,
                                    OnDebugCollisionMeshCallback callback, void *const userData) const {
	dgCollisionBVHShowPolyContext context;

	context.m_matrix = matrixPtr;
	context.m_userData = userData;
	;
	context.m_callback = callback;

	dgVector p0(dgFloat32(-1.0e20f), dgFloat32(-1.0e20f), dgFloat32(-1.0e20f),
	            dgFloat32(0.0f));
	dgVector p1(dgFloat32(1.0e20f), dgFloat32(1.0e20f), dgFloat32(1.0e20f),
	            dgFloat32(0.0f));
	ForAllSectors(p0, p1, ShowDebugPolygon, &context);
}