//
// 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 "PxPhysicsAPI.h"
#include "extensions/PxExtensionsAPI.h"
#include "SampleSubmarine.h"
#include "Crab.h"
#include "RendererColor.h"
#include "RenderPhysX3Debug.h"

#include "PxTkStream.h"

#include "PhysXSample.h"
#include "PxTkFile.h"
using namespace PxToolkit;
// if enabled: runs the crab AI in sync, not as a parallel task to physx.
#define DEBUG_RENDERING 0

void setupFiltering(PxRigidActor* actor, PxU32 filterGroup, PxU32 filterMask);

// table with default times in seconds how the crab AI will try to stay in a state
static const PxReal gDefaultStateTime[CrabState::eNUM_STATES] = {5.0f, 10.0f, 10.0f, 10.0f, 10.0f, 6.0f};

Crab::Crab(SampleSubmarine& sample, const PxVec3& crabPos, RenderMaterial* material)
: ClassType(ClassType::eCRAB)
, mSampleSubmarine(&sample)
, mMaterial(material)
{
	initMembers();
	create(crabPos);
}

Crab::Crab(SampleSubmarine& sample, const char* filename, RenderMaterial* material)
: ClassType(ClassType::eCRAB)
, mSampleSubmarine(&sample)
, mMaterial(material)
{
	initMembers();
	load(filename);
}

void Crab::initMembers()
{
	mMemory = NULL;
	mCrabBody = NULL;
	mSqRayBuffer = NULL;
	mLegHeight = 0;
	mRespawnMe = false;
	mCrabState = CrabState::eWAITING;
	mStateTime = gDefaultStateTime[CrabState::eWAITING];
	mAccumTime = 0;
	mElapsedTime = 0;
	mRunning = 0;

	mAcceleration[0] = 0;
	mAcceleration[1] = 0;

	mAccelerationBuffer[0] = 0;
	mAccelerationBuffer[1] = 0;

	// setup buffer for 10 batched rays and 10 hits
	mSqRayBuffer = SAMPLE_NEW(SqRayBuffer)(*mSampleSubmarine, 10, 10);

	mSubmarinePos = PxVec3(0);

}
Crab::~Crab()
{
	// wait until background task is finished
	while(mRunning)
		;

	for(PxU32 i = 0; i < mJoints.size(); i++)
		mJoints[i]->release();
	mJoints.clear();

	for(PxU32 i = 0; i < mActors.size(); i++)
		mSampleSubmarine->removeActor(mActors[i]);
	mActors.clear();
	
	if(mMemory)
	{
		SAMPLE_FREE(mMemory);
	}

	delete mSqRayBuffer;
}

static void setShapeFlag(PxRigidActor* actor, PxShapeFlag::Enum flag, bool flagValue)
{
	const PxU32 numShapes = actor->getNbShapes();
	PxShape** shapes = (PxShape**)SAMPLE_ALLOC(sizeof(PxShape*)*numShapes);
	actor->getShapes(shapes, numShapes);
	for(PxU32 i = 0; i < numShapes; i++)
	{
		PxShape* shape = shapes[i];
		shape->setFlag(flag, flagValue);
	}
	SAMPLE_FREE(shapes);
}

PxVec3 Crab::getPlaceOnFloor(PxVec3 start)
{
	PxRaycastBuffer rayHit;
	mSampleSubmarine->getActiveScene().raycast(start, PxVec3(0,-1,0), 1000.0f, rayHit);

	return rayHit.block.position + PxVec3(0,mLegHeight,0);
}

static const PxSerialObjectId mMaterial_id		= (PxSerialObjectId)0x01;
static const PxSerialObjectId mCrabBody_id		= (PxSerialObjectId)0x02;
static const PxSerialObjectId mMotorJoint0_id	= (PxSerialObjectId)0x03;
static const PxSerialObjectId mMotorJoint1_id	= (PxSerialObjectId)0x04; 

void Crab::save(const char* filename)
{
	PxPhysics& physics = mSampleSubmarine->getPhysics();
	PxCollection* thePxCollection = PxCreateCollection();
	PxSerializationRegistry* sr = PxSerialization::createSerializationRegistry(physics);	
	for(PxU32 i = 0; i < mActors.size(); ++i)
	{		
		thePxCollection->add(*mActors[i]);
	}
	
	for(PxU32 i = 0; i < mJoints.size(); ++i)
	{
		thePxCollection->add(*mJoints[i]);
	}
	
	thePxCollection->addId(*mCrabBody,		 mCrabBody_id);
	thePxCollection->addId(*mMotorJoint[0],  mMotorJoint0_id);
	thePxCollection->addId(*mMotorJoint[1],  mMotorJoint1_id);
	
	PxCollection* theExtRef = PxCreateCollection();
	theExtRef->add(*mSampleSubmarine->mMaterial, mMaterial_id);
	
	PxSerialization::complete(*thePxCollection, *sr, theExtRef);

	PxDefaultFileOutputStream s(filename);
	PxSerialization::serializeCollectionToBinary(s, *thePxCollection, *sr, theExtRef);
	
	theExtRef->release();
	thePxCollection->release();
	sr->release();
}

static PxU32 GetFileSize(const char* name)
{
	if(!name)	return 0;

#ifndef SEEK_END
#define SEEK_END 2
#endif

	SampleFramework::File* fp;
	if (PxToolkit::fopen_s(&fp, name, "rb"))
		return 0;
	fseek(fp, 0, SEEK_END);
	PxU32 eof_ftell = (PxU32)ftell(fp);
	fclose(fp);
	return eof_ftell;
}

void Crab::load(const char* filename)
{
	PxPhysics& thePhysics = mSampleSubmarine->getPhysics();

	SampleFramework::File* fp = NULL;
	if (!PxToolkit::fopen_s(&fp, filename, "rb"))
	{
		PxU32 theFileSize = GetFileSize(filename);
		
		if(!mMemory)
			mMemory = SAMPLE_ALLOC(theFileSize + PX_SERIAL_FILE_ALIGN);

		void* theMemory16 = (void*)((size_t(mMemory) + PX_SERIAL_FILE_ALIGN)&~(PX_SERIAL_FILE_ALIGN-1));
		const size_t theNumRead = fread(theMemory16, 1, theFileSize, fp);
		PX_ASSERT(PxU32(theNumRead) == theFileSize);
		PX_UNUSED(theNumRead);
		fclose(fp);

		PxCollection* theExtRef = PxCreateCollection();
		theExtRef->add(*mSampleSubmarine->mMaterial, mMaterial_id);
		PxSerializationRegistry* sr = PxSerialization::createSerializationRegistry(thePhysics);
		PxCollection* thePxCollection = PxSerialization::createCollectionFromBinary(theMemory16, *sr, theExtRef);
		PX_ASSERT(thePxCollection);

		mSampleSubmarine->getActiveScene().addCollection(*thePxCollection);
			
		mMotorJoint[0] = reinterpret_cast<PxRevoluteJoint*>( thePxCollection->find(mMotorJoint0_id));
		mMotorJoint[1] = reinterpret_cast<PxRevoluteJoint*>( thePxCollection->find(mMotorJoint1_id));
		mCrabBody = reinterpret_cast<PxRigidDynamic*>( thePxCollection->find(mCrabBody_id));
		PX_ASSERT(mMotorJoint[0] && mMotorJoint[1] && mCrabBody );

		PxU32 nbObjs = thePxCollection->getNbObjects();
		PX_ASSERT(nbObjs != 0);
		for(PxU32 i = 0; i < nbObjs; ++i)
		{
			PxBase* object = &thePxCollection->getObject(i);
			if(object)
			{
				const PxType serialType = object->getConcreteType();
				if(serialType == PxConcreteType::eRIGID_DYNAMIC)
				{
					PxRigidDynamic* actor = reinterpret_cast<PxRigidDynamic*>(object);

					mSampleSubmarine->createRenderObjectsFromActor(actor , mMaterial ); 
					mSampleSubmarine->addPhysicsActors( actor );
					mActors.push_back( actor );
				}
				else if(serialType == PxConcreteType::eCONSTRAINT)
				{
					PxU32 typeID = 0;
					PxConstraint* constraint = reinterpret_cast<PxConstraint*>(object);
					PxJoint* joint = reinterpret_cast<PxJoint*>(constraint->getExternalReference(typeID));
					mJoints.push_back( joint );
				}
				else if(serialType == PxConcreteType::eSHAPE)
				{
					//giving up application shape ownership early
					PxShape* shape = reinterpret_cast<PxShape*>(object);
					shape->release();
				}
			}
		}
		
		theExtRef->release();
		thePxCollection->release();
		sr->release();
	}

	if( !mCrabBody ) mSampleSubmarine->fatalError( "createBox failed!" );
	setupFiltering( mCrabBody, FilterGroup::eCRAB, FilterGroup::eHEIGHTFIELD );
}


void Crab::create(const PxVec3& _crabPos)
{
	static const PxReal scale = 0.8f;
	static const PxReal crabDepth = 2.0f;
	static const PxVec3 crabBodyDim = PxVec3(0.8f, 0.8f, crabDepth*0.5f)*scale;
	static const PxReal legMass = 0.03f;
	static const PxReal velocity = 0.0f;
	static const PxReal maxForce = 4000.0f;

	LegParameters params; // check edge ascii art in Crab.h
	params.a = 0.5f;
	params.b = 0.6f;
	params.c = 0.5f;
	params.d = 0.5f;
	params.e = 1.5f;
	params.m = 0.3f;
	params.n = 0.1f;

	mLegHeight = scale*2.0f*(params.d+params.c);
	mLegHeight += 0.5f;
	PxVec3 crabPos = getPlaceOnFloor(_crabPos); 
	mCrabBody = mSampleSubmarine->createBox(crabPos, crabBodyDim, NULL, mMaterial, 1.0f)->is<PxRigidDynamic>();
	if(!mCrabBody) mSampleSubmarine->fatalError("createBox failed!");
	PxShape* shape; mCrabBody->getShapes(&shape, 1);
	shape->setLocalPose(PxTransform(PxQuat(PxHalfPi*0.5f, PxVec3(0,0,1))));
	PxRigidBodyExt::setMassAndUpdateInertia(*mCrabBody, legMass*10.0f);
	PxTransform cmPose = mCrabBody->getCMassLocalPose();
	cmPose.p.y -= 0.8f;
	mCrabBody->setCMassLocalPose(cmPose);
	mCrabBody->setAngularDamping(100.0f);
	mCrabBody->userData = this;
	mActors.push_back(mCrabBody);

	// legs
	static const PxU32 numLegs = 4;
	PxReal recipNumLegs = 1.0f/PxReal(numLegs);
	PxReal recipNumLegsMinus1 = 1.0f/PxReal(numLegs-1);
	PX_COMPILE_TIME_ASSERT((numLegs&1) == 0);

	PxRigidDynamic* motor[2];
	{
		static const PxReal density = 1.0f;
		static const PxReal m = params.m * scale;
		static const PxReal n = params.n * scale;
		static const PxBoxGeometry boxGeomM = PxBoxGeometry(m, m, crabBodyDim.z * 0.5f);

		// create left and right motor
		PxVec3 motorPos = crabPos+PxVec3(0,n,0);
		for(PxU32 i = 0; i < 2; i++)
		{
			PxVec3 motorOfs = i==0 ? PxVec3(0,0, boxGeomM.halfExtents.z) : -PxVec3(0,0,boxGeomM.halfExtents.z);
			motor[i] = mSampleSubmarine->createBox(motorPos+motorOfs, boxGeomM.halfExtents, NULL, mMaterial, density)->is<PxRigidDynamic>();
			if(!motor[i]) mSampleSubmarine->fatalError("createBox failed!");
			
			PxRigidBodyExt::setMassAndUpdateInertia(*motor[i], legMass);
			motor[i]->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
			setShapeFlag(motor[i], PxShapeFlag::eSIMULATION_SHAPE, false);

			mMotorJoint[i] = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
				mCrabBody,		PxTransform(motorOfs,			PxQuat(-PxHalfPi, PxVec3(0,1,0))),
				motor[i],		PxTransform(PxVec3(0, 0, 0),	PxQuat(-PxHalfPi, PxVec3(0,1,0))));
			if(!mMotorJoint[i]) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");

			mMotorJoint[i]->setDriveVelocity(velocity);
			mMotorJoint[i]->setDriveForceLimit(maxForce);
			mMotorJoint[i]->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_ENABLED, true);
			mActors.push_back(motor[i]);
			mJoints.push_back(mMotorJoint[i]);
		}
		
		// create legs and attach to left and right motor
		PxReal legSpacing = crabDepth*recipNumLegsMinus1*scale;
		PxVec3 bodyToLegPos0 = PxVec3(0, 0, crabBodyDim.z);
		PxVec3 bodyToLegPos1 = PxVec3(0, 0, crabBodyDim.z-(numLegs/2)*legSpacing);
		PxVec3 motorToLegPos0 = PxVec3(0, 0, crabBodyDim.z*0.5f);
		PxVec3 motorToLegPos1 = PxVec3(0, 0, (crabBodyDim.z - legSpacing)*0.5f);
		for(PxU32 i = 0; i < numLegs/2; i++)
		{
			PxReal angle0 = -PxHalfPi + PxTwoPi*recipNumLegs*i;
			PxReal angle1 =  angle0 + PxPi;

			createLeg(mCrabBody, bodyToLegPos0, legMass, params, scale, motor[0],	motorToLegPos0 + m * PxVec3(PxCos(angle0), PxSin(angle0), 0));
			createLeg(mCrabBody, bodyToLegPos1, legMass, params, scale, motor[1],	motorToLegPos1 + m * PxVec3(PxCos(angle1), PxSin(angle1), 0));
			bodyToLegPos0.z -= legSpacing;
			bodyToLegPos1.z -= legSpacing;
			motorToLegPos0.z -= legSpacing;
			motorToLegPos1.z -= legSpacing;
		}
	}

	setupFiltering(mCrabBody, FilterGroup::eCRAB, FilterGroup::eHEIGHTFIELD);
}

void Crab::createLeg(PxRigidDynamic* mainBody, PxVec3 localPos, PxReal mass, const LegParameters& params, PxReal scale, PxRigidDynamic* motor, PxVec3 motorAttachmentPos)
{
	PxVec3 crabLegPos = mainBody->getGlobalPose().p + localPos;

	// params for Theo Jansen's machine
	// check edge ascii art in Crab.h
	static const PxReal stickExt = 0.125f * 0.5f * scale;
	const PxReal a = params.a * scale;
	const PxReal b = params.b * scale;
	const PxReal c = params.c * scale;
	const PxReal d = params.d * scale;
	const PxReal e = params.e * scale;
	const PxReal m = params.m * scale;
	const PxReal n = params.n * scale;

	const PxReal density = 1.0f;

	std::vector<PxTransform> poses;
	std::vector<const PxGeometry*> geometries;

	PxBoxGeometry boxGeomA = PxBoxGeometry(a, stickExt, stickExt);
	PxBoxGeometry boxGeomB = PxBoxGeometry(stickExt, b, stickExt);
	PxBoxGeometry boxGeomC = PxBoxGeometry(stickExt, c, stickExt);

	PxCapsuleGeometry capsGeomD = PxCapsuleGeometry(stickExt*2.0f, d);

	for(PxU32 leg = 0; leg < 2; leg++)
	{
		bool left = (leg==0);
		#define MIRROR(X) left ? -1.0f*(X) : (X)
		PxVec3 startPos = crabLegPos + PxVec3(MIRROR(e), 0, 0);

		// create upper triangle from boxes
		PxRigidDynamic* upperTriangle = NULL;
		{
			PxTransform poseA = PxTransform(PxVec3(MIRROR(a), 0, 0));
			PxTransform poseB = PxTransform(PxVec3(MIRROR(0), b, 0));
			poses.clear(); geometries.clear();
			poses.push_back(poseA); poses.push_back(poseB);
			geometries.push_back(&boxGeomA); geometries.push_back(&boxGeomB);
			upperTriangle = mSampleSubmarine->createCompound(startPos, poses, geometries, NULL, mMaterial, density)->is<PxRigidDynamic>();
			if(!upperTriangle) mSampleSubmarine->fatalError("createCompound failed!");
			mActors.push_back(upperTriangle);
		}

		// create lower triangle from boxes
		PxRigidDynamic* lowerTriangle = NULL;
		{
			PxTransform poseA = PxTransform(PxVec3(MIRROR(a), 0, 0));
			//PxTransform poseD = PxTransform(PxVec3(MIRROR(0), -d, 0));
			PxTransform poseD = PxTransform(PxVec3(MIRROR(0), -d, 0), PxQuat(PxHalfPi, PxVec3(0,0,1)));
			poses.clear(); geometries.clear();
			poses.push_back(poseA); poses.push_back(poseD);
			//geometries.push_back(&boxGeomA); geometries.push_back(&boxGeomD);
			geometries.push_back(&boxGeomA); geometries.push_back(&capsGeomD);
			lowerTriangle = mSampleSubmarine->createCompound(startPos + PxVec3(0, -2.0f*c, 0), poses, geometries, NULL, mMaterial, density)->is<PxRigidDynamic>();
			if(!lowerTriangle) mSampleSubmarine->fatalError("createCompound failed!");
			mActors.push_back(lowerTriangle);
		}

		// create vertical boxes to connect the triangles
		PxRigidDynamic* verticalBox0 = mSampleSubmarine->createBox(startPos + PxVec3(0, -c, 0), boxGeomC.halfExtents ,NULL, mMaterial, density)->is<PxRigidDynamic>();
		if(!verticalBox0) mSampleSubmarine->fatalError("createBox failed!");
		PxRigidDynamic* verticalBox1 = mSampleSubmarine->createBox(startPos + PxVec3(MIRROR(2.0f*a), -c, 0), boxGeomC.halfExtents ,NULL, mMaterial, density)->is<PxRigidDynamic>();
		if(!verticalBox1) mSampleSubmarine->fatalError("createBox failed!");
		mActors.push_back(verticalBox0);
		mActors.push_back(verticalBox1);

		// disable gravity
		upperTriangle->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
		lowerTriangle->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
		verticalBox0->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);
		verticalBox1->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, true);

		// set mass
		PxRigidBodyExt::setMassAndUpdateInertia(*upperTriangle, mass); 
		PxRigidBodyExt::setMassAndUpdateInertia(*lowerTriangle, mass); 
		PxRigidBodyExt::setMassAndUpdateInertia(*verticalBox0, mass); 
		PxRigidBodyExt::setMassAndUpdateInertia(*verticalBox1, mass); 

		// turn off collision upper triangle and vertical boxes
		setShapeFlag(upperTriangle, PxShapeFlag::eSIMULATION_SHAPE, false);
		setShapeFlag(verticalBox0, PxShapeFlag::eSIMULATION_SHAPE, false);
		setShapeFlag(verticalBox1, PxShapeFlag::eSIMULATION_SHAPE, false);

		// revolute joint in lower corner of upper triangle
		PxRevoluteJoint* joint;
		joint = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
			mainBody, PxTransform(PxVec3(MIRROR(e), 0, 0)+localPos, PxQuat(-PxHalfPi, PxVec3(0,1,0))),
			upperTriangle, PxTransform(PxVec3(0, 0, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))));
		if(!joint) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");
		mJoints.push_back(joint);

		// 4 revolute joints to connect triangles
		joint = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
			upperTriangle, PxTransform(PxVec3(0, 0, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))),
			verticalBox0, PxTransform(PxVec3(0, c, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))));
		if(!joint) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");
		mJoints.push_back(joint);

		joint = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
			upperTriangle, PxTransform(PxVec3(MIRROR(2.0f*a), 0, 0),PxQuat(-PxHalfPi, PxVec3(0,1,0))),
			verticalBox1, PxTransform(PxVec3(0, c, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))));
		if(!joint) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");
		mJoints.push_back(joint);

		joint = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
			lowerTriangle, PxTransform(PxVec3(0, 0, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))),
			verticalBox0, PxTransform(PxVec3(0, -c, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))));
		if(!joint) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");
		mJoints.push_back(joint);

		joint = PxRevoluteJointCreate(mSampleSubmarine->getPhysics(),
			lowerTriangle, PxTransform(PxVec3(MIRROR(2.0f*a), 0, 0),PxQuat(-PxHalfPi, PxVec3(0,1,0))),
			verticalBox1, PxTransform(PxVec3(0, -c, 0),				PxQuat(-PxHalfPi, PxVec3(0,1,0))));
		if(!joint) mSampleSubmarine->fatalError("PxRevoluteJointCreate failed!");
		mJoints.push_back(joint);

		// 2 distance constraints to connect motor with the triangles
		PxTransform motorTransform = PxTransform(motorAttachmentPos);
		PxReal dist0 = PxSqrt( (2.0f*b - n)*(2.0f*b - n) + (e-m)*(e-m));
		PxReal dist1 = PxSqrt( (2.0f*c + n)*(2.0f*c + n) + (e-m)*(e-m));
		
		PxDistanceJoint* distJoint0 = PxDistanceJointCreate(mSampleSubmarine->getPhysics(), upperTriangle, PxTransform(PxVec3(0, 2.0f*b, 0)), motor, motorTransform);
		if(!distJoint0) mSampleSubmarine->fatalError("PxDistanceJointCreate failed!");
		// set min & max distance to dist0
		distJoint0->setMaxDistance(dist0);
		distJoint0->setMinDistance(dist0);
		// setup damping & spring
		distJoint0->setDamping(0.1f);
		distJoint0->setStiffness(100.0f);
		distJoint0->setDistanceJointFlags(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED | PxDistanceJointFlag::eMIN_DISTANCE_ENABLED | PxDistanceJointFlag::eSPRING_ENABLED);

		PxDistanceJoint* distJoint1 = PxDistanceJointCreate(mSampleSubmarine->getPhysics(), lowerTriangle, PxTransform(PxVec3(0, 0, 0)), motor, motorTransform);
		if(!distJoint1) mSampleSubmarine->fatalError("PxDistanceJointCreate failed!");
		// set min & max distance to dist0
		distJoint1->setMaxDistance(dist1);
		distJoint1->setMinDistance(dist1);
		// setup damping & spring
		distJoint1->setDamping(0.1f);
		distJoint1->setStiffness(100.0f);
		distJoint1->setDistanceJointFlags(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED | PxDistanceJointFlag::eMIN_DISTANCE_ENABLED | PxDistanceJointFlag::eSPRING_ENABLED);

		// one distance joint to ensure that the vertical boxes do not get stuck if they cross the diagonal.
		PxReal halfDiagDist = PxSqrt(a*a + c*c);
		PxDistanceJoint* noFlip = PxDistanceJointCreate(mSampleSubmarine->getPhysics(), lowerTriangle, PxTransform(PxVec3(MIRROR(2.0f*a), 0, 0)), upperTriangle, PxTransform(PxVec3(0)));
		if(!noFlip) mSampleSubmarine->fatalError("PxDistanceJointCreate failed!");
		// set min & max distance to dist0
		noFlip->setMaxDistance(2.0f * (a+c));
		noFlip->setMinDistance(halfDiagDist);
		// setup damping & spring
		noFlip->setDamping(1.0f);
		noFlip->setStiffness(100.0f);
		noFlip->setDistanceJointFlags(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED | PxDistanceJointFlag::eMIN_DISTANCE_ENABLED | PxDistanceJointFlag::eSPRING_ENABLED);

		mJoints.push_back(distJoint0);
		mJoints.push_back(distJoint1);
		mJoints.push_back(noFlip);
	}
}


void Crab::update(PxReal dt)
{
	PxSceneWriteLock scopedLock(mSampleSubmarine->getActiveScene());

	{
		// check if I have to be reset
		PxTransform pose = mCrabBody->getGlobalPose();
		PxVec3 upVect = PxVec3(0,1,0);
		PxVec3 crabUp = pose.rotate(upVect);
		PxReal angle = upVect.dot(crabUp);
		if(angle <= 0.1f)
		{
			mRespawnMe = true;
		}
	}

	PxReal maxVelocity = 16.0f;
	PxReal velDamping = 0.8f;

	if(mRunning == 0)
		flushAccelerationBuffer();

	for(PxU32 i = 0; i < 2; i++)
	{
		PxReal prevVelocity = mMotorJoint[i]->getDriveVelocity();
		PxReal velocityChange = mAcceleration[i] ? mAcceleration[i]*dt : -prevVelocity*velDamping*dt;
		PxReal newVelocity = PxClamp(prevVelocity + velocityChange, -maxVelocity, maxVelocity);
		mMotorJoint[i]->setDriveVelocity(newVelocity);

		if(mAcceleration[i] != 0.0f)
			mCrabBody->wakeUp();

		mAcceleration[i] = 0;
	}

	// add up elapsed time
	mAccumTime += dt;

	// submit accum time to AI time before starting the PxTask
	if(mRunning == 0)
	{
		mElapsedTime = mAccumTime;
		mAccumTime = 0;
		mSubmarinePos = mSampleSubmarine->mSubmarineActor ? mSampleSubmarine->mSubmarineActor->getGlobalPose().p : PxVec3(0);

#if DEBUG_RENDERING
		// run immediately
		scanForObstacles();
		updateState();
#endif
	}
}

void Crab::run()
{
#if !DEBUG_RENDERING
	mRunning = 1;

	// run as a separate task/thread
	scanForObstacles();
	updateState();

	mRunning = 0;
#endif
}


void Crab::setAcceleration(PxReal leftAcc, PxReal rightAcc)
{
	mAccelerationBuffer[0] = -leftAcc;
	mAccelerationBuffer[1] = -rightAcc;
}


void Crab::flushAccelerationBuffer()
{
	mAcceleration[0] =  mAccelerationBuffer[0];
	mAcceleration[1] =  mAccelerationBuffer[1];
}


void Crab::scanForObstacles()
{
	PxSceneReadLock scopedLock(mSampleSubmarine->getActiveScene());

	PxTransform crabPose = mCrabBody->getGlobalPose();
	PxVec3 rayStart[2] = {PxVec3(2.0f, 0.0f, 0.0f), PxVec3(-2.0f, 0.0f, 0.0f)} ;
	rayStart[0] = crabPose.transform(rayStart[0]);
	rayStart[1] = crabPose.transform(rayStart[1]);
	PxReal rayDist = 100.0f;
	
	// setup raycasts
	// 3 front & 3 back
	for(PxU32 j = 0; j < 2; j++)
	{
		PxVec3 rayDir = crabPose.rotate(PxVec3(j?-1.0f:1.0f,0,0));
		PxQuat rotY = PxQuat(0.4f, PxVec3(0,1,0));
		rayDir = rotY.rotateInv(rayDir);
		for(PxU32 i = 0; i < 3; i++)
		{
			mSqRayBuffer->mBatchQuery->raycast(rayStart[j], rayDir, rayDist);
			rayDir = rotY.rotate(rayDir);
		}
	}

	// add submarine visibility query
	if(mSampleSubmarine->mSubmarineActor)
	{
		PxVec3 rayStart = crabPose.transform(PxVec3(0,2,0));
		PxVec3 crabToSub = mSubmarinePos - rayStart;
		mSqRayBuffer->mBatchQuery->raycast(rayStart, crabToSub.getNormalized(), rayDist);
	}

	mSqRayBuffer->mBatchQuery->execute();

	for(PxU32 i = 0; i < mSqRayBuffer->mQueryResultSize; i++)
	{
		PxRaycastQueryResult& result = mSqRayBuffer->mRayCastResults[i];
		if(result.queryStatus == PxBatchQueryStatus::eSUCCESS && result.getNbAnyHits() == 1)
		{
			const PxRaycastHit& hit = result.getAnyHit(0);
			mDistances[i] = hit.distance;

			// don't see flat terrain as wall
			SampleRenderer::RendererColor rayColor(0,0,255);
			PxReal angle = hit.normal.dot(crabPose.q.rotate(PxVec3(0,1,0)));
			if(angle > 0.98f) // = 11.5 degree difference
			{
				mDistances[i] = rayDist;
				rayColor = SampleRenderer::RendererColor(0,255,0);
			}
#if DEBUG_RENDERING
			// debug rendering
			PxU8 blue = PxU8(mDistances[i] * (255.0f/rayDist));
			const SampleRenderer::RendererColor color(255, 0, blue);
			mSampleSubmarine->getDebugRenderer()->addLine(rayStart[i<3?0:1], hit.position, color);
			mSampleSubmarine->getDebugRenderer()->addLine(hit.position, hit.position + hit.normal*3.0f, rayColor);
#endif
		}
		else
			mDistances[i] = rayDist;
	}
}

void Crab::initState(CrabState::Enum state)
{
	mCrabState = state;
	mStateTime = gDefaultStateTime[mCrabState];
}

void Crab::updateState()
{
	// update remaining time in current state
	// transition if needed
	mStateTime -= mElapsedTime;
	mElapsedTime = 0;
	if(mStateTime <= 0.0f)
	{
		initState(CrabState::eMOVE_FWD);
	}

	PxTransform crabPose;
	{
		PxSceneReadLock scopedLock(mSampleSubmarine->getActiveScene());
		crabPose = mCrabBody->getGlobalPose();
	}

	// check if we should go into panic mode
	static const PxReal subMarinePanicDist = 50.0f;
	if(mSampleSubmarine->mSubmarineActor && mCrabState != CrabState::ePANIC)
	{
		PxRaycastQueryResult& rayResult = mSqRayBuffer->mRayCastResults[6];
		if(rayResult.queryStatus == PxBatchQueryStatus::eSUCCESS && rayResult.getNbAnyHits() == 1)
		{
			const PxRaycastHit& hit = rayResult.getAnyHit(0);
			PxVec3 subToCrab = crabPose.p - mSubmarinePos;
			PxReal distanceToSubMarine = subToCrab.magnitude();
			if(hit.actor == mSampleSubmarine->mSubmarineActor && distanceToSubMarine <= subMarinePanicDist)
			{
				initState(CrabState::ePANIC);
			}
		}
	}

	PxReal leftAcc = 0, rightAcc = 0;
	// compute fwd and bkwd distances
	static const PxReal minDist = 10.0f;
	static const PxReal fullSpeedDist = 50.0f;
	static const PxReal recipFullSpeedDist = 1.0f/fullSpeedDist;
	PxReal fDist = 0, bDist = 0;
	fDist = PxMin(mDistances[0], PxMin(mDistances[1], mDistances[2]));
	bDist = PxMin(mDistances[3], PxMin(mDistances[4], mDistances[5]));

	// handle states
	if(mCrabState == CrabState::eMOVE_FWD)
	{
		if(fDist < minDist)
		{
			initState(CrabState::eMOVE_BKWD);
		}
		else
		{
			leftAcc = PxMin(fullSpeedDist, mDistances[0])*recipFullSpeedDist*2.0f - 1.0f;
			rightAcc = PxMin(fullSpeedDist, mDistances[2])*recipFullSpeedDist*2.0f - 1.0f;
			leftAcc *= 3.0f;
			rightAcc *= 3.0f;
		}
	}
	else if (mCrabState == CrabState::eMOVE_BKWD)
	{
		if(bDist < minDist)
		{
			// find rotation dir, where we have some free space
			bool rotateLeft = mDistances[0] < mDistances[2];
			initState(rotateLeft ? CrabState::eROTATE_LEFT : CrabState::eROTATE_RIGHT);
		}
		else
		{
			leftAcc = -(PxMin(fullSpeedDist, mDistances[5])*recipFullSpeedDist*2.0f - 1.0f);
			rightAcc = -(PxMin(fullSpeedDist, mDistances[3])*recipFullSpeedDist*2.0f - 1.0f);
			leftAcc *= 3.0f;
			rightAcc *= 3.0f;
		}
	}
	else if (mCrabState == CrabState::eROTATE_LEFT)
	{
		leftAcc = -3.0f;
		rightAcc = 3.0f;
		if(fDist > minDist)
		{
			initState(CrabState::eMOVE_FWD);
		}
	}
	else if (mCrabState == CrabState::eROTATE_RIGHT)
	{
		leftAcc = 3.0f;
		rightAcc = -3.0f;
		if(fDist > minDist)
		{
			initState(CrabState::eMOVE_FWD);
		}
	}
	else if (mCrabState == CrabState::ePANIC)
	{
		if(mSampleSubmarine->mSubmarineActor)
		{
			PxVec3 subToCrab = crabPose.p - mSubmarinePos;
			PxReal distanceToSubMarine = subToCrab.magnitude();
			if(distanceToSubMarine <= subMarinePanicDist)
			{
				PxVec3 dir = crabPose.q.rotateInv(subToCrab);
				dir.y = 0;
				dir.normalize();
#if DEBUG_RENDERING
				PxVec3 startPos = crabPose.p + PxVec3(0,2,0);
				mSampleSubmarine->getDebugRenderer()->addLine(startPos, startPos + crabPose.q.rotate(dir)*2.0f, SampleRenderer::RendererColor(0,255,0));
#endif
				leftAcc =  (1.0f*dir.x + 0.2f*dir.z) * 6.0f;
				rightAcc = (1.0f*dir.x - 0.2f*dir.z) * 6.0f;
			}
		}
	}
	else if (mCrabState == CrabState::eWAITING)
	{
		// have a break
	}

	// change acceleration
	setAcceleration(leftAcc, rightAcc);

#if DEBUG_RENDERING
	PxVec3 startPosL = crabPose.transform(PxVec3(0,2,-1));
	PxVec3 startPosR =  crabPose.transform(PxVec3(0,2,1));
	mSampleSubmarine->getDebugRenderer()->addLine(startPosL, startPosL + crabPose.q.rotate(PxVec3(1,0,0))*leftAcc, SampleRenderer::RendererColor(255,255,0));
	mSampleSubmarine->getDebugRenderer()->addLine(startPosR, startPosR + crabPose.q.rotate(PxVec3(1,0,0))*rightAcc, SampleRenderer::RendererColor(0,255,0));
#endif
}


SqRayBuffer::SqRayBuffer(SampleSubmarine& sampleSubmarine, PxU32 numRays, PxU32 numHits)
: mSampleSubmarine(sampleSubmarine)
, mQueryResultSize(numRays)
, mHitSize(numHits) 
{
	mOrigAddresses[0] = malloc(mQueryResultSize*sizeof(PxRaycastQueryResult) + 15);
	mOrigAddresses[1] = malloc(mHitSize*sizeof(PxRaycastHit) + 15);

	mRayCastResults = reinterpret_cast<PxRaycastQueryResult*>((size_t(mOrigAddresses[0]) + 15) & ~15);
	mRayCastHits = reinterpret_cast<PxRaycastHit*>((size_t(mOrigAddresses[1]) + 15 )& ~15);

	PxBatchQueryDesc batchQueryDesc(mQueryResultSize, 0, 0);
	batchQueryDesc.queryMemory.userRaycastResultBuffer = mRayCastResults;
	batchQueryDesc.queryMemory.userRaycastTouchBuffer = mRayCastHits;
	batchQueryDesc.queryMemory.raycastTouchBufferSize = mHitSize;

	mBatchQuery = mSampleSubmarine.getActiveScene().createBatchQuery(batchQueryDesc);
	if(!mBatchQuery) mSampleSubmarine.fatalError("createBatchQuery failed!");
}

SqRayBuffer::~SqRayBuffer()
{
	mBatchQuery->release();
	free(mOrigAddresses[0]);
	free(mOrigAddresses[1]);
}