/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

/*
 *  Sweep and Prune adaptation/tweaks for ODE by Aras Pranckevicius.
 *  Additional work by David Walters
 *  Original code:
 *		OPCODE - Optimized Collision Detection
 *		Copyright (C) 2001 Pierre Terdiman
 *		Homepage: http://www.codercorner.com/Opcode.htm
 *
 *	This version does complete radix sort, not "classical" SAP. So, we
 *	have no temporal coherence, but are able to handle any movement
 *	velocities equally well.
 */

#include <gazebo/ode/common.h>
#include <gazebo/ode/odemath.h>
#include <gazebo/ode/matrix.h>
#include <gazebo/ode/collision_space.h>
#include <gazebo/ode/collision.h>

#include "config.h"
#include "collision_kernel.h"
#include "collision_space_internal.h"

// Reference counting helper for radix sort global data.
//static void RadixSortRef();
//static void RadixSortDeref();


// --------------------------------------------------------------------------
//  Radix Sort Context
// --------------------------------------------------------------------------

struct RaixSortContext
{
public:
	RaixSortContext(): mCurrentSize(0), mCurrentUtilization(0), mRanksValid(false), mRanksBuffer(NULL), mPrimaryRanks(NULL) {}
	~RaixSortContext() { FreeRanks(); }

	// OPCODE's Radix Sorting, returns a list of indices in sorted order
	const uint32* RadixSort( const float* input2, uint32 nb );

private:
	void FreeRanks();
	void AllocateRanks(size_t nNewSize);

	void ReallocateRanksIfNecessary(size_t nNewSize);

private:
	void SetCurrentSize(size_t nValue) { mCurrentSize = nValue; }
	size_t GetCurrentSize() const { return mCurrentSize; }

    void SetCurrentUtilization(size_t nValue) { mCurrentUtilization = nValue; }
    size_t GetCurrentUtilization() const { return mCurrentUtilization; }

	uint32 *GetRanks1() const { return mPrimaryRanks; }
	uint32 *GetRanks2() const { return mRanksBuffer + ((mRanksBuffer + mCurrentSize) - mPrimaryRanks); }
	void SwapRanks() { mPrimaryRanks = GetRanks2(); }

	bool AreRanksValid() const { return mRanksValid; }
	void InvalidateRanks() { mRanksValid = false; }
	void ValidateRanks() { mRanksValid = true; }

private:
	size_t mCurrentSize;						//!< Current size of the indices list
    size_t mCurrentUtilization;					//!< Current utilization of the indices list
	bool mRanksValid;
	uint32* mRanksBuffer;						//!< Two lists allocated sequentially in a single block
	uint32* mPrimaryRanks;
};

void RaixSortContext::AllocateRanks(size_t nNewSize)
{
	dIASSERT(GetCurrentSize() == 0);

	mRanksBuffer = new uint32[2 * nNewSize];
	mPrimaryRanks = mRanksBuffer;

	SetCurrentSize(nNewSize);
}

void RaixSortContext::FreeRanks()
{
	SetCurrentSize(0);

	delete[] mRanksBuffer;
}

void RaixSortContext::ReallocateRanksIfNecessary(size_t nNewSize)
{
	size_t nCurUtilization = GetCurrentUtilization();
	
	if (nNewSize != nCurUtilization)
	{
        size_t nCurSize = GetCurrentSize();

		if ( nNewSize > nCurSize )
		{
			// Free previously used ram
			FreeRanks();

			// Get some fresh one
			AllocateRanks(nNewSize);
		}

		InvalidateRanks();
        SetCurrentUtilization(nNewSize);
	}
}

// --------------------------------------------------------------------------
//  SAP space code
// --------------------------------------------------------------------------

struct dxSAPSpace : public dxSpace
{
	// Constructor / Destructor
	dxSAPSpace( dSpaceID _space, int sortaxis );
	~dxSAPSpace();

	// dxSpace
	virtual dxGeom* getGeom(int i);
	virtual void add(dxGeom* g);
	virtual void remove(dxGeom* g);
	virtual void dirty(dxGeom* g);
	virtual void computeAABB();
	virtual void cleanGeoms();
	virtual void collide( void *data, dNearCallback *callback );
	virtual void collide2( void *data, dxGeom *geom, dNearCallback *callback );

private:

	//--------------------------------------------------------------------------
	// Local Declarations
	//--------------------------------------------------------------------------

	//! A generic couple structure
	struct Pair
	{
		uint32 id0;	//!< First index of the pair
		uint32 id1;	//!< Second index of the pair

		// Default and Value Constructor
		Pair() {}
		Pair( uint32 i0, uint32 i1 ) : id0( i0 ), id1( i1 ) {}
	};

	//--------------------------------------------------------------------------
	// Helpers
	//--------------------------------------------------------------------------

	/**
	 *	Complete box pruning.
	 *  Returns a list of overlapping pairs of boxes, each box of the pair
	 *  belongs to the same set.
	 *
	 *	@param	count	[in] number of boxes.
	 *	@param	geoms	[in] geoms of boxes.
	 *	@param	pairs	[out] array of overlapping pairs.
	 */
	void BoxPruning( int count, const dxGeom** geoms, dArray< Pair >& pairs );


	//--------------------------------------------------------------------------
	// Implementation Data
	//--------------------------------------------------------------------------

	// We have two lists (arrays of pointers) to dirty and clean
	// geoms. Each geom knows it's index into the corresponding list
	// (see macros above).
	dArray<dxGeom*> DirtyList; // dirty geoms
	dArray<dxGeom*> GeomList;	// clean geoms

	// For SAP, we ultimately separate "normal" geoms and the ones that have
	// infinite AABBs. No point doing SAP on infinite ones (and it doesn't handle
	// infinite geoms anyway).
	dArray<dxGeom*> TmpGeomList;	// temporary for normal geoms
	dArray<dxGeom*> TmpInfGeomList;	// temporary for geoms with infinite AABBs

	// Our sorting axes. (X,Z,Y is often best). Stored *2 for minor speedup
	// Axis indices into geom's aabb are: min=idx, max=idx+1
	uint32 ax0idx;
	uint32 ax1idx;
	uint32 ax2idx;

	// pruning position array scratch pad
	// NOTE: this is float not dReal because of the OPCODE radix sorter
	dArray< float > poslist;
	RaixSortContext	sortContext;
};

// Creation
dSpaceID dSweepAndPruneSpaceCreate( dxSpace* space, int axisorder ) {
	return new dxSAPSpace( space, axisorder );
}


//==============================================================================

#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)

// HACK: We abuse 'next' and 'tome' members of dxGeom to store indice into dirty/geom lists.
#define GEOM_SET_DIRTY_IDX(g,idx) { (g)->next = (dxGeom*)(size_t)(idx); }
#define GEOM_SET_GEOM_IDX(g,idx) { (g)->tome = (dxGeom**)(size_t)(idx); }
#define GEOM_GET_DIRTY_IDX(g) ((int)(size_t)(g)->next)
#define GEOM_GET_GEOM_IDX(g) ((int)(size_t)(g)->tome)
#define GEOM_INVALID_IDX (-1)


/*
 *  A bit of repetitive work - similar to collideAABBs, but doesn't check
 *  if AABBs intersect (because SAP returns pairs with overlapping AABBs).
 */
static void collideGeomsNoAABBs( dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback )
{
	dIASSERT( (g1->gflags & GEOM_AABB_BAD)==0 );
	dIASSERT( (g2->gflags & GEOM_AABB_BAD)==0 );

	// no contacts if both geoms on the same body, and the body is not 0
	if (g1->body == g2->body && g1->body) return;

	// test if the category and collide bitfields match
	if ( ((g1->category_bits & g2->collide_bits) ||
		  (g2->category_bits & g1->collide_bits)) == 0) {
		return;
	}

	dReal *bounds1 = g1->aabb;
	dReal *bounds2 = g2->aabb;

	// check if either object is able to prove that it doesn't intersect the
	// AABB of the other
	if (g1->AABBTest (g2,bounds2) == 0) return;
	if (g2->AABBTest (g1,bounds1) == 0) return;

	// the objects might actually intersect - call the space callback function
	callback (data,g1,g2);
}


dxSAPSpace::dxSAPSpace( dSpaceID _space, int axisorder ) : dxSpace( _space )
{
	type = dSweepAndPruneSpaceClass;

	// Init AABB to infinity
	aabb[0] = -dInfinity;
	aabb[1] = dInfinity;
	aabb[2] = -dInfinity;
	aabb[3] = dInfinity;
	aabb[4] = -dInfinity;
	aabb[5] = dInfinity;

	ax0idx = ( ( axisorder ) & 3 ) << 1;
	ax1idx = ( ( axisorder >> 2 ) & 3 ) << 1;
	ax2idx = ( ( axisorder >> 4 ) & 3 ) << 1;
}

dxSAPSpace::~dxSAPSpace()
{
	CHECK_NOT_LOCKED(this);
	if ( cleanup ) {
		// note that destroying each geom will call remove()
		for ( ; DirtyList.size(); dGeomDestroy( DirtyList[ 0 ] ) ) {}
		for ( ; GeomList.size(); dGeomDestroy( GeomList[ 0 ] ) ) {}
	}
	else {
		// just unhook them
		for ( ; DirtyList.size(); remove( DirtyList[ 0 ] ) ) {}
		for ( ; GeomList.size(); remove( GeomList[ 0 ] ) ) {}
	}
}

dxGeom* dxSAPSpace::getGeom( int i )
{
	dUASSERT( i >= 0 && i < count, "index out of range" );
	int dirtySize = DirtyList.size();
	if( i < dirtySize )
		return DirtyList[i];
	else
		return GeomList[i-dirtySize];
}

void dxSAPSpace::add( dxGeom* g )
{
	CHECK_NOT_LOCKED (this);
	dAASSERT(g);
	dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");

	g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;

	// add to dirty list
	GEOM_SET_DIRTY_IDX( g, DirtyList.size() );
	GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX );
	DirtyList.push( g );

	g->parent_space = this;
	this->count++;

	dGeomMoved(this);
}

void dxSAPSpace::remove( dxGeom* g )
{
	CHECK_NOT_LOCKED(this);
	dAASSERT(g);
	dUASSERT(g->parent_space == this,"object is not in this space");

	// remove
	int dirtyIdx = GEOM_GET_DIRTY_IDX(g);
	int geomIdx = GEOM_GET_GEOM_IDX(g);
	// must be in one list, not in both
	dUASSERT(
		(dirtyIdx==GEOM_INVALID_IDX && geomIdx>=0 && geomIdx<GeomList.size()) ||
		(geomIdx==GEOM_INVALID_IDX && dirtyIdx>=0 && dirtyIdx<DirtyList.size()),
		"geom indices messed up" );
	if( dirtyIdx != GEOM_INVALID_IDX ) {
		// we're in dirty list, remove
		int dirtySize = DirtyList.size();
		dxGeom* lastG = DirtyList[dirtySize-1];
		DirtyList[dirtyIdx] = lastG;
		GEOM_SET_DIRTY_IDX(lastG,dirtyIdx);
		GEOM_SET_DIRTY_IDX(g,GEOM_INVALID_IDX);
		DirtyList.setSize( dirtySize-1 );
	} else {
		// we're in geom list, remove
		int geomSize = GeomList.size();
		dxGeom* lastG = GeomList[geomSize-1];
		GeomList[geomIdx] = lastG;
		GEOM_SET_GEOM_IDX(lastG,geomIdx);
		GEOM_SET_GEOM_IDX(g,GEOM_INVALID_IDX);
		GeomList.setSize( geomSize-1 );
	}
	count--;

	// safeguard
	g->parent_space = 0;

	// the bounding box of this space (and that of all the parents) may have
	// changed as a consequence of the removal.
	dGeomMoved(this);
}

void dxSAPSpace::dirty( dxGeom* g )
{
	dAASSERT(g);
	dUASSERT(g->parent_space == this,"object is not in this space");

	// check if already dirtied
	int dirtyIdx = GEOM_GET_DIRTY_IDX(g);
	if( dirtyIdx != GEOM_INVALID_IDX )
		return;

	int geomIdx = GEOM_GET_GEOM_IDX(g);
	dUASSERT( geomIdx>=0 && geomIdx<GeomList.size(), "geom indices messed up" );

	// remove from geom list, place last in place of this
	int geomSize = GeomList.size();
	dxGeom* lastG = GeomList[geomSize-1];
	GeomList[geomIdx] = lastG;
	GEOM_SET_GEOM_IDX(lastG,geomIdx);
	GeomList.setSize( geomSize-1 );

	// add to dirty list
	GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX );
	GEOM_SET_DIRTY_IDX( g, DirtyList.size() );
	DirtyList.push( g );
}

void dxSAPSpace::computeAABB()
{
	// TODO?
}

void dxSAPSpace::cleanGeoms()
{
	int dirtySize = DirtyList.size();
	if( !dirtySize )
		return;

	// compute the AABBs of all dirty geoms, clear the dirty flags,
	// remove from dirty list, place into geom list
	lock_count++;

	int geomSize = GeomList.size();
	GeomList.setSize( geomSize + dirtySize ); // ensure space in geom list

	for( int i = 0; i < dirtySize; ++i ) {
		dxGeom* g = DirtyList[i];
		if( IS_SPACE(g) ) {
			((dxSpace*)g)->cleanGeoms();
		}
		g->recomputeAABB();
		g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
		// remove from dirty list, add to geom list
		GEOM_SET_DIRTY_IDX( g, GEOM_INVALID_IDX );
		GEOM_SET_GEOM_IDX( g, geomSize + i );
		GeomList[geomSize+i] = g;
	}
	// clear dirty list
	DirtyList.setSize( 0 );

	lock_count--;
}

void dxSAPSpace::collide( void *_data, dNearCallback *callback )
{
	dAASSERT (callback);

	lock_count++;

	cleanGeoms();

	// by now all geoms are in GeomList, and DirtyList must be empty
	int geom_count = GeomList.size();
	dUASSERT( geom_count == count, "geom counts messed up" );

	// separate all ENABLED geoms into infinite AABBs and normal AABBs
	TmpGeomList.setSize(0);
	TmpInfGeomList.setSize(0);
	int axis0max = ax0idx + 1;
	for( int i = 0; i < geom_count; ++i ) {
		dxGeom* g = GeomList[i];
		if( !GEOM_ENABLED(g) ) // skip disabled ones
			continue;
		const dReal& amax = g->aabb[axis0max];
		if(_dequal(amax, dInfinity)) // HACK? probably not...
			TmpInfGeomList.push( g );
		else
			TmpGeomList.push( g );
	}

	// do SAP on normal AABBs
	dArray< Pair > overlapBoxes;
	int tmp_geom_count = TmpGeomList.size();
	if ( tmp_geom_count > 0 )
	{
		// Size the poslist (+1 for infinity end cap)
		poslist.setSize( tmp_geom_count + 1 );

		// Generate a list of overlapping boxes
		BoxPruning( tmp_geom_count, (const dxGeom**)TmpGeomList.data(), overlapBoxes );
	}

	// collide overlapping
	int overlapCount = overlapBoxes.size();
	for( int j = 0; j < overlapCount; ++j )
	{
		const Pair& pair = overlapBoxes[ j ];
		dxGeom* g1 = TmpGeomList[ pair.id0 ];
		dxGeom* g2 = TmpGeomList[ pair.id1 ];
		collideGeomsNoAABBs( g1, g2, _data, callback );
	}

	int infSize = TmpInfGeomList.size();
	int normSize = TmpGeomList.size();
	int m, n;

	for ( m = 0; m < infSize; ++m )
	{
		dxGeom* g1 = TmpInfGeomList[ m ];

		// collide infinite ones
		for( n = m+1; n < infSize; ++n ) {
			dxGeom* g2 = TmpInfGeomList[n];
			collideGeomsNoAABBs( g1, g2, _data, callback );
		}

		// collide infinite ones with normal ones
		for( n = 0; n < normSize; ++n ) {
			dxGeom* g2 = TmpGeomList[n];
			collideGeomsNoAABBs( g1, g2, _data, callback );
		}
	}

	lock_count--;
}

void dxSAPSpace::collide2( void *_data, dxGeom *geom, dNearCallback *callback )
{
	dAASSERT (geom && callback);

	// TODO: This is just a simple N^2 implementation

	lock_count++;

	cleanGeoms();
	geom->recomputeAABB();

	// intersect bounding boxes
	int geom_count = GeomList.size();
	for ( int i = 0; i < geom_count; ++i ) {
		dxGeom* g = GeomList[i];
		if ( GEOM_ENABLED(g) )
			collideAABBs (g,geom,_data,callback);
	}

	lock_count--;
}


void dxSAPSpace::BoxPruning( int _count, const dxGeom** geoms, dArray< Pair >& pairs )
{
	// 1) Build main list using the primary axis
	//  NOTE: uses floats instead of dReals because that's what radix sort wants
	for( int i = 0; i < _count; ++i )
		poslist[ i ] = (float)TmpGeomList[i]->aabb[ ax0idx ];
	poslist[ _count++ ] = FLT_MAX;

	// 2) Sort the list
	const uint32* Sorted = sortContext.RadixSort( poslist.data(), _count );

	// 3) Prune the list
	const uint32* const LastSorted = Sorted + _count;
	const uint32* RunningAddress = Sorted;

	Pair IndexPair;
	while ( RunningAddress < LastSorted && Sorted < LastSorted )
	{
		IndexPair.id0 = *Sorted++;

		// empty, this loop just advances RunningAddress
		while ( poslist[*RunningAddress++] < poslist[IndexPair.id0] ) {}

		if ( RunningAddress < LastSorted )
		{
			const uint32* RunningAddress2 = RunningAddress;

			const dReal idx0ax0max = geoms[IndexPair.id0]->aabb[ax0idx+1];
			const dReal idx0ax1max = geoms[IndexPair.id0]->aabb[ax1idx+1];
			const dReal idx0ax2max = geoms[IndexPair.id0]->aabb[ax2idx+1];

			while ( poslist[ IndexPair.id1 = *RunningAddress2++ ] <= idx0ax0max )
			{
				const dReal* aabb0 = geoms[ IndexPair.id0 ]->aabb;
				const dReal* aabb1 = geoms[ IndexPair.id1 ]->aabb;

				// Intersection?
				if ( idx0ax1max >= aabb1[ax1idx] && aabb1[ax1idx+1] >= aabb0[ax1idx] )
				if ( idx0ax2max >= aabb1[ax2idx] && aabb1[ax2idx+1] >= aabb0[ax2idx] )
				{
					pairs.push( IndexPair );
				}
			}
		}

	}; // while ( RunningAddress < LastSorted && Sorted < LastSorted )
}


//==============================================================================

//------------------------------------------------------------------------------
// Radix Sort
//------------------------------------------------------------------------------



#define CHECK_PASS_VALIDITY(pass)															\
	/* Shortcut to current counters */														\
	uint32* CurCount = &mHistogram[pass<<8];												\
																							\
	/* Reset flag. The sorting pass is supposed to be performed. (default) */				\
	bool PerformPass = true;																\
																							\
	/* Check pass validity */																\
																							\
	/* If all values have the same byte, sorting is useless. */								\
	/* It may happen when sorting bytes or words instead of dwords. */						\
	/* This routine actually sorts words faster than dwords, and bytes */					\
	/* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */				\
	/* for words and O(n) for bytes. Running time for floats depends on actual values... */	\
																							\
	/* Get first byte */																	\
	uint8 UniqueVal = *(((uint8*)input)+pass);												\
																							\
	/* Check that byte's counter */															\
	if(CurCount[UniqueVal]==nb)	PerformPass=false;

// WARNING ONLY SORTS IEEE FLOATING-POINT VALUES
const uint32* RaixSortContext::RadixSort( const float* input2, uint32 nb )
{
	uint32* input = (uint32*)input2;

	// Resize lists if needed
	ReallocateRanksIfNecessary(nb);

	// Allocate histograms & offsets on the stack
	uint32 mHistogram[256*4];
	uint32* mLink[256];

	// Create histograms (counters). Counters for all passes are created in one run.
	// Pros:	read input buffer once instead of four times
	// Cons:	mHistogram is 4Kb instead of 1Kb
	// Floating-point values are always supposed to be signed values, so there's only one code path there.
	// Please note the floating point comparison needed for temporal coherence! Although the resulting asm code
	// is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first
	// generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just
	// wouldn't work with mixed positive/negative values....
	{
		/* Clear counters/histograms */
		memset(mHistogram, 0, 256*4*sizeof(uint32));

		/* Prepare to count */
		uint8* p = (uint8*)input;
		uint8* pe = &p[nb*4];
		uint32* h0= &mHistogram[0];		/* Histogram for first pass (LSB)	*/
		uint32* h1= &mHistogram[256];	/* Histogram for second pass		*/
		uint32* h2= &mHistogram[512];	/* Histogram for third pass			*/
		uint32* h3= &mHistogram[768];	/* Histogram for last pass (MSB)	*/

		bool AlreadySorted = true;	/* Optimism... */

		if (!AreRanksValid())
		{
			/* Prepare for temporal coherence */
			float* Running = (float*)input2;
			float PrevVal = *Running;

			while(p!=pe)
			{
				/* Read input input2 in previous sorted order */
				float Val = *Running++;
				/* Check whether already sorted or not */
				if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */
				/* Update for next iteration */
				PrevVal = Val;

				/* Create histograms */
				h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
			}

			/* If all input values are already sorted, we just have to return and leave the */
			/* previous list unchanged. That way the routine may take advantage of temporal */
			/* coherence, for example when used to sort transparent faces.					*/
			if(AlreadySorted)
			{
				uint32* const Ranks1 = GetRanks1();
				for(uint32 i=0;i<nb;i++)	Ranks1[i] = i;
				return Ranks1;
			}
		}
		else
		{
			/* Prepare for temporal coherence */
			uint32* const Ranks1 = GetRanks1();

			uint32* Indices = Ranks1;
			float PrevVal = (float)input2[*Indices];

			while(p!=pe)
			{
				/* Read input input2 in previous sorted order */
				float Val = (float)input2[*Indices++];
				/* Check whether already sorted or not */
				if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */
				/* Update for next iteration */
				PrevVal = Val;

				/* Create histograms */
				h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
			}

			/* If all input values are already sorted, we just have to return and leave the */
			/* previous list unchanged. That way the routine may take advantage of temporal */
			/* coherence, for example when used to sort transparent faces.					*/
			if(AlreadySorted)	{ return Ranks1;	}
		}

		/* Else there has been an early out and we must finish computing the histograms */
		while(p!=pe)
		{
			/* Create histograms without the previous overhead */
			h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
		}
	}

	// Compute #negative values involved if needed
	uint32 NbNegativeValues = 0;

	// An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
	// last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
	// responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
	uint32* h3= &mHistogram[768];
	for(uint32 i=128;i<256;i++)	NbNegativeValues += h3[i];	// 768 for last histogram, 128 for negative part

	// Radix sort, j is the pass number (0=LSB, 3=MSB)
	for(uint32 j=0;j<4;j++)
	{
		// Should we care about negative values?
		if(j!=3)
		{
			// Here we deal with positive values only
			CHECK_PASS_VALIDITY(j);

			if(PerformPass)
			{
				uint32* const Ranks2 = GetRanks2();
				// Create offsets
				mLink[0] = Ranks2;
				for(uint32 i=1;i<256;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];

				// Perform Radix Sort
				uint8* InputBytes = (uint8*)input;
				InputBytes += j;
				if (!AreRanksValid())
				{
					for(uint32 i=0;i<nb;i++)
					{
						*mLink[InputBytes[i<<2]]++ = i;
					}

					ValidateRanks();
				}
				else
				{
					uint32* const Ranks1 = GetRanks1();

					uint32* Indices				= Ranks1;
					uint32* const IndicesEnd	= Ranks1 + nb;
					while(Indices!=IndicesEnd)
					{
						uint32 id = *Indices++;
						*mLink[InputBytes[id<<2]]++ = id;
					}
				}

				// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
				SwapRanks();
			}
		}
		else
		{
			// This is a special case to correctly handle negative values
			CHECK_PASS_VALIDITY(j);

			if(PerformPass)
			{
				uint32* const Ranks2 = GetRanks2();

				// Create biased offsets, in order for negative numbers to be sorted as well
				mLink[0] = Ranks2 + NbNegativeValues;										// First positive number takes place after the negative ones
				for(uint32 i=1;i<128;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];		// 1 to 128 for positive numbers

				// We must reverse the sorting order for negative numbers!
				mLink[255] = Ranks2;
				for(uint32 i=0;i<127;i++)	mLink[254-i] = mLink[255-i] + CurCount[255-i];		// Fixing the wrong order for negative values
				for(uint32 i=128;i<256;i++)	mLink[i] += CurCount[i];							// Fixing the wrong place for negative values

				// Perform Radix Sort
				if (!AreRanksValid())
				{
					for(uint32 i=0;i<nb;i++)
					{
						uint32 Radix = input[i]>>24;							// Radix byte, same as above. AND is useless here (uint32).
						// ### cmp to be killed. Not good. Later.
						if(Radix<128)		*mLink[Radix]++ = i;		// Number is positive, same as above
						else				*(--mLink[Radix]) = i;		// Number is negative, flip the sorting order
					}

					ValidateRanks();
				}
				else
				{
					uint32* const Ranks1 = GetRanks1();

					for(uint32 i=0;i<nb;i++)
					{
						uint32 Radix = input[Ranks1[i]]>>24;							// Radix byte, same as above. AND is useless here (uint32).
						// ### cmp to be killed. Not good. Later.
						if(Radix<128)		*mLink[Radix]++ = Ranks1[i];		// Number is positive, same as above
						else				*(--mLink[Radix]) = Ranks1[i];		// Number is negative, flip the sorting order
					}
				}
				// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
				SwapRanks();
			}
			else
			{
				// The pass is useless, yet we still have to reverse the order of current list if all values are negative.
				if(UniqueVal>=128)
				{
					if (!AreRanksValid())
					{
						uint32* const Ranks2 = GetRanks2();
						// ###Possible?
						for(uint32 i=0;i<nb;i++)
						{
							Ranks2[i] = nb-i-1;
						}

						ValidateRanks();
					}
					else
					{
						uint32* const Ranks1 = GetRanks1();
						uint32* const Ranks2 = GetRanks2();
						for(uint32 i=0;i<nb;i++)	Ranks2[i] = Ranks1[nb-i-1];
					}

					// Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
					SwapRanks();
				}
			}
		}
	}

	// Return indices
	uint32* const Ranks1 = GetRanks1();
	return Ranks1;
}

