/*
Copyright (c) 2012 Advanced Micro Devices, Inc.  

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.
*/
//Originally written by Takahiro Harada

#include "b3Solver.h"

///useNewBatchingKernel  is a rewritten kernel using just a single thread of the warp, for experiments
bool useNewBatchingKernel = true;
bool gConvertConstraintOnCpu = false;

#define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl"
#define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl"
#define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl"
#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl"
#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl"
#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl"

#include "Bullet3Dynamics/shared/b3ConvertConstraint4.h"

#include "kernels/solverSetup.h"
#include "kernels/solverSetup2.h"

#include "kernels/solveContact.h"
#include "kernels/solveFriction.h"

#include "kernels/batchingKernels.h"
#include "kernels/batchingKernelsNew.h"

#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
#include "Bullet3Common/b3Vector3.h"

struct SolverDebugInfo
{
	int m_valInt0;
	int m_valInt1;
	int m_valInt2;
	int m_valInt3;

	int m_valInt4;
	int m_valInt5;
	int m_valInt6;
	int m_valInt7;

	int m_valInt8;
	int m_valInt9;
	int m_valInt10;
	int m_valInt11;

	int m_valInt12;
	int m_valInt13;
	int m_valInt14;
	int m_valInt15;

	float m_val0;
	float m_val1;
	float m_val2;
	float m_val3;
};

class SolverDeviceInl
{
public:
	struct ParallelSolveData
	{
		b3OpenCLArray<unsigned int>* m_numConstraints;
		b3OpenCLArray<unsigned int>* m_offsets;
	};
};

b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity)
	: m_context(ctx),
	  m_device(device),
	  m_queue(queue),
	  m_batchSizes(ctx, queue),
	  m_nIterations(4)
{
	m_sort32 = new b3RadixSort32CL(ctx, device, queue);
	m_scan = new b3PrefixScanCL(ctx, device, queue, B3_SOLVER_N_CELLS);
	m_search = new b3BoundSearchCL(ctx, device, queue, B3_SOLVER_N_CELLS);

	const int sortSize = B3NEXTMULTIPLEOF(pairCapacity, 512);

	m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx, queue, sortSize);
	m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx, queue);

	m_numConstraints = new b3OpenCLArray<unsigned int>(ctx, queue, B3_SOLVER_N_CELLS);
	m_numConstraints->resize(B3_SOLVER_N_CELLS);

	m_offsets = new b3OpenCLArray<unsigned int>(ctx, queue, B3_SOLVER_N_CELLS);
	m_offsets->resize(B3_SOLVER_N_CELLS);
	const char* additionalMacros = "";
	//	const char* srcFileNameForCaching="";

	cl_int pErrNum;
	const char* batchKernelSource = batchingKernelsCL;
	const char* batchKernelNewSource = batchingKernelsNewCL;

	const char* solverSetupSource = solverSetupCL;
	const char* solverSetup2Source = solverSetup2CL;
	const char* solveContactSource = solveContactCL;
	const char* solveFrictionSource = solveFrictionCL;

	{
		cl_program solveContactProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveContactSource, &pErrNum, additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH);
		b3Assert(solveContactProg);

		cl_program solveFrictionProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveFrictionSource, &pErrNum, additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH);
		b3Assert(solveFrictionProg);

		cl_program solverSetup2Prog = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetup2Source, &pErrNum, additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH);
		b3Assert(solverSetup2Prog);

		cl_program solverSetupProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetupSource, &pErrNum, additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH);
		b3Assert(solverSetupProg);

		m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg, additionalMacros);
		b3Assert(m_solveFrictionKernel);

		m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg, additionalMacros);
		b3Assert(m_solveContactKernel);

		m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg, additionalMacros);
		b3Assert(m_contactToConstraintKernel);

		m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog, additionalMacros);
		b3Assert(m_setSortDataKernel);

		m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog, additionalMacros);
		b3Assert(m_reorderContactKernel);

		m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog, additionalMacros);
		b3Assert(m_copyConstraintKernel);
	}

	{
		cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelSource, &pErrNum, additionalMacros, B3_BATCHING_PATH);
		//cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true);
		b3Assert(batchingProg);

		m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg, additionalMacros);
		b3Assert(m_batchingKernel);
	}
	{
		cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelNewSource, &pErrNum, additionalMacros, B3_BATCHING_NEW_PATH);
		b3Assert(batchingNewProg);

		m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg, additionalMacros);
		//m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros );
		b3Assert(m_batchingKernelNew);
	}
}

b3Solver::~b3Solver()
{
	delete m_offsets;
	delete m_numConstraints;
	delete m_sortDataBuffer;
	delete m_contactBuffer2;

	delete m_sort32;
	delete m_scan;
	delete m_search;

	clReleaseKernel(m_batchingKernel);
	clReleaseKernel(m_batchingKernelNew);

	clReleaseKernel(m_solveContactKernel);
	clReleaseKernel(m_solveFrictionKernel);

	clReleaseKernel(m_contactToConstraintKernel);
	clReleaseKernel(m_setSortDataKernel);
	clReleaseKernel(m_reorderContactKernel);
	clReleaseKernel(m_copyConstraintKernel);
}

template <bool JACOBI>
static __inline void solveContact(b3GpuConstraint4& cs,
								  const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
								  const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
								  float maxRambdaDt[4], float minRambdaDt[4])
{
	b3Vector3 dLinVelA;
	dLinVelA.setZero();
	b3Vector3 dAngVelA;
	dAngVelA.setZero();
	b3Vector3 dLinVelB;
	dLinVelB.setZero();
	b3Vector3 dAngVelB;
	dAngVelB.setZero();

	for (int ic = 0; ic < 4; ic++)
	{
		//	dont necessary because this makes change to 0
		if (cs.m_jacCoeffInv[ic] == 0.f) continue;

		{
			b3Vector3 angular0, angular1, linear;
			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
			setLinearAndAngular((const b3Vector3&)cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, &linear, &angular0, &angular1);

			float rambdaDt = calcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
										linVelA, angVelA, linVelB, angVelB) +
							 cs.m_b[ic];
			rambdaDt *= cs.m_jacCoeffInv[ic];

			{
				float prevSum = cs.m_appliedRambdaDt[ic];
				float updated = prevSum;
				updated += rambdaDt;
				updated = b3Max(updated, minRambdaDt[ic]);
				updated = b3Min(updated, maxRambdaDt[ic]);
				rambdaDt = updated - prevSum;
				cs.m_appliedRambdaDt[ic] = updated;
			}

			b3Vector3 linImp0 = invMassA * linear * rambdaDt;
			b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
			b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
			b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
			b3Assert(_finite(linImp0.getX()));
			b3Assert(_finite(linImp1.getX()));
#endif
			if (JACOBI)
			{
				dLinVelA += linImp0;
				dAngVelA += angImp0;
				dLinVelB += linImp1;
				dAngVelB += angImp1;
			}
			else
			{
				linVelA += linImp0;
				angVelA += angImp0;
				linVelB += linImp1;
				angVelB += angImp1;
			}
		}
	}

	if (JACOBI)
	{
		linVelA += dLinVelA;
		angVelA += dAngVelA;
		linVelB += dLinVelB;
		angVelB += dAngVelB;
	}
}

static __inline void solveFriction(b3GpuConstraint4& cs,
								   const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
								   const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
								   float maxRambdaDt[4], float minRambdaDt[4])
{
	if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
	const b3Vector3& center = (const b3Vector3&)cs.m_center;

	b3Vector3 n = -(const b3Vector3&)cs.m_linear;

	b3Vector3 tangent[2];
#if 1
	b3PlaneSpace1(n, tangent[0], tangent[1]);
#else
	b3Vector3 r = cs.m_worldPos[0] - center;
	tangent[0] = cross3(n, r);
	tangent[1] = cross3(tangent[0], n);
	tangent[0] = normalize3(tangent[0]);
	tangent[1] = normalize3(tangent[1]);
#endif

	b3Vector3 angular0, angular1, linear;
	b3Vector3 r0 = center - posA;
	b3Vector3 r1 = center - posB;
	for (int i = 0; i < 2; i++)
	{
		setLinearAndAngular(tangent[i], r0, r1, &linear, &angular0, &angular1);
		float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
									linVelA, angVelA, linVelB, angVelB);
		rambdaDt *= cs.m_fJacCoeffInv[i];

		{
			float prevSum = cs.m_fAppliedRambdaDt[i];
			float updated = prevSum;
			updated += rambdaDt;
			updated = b3Max(updated, minRambdaDt[i]);
			updated = b3Min(updated, maxRambdaDt[i]);
			rambdaDt = updated - prevSum;
			cs.m_fAppliedRambdaDt[i] = updated;
		}

		b3Vector3 linImp0 = invMassA * linear * rambdaDt;
		b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
		b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
		b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
#ifdef _WIN32
		b3Assert(_finite(linImp0.getX()));
		b3Assert(_finite(linImp1.getX()));
#endif
		linVelA += linImp0;
		angVelA += angImp0;
		linVelB += linImp1;
		angVelB += angImp1;
	}

	{  //	angular damping for point constraint
		b3Vector3 ab = (posB - posA).normalized();
		b3Vector3 ac = (center - posA).normalized();
		if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
		{
			float angNA = b3Dot(n, angVelA);
			float angNB = b3Dot(n, angVelB);

			angVelA -= (angNA * 0.1f) * n;
			angVelB -= (angNB * 0.1f) * n;
		}
	}
}
/*
 b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
	b3AlignedObjectArray<b3InertiaData>& m_shapes;
	b3AlignedObjectArray<b3GpuConstraint4>& m_constraints;
	b3AlignedObjectArray<int>* m_batchSizes;
	int m_cellIndex;
	int m_curWgidx;
	int m_start;
	int m_nConstraints;
	bool m_solveFriction;
	int m_maxNumBatches;
 */

struct SolveTask  // : public ThreadPool::Task
{
	SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, b3AlignedObjectArray<b3InertiaData>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints,
			  int start, int nConstraints, int maxNumBatches, b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx, b3AlignedObjectArray<int>* batchSizes, int cellIndex)
		: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_batchSizes(batchSizes), m_cellIndex(cellIndex), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
	{
	}

	unsigned short int getType() { return 0; }

	void run(int tIdx)
	{
		int offset = 0;
		for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++)
		{
			int numInBatch = m_batchSizes->at(m_cellIndex * B3_MAX_NUM_BATCHES + ii);
			if (!numInBatch)
				break;

			for (int jj = 0; jj < numInBatch; jj++)
			{
				int i = m_start + offset + jj;
				int batchId = m_constraints[i].m_batchIdx;
				b3Assert(batchId == ii);
				float frictionCoeff = m_constraints[i].getFrictionCoeff();
				int aIdx = (int)m_constraints[i].m_bodyA;
				int bIdx = (int)m_constraints[i].m_bodyB;
				//				int localBatch = m_constraints[i].m_batchIdx;
				b3RigidBodyData& bodyA = m_bodies[aIdx];
				b3RigidBodyData& bodyB = m_bodies[bIdx];

				if (!m_solveFriction)
				{
					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};

					solveContact<false>(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
										(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
										maxRambdaDt, minRambdaDt);
				}
				else
				{
					float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
					float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
					float sum = 0;
					for (int j = 0; j < 4; j++)
					{
						sum += m_constraints[i].m_appliedRambdaDt[j];
					}
					frictionCoeff = 0.7f;
					for (int j = 0; j < 4; j++)
					{
						maxRambdaDt[j] = frictionCoeff * sum;
						minRambdaDt[j] = -maxRambdaDt[j];
					}
					solveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
								  (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
								  maxRambdaDt, minRambdaDt);
				}
			}
			offset += numInBatch;
		}
		/*		for (int bb=0;bb<m_maxNumBatches;bb++)
		{
			//for(int ic=m_nConstraints-1; ic>=0; ic--)
			for(int ic=0; ic<m_nConstraints; ic++)
			{
				
				int i = m_start + ic;
				if (m_constraints[i].m_batchIdx != bb)
					continue;

				float frictionCoeff = m_constraints[i].getFrictionCoeff();
				int aIdx = (int)m_constraints[i].m_bodyA;
				int bIdx = (int)m_constraints[i].m_bodyB;
				int localBatch = m_constraints[i].m_batchIdx;
				b3RigidBodyData& bodyA = m_bodies[aIdx];
				b3RigidBodyData& bodyB = m_bodies[bIdx];

				if( !m_solveFriction )
				{
					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

					solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
				}
				else
				{
					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
					float sum = 0;
					for(int j=0; j<4; j++)
					{
						sum +=m_constraints[i].m_appliedRambdaDt[j];
					}
					frictionCoeff = 0.7f;
					for(int j=0; j<4; j++)
					{
						maxRambdaDt[j] = frictionCoeff*sum;
						minRambdaDt[j] = -maxRambdaDt[j];
					}
					solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
						maxRambdaDt, minRambdaDt );
			
				}
			}
		}
		*/
	}

	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
	b3AlignedObjectArray<b3InertiaData>& m_shapes;
	b3AlignedObjectArray<b3GpuConstraint4>& m_constraints;
	b3AlignedObjectArray<int>* m_batchSizes;
	int m_cellIndex;
	int m_curWgidx;
	int m_start;
	int m_nConstraints;
	bool m_solveFriction;
	int m_maxNumBatches;
};

void b3Solver::solveContactConstraintHost(b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf,
										  b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, b3AlignedObjectArray<int>* batchSizes)
{
#if 0
	{	
		int nSplitX = B3_SOLVER_N_SPLIT_X;
		int nSplitY = B3_SOLVER_N_SPLIT_Y;
		int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES;
		for (int z=0;z<4;z++)
		{
			for (int y=0;y<4;y++)
			{
				for (int x=0;x<4;x++)
				{
					int newIndex = (x+y*nSplitX+z*nSplitX*nSplitY);
				//	printf("newIndex=%d\n",newIndex);

					int zIdx = newIndex/(nSplitX*nSplitY);
					int remain = newIndex%(nSplitX*nSplitY);
					int yIdx = remain/nSplitX;
					int xIdx = remain%nSplitX;
				//	printf("newIndex=%d\n",newIndex);
				}
			}
		}

		//for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--)
		for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++)
		{
			for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++)
			{
				int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2);
				int remain= (wgIdx%((nSplitX*nSplitY)/4));
				int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1);
				int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1);
				
				/*int zIdx = newIndex/(nSplitX*nSplitY);
				int remain = newIndex%(nSplitX*nSplitY);
				int yIdx = remain/nSplitX;
				int xIdx = remain%nSplitX;
				*/
				int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY);
			//	printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch);
			}
		}
	}
#endif

	b3AlignedObjectArray<b3RigidBodyData> bodyNative;
	bodyBuf->copyToHost(bodyNative);
	b3AlignedObjectArray<b3InertiaData> shapeNative;
	shapeBuf->copyToHost(shapeNative);
	b3AlignedObjectArray<b3GpuConstraint4> constraintNative;
	constraint->copyToHost(constraintNative);

	b3AlignedObjectArray<unsigned int> numConstraintsHost;
	m_numConstraints->copyToHost(numConstraintsHost);

	//printf("------------------------\n");
	b3AlignedObjectArray<unsigned int> offsetsHost;
	m_offsets->copyToHost(offsetsHost);
	static int frame = 0;
	bool useBatches = true;
	if (useBatches)
	{
		for (int iter = 0; iter < m_nIterations; iter++)
		{
			for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++)
			{
				int nSplitX = B3_SOLVER_N_SPLIT_X;
				int nSplitY = B3_SOLVER_N_SPLIT_Y;
				int numWorkgroups = B3_SOLVER_N_CELLS / B3_SOLVER_N_BATCHES;
				//printf("cell Batch %d\n",cellBatch);
				b3AlignedObjectArray<int> usedBodies[B3_SOLVER_N_CELLS];
				for (int i = 0; i < B3_SOLVER_N_CELLS; i++)
				{
					usedBodies[i].resize(0);
				}

				//for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--)
				for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++)
				{
					int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 2);
					int remain = (wgIdx % ((nSplitX * nSplitY) / 4));
					int yIdx = (remain / (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1);
					int xIdx = (remain % (nSplitX / 2)) * 2 + (cellBatch & 1);
					int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY);

					if (numConstraintsHost[cellIdx] == 0)
						continue;

					//printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch);
					//printf("cell %d has %d constraints\n", cellIdx,numConstraintsHost[cellIdx]);
					if (zIdx)
					{
						//printf("?\n");
					}

					if (iter == 0)
					{
						//printf("frame=%d, Cell xIdx=%x, yIdx=%d ",frame, xIdx,yIdx);
						//printf("cellBatch=%d, wgIdx=%d, #constraints in cell=%d\n",cellBatch,wgIdx,numConstraintsHost[cellIdx]);
					}
					const int start = offsetsHost[cellIdx];
					int numConstraintsInCell = numConstraintsHost[cellIdx];
					//				const int end = start + numConstraintsInCell;

					SolveTask task(bodyNative, shapeNative, constraintNative, start, numConstraintsInCell, maxNumBatches, usedBodies, wgIdx, batchSizes, cellIdx);
					task.m_solveFriction = false;
					task.run(0);
				}
			}
		}

		for (int iter = 0; iter < m_nIterations; iter++)
		{
			for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++)
			{
				int nSplitX = B3_SOLVER_N_SPLIT_X;
				int nSplitY = B3_SOLVER_N_SPLIT_Y;

				int numWorkgroups = B3_SOLVER_N_CELLS / B3_SOLVER_N_BATCHES;

				for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++)
				{
					int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 2);
					int remain = (wgIdx % ((nSplitX * nSplitY) / 4));
					int yIdx = (remain / (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1);
					int xIdx = (remain % (nSplitX / 2)) * 2 + (cellBatch & 1);

					int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY);

					if (numConstraintsHost[cellIdx] == 0)
						continue;

					//printf("yIdx=%d\n",yIdx);

					const int start = offsetsHost[cellIdx];
					int numConstraintsInCell = numConstraintsHost[cellIdx];
					//				const int end = start + numConstraintsInCell;

					SolveTask task(bodyNative, shapeNative, constraintNative, start, numConstraintsInCell, maxNumBatches, 0, 0, batchSizes, cellIdx);
					task.m_solveFriction = true;
					task.run(0);
				}
			}
		}
	}
	else
	{
		for (int iter = 0; iter < m_nIterations; iter++)
		{
			SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0);
			task.m_solveFriction = false;
			task.run(0);
		}

		for (int iter = 0; iter < m_nIterations; iter++)
		{
			SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0);
			task.m_solveFriction = true;
			task.run(0);
		}
	}

	bodyBuf->copyFromHost(bodyNative);
	shapeBuf->copyFromHost(shapeNative);
	constraint->copyFromHost(constraintNative);
	frame++;
}

void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
						  const b3OpenCLArray<b3InertiaData>* shapeBuf,
						  b3OpenCLArray<b3GpuConstraint4>* constraint,
						  b3OpenCLArray<unsigned int>* m_numConstraints,
						  b3OpenCLArray<unsigned int>* m_offsets,
						  int batchId)
{
	//						b3BufferInfoCL( m_numConstraints->getBufferCL() ),
	//						b3BufferInfoCL( m_offsets->getBufferCL() )

	int cellBatch = batchId;
	const int nn = B3_SOLVER_N_CELLS;
	//	int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES;

	b3AlignedObjectArray<unsigned int> gN;
	m_numConstraints->copyToHost(gN);
	b3AlignedObjectArray<unsigned int> gOffsets;
	m_offsets->copyToHost(gOffsets);
	int nSplitX = B3_SOLVER_N_SPLIT_X;
	int nSplitY = B3_SOLVER_N_SPLIT_Y;

	//	int bIdx = batchId;

	b3AlignedObjectArray<b3GpuConstraint4> cpuConstraints;
	constraint->copyToHost(cpuConstraints);

	printf("batch = %d\n", batchId);

	int numWorkgroups = nn / B3_SOLVER_N_BATCHES;
	b3AlignedObjectArray<int> usedBodies;

	for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++)
	{
		printf("wgIdx = %d           ", wgIdx);

		int zIdx = (wgIdx / ((nSplitX * nSplitY)) / 2) * 2 + ((cellBatch & 4) >> 2);
		int remain = wgIdx % ((nSplitX * nSplitY));
		int yIdx = (remain % (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1);
		int xIdx = (remain / (nSplitX / 2)) * 2 + (cellBatch & 1);

		int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY);
		printf("cellIdx=%d\n", cellIdx);
		if (gN[cellIdx] == 0)
			continue;

		const int start = gOffsets[cellIdx];
		const int end = start + gN[cellIdx];

		for (int c = start; c < end; c++)
		{
			b3GpuConstraint4& constraint = cpuConstraints[c];
			//printf("constraint (%d,%d)\n", constraint.m_bodyA,constraint.m_bodyB);
			if (usedBodies.findLinearSearch(constraint.m_bodyA) < usedBodies.size())
			{
				printf("error?\n");
			}
			if (usedBodies.findLinearSearch(constraint.m_bodyB) < usedBodies.size())
			{
				printf("error?\n");
			}
		}

		for (int c = start; c < end; c++)
		{
			b3GpuConstraint4& constraint = cpuConstraints[c];
			usedBodies.push_back(constraint.m_bodyA);
			usedBodies.push_back(constraint.m_bodyB);
		}
	}
}

static bool verify = false;

void b3Solver::solveContactConstraint(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf,
									  b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches)
{
	b3Int4 cdata = b3MakeInt4(n, 0, 0, 0);
	{
		const int nn = B3_SOLVER_N_CELLS;

		cdata.x = 0;
		cdata.y = maxNumBatches;  //250;

		int numWorkItems = 64 * nn / B3_SOLVER_N_BATCHES;
#ifdef DEBUG_ME
		SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems];
		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device, numWorkItems);
#endif

		{
			B3_PROFILE("m_batchSolveKernel iterations");
			for (int iter = 0; iter < m_nIterations; iter++)
			{
				for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++)
				{
					if (verify)
					{
						checkConstraintBatch(bodyBuf, shapeBuf, constraint, m_numConstraints, m_offsets, ib);
					}

#ifdef DEBUG_ME
					memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems);
					gpuDebugInfo.write(debugInfo, numWorkItems);
#endif

					cdata.z = ib;

					b3LauncherCL launcher(m_queue, m_solveContactKernel, "m_solveContactKernel");
#if 1

					b3BufferInfoCL bInfo[] = {

						b3BufferInfoCL(bodyBuf->getBufferCL()),
						b3BufferInfoCL(shapeBuf->getBufferCL()),
						b3BufferInfoCL(constraint->getBufferCL()),
						b3BufferInfoCL(m_numConstraints->getBufferCL()),
						b3BufferInfoCL(m_offsets->getBufferCL())
#ifdef DEBUG_ME
							,
						b3BufferInfoCL(&gpuDebugInfo)
#endif
					};

					launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
					//launcher.setConst(  cdata.x );
					launcher.setConst(cdata.y);
					launcher.setConst(cdata.z);
					b3Int4 nSplit;
					nSplit.x = B3_SOLVER_N_SPLIT_X;
					nSplit.y = B3_SOLVER_N_SPLIT_Y;
					nSplit.z = B3_SOLVER_N_SPLIT_Z;

					launcher.setConst(nSplit);
					launcher.launch1D(numWorkItems, 64);

#else
					const char* fileName = "m_batchSolveKernel.bin";
					FILE* f = fopen(fileName, "rb");
					if (f)
					{
						int sizeInBytes = 0;
						if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))
						{
							printf("error, cannot get file size\n");
							exit(0);
						}

						unsigned char* buf = (unsigned char*)malloc(sizeInBytes);
						fread(buf, sizeInBytes, 1, f);
						int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes, m_context);
						int num = *(int*)&buf[serializedBytes];

						launcher.launch1D(num);

						//this clFinish is for testing on errors
						clFinish(m_queue);
					}

#endif

#ifdef DEBUG_ME
					clFinish(m_queue);
					gpuDebugInfo.read(debugInfo, numWorkItems);
					clFinish(m_queue);
					for (int i = 0; i < numWorkItems; i++)
					{
						if (debugInfo[i].m_valInt2 > 0)
						{
							printf("debugInfo[i].m_valInt2 = %d\n", i, debugInfo[i].m_valInt2);
						}

						if (debugInfo[i].m_valInt3 > 0)
						{
							printf("debugInfo[i].m_valInt3 = %d\n", i, debugInfo[i].m_valInt3);
						}
					}
#endif  //DEBUG_ME
				}
			}

			clFinish(m_queue);
		}

		cdata.x = 1;
		bool applyFriction = true;
		if (applyFriction)
		{
			B3_PROFILE("m_batchSolveKernel iterations2");
			for (int iter = 0; iter < m_nIterations; iter++)
			{
				for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++)
				{
					cdata.z = ib;

					b3BufferInfoCL bInfo[] = {
						b3BufferInfoCL(bodyBuf->getBufferCL()),
						b3BufferInfoCL(shapeBuf->getBufferCL()),
						b3BufferInfoCL(constraint->getBufferCL()),
						b3BufferInfoCL(m_numConstraints->getBufferCL()),
						b3BufferInfoCL(m_offsets->getBufferCL())
#ifdef DEBUG_ME
							,
						b3BufferInfoCL(&gpuDebugInfo)
#endif  //DEBUG_ME
					};
					b3LauncherCL launcher(m_queue, m_solveFrictionKernel, "m_solveFrictionKernel");
					launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
					//launcher.setConst(  cdata.x );
					launcher.setConst(cdata.y);
					launcher.setConst(cdata.z);
					b3Int4 nSplit;
					nSplit.x = B3_SOLVER_N_SPLIT_X;
					nSplit.y = B3_SOLVER_N_SPLIT_Y;
					nSplit.z = B3_SOLVER_N_SPLIT_Z;

					launcher.setConst(nSplit);

					launcher.launch1D(64 * nn / B3_SOLVER_N_BATCHES, 64);
				}
			}
			clFinish(m_queue);
		}
#ifdef DEBUG_ME
		delete[] debugInfo;
#endif  //DEBUG_ME
	}
}

void b3Solver::convertToConstraints(const b3OpenCLArray<b3RigidBodyData>* bodyBuf,
									const b3OpenCLArray<b3InertiaData>* shapeBuf,
									b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData,
									int nContacts, const ConstraintCfg& cfg)
{
	//	b3OpenCLArray<b3GpuConstraint4>* constraintNative =0;
	contactCOut->resize(nContacts);
	struct CB
	{
		int m_nContacts;
		float m_dt;
		float m_positionDrift;
		float m_positionConstraintCoeff;
	};

	{
		CB cdata;
		cdata.m_nContacts = nContacts;
		cdata.m_dt = cfg.m_dt;
		cdata.m_positionDrift = cfg.m_positionDrift;
		cdata.m_positionConstraintCoeff = cfg.m_positionConstraintCoeff;

		if (gConvertConstraintOnCpu)
		{
			b3AlignedObjectArray<b3RigidBodyData> gBodies;
			bodyBuf->copyToHost(gBodies);

			b3AlignedObjectArray<b3Contact4> gContact;
			contactsIn->copyToHost(gContact);

			b3AlignedObjectArray<b3InertiaData> gShapes;
			shapeBuf->copyToHost(gShapes);

			b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut;
			gConstraintOut.resize(nContacts);

			B3_PROFILE("cpu contactToConstraintKernel");
			for (int gIdx = 0; gIdx < nContacts; gIdx++)
			{
				int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);
				int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);

				b3Float4 posA = gBodies[aIdx].m_pos;
				b3Float4 linVelA = gBodies[aIdx].m_linVel;
				b3Float4 angVelA = gBodies[aIdx].m_angVel;
				float invMassA = gBodies[aIdx].m_invMass;
				b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia;

				b3Float4 posB = gBodies[bIdx].m_pos;
				b3Float4 linVelB = gBodies[bIdx].m_linVel;
				b3Float4 angVelB = gBodies[bIdx].m_angVel;
				float invMassB = gBodies[bIdx].m_invMass;
				b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia;

				b3ContactConstraint4_t cs;

				setConstraint4(posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,
							   &gContact[gIdx], cdata.m_dt, cdata.m_positionDrift, cdata.m_positionConstraintCoeff,
							   &cs);

				cs.m_batchIdx = gContact[gIdx].m_batchIdx;

				gConstraintOut[gIdx] = (b3GpuConstraint4&)cs;
			}

			contactCOut->copyFromHost(gConstraintOut);
		}
		else
		{
			B3_PROFILE("gpu m_contactToConstraintKernel");

			b3BufferInfoCL bInfo[] = {b3BufferInfoCL(contactsIn->getBufferCL()), b3BufferInfoCL(bodyBuf->getBufferCL()), b3BufferInfoCL(shapeBuf->getBufferCL()),
									  b3BufferInfoCL(contactCOut->getBufferCL())};
			b3LauncherCL launcher(m_queue, m_contactToConstraintKernel, "m_contactToConstraintKernel");
			launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
			//launcher.setConst(  cdata );

			launcher.setConst(cdata.m_nContacts);
			launcher.setConst(cdata.m_dt);
			launcher.setConst(cdata.m_positionDrift);
			launcher.setConst(cdata.m_positionConstraintCoeff);

			launcher.launch1D(nContacts, 64);
			clFinish(m_queue);
		}
	}
}

/*
void b3Solver::sortContacts(  const b3OpenCLArray<b3RigidBodyData>* bodyBuf, 
			b3OpenCLArray<b3Contact4>* contactsIn, void* additionalData, 
			int nContacts, const b3Solver::ConstraintCfg& cfg )
{
	
	

	const int sortAlignment = 512; // todo. get this out of sort
	if( cfg.m_enableParallelSolve )
	{
		

		int sortSize = NEXTMULTIPLEOF( nContacts, sortAlignment );

		b3OpenCLArray<unsigned int>* countsNative = m_numConstraints;//BufferUtils::map<TYPE_CL, false>( data->m_device, &countsHost );
		b3OpenCLArray<unsigned int>* offsetsNative = m_offsets;//BufferUtils::map<TYPE_CL, false>( data->m_device, &offsetsHost );

		{	//	2. set cell idx
			struct CB
			{
				int m_nContacts;
				int m_staticIdx;
				float m_scale;
				int m_nSplit;
			};

			b3Assert( sortSize%64 == 0 );
			CB cdata;
			cdata.m_nContacts = nContacts;
			cdata.m_staticIdx = cfg.m_staticIdx;
			cdata.m_scale = 1.f/(N_OBJ_PER_SPLIT*cfg.m_averageExtent);
			cdata.m_nSplit = B3_SOLVER_N_SPLIT;

			
			b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };
			b3LauncherCL launcher( m_queue, m_setSortDataKernel );
			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
			launcher.setConst(  cdata );
			launcher.launch1D( sortSize, 64 );
		}

		{	//	3. sort by cell idx
			int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT;
			int sortBit = 32;
			//if( n <= 0xffff ) sortBit = 16;
			//if( n <= 0xff ) sortBit = 8;
			m_sort32->execute(*m_sortDataBuffer,sortSize);
		}
		{	//	4. find entries
			m_search->execute( *m_sortDataBuffer, nContacts, *countsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, b3BoundSearchCL::COUNT);

			m_scan->execute( *countsNative, *offsetsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT );
		}

		{	//	5. sort constraints by cellIdx
			//	todo. preallocate this
//			b3Assert( contactsIn->getType() == TYPE_HOST );
//			b3OpenCLArray<b3Contact4>* out = BufferUtils::map<TYPE_CL, false>( data->m_device, contactsIn );	//	copying contacts to this buffer

			{
				

				b3Int4 cdata; cdata.x = nContacts;
				b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };
				b3LauncherCL launcher( m_queue, m_reorderContactKernel );
				launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
				launcher.setConst(  cdata );
				launcher.launch1D( nContacts, 64 );
			}
//			BufferUtils::unmap<true>( out, contactsIn, nContacts );
		}
	}

	
}

*/
void b3Solver::batchContacts(b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* nNative, b3OpenCLArray<unsigned int>* offsetsNative, int staticIdx)
{
	int numWorkItems = 64 * B3_SOLVER_N_CELLS;
	{
		B3_PROFILE("batch generation");

		b3Int4 cdata;
		cdata.x = nContacts;
		cdata.y = 0;
		cdata.z = staticIdx;

#ifdef BATCH_DEBUG
		SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems];
		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device, numWorkItems);
		memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems);
		gpuDebugInfo.write(debugInfo, numWorkItems);
#endif

#if 0
		b3BufferInfoCL bInfo[] = { 
			b3BufferInfoCL( contacts->getBufferCL() ), 
			b3BufferInfoCL(  m_contactBuffer2->getBufferCL()),
			b3BufferInfoCL( nNative->getBufferCL() ), 
			b3BufferInfoCL( offsetsNative->getBufferCL() ),
#ifdef BATCH_DEBUG
			,	b3BufferInfoCL(&gpuDebugInfo)
#endif
		};
#endif

		{
			m_batchSizes.resize(nNative->size());
			B3_PROFILE("batchingKernel");
			//b3LauncherCL launcher( m_queue, m_batchingKernel);
			cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel;

			b3LauncherCL launcher(m_queue, k, "*batchingKernel");
			if (!useNewBatchingKernel)
			{
				launcher.setBuffer(contacts->getBufferCL());
			}
			launcher.setBuffer(m_contactBuffer2->getBufferCL());
			launcher.setBuffer(nNative->getBufferCL());
			launcher.setBuffer(offsetsNative->getBufferCL());

			launcher.setBuffer(m_batchSizes.getBufferCL());

			//launcher.setConst(  cdata );
			launcher.setConst(staticIdx);

			launcher.launch1D(numWorkItems, 64);
			//clFinish(m_queue);
			//b3AlignedObjectArray<int> batchSizesCPU;
			//m_batchSizes.copyToHost(batchSizesCPU);
			//printf(".\n");
		}

#ifdef BATCH_DEBUG
		aaaa
			b3Contact4* hostContacts = new b3Contact4[nContacts];
		m_contactBuffer->read(hostContacts, nContacts);
		clFinish(m_queue);

		gpuDebugInfo.read(debugInfo, numWorkItems);
		clFinish(m_queue);

		for (int i = 0; i < numWorkItems; i++)
		{
			if (debugInfo[i].m_valInt1 > 0)
			{
				printf("catch\n");
			}
			if (debugInfo[i].m_valInt2 > 0)
			{
				printf("catch22\n");
			}

			if (debugInfo[i].m_valInt3 > 0)
			{
				printf("catch666\n");
			}

			if (debugInfo[i].m_valInt4 > 0)
			{
				printf("catch777\n");
			}
		}
		delete[] debugInfo;
#endif  //BATCH_DEBUG
	}

	//	copy buffer to buffer
	//b3Assert(m_contactBuffer->size()==nContacts);
	//contacts->copyFromOpenCLArray( *m_contactBuffer);
	//clFinish(m_queue);//needed?
}