#include <v_lib.h>
#include "config.h"
#include "phasesp.h"
#include "ctrl.h"
#include "psys.h"

#ifdef		PS_PARALLEL

volatile int spinlock_init;
volatile int thread_num = 0;
volatile int all_jobs_done = 0;

vhandle_t	ready_count_mutex;
volatile int ready_count;

void
apply_binary_forces_partial(particle_system_p ps,int *ptr, int *sentinel);

void V_CALL
ps_thread_proc(void *arg)
{
	int thread_idx = thread_num++;
	int *istart, *sentinel;
	particle_system_p ps = (particle_system_p)arg;
	istart = ps->ij_start[thread_idx];
	sentinel = ps->ij_start[thread_idx + 1];
	conPrintf("Thread %i initialized (sentinel - istart == %i).\n",
		thread_idx, (sentinel - istart) >> 1);
	spinlock_init = 0;
do_job:;
	/* printf("Thread %i: waiting for job.\n", thread_idx); */
	mutexLock(ready_count_mutex);
	ready_count++;
	mutexUnlock(ready_count_mutex);
	condWait(ps->job_ready);
	if(all_jobs_done)
	{
		conPrintf("Thread %i: exiting...\n", thread_idx);
		threadExit(0);
	}
	/*
	 * do the job
	 */
/*
	for(int *ptr = istart;ptr < sentinel;ptr+=2)
	{
		printf("(%i,%i);",ptr[0],ptr[1]);
	}
*/
	/* apply_binary_forces_partial(ps,istart,sentinel); */
	mutexLock(ready_count_mutex);
	/* printf("JOB finished for thread %i.\n", thread_idx); */
	mutexUnlock(ready_count_mutex);
	goto do_job;
}

void do_job_parallel(particle_system_p ps)
{
	int i;
	int count;
	/* puts("do_job_parallel(): waiting for completion of all threads"); */
	for(;;)
	{
		mutexLock(ready_count_mutex);
		count = ready_count;
		mutexUnlock(ready_count_mutex);
		if(count == ps->num_slices)
			break;
		/* timeDelay(1); */
	}
/*
	while(ready_count < ps->num_slices)
		timeDelay(0);
*/
	mutexLock(ready_count_mutex);
	ready_count = 0;
	mutexUnlock(ready_count_mutex);
	/* puts("do_job_parallel(): starting threads."); */
	condPulse(ps->job_ready);
/*
	while(ready_count < ps->num_slices)
		timeDelay(0);
*/
	for(;;)
	{
		i = 0;
		mutexLock(ready_count_mutex);
		count = ready_count;
		mutexUnlock(ready_count_mutex);
		if(count == ps->num_slices)
			break;
		timeDelay(10);
		/* printf("wait %i, count %i\n",i++,count); */
	}


	/* while(ready_count < ps->num_slices);
	puts("do_job_parallel(): all threads idle."); */
}

#endif	/* PS_PARALLEL */

particle_system_p
alloc_particle_system(u32_t num_particles)
{
	particle_system_p ps = NULL;
	ps = memAllocZ(sizeof(particle_system_t));

	ps->num_particles = num_particles;
	ps->particle_radius = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t));
	ps->particle_density = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t));
	ps->particle_mass = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t));
	ps->particle_reciprocal_mass = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t));
	ps->particle_6_pi_eta_r = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t));
	ps->phase = 
		(vec_p)memAllocZ(num_particles * sizeof(vec_t) * PHASESPACE_DIM);
	ps->force = 
		(vec3_t*)memAllocZ(num_particles * sizeof(vec3_t));
	ps->saves_per_second = 10;
	ps->solution_RK_tolerance = 1e-2;
	ps->solution_RK_timeout = 10;
	ps->solution_maximal_step = MAXFLOAT;
	ps->rt_timer = timerCreate();
	ps->contact = matAlloc(num_particles, num_particles);
	ps->dist = matAlloc(num_particles, num_particles);
	ps->dglue = 1e-6;
	ps->hglue = 0;
	ps->deltaz = 0;
	strCopy(ps->file_name,"[null]",_MAX_PATH);
#ifdef	PS_PARALLEL
#define	NUM_SLICES		4
	{
		int i, j, *pij, slice_len2;
		int inner_limit, outer_limit;
		int num_ij = (num_particles * num_particles - num_particles);
		ps->ij = pij = (int*)memAlloc(num_ij * sizeof(int));
		num_ij = num_ij >> 1;
		inner_limit = num_particles;
		outer_limit = num_particles - 1;
		for(i = 0;i < outer_limit;i++)
			for(j = i+1;j < inner_limit;j++)
			{
				pij[0] = i;
				pij[1] = j;
				pij += 2;
			}
		/* TODO */
		ps->num_slices = NUM_SLICES;
		ps->ij_start = (int**)memAlloc((ps->num_slices + 1) * sizeof(int*));
		/*
		 * Split the job.
		 * pij now points at the 2nd element after end of ij.
		 */
		slice_len2 = (num_ij / ps->num_slices) << 1;
		ps->ij_start[ps->num_slices] = pij;
		pij = ps->ij;
		for(i = 0;i < ps->num_slices;i++)
		{
			ps->ij_start[i] = pij;
			pij += slice_len2;
		}
		ready_count_mutex = mutexCreate();
		ps->job_ready = condCreate(false);
		condReset(ps->job_ready);
		conPuts("Thread initialization...");
		ps->threads = (vhandle_t*)memAlloc(ps->num_slices * sizeof(vhandle_t));
		ready_count = 0;
		for(i = 0;i < ps->num_slices;i++)
		{
			spinlock_init = 1;
			ps->threads[i] = threadCreate(ps_thread_proc, ps);
			while(spinlock_init);
		}
		conPrintf("Thread initialization complete, ready_count %i\n", ready_count);
	}
#endif	/* PS_PARALLEL */

	return ps;
}

extern vboolean
psy_save_pos(particle_system_p ps, char *filename, vboolean msform);

void
free_particle_system(particle_system_p ps)
{
#ifdef	PS_PARALLEL
	int i;
#endif	/* PS_PARALLEL */
	if(!ps)
		return;
#ifdef	PS_PARALLEL
	/* puts("free_particle_system() waiting for completion of all threads..."); */
	while(ready_count < ps->num_slices);
	/* puts("free_particle_system() all threads idle."); */
#endif	/* PS_PARALLEL */
	if(ps->final_filename[0])
	{
		char ext[_MAX_PATH];
		fsSplitPath(ps->final_filename,NULL,NULL,ext);
		psy_save_pos(ps, ps->final_filename, strcasecmp(ext, "ms") ? false : true);
	}
	if(ps->rt_timer != LIBV_INVALID_HANDLE)
		timerDestroy(ps->rt_timer);
	if(ps->particle_radius)
		memFree(ps->particle_radius);
	if(ps->particle_density)
		memFree(ps->particle_density);
	if(ps->particle_mass)
		memFree(ps->particle_mass);
	if(ps->particle_reciprocal_mass)
		memFree(ps->particle_reciprocal_mass);
	if(ps->particle_6_pi_eta_r)
		memFree(ps->particle_6_pi_eta_r);
	if(ps->phase)
		memFree(ps->phase);
	if(ps->force)
		memFree(ps->force);
	if(ps->contact)
		memFree(ps->contact);
	if(ps->dist)
		memFree(ps->dist);
#ifdef	PS_PARALLEL
	if(ps->ij)
		memFree(ps->ij);
	if(ps->ij_start)
		memFree(ps->ij_start);
	all_jobs_done = 1;
	condPulse(ps->job_ready);
	for(i = 0;i < ps->num_slices;i++)
	{
		threadJoin(ps->threads[i], NULL);
		conPrintf("Thread %i finished.\n", i);
	}
	memFree(ps->threads);
	condDestroy(ps->job_ready);
	mutexDestroy(ready_count_mutex);
#endif	/* PS_PARALLEL */
	memFree(ps);
}
