//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  


#include "geometry/PxTriangleMesh.h"

#include "PxcNpBatch.h"
#include "PxcNpWorkUnit.h"
#include "PxcContactCache.h"
#include "PxcMaterialMethodImpl.h"
#include "PxcNpContactPrepShared.h"
#include "PxvDynamics.h"			// for PxsBodyCore
#include "PxvGeometry.h"			// for PxsShapeCore
#include "CmFlushPool.h"
#include "CmTask.h"
#include "PxsContactManager.h"
#include "PxsMaterialManager.h"
#include "PxsTransformCache.h"
#include "PxsContactManagerState.h"
#include "GuGeometryUnion.h"
#include "GuPersistentContactManifold.h"
#include "PsFoundation.h"

using namespace physx;
using namespace Gu;


static void startContacts(PxsContactManagerOutput& output, PxcNpThreadContext& context)
{
	context.mContactBuffer.reset();

	output.contactForces = NULL;
	output.contactPatches = NULL;
	output.contactPoints = NULL;
	output.nbContacts = 0;
	output.nbPatches = 0;
	output.statusFlag = 0;	
}

static void flipContacts(PxcNpThreadContext& threadContext, PxsMaterialInfo* PX_RESTRICT materialInfo)
{
	ContactBuffer& buffer = threadContext.mContactBuffer;
	for(PxU32 i=0; i<buffer.count; ++i)
	{
		Gu::ContactPoint& contactPoint = buffer.contacts[i];
		contactPoint.normal = -contactPoint.normal;
		Ps::swap(materialInfo[i].mMaterialIndex0, materialInfo[i].mMaterialIndex1);
	}
}

static PX_FORCE_INLINE void updateDiscreteContactStats(PxcNpThreadContext& context, PxGeometryType::Enum type0, PxGeometryType::Enum type1)
{
#if PX_ENABLE_SIM_STATS
	PX_ASSERT(type0<=type1);
	context.mDiscreteContactPairs[type0][type1]++;
#endif
}

static bool copyBuffers(PxsContactManagerOutput& cmOutput, Gu::Cache& cache, PxcNpThreadContext& context, const bool useContactCache, const bool isMeshType)
{
	bool ret = false;
	//Copy the contact stream from previous buffer to current buffer...
	PxU32 oldSize = sizeof(PxContact) * cmOutput.nbContacts + sizeof(PxContactPatch)*cmOutput.nbPatches;
	if(oldSize)
	{
		ret = true;
		PxU8* oldPatches = cmOutput.contactPatches;
		PxU8* oldContacts = cmOutput.contactPoints;
		PxReal* oldForces = cmOutput.contactForces;

		PxU32 forceSize = cmOutput.nbContacts * sizeof(PxReal);
		if(isMeshType)
			forceSize += cmOutput.nbContacts * sizeof(PxU32);

		PxU8* PX_RESTRICT contactPatches = NULL;
		PxU8* PX_RESTRICT contactPoints = NULL;

		PxReal* forceBuffer = NULL;

		bool isOverflown = false;

		//ML: if we are using contactStreamPool, which means we are running the GPU codepath
		if(context.mContactStreamPool)
		{
			const PxU32 patchSize = cmOutput.nbPatches * sizeof(PxContactPatch);
			const PxU32 contactSize = cmOutput.nbContacts * sizeof(PxContact);

			PxU32 index = PxU32(Ps::atomicAdd(&context.mContactStreamPool->mSharedDataIndex, PxI32(contactSize)));
			
			if(context.mContactStreamPool->isOverflown())
			{
				PX_WARN_ONCE("Contact buffer overflow detected, please increase its size in the scene desc!\n");
				isOverflown = true;
			}
			contactPoints = context.mContactStreamPool->mDataStream + context.mContactStreamPool->mDataStreamSize - index;

			const PxU32 patchIndex = PxU32(Ps::atomicAdd(&context.mPatchStreamPool->mSharedDataIndex, PxI32(patchSize)));
			
			if(context.mPatchStreamPool->isOverflown())
			{
				PX_WARN_ONCE("Patch buffer overflow detected, please increase its size in the scene desc!\n");
				isOverflown = true;
			}
			contactPatches = context.mPatchStreamPool->mDataStream + context.mPatchStreamPool->mDataStreamSize - patchIndex;

			if(forceSize)
			{
				index = PxU32(Ps::atomicAdd(&context.mForceAndIndiceStreamPool->mSharedDataIndex, PxI32(forceSize)));
			
				if(context.mForceAndIndiceStreamPool->isOverflown())
				{
					PX_WARN_ONCE("Force buffer overflow detected, please increase its size in the scene desc!\n");
					isOverflown = true;
				}
				forceBuffer = reinterpret_cast<PxReal*>(context.mForceAndIndiceStreamPool->mDataStream + context.mForceAndIndiceStreamPool->mDataStreamSize - index);
			}

			if(isOverflown)
			{
				contactPatches = NULL;
				contactPoints = NULL;
				forceBuffer = NULL;
				cmOutput.nbContacts = cmOutput.nbPatches = 0;
			}
			else
			{
				PxMemCopy(contactPatches, oldPatches, patchSize);
				PxMemCopy(contactPoints, oldContacts, contactSize);
				if (isMeshType)
				{
					PxMemCopy(forceBuffer + cmOutput.nbContacts, oldForces + cmOutput.nbContacts, sizeof(PxU32) * cmOutput.nbContacts);
				}
			}
		}
		else
		{
			const PxU32 alignedOldSize = (oldSize + 0xf) & 0xfffffff0;

			PxU8* data = context.mContactBlockStream.reserve(alignedOldSize + forceSize);
			if(forceSize)
				forceBuffer = reinterpret_cast<PxReal*>(data + alignedOldSize);

			contactPatches = data;
			contactPoints = data + cmOutput.nbPatches * sizeof(PxContactPatch);

			PxMemCopy(data, oldPatches, oldSize);
			if (isMeshType)
			{
				PxMemCopy(forceBuffer + cmOutput.nbContacts, oldForces + cmOutput.nbContacts, sizeof(PxU32) * cmOutput.nbContacts);
			}
		}

		if(forceSize)
			PxMemZero(forceBuffer, forceSize);
		
		cmOutput.contactPatches= contactPatches;
		cmOutput.contactPoints = contactPoints;
		cmOutput.contactForces = forceBuffer;
	}

	if(cache.mCachedSize)
	{
		if(cache.isMultiManifold())
		{
			PX_ASSERT((cache.mCachedSize & 0xF) == 0);
			PxU8* newData = context.mNpCacheStreamPair.reserve(cache.mCachedSize);
			PX_ASSERT((reinterpret_cast<uintptr_t>(newData)& 0xF) == 0);
			PxMemCopy(newData, & cache.getMultipleManifold(), cache.mCachedSize);
			cache.setMultiManifold(newData);
		}
		else if(useContactCache)
		{
			//Copy cache information as well...
			const PxU8* cachedData = cache.mCachedData;
			PxU8* newData = context.mNpCacheStreamPair.reserve(PxU32(cache.mCachedSize + 0xf) & 0xfff0);
			PxMemCopy(newData, cachedData, cache.mCachedSize);
			cache.mCachedData = newData;
		}
	}
	return ret;
}

//ML: isMeshType is used in the GPU codepath. If the collision pair is mesh/heightfield vs primitives, we need to allocate enough memory for the mForceAndIndiceStreamPool in the threadContext. 
static bool finishContacts(const PxcNpWorkUnit& input, PxsContactManagerOutput& npOutput, PxcNpThreadContext& threadContext, PxsMaterialInfo* PX_RESTRICT pMaterials, const bool isMeshType)
{
	ContactBuffer& buffer = threadContext.mContactBuffer;

	PX_ASSERT((npOutput.statusFlag & PxsContactManagerStatusFlag::eTOUCH_KNOWN) != PxsContactManagerStatusFlag::eTOUCH_KNOWN);
	PxU8 statusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
	if(buffer.count)
		statusFlags |= PxsContactManagerStatusFlag::eHAS_TOUCH;
	else
		statusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;

	npOutput.nbContacts = Ps::to8(buffer.count);

	if(!buffer.count)
	{
		npOutput.statusFlag = statusFlags;
		npOutput.nbContacts = 0;
		npOutput.nbPatches = 0;
		return true;
	}
	PX_ASSERT(buffer.count);

#if PX_ENABLE_SIM_STATS
	threadContext.mNbDiscreteContactPairsWithContacts++;
#endif

	npOutput.statusFlag = statusFlags;

	PxU32 contactForceByteSize = buffer.count * sizeof(PxReal);

	//Regardless of the flags, we need to now record the compressed contact stream

	PxU16 compressedContactSize;

	const bool createReports =
		input.flags & PxcNpWorkUnitFlag::eOUTPUT_CONTACTS
		|| threadContext.mCreateContactStream
		|| (input.flags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);

	if((!isMeshType && !createReports))
		contactForceByteSize = 0;

	bool res = (writeCompressedContact(buffer.contacts, buffer.count, &threadContext, npOutput.nbContacts, npOutput.contactPatches, npOutput.contactPoints, compressedContactSize,
		reinterpret_cast<PxReal*&>(npOutput.contactForces), contactForceByteSize, threadContext.mMaterialManager, ((input.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT) != 0), 
		false, pMaterials, npOutput.nbPatches, 0, NULL, NULL, threadContext.mCreateAveragePoint, threadContext.mContactStreamPool, 
		threadContext.mPatchStreamPool, threadContext.mForceAndIndiceStreamPool, isMeshType) != 0);

	//handle buffer overflow
	if(!npOutput.nbContacts)
	{
		PxU8 thisStatusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
		thisStatusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;

		npOutput.statusFlag = thisStatusFlags;
		npOutput.nbContacts = 0;
		npOutput.nbPatches = 0;
#if PX_ENABLE_SIM_STATS
		threadContext.mNbDiscreteContactPairsWithContacts--;
#endif
	}
	return res;
}

template<bool useContactCacheT>
static PX_FORCE_INLINE bool checkContactsMustBeGenerated(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output,
										 const PxsCachedTransform* cachedTransform0, const PxsCachedTransform* cachedTransform1,
										 const bool flip, PxGeometryType::Enum type0, PxGeometryType::Enum type1)
{
	PX_ASSERT(cachedTransform0->transform.isSane() && cachedTransform1->transform.isSane());

	//ML : if user doesn't raise the eDETECT_DISCRETE_CONTACT, we should not generate contacts
	if(!(input.flags & PxcNpWorkUnitFlag::eDETECT_DISCRETE_CONTACT))
		return false;

	if(!(output.statusFlag & PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER) && !(input.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT))
	{
		const PxU32 body0Dynamic = PxU32(input.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY0);
		const PxU32 body1Dynamic = PxU32(input.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1);

		const PxU32 active0 = PxU32(body0Dynamic && !cachedTransform0->isFrozen());
		const PxU32 active1 = PxU32(body1Dynamic && !cachedTransform1->isFrozen());

		if(!(active0 || active1))
		{
			if(flip)
				Ps::swap(type0, type1);

			const bool useContactCache = useContactCacheT ? context.mContactCache && g_CanUseContactCache[type0][type1] : false;
			
#if PX_ENABLE_SIM_STATS
			if(output.nbContacts)
				context.mNbDiscreteContactPairsWithContacts++;
#endif
			const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH;
			copyBuffers(output, cache, context, useContactCache, isMeshType);
			return false;
		}
	}

	output.statusFlag &= (~PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER);

	const PxReal contactDist0 = context.mContactDistance[input.mTransformCache0];
	const PxReal contactDist1 = context.mContactDistance[input.mTransformCache1];
	//context.mNarrowPhaseParams.mContactDistance = shape0->contactOffset + shape1->contactOffset;
	context.mNarrowPhaseParams.mContactDistance = contactDist0 + contactDist1;

	return true;
}

template<bool useLegacyCodepath>
static PX_FORCE_INLINE void discreteNarrowPhase(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
{
	PxGeometryType::Enum type0 = static_cast<PxGeometryType::Enum>(input.geomType0);
	PxGeometryType::Enum type1 = static_cast<PxGeometryType::Enum>(input.geomType1);

	const bool flip = (type1<type0);

	const PxsCachedTransform* cachedTransform0 = &context.mTransformCache->getTransformCache(input.mTransformCache0);
	const PxsCachedTransform* cachedTransform1 = &context.mTransformCache->getTransformCache(input.mTransformCache1);

	if(!checkContactsMustBeGenerated<useLegacyCodepath>(context, input, cache, output, cachedTransform0, cachedTransform1, flip, type0, type1))
		return;

	PxsShapeCore* shape0 = const_cast<PxsShapeCore*>(input.shapeCore0);
	PxsShapeCore* shape1 = const_cast<PxsShapeCore*>(input.shapeCore1);

	if(flip)
	{
		Ps::swap(type0, type1);
		Ps::swap(shape0, shape1);
		Ps::swap(cachedTransform0, cachedTransform1);
	}

	PxsMaterialInfo materialInfo[ContactBuffer::MAX_CONTACTS];

	Gu::MultiplePersistentContactManifold& manifold = context.mTempManifold;
	bool isMultiManifold = false;

	if(!useLegacyCodepath)
	{
		if(cache.isMultiManifold())
		{
			//We are using a multi-manifold. This is cached in a reduced npCache...
			isMultiManifold = true;
			uintptr_t address = uintptr_t(&cache.getMultipleManifold());
			manifold.fromBuffer(reinterpret_cast<PxU8*>(address));
			cache.setMultiManifold(&manifold);
		}
		else if(cache.isManifold())
		{
			void* address = reinterpret_cast<void*>(&cache.getManifold());
			Ps::prefetch(address);
			Ps::prefetch(address, 128);
			Ps::prefetch(address, 256);
		}
	}

	updateDiscreteContactStats(context, type0, type1);

	startContacts(output, context);

	const PxTransform* tm0 = &cachedTransform0->transform;
	const PxTransform* tm1 = &cachedTransform1->transform;
	PX_ASSERT(tm0->isSane() && tm1->isSane());

	if(useLegacyCodepath)
	{
		// PT: many cache misses here...
	
		Ps::prefetchLine(shape1, 0);	// PT: at least get rid of L2s for shape1

		const PxcContactMethod conMethod = g_ContactMethodTable[type0][type1];
		PX_ASSERT(conMethod);

		const bool useContactCache = context.mContactCache && g_CanUseContactCache[type0][type1];
		if(useContactCache)
		{
#if PX_ENABLE_SIM_STATS
			if(PxcCacheLocalContacts(context, cache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry))
				context.mNbDiscreteContactPairsWithCacheHits++;
#else
			PxcCacheLocalContacts(context, n.pairCache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry);
#endif
		}
		else
		{
			conMethod(shape0->geometry, shape1->geometry, *tm0, *tm1, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
		}
	}
	else
	{
		const PxcContactMethod conMethod = g_PCMContactMethodTable[type0][type1];
		PX_ASSERT(conMethod);

		conMethod(shape0->geometry, shape1->geometry, *tm0, *tm1, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
	}

	const PxcGetMaterialMethod materialMethod = g_GetMaterialMethodTable[type0][type1];
//	PX_ASSERT(materialMethod);
	if(materialMethod)
		materialMethod(shape0, shape1, context,  materialInfo);

	if(flip)
		flipContacts(context, materialInfo);

	if(!useLegacyCodepath)
	{
		if(isMultiManifold)
		{
			//Store the manifold back...
			const PxU32 size = (sizeof(MultiPersistentManifoldHeader) +
				manifold.mNumManifolds * sizeof(SingleManifoldHeader) +
				manifold.mNumTotalContacts * sizeof(Gu::CachedMeshPersistentContact));

			PxU8* buffer = context.mNpCacheStreamPair.reserve(size);

			PX_ASSERT((reinterpret_cast<uintptr_t>(buffer)& 0xf) == 0);
			manifold.toBuffer(buffer);
			cache.setMultiManifold(buffer);
			cache.mCachedSize = Ps::to16(size);
		}
	}

	const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH; 
	finishContacts(input, output, context, materialInfo, isMeshType);
}

void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
{
	discreteNarrowPhase<true>(context, input, cache, output);
}

void physx::PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
{
	discreteNarrowPhase<false>(context, input, cache, output);
}