/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 *
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 *
 * 3. This notice may not be removed or altered from any source distribution.
 */

#ifndef __AFX_BROADPHASE__INCLUDED__
#define __AFX_BROADPHASE__INCLUDED__

#include "dgBody.h"
#include "dgContact.h"
#include "hpl1/engine/libraries/newton/core/dg.h"


//#define DG_OCTREE_MAX_DEPTH       1
//#define DG_OCTREE_MAX_DEPTH       6
#define DG_OCTREE_MAX_DEPTH 7

typedef void(dgApi *OnBodiesInAABB)(dgBody *body, void *const userData);
typedef void(dgApi *OnLeavingWorldAction)(dgBody *body, dgInt32 threadIndex);

class dgCellPair {
public:
	dgBroadPhaseCell *m_cell_A;
	dgBroadPhaseCell *m_cell_B;
};

class dgSortArrayEntry {
public:
	dgBody *m_body;
	dgFloat32 m_key;
};

class dgSortArray : public dgList<dgSortArrayEntry> {
public:
	dgSortArray();
	~dgSortArray();

	void Add(dgBody *const body);
	void Remove(dgBody *const body);
	dgFloat32 Sort();
	void InvalidateCache();

	dgFloat32 RayCast(dgFloat32 minT, const dgLineBox &line, OnRayCastAction filter, OnRayPrecastAction prefilter, void *const userData) const;
	bool SanityCheck();

	dgInt8 m_index;
	dgInt8 m_isSorted;
};

class dgBroadPhaseCell {
public:
	dgBroadPhaseCell();
	~dgBroadPhaseCell();

private:
	void Init(dgInt32 layer, dgMemoryAllocator *allocator);
	void Add(dgBody *const body);
	void Remove(dgBody *const body);

	void Sort();
	void UpdateAutoPair(dgWorld *const world, dgInt32 threadIndex);

	dgSortArray m_sort[3];
	dgSortArray *m_lastSortArray;

	dgInt32 m_count;
	dgInt8 m_active;
	dgInt8 m_layerIndex;

	friend class dgBody;
	friend class dgBroadPhaseLayer;
	friend class dgBroadPhaseCollision;
	friend class dgBroadPhaseCellPairsWorkerThread;
};

class dgBroadPhaseLayer : public dgTree<dgBroadPhaseCell, dgUnsigned32> {
public:
	dgBroadPhaseLayer();
	~dgBroadPhaseLayer();

private:
	void Init(dgWorld *const world, dgFloat32 cellSize, dgInt32 layerIndex);

	inline dgUnsigned32 GetKey(dgInt32 x, dgInt32 z) const {
		dgUnsigned32 key = dgUnsigned32((z << DG_OCTREE_MAX_DEPTH) + x);
		return key;
	}

	inline void KeyToIndex(dgInt32 key, dgInt32 &x, dgInt32 &z) const {
		x = key & ((1 << DG_OCTREE_MAX_DEPTH) - 1);
		z = key >> DG_OCTREE_MAX_DEPTH;
	}

	inline dgBroadPhaseCell *Find(dgInt32 x, dgInt32 z) const {
		dgTreeNode *const node = dgTree<dgBroadPhaseCell, dgUnsigned32>::Find(GetKey(x, z));
		return node ? &node->GetInfo() : NULL;
	}
	dgBroadPhaseCell *FindCreate(dgInt32 x, dgInt32 z);

	dgWorld *m_me;
	dgFloat32 m_cellSize;
	dgFloat32 m_invCellSize;
	dgInt16 m_layerIndex;

	friend class dgBroadPhaseCollision;
};

class dgBroadPhaseApplyExternalForce : public dgWorkerThread {
public:
	virtual void ThreadExecute();

	dgInt32 m_step;
	dgInt32 m_count;
	dgInt32 m_skipForceUpdate;
	dgFloat32 m_timeStep;
	dgWorld *m_world;
	dgBody **m_bodies;
};

class dgBroadPhaseCellPairsWorkerThread : public dgWorkerThread {
public:
	virtual void ThreadExecute();

	dgInt32 m_step;
	dgInt32 m_count;
	dgWorld *m_world;
	dgCellPair *m_pairs;
};

class dgBroadPhaseCalculateContactsWorkerThread : public dgWorkerThread {
public:
	void Realloc(dgInt32 jointsCount, dgInt32 contactCount, dgInt32 threadIndex);
	virtual void ThreadExecute();

	dgInt32 m_step;
	dgInt32 m_count;
	dgInt32 m_useSimd;
	dgFloat32 m_timestep;
	dgWorld *m_world;
};

class dgBroadPhaseMaterialCallbackWorkerThread : public dgWorkerThread {
public:
	virtual void ThreadExecute();

	dgInt32 m_step;
	dgInt32 m_count;
	dgInt32 m_useSimd;
	dgFloat32 m_timestep;
	dgWorld *m_world;
	dgCollidingPairCollector::dgPair *m_pairs;
};

class dgBroadPhaseCollision {
public:
	void GetWorldSize(dgVector &p0, dgVector &p1) const;
	void SetWorldSize(const dgVector &min, const dgVector &max);
	void RayCast(const dgVector &p0, const dgVector &p1, OnRayCastAction filter, OnRayPrecastAction prefilter, void *const userData);
	dgInt32 ConvexCast(dgCollision *const shape, const dgMatrix &p0, const dgVector &p1, dgFloat32 &timetoImpact, OnRayPrecastAction prefilter, void *const userData, dgConvexCastReturnInfo *const info, dgInt32 maxContacts, dgInt32 threadIndex);
	void ForEachBodyInAABB(const dgVector &q0, const dgVector &q1, OnBodiesInAABB callback, void *const userData) const;

private:
	dgBroadPhaseCollision(dgMemoryAllocator *allocator);
	~dgBroadPhaseCollision();

	void Init();
	void Add(dgBody *const body);
	void Remove(dgBody *const body);
	void InvalidateCache();

	dgUnsigned32 UpdateContactsBroadPhaseBegin(dgFloat32 tiemstep, bool collisioUpdate, dgUnsigned32 ticks);
	void UpdateContactsBroadPhaseEnd(dgFloat32 tiemstep);

	void UpdateContacts(dgFloat32 tiemstep, bool collisioUpdate);
	void UpdateContactsSimd(dgFloat32 tiemstep, bool collisioUpdate);

	void UpdateBodyBroadphase(dgBody *const body, dgInt32 threadIndex);

	void UpdatePairs(dgBroadPhaseCell &cellA, dgBroadPhaseCell &cellB, dgInt32 threadIndex);
	void UpdatePairs(dgBody *const body0, dgSortArray::dgListNode *const listNode, dgInt32 axisX, dgInt32 threadIndex);

	dgVector m_min;
	dgVector m_max;
	dgVector m_appMinBox;
	dgVector m_appMaxBox;

	dgVector m_boxSize;
	dgBroadPhaseCell m_inactiveList;
	dgBroadPhaseLayer m_layerMap[DG_OCTREE_MAX_DEPTH];
	dgBroadPhaseApplyExternalForce m_applyExtForces[DG_MAXIMUN_THREADS];
	dgBroadPhaseCellPairsWorkerThread m_cellPairsWorkerThreads[DG_MAXIMUN_THREADS];
	dgBroadPhaseMaterialCallbackWorkerThread m_materialCallbackWorkerThreads[DG_MAXIMUN_THREADS];
	dgBroadPhaseCalculateContactsWorkerThread m_calculateContactsWorkerThreads[DG_MAXIMUN_THREADS];

	//  static void ForceAndtorque (void** const m_userParamArray, dgInt32 threadID);
	//  dgWorld* m_me;
	dgFloat32 m_worlSize;

	friend class dgBody;
	friend class dgWorld;
	friend class dgBroadPhaseCellPairsWorkerThread;
};

#endif