//
// 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-2019 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  


#ifndef PX_PHYSICS_SCP_ARTICULATION_CORE
#define PX_PHYSICS_SCP_ARTICULATION_CORE

#include "ScActorCore.h"
#include "DyArticulation.h"
#include "DyFeatherstoneArticulation.h"

namespace physx
{

class PxvArticulation;

namespace IG
{
	class NodeIndex;
}

namespace Sc
{
	typedef Dy::FsData ArticulationDriveCache;

	class ArticulationSim;
	class BodyCore;
	class BodySim;
	class ArticulationJointCore;

	class ArticulationCore
	{
	//= ATTENTION! =====================================================================================
	// Changing the data layout of this class breaks the binary serialization format.  See comments for 
	// PX_BINARY_SERIAL_VERSION.  If a modification is required, please adjust the getBinaryMetaData 
	// function.  If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
	// accordingly.
	//==================================================================================================

		//---------------------------------------------------------------------------------
		// Construction, destruction & initialization
		//---------------------------------------------------------------------------------

// PX_SERIALIZATION
		public:
													ArticulationCore(const PxEMPTY) : mSim(NULL)	{}
		static		void							getBinaryMetaData(PxOutputStream& stream);
//~PX_SERIALIZATION
													ArticulationCore(bool reducedCoordinate);
													~ArticulationCore();

		//---------------------------------------------------------------------------------
		// External API
		//---------------------------------------------------------------------------------
		PX_FORCE_INLINE	PxU32						getInternalDriveIterations()		const	{ return mCore.internalDriveIterations;	}
		PX_FORCE_INLINE	void						setInternalDriveIterations(const PxU32 v)	{ mCore.internalDriveIterations = v;	}

		PX_FORCE_INLINE	PxU32						getExternalDriveIterations()		const	{ return mCore.externalDriveIterations;	}
		PX_FORCE_INLINE	void						setExternalDriveIterations(const PxU32 v)	{ mCore.externalDriveIterations = v;	}

		PX_FORCE_INLINE	PxU32						getMaxProjectionIterations()		const	{ return mCore.maxProjectionIterations;	}
		PX_FORCE_INLINE	void						setMaxProjectionIterations(const PxU32 v)	{ mCore.maxProjectionIterations = v;	}

		PX_FORCE_INLINE	PxReal						getSeparationTolerance()			const	{ return mCore.separationTolerance;		}
		PX_FORCE_INLINE	void						setSeparationTolerance(const PxReal v)		{ mCore.separationTolerance = v;		}

		PX_FORCE_INLINE	PxReal						getSleepThreshold()					const	{ return mCore.sleepThreshold;			}
		PX_FORCE_INLINE	void						setSleepThreshold(const PxReal v)			{ mCore.sleepThreshold = v;				}

		PX_FORCE_INLINE	PxReal						getFreezeThreshold()				const	{ return mCore.freezeThreshold;			}
		PX_FORCE_INLINE	void						setFreezeThreshold(const PxReal v)			{ mCore.freezeThreshold = v;			}

		PX_FORCE_INLINE	PxU16						getSolverIterationCounts()			const	{ return mCore.solverIterationCounts;	}
		PX_FORCE_INLINE	void						setSolverIterationCounts(PxU16 c)			{ mCore.solverIterationCounts = c;		}

		PX_FORCE_INLINE	PxReal						getWakeCounter()					const	{ return mCore.wakeCounter;				}
		PX_FORCE_INLINE	void						setWakeCounterInternal(const PxReal v)		{ mCore.wakeCounter = v;				}
						void						setWakeCounter(const PxReal v);

						bool						isSleeping() const;
						void						wakeUp(PxReal wakeCounter);
						void						putToSleep();

						PxArticulationBase*			getPxArticulationBase();
						const PxArticulationBase*	getPxArticulationBase() const;

		//---------------------------------------------------------------------------------
		// Drive Cache API
		//---------------------------------------------------------------------------------
						ArticulationDriveCache*		createDriveCache(PxReal compliance,
																	 PxU32 driveIterations) const;

						void						updateDriveCache(ArticulationDriveCache& cache,
																	 PxReal compliance,
																	 PxU32 driveIterations) const;

						void						releaseDriveCache(ArticulationDriveCache& cache) const;

						PxU32						getCacheLinkCount(const ArticulationDriveCache& cache) const;

						void						applyImpulse(BodyCore& link,
																 const ArticulationDriveCache& driveCache,
																 const PxVec3& force,
																 const PxVec3& torque);

						void						computeImpulseResponse(BodyCore& link,
																		   PxVec3& linearResponse, 
																		   PxVec3& angularResponse,
																		   const ArticulationDriveCache& driveCache,
																		   const PxVec3& force,
																		   const PxVec3& torque) const;

		//---------------------------------------------------------------------------------
		// external reduced coordinate API
		//---------------------------------------------------------------------------------
						void						setArticulationFlags(PxArticulationFlags flags);
						PxArticulationFlags			getArticulationFlags() const { return mCore.flags; }

						PxU32						getDofs() const;

						PxArticulationCache*		createCache() const;

						PxU32						getCacheDataSize() const;

						void						zeroCache(PxArticulationCache& cache) const;

						void						applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag)const;
		
						void						copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const;

						void						releaseCache(PxArticulationCache& cache) const;

						void						packJointData(const PxReal* maximum, PxReal* reduced) const;

						void						unpackJointData(const PxReal* reduced, PxReal* maximum) const;

						void						commonInit() const;

						void						computeGeneralizedGravityForce(PxArticulationCache& cache) const;

						void						computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const;

						void						computeGeneralizedExternalForce(PxArticulationCache& cache) const;

						void						computeJointAcceleration(PxArticulationCache& cache) const;

						void						computeJointForce(PxArticulationCache& cache) const;


						void						computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const;

						void						computeCoefficientMatrix(PxArticulationCache& cache) const;

						bool						computeLambda(PxArticulationCache& cache, PxArticulationCache& rollBackCache, const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter) const;

						void						computeGeneralizedMassMatrix(PxArticulationCache& cache) const;

						PxU32						getCoefficientMatrixSize() const;

						PxSpatialVelocity			getLinkVelocity(const PxU32 linkId) const;

						PxSpatialVelocity			getLinkAcceleration(const PxU32 linkId) const;

		//---------------------------------------------------------------------------------
		// Internal API
		//---------------------------------------------------------------------------------
	public:
		PX_FORCE_INLINE	void						setSim(ArticulationSim* sim)
													{
														PX_ASSERT((sim==0) ^ (mSim == 0));
														mSim = sim;
													}
		PX_FORCE_INLINE	ArticulationSim*			getSim()			const	{ return mSim;			}

		PX_FORCE_INLINE	const Dy::ArticulationCore&	getCore()					{ return mCore;			}

		static PX_FORCE_INLINE ArticulationCore&	getArticulationCore(ArticulationCore& core)
													{
														const size_t offset = PX_OFFSET_OF(ArticulationCore, mCore);
														return *reinterpret_cast<ArticulationCore*>(reinterpret_cast<PxU8*>(&core) - offset);
													}

		PX_INLINE		bool						isReducedCoordinate() const	{ return mIsReducedCoordinate;	}

						IG::NodeIndex				getIslandNodeIndex() const;

						void						setGlobalPose();

						void						setDirty(const bool dirty);

	private:
						ArticulationSim*			mSim;
						Dy::ArticulationCore		mCore;
						bool						mIsReducedCoordinate;
	};

} // namespace Sc

}

#endif