//
// 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.  

#include "foundation/PxMath.h"
#include "vehicle/PxVehicleUtilSetup.h"
#include "vehicle/PxVehicleDrive4W.h"
#include "vehicle/PxVehicleDriveNW.h"
#include "vehicle/PxVehicleDriveTank.h"
#include "vehicle/PxVehicleNoDrive.h"
#include "vehicle/PxVehicleWheels.h"
#include "vehicle/PxVehicleUtil.h"
#include "vehicle/PxVehicleUpdate.h"

#include "PsFoundation.h"
#include "CmPhysXCommon.h"

namespace physx
{

void enable3WMode(const PxU32 rightDirection, const PxU32 upDirection, const bool removeFrontWheel, PxVehicleWheelsSimData& wheelsSimData, PxVehicleWheelsDynData& wheelsDynData, PxVehicleDriveSimData4W& driveSimData);

void computeDirection(PxU32& rightDirection, PxU32& upDirection);

void PxVehicle4WEnable3WTadpoleMode(PxVehicleWheelsSimData& wheelsSimData, PxVehicleWheelsDynData& wheelsDynData, PxVehicleDriveSimData4W& driveSimData)
{
	PX_CHECK_AND_RETURN
		(!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_LEFT) &&
		 !wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT) &&
		 !wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_LEFT) &&
		 !wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_RIGHT), "PxVehicle4WEnable3WTadpoleMode requires no wheels to be disabled");

	PxU32 rightDirection=0xffffffff;
	PxU32 upDirection=0xffffffff;
	computeDirection(rightDirection, upDirection);
	PX_CHECK_AND_RETURN(rightDirection<3 && upDirection<3, "PxVehicle4WEnable3WTadpoleMode requires the vectors set in PxVehicleSetBasisVectors to be axis-aligned");

	enable3WMode(rightDirection, upDirection, false, wheelsSimData, wheelsDynData, driveSimData);
}

void PxVehicle4WEnable3WDeltaMode(PxVehicleWheelsSimData& wheelsSimData, PxVehicleWheelsDynData& wheelsDynData, PxVehicleDriveSimData4W& driveSimData)
{
	PX_CHECK_AND_RETURN
		(!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_LEFT) &&
		!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT) &&
		!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_LEFT) &&
		!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_RIGHT), "PxVehicle4WEnable3WDeltaMode requires no wheels to be disabled");

	PxU32 rightDirection=0xffffffff;
	PxU32 upDirection=0xffffffff;
	computeDirection(rightDirection, upDirection);
	PX_CHECK_AND_RETURN(rightDirection<3 && upDirection<3, "PxVehicle4WEnable3WTadpoleMode requires the vectors set in PxVehicleSetBasisVectors to be axis-aligned");

	enable3WMode(rightDirection, upDirection, true, wheelsSimData, wheelsDynData, driveSimData);
}

void computeSprungMasses(const PxU32 numSprungMasses, const PxVec3* sprungMassCoordinates, const PxVec3& centreOfMass, const PxReal totalMass, const PxU32 gravityDirection, PxReal* sprungMasses);

void PxVehicleComputeSprungMasses(const PxU32 numSprungMasses, const PxVec3* sprungMassCoordinates, const PxVec3& centreOfMass, const PxReal totalMass, const PxU32 gravityDirection, PxReal* sprungMasses)
{
	computeSprungMasses(numSprungMasses, sprungMassCoordinates, centreOfMass, totalMass, gravityDirection, sprungMasses);
}

void PxVehicleCopyDynamicsData(const PxVehicleCopyDynamicsMap& wheelMap, const PxVehicleWheels& src, PxVehicleWheels* trg)
{
	PX_CHECK_AND_RETURN(trg, "PxVehicleCopyDynamicsData requires that trg is a valid vehicle pointer");

	PX_CHECK_AND_RETURN(src.getVehicleType() == trg->getVehicleType(), "PxVehicleCopyDynamicsData requires that both src and trg are the same type of vehicle");

#if PX_CHECKED
	{
		const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
		const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();
		PxU8 copiedWheelsSrc[PX_MAX_NB_WHEELS];
		PxMemZero(copiedWheelsSrc, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
		PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		for(PxU32 i = 0; i < PxMin(numWheelsSrc, numWheelsTrg); i++)
		{
			const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
			PX_CHECK_AND_RETURN(srcWheelId < numWheelsSrc, "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			PX_CHECK_AND_RETURN(0 == copiedWheelsSrc[srcWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			copiedWheelsSrc[srcWheelId] = 1;

			const PxU32 trgWheelId = wheelMap.targetWheelIds[i];
			PX_CHECK_AND_RETURN(trgWheelId < numWheelsTrg, "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			PX_CHECK_AND_RETURN(0 == setWheelsTrg[trgWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			setWheelsTrg[trgWheelId]=1;
		}
	}
#endif


	const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
	const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();

	//Set all dynamics data on the target to zero.
	//Be aware that setToRestState sets the rigid body to 
	//rest so set the momentum back after calling setToRestState.
	PxRigidDynamic* actorTrg = trg->getRigidDynamicActor();
	PxVec3 linVel = actorTrg->getLinearVelocity();
	PxVec3 angVel = actorTrg->getAngularVelocity();
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
		static_cast<PxVehicleDrive4W*>(trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVENW:
		static_cast<PxVehicleDriveNW*>(trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVETANK:
		static_cast<PxVehicleDriveTank*>(trg)->setToRestState();
		break;
	case PxVehicleTypes::eNODRIVE:
		static_cast<PxVehicleNoDrive*>(trg)->setToRestState();
		break;
	default:
		break;
	}
	actorTrg->setLinearVelocity(linVel);
	actorTrg->setAngularVelocity(angVel);


	//Keep a track of the wheels on trg that have their dynamics data set as a copy from src.
	PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
	PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);

	//Keep a track of the average wheel rotation speed of all enabled wheels on src.
	PxU32 numEnabledWheelsSrc = 0;
	PxF32 accumulatedWheelRotationSpeedSrc = 0.0f;

	//Copy wheel dynamics data from src wheels to trg wheels.
	//Track the target wheels that have been given dynamics data from src wheels.
	//Compute the accumulated wheel rotation speed of all enabled src wheels.
	const PxU32 numMappedWheels = PxMin(numWheelsSrc, numWheelsTrg);
	for(PxU32 i = 0; i < numMappedWheels; i++)
	{
		const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
		const PxU32 trgWheelId = wheelMap.targetWheelIds[i];

		trg->mWheelsDynData.copy(src.mWheelsDynData, srcWheelId, trgWheelId);

		setWheelsTrg[trgWheelId] = 1;

		if(!src.mWheelsSimData.getIsWheelDisabled(srcWheelId))
		{
			numEnabledWheelsSrc++;
			accumulatedWheelRotationSpeedSrc += src.mWheelsDynData.getWheelRotationSpeed(srcWheelId);
		}
	}

	//Compute the average wheel rotation speed of src.
	PxF32 averageWheelRotationSpeedSrc = 0;
	if(numEnabledWheelsSrc > 0)
	{
		averageWheelRotationSpeedSrc = (accumulatedWheelRotationSpeedSrc/ (1.0f * numEnabledWheelsSrc));
	}

	//For wheels on trg that have not had their dynamics data copied from src just set
	//their wheel rotation speed to the average wheel rotation speed.
	for(PxU32 i = 0; i < numWheelsTrg; i++)
	{
		if(0 == setWheelsTrg[i] && !trg->mWheelsSimData.getIsWheelDisabled(i))
		{
			trg->mWheelsDynData.setWheelRotationSpeed(i, averageWheelRotationSpeedSrc);
		}
	}

	//Copy the engine rotation speed/gear states/autobox states/etc.
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
	case PxVehicleTypes::eDRIVENW:
	case PxVehicleTypes::eDRIVETANK:
		{
			const PxVehicleDriveDynData& driveDynDataSrc = static_cast<const PxVehicleDrive&>(src).mDriveDynData;
			PxVehicleDriveDynData* driveDynDataTrg = &static_cast<PxVehicleDrive*>(trg)->mDriveDynData;
			*driveDynDataTrg = driveDynDataSrc;
		}
		break;
	default:
		break;
	}
}

bool areEqual(const PxQuat& q0, const PxQuat& q1)
{
	return ((q0.x == q1.x) && (q0.y == q1.y) && (q0.z == q1.z) && (q0.w == q1.w)); 
}

void PxVehicleUpdateCMassLocalPose(const PxTransform& oldCMassLocalPose, const PxTransform& newCMassLocalPose, const PxU32 gravityDirection, PxVehicleWheels* vehicle)
{
	PX_CHECK_AND_RETURN(areEqual(PxQuat(PxIdentity), oldCMassLocalPose.q), "Only center of mass poses with identity rotation are supported");
	PX_CHECK_AND_RETURN(areEqual(PxQuat(PxIdentity), newCMassLocalPose.q), "Only center of mass poses with identity rotation are supported");
	PX_CHECK_AND_RETURN(0==gravityDirection || 1==gravityDirection || 2==gravityDirection, "gravityDirection must be 0 or 1 or 2.");

	//Update the offsets from the rigid body center of mass.
	PxVec3 wheelCenterCMOffsets[PX_MAX_NB_WHEELS];
	const PxU32 nbWheels = vehicle->mWheelsSimData.getNbWheels();
	for(PxU32 i = 0; i < nbWheels; i++)
	{
		wheelCenterCMOffsets[i] = vehicle->mWheelsSimData.getWheelCentreOffset(i) + oldCMassLocalPose.p - newCMassLocalPose.p;
		vehicle->mWheelsSimData.setWheelCentreOffset(i, vehicle->mWheelsSimData.getWheelCentreOffset(i) + oldCMassLocalPose.p - newCMassLocalPose.p);
		vehicle->mWheelsSimData.setSuspForceAppPointOffset(i, vehicle->mWheelsSimData.getSuspForceAppPointOffset(i) + oldCMassLocalPose.p - newCMassLocalPose.p);
		vehicle->mWheelsSimData.setTireForceAppPointOffset(i, vehicle->mWheelsSimData.getTireForceAppPointOffset(i) + oldCMassLocalPose.p - newCMassLocalPose.p);
	}

	//Now update the sprung masses.
	PxF32 sprungMasses[PX_MAX_NB_WHEELS];
	PxVehicleComputeSprungMasses(nbWheels, wheelCenterCMOffsets, PxVec3(0,0,0), vehicle->getRigidDynamicActor()->getMass(), gravityDirection, sprungMasses);
	for(PxU32 i = 0; i < nbWheels; i++)
	{
		PxVehicleSuspensionData suspData = vehicle->mWheelsSimData.getSuspensionData(i);
		const PxF32 massRatio = sprungMasses[i]/suspData.mSprungMass;
		suspData.mSprungMass = sprungMasses[i];
		suspData.mSpringStrength *= massRatio;
		suspData.mSpringDamperRate *= massRatio;
		vehicle->mWheelsSimData.setSuspensionData(i, suspData);
	}
}

}//physx