#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER

struct b3Contact4;
struct b3ContactPoint;

class b3Dispatcher;

#include "b3TypedConstraint.h"
#include "b3ContactSolverInfo.h"
#include "b3SolverBody.h"
#include "b3SolverConstraint.h"

struct b3RigidBodyData;
struct b3InertiaData;

class b3PgsJacobiSolver
{
protected:
	b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
	b3ConstraintArray m_tmpSolverContactConstraintPool;
	b3ConstraintArray m_tmpSolverNonContactConstraintPool;
	b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
	b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;

	b3AlignedObjectArray<int> m_orderTmpConstraintPool;
	b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
	b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
	b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;

	b3AlignedObjectArray<int> m_bodyCount;
	b3AlignedObjectArray<int> m_bodyCountCheck;

	b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
	b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;

	bool m_usePgs;
	void averageVelocities();

	int m_maxOverrideNumSolverIterations;

	int m_numSplitImpulseRecoveries;

	b3Scalar getContactProcessingThreshold(b3Contact4* contact)
	{
		return 0.02f;
	}
	void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
								 b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
								 b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
								 b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);

	void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
										b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
										b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
										b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);

	b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
	b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);

	void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
								b3Vector3& rel_pos1, b3Vector3& rel_pos2);

	void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
									  b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);

	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
	unsigned long m_btSeed2;

	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);

	void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);

	void resolveSplitPenetrationSIMD(
		b3SolverBody& bodyA, b3SolverBody& bodyB,
		const b3SolverConstraint& contactConstraint);

	void resolveSplitPenetrationImpulseCacheFriendly(
		b3SolverBody& bodyA, b3SolverBody& bodyB,
		const b3SolverConstraint& contactConstraint);

	//internal method
	int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
	void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);

	void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);

	void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);

	void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);

	void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);

protected:
	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);

	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);

	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);

public:
	B3_DECLARE_ALIGNED_ALLOCATOR();

	b3PgsJacobiSolver(bool usePgs);
	virtual ~b3PgsJacobiSolver();

	//	void	solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
	void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);

	b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);

	///clear internal cached data and reset random seed
	virtual void reset();

	unsigned long b3Rand2();

	int b3RandInt2(int n);

	void setRandSeed(unsigned long seed)
	{
		m_btSeed2 = seed;
	}
	unsigned long getRandSeed() const
	{
		return m_btSeed2;
	}
};

#endif  //B3_PGS_JACOBI_SOLVER