//
// Asura_Platform_Physics_Solver.cpp
//
// Created by KJFL - 15 July 2008
//
// Copyright (C) 2008 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

// This file's header.
#include "Physics/Asura_Physics_Solver.h"
#include "Core/THreads/Asura_ProducerConsumerQueue.h"

#ifndef ASURA_PHYSICS_USE_OLD_SOLVER

static u_char*	g_pucSolverMem = 0;

static const u_int uSOLVER_BUFFER_SIZE = 128 * 1024;
static const u_int uNUMBER_OF_BUFFERS = 4;

static Asura_Semaphore* g_pxSolverAvailable;
static Asura_ProducerConsumerQueue* g_pxQueue;

static bool g_abSolverAvailable[uNUMBER_OF_BUFFERS];

void Asura_Physics_Solver::Platform_Initialise( void )
{
	g_pucSolverMem = (u_char*)Asura_AllocateAlign( uSOLVER_BUFFER_SIZE * uNUMBER_OF_BUFFERS, ASURA_ALIGN_16_BYTE );

	g_pxSolverAvailable = new Asura_Semaphore( "SolverAvailable", uNUMBER_OF_BUFFERS, uNUMBER_OF_BUFFERS );

	g_pxQueue = new Asura_ProducerConsumerQueue( 1, uNUMBER_OF_BUFFERS );

	for (u_int u=0; u<uNUMBER_OF_BUFFERS; ++u)
	{
		g_abSolverAvailable[u] = true;
	}
}
	
void Asura_Physics_Solver::Platform_Shutdown( void )
{
	if ( g_pucSolverMem )
	{
		Asura_DeallocateAlign( g_pucSolverMem );
		g_pucSolverMem = 0;
	}

	delete g_pxSolverAvailable;
	delete g_pxQueue;
}

void Asura_Physics_Solver::Platform_Begin( void )
{

}

void Asura_Physics_Solver::Platform_End( void )
{
	// wait for all the solver threads to finish
	for (u_int u=0; u<uNUMBER_OF_BUFFERS; ++u)
	{
		g_pxSolverAvailable->Wait();
	}

	// signal the solver threads are ready for work again
	for (u_int u=0; u<uNUMBER_OF_BUFFERS; ++u)
	{
		g_pxSolverAvailable->Signal();
	}
}

u_char* Asura_Physics_Solver::Platform_GetSolverBuffer( void )
{
	g_pxSolverAvailable->Wait();

	u_int uSolverToUse=0;

	for (u_int u=0; u<uNUMBER_OF_BUFFERS; ++u)
	{
		if (g_abSolverAvailable[u])
		{
			g_abSolverAvailable[u] = false;
			uSolverToUse = u;
			break;
		}
	}

	return (g_pucSolverMem + uSOLVER_BUFFER_SIZE* (uSolverToUse));
}

u_int Asura_Physics_Solver::Platform_GetSolverBufferSize( void )
{
	return uSOLVER_BUFFER_SIZE;
}

static void Worker( void* pData, u_int uSize )
{

	Asura_Physics_Solver::Reference_SolveAllContacts( (Asura_Physics_Solver_Data*)pData );

	g_abSolverAvailable[ uSize ] = true;
	g_pxSolverAvailable->Signal();
}

bool Asura_Physics_Solver::Platform_SolveAllContacts( Asura_Physics_Solver_Data* pxTask )
{
	u_int uSolverToUse = ((u_char*)pxTask - g_pucSolverMem) / uSOLVER_BUFFER_SIZE;

	if( pxTask->m_uNumberOfSingleBodyContacts + pxTask->m_uNumberOfPairingContacts + pxTask->m_uNumberOfConeTwistConstraints == 0 )
    {
		g_abSolverAvailable[ uSolverToUse ] = true;
		g_pxSolverAvailable->Signal();
        return true;
    }

	g_pxQueue->AddToQueue( Worker, pxTask, uSolverToUse );

//	Worker( pxTask, uSolverToUse );

    return true;
}

#endif