//
// 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 "PulleyJoint.h"
#include <assert.h>
#include "PxConstraint.h"

using namespace physx;


PxConstraintShaderTable PulleyJoint::sShaderTable = { &PulleyJoint::solverPrep, &PulleyJoint::project, &PulleyJoint::visualize, PxConstraintFlag::Enum(0) };

PulleyJoint::PulleyJoint(PxPhysics& physics, PxRigidBody& body0, const PxTransform& localFrame0, const PxVec3& attachment0,
											 PxRigidBody& body1, const PxTransform& localFrame1, const PxVec3& attachment1)
{
	mConstraint = physics.createConstraint(&body0, &body1, *this, PulleyJoint::sShaderTable, sizeof(PulleyJointData));

	mBody[0] = &body0;
	mBody[1] = &body1;

	// keep these around in case the CoM gets relocated
	mLocalPose[0] = localFrame0.getNormalized();
	mLocalPose[1] = localFrame1.getNormalized();

	// the data which will be fed to the joint solver and projection shaders
	mData.attachment0 = attachment0;
	mData.attachment1 = attachment1;
	mData.distance = 1.0f;
	mData.ratio = 1.0f;
	mData.c2b[0] = body0.getCMassLocalPose().transformInv(mLocalPose[0]);
	mData.c2b[1] = body1.getCMassLocalPose().transformInv(mLocalPose[1]);
}

void PulleyJoint::release()
{
	mConstraint->release();
}

///////////////////////////////////////////// attribute accessors and mutators


void PulleyJoint::setAttachment0(const PxVec3& pos)
{
	mData.attachment0 = pos;
	mConstraint->markDirty();
}

PxVec3 PulleyJoint::getAttachment0() const
{
	return mData.attachment0;
}

void PulleyJoint::setAttachment1(const PxVec3& pos)
{
	mData.attachment1 = pos;
	mConstraint->markDirty();
}

PxVec3 PulleyJoint::getAttachment1() const
{
	return mData.attachment1;
}

void PulleyJoint::setDistance(float totalDistance)
{
	mData.distance = totalDistance;
	mConstraint->markDirty();
}

float PulleyJoint::getDistance() const
{
	return mData.distance;
}
	
void PulleyJoint::setRatio(float ratio)
{
	mData.ratio = ratio;
	mConstraint->markDirty();
}

float PulleyJoint::getRatio() const
{
	return mData.ratio;
}


///////////////////////////////////////////// PxConstraintConnector methods

void* PulleyJoint::prepareData()
{
	return &mData;
}

void  PulleyJoint::onConstraintRelease()
{
	delete this;
}

void PulleyJoint::onComShift(PxU32 actor)
{
	mData.c2b[actor] = mBody[actor]->getCMassLocalPose().transformInv(mLocalPose[actor]); 
	mConstraint->markDirty();
}

void  PulleyJoint::onOriginShift(const PxVec3& shift)
{
	mData.attachment0 -= shift;
	mData.attachment1 -= shift;
}

void* PulleyJoint::getExternalReference(PxU32& typeID)
{
	typeID = TYPE_ID;
	return this;
}


///////////////////////////////////////////// work functions


PxU32 PulleyJoint::solverPrep(Px1DConstraint* constraints,
							  PxVec3& body0WorldOffset,
							  PxU32 maxConstraints,
							  PxConstraintInvMassScale&,
							  const void* constantBlock,
							  const PxTransform& bA2w,
							  const PxTransform& bB2w,
							  bool /*useExtendedLimits*/,
							  PxVec3& cA2wOut, PxVec3& cB2wOut)
{		
	PX_UNUSED(maxConstraints);

	const PulleyJointData& data = *reinterpret_cast<const PulleyJointData*>(constantBlock);

	PxTransform cA2w = bA2w.transform(data.c2b[0]);
	PxTransform cB2w = bB2w.transform(data.c2b[1]);

	cA2wOut = cA2w.p;
	cB2wOut = cB2w.p;

	body0WorldOffset = cB2w.p - bA2w.p;

	PxVec3 directionA = data.attachment0 - cA2w.p;
	PxReal distanceA = directionA.normalize();

	PxVec3 directionB = data.attachment1 - cB2w.p;
	PxReal distanceB = directionB.normalize();

	directionB *= data.ratio;

	PxReal totalDistance = distanceA + distanceB;

	// compute geometric error:
	PxReal geometricError = (data.distance - totalDistance); 

	Px1DConstraint *c = constraints;
	// constraint is breakable, so we need to output forces
	c->flags = Px1DConstraintFlag::eOUTPUT_FORCE;

	if (geometricError < 0.0f) 
	{
		c->maxImpulse = PX_MAX_F32;
		c->minImpulse = 0;
		c->geometricError = geometricError; 
	}
	else if(geometricError > 0.0f)
	{
		c->maxImpulse = 0;
		c->minImpulse = -PX_MAX_F32;
		c->geometricError = geometricError; 
	}

	c->linear0 = directionA;		c->angular0 = (cA2w.p - bA2w.p).cross(c->linear0);		
	c->linear1 = -directionB;		c->angular1 = (cB2w.p - bB2w.p).cross(c->linear1);		

	return 1;
}

void PulleyJoint::project(const void* constantBlock,
						  PxTransform& bodyAToWorld,
						  PxTransform& bodyBToWorld,
						  bool projectToA)
{
	PX_UNUSED(constantBlock);
	PX_UNUSED(bodyAToWorld);
	PX_UNUSED(bodyBToWorld);
	PX_UNUSED(projectToA);
}

void PulleyJoint::visualize(PxConstraintVisualizer&		viz,
							const void*					constantBlock,
							const PxTransform&			body0Transform,
							const PxTransform&			body1Transform,
							PxU32						flags)
{
	PX_UNUSED(flags);
	const PulleyJointData& data = *reinterpret_cast<const PulleyJointData*>(constantBlock);

	PxTransform cA2w = body0Transform * data.c2b[0];
	PxTransform cB2w = body1Transform * data.c2b[1];
	viz.visualizeJointFrames(cA2w, cB2w);
	viz.visualizeJointFrames(PxTransform(data.attachment0), PxTransform(data.attachment1));
}