#include <v_lib.h>
#include "phasesp.h"
#include "icond.h"

/*
 * Routine to fill array with uniformly distributed
 * float random numbers. This is a wrapper for urnNextVectorRange().
 */
void
uniform_random_vector(vec_t dest[],u32_t nval,vec_t min,vec_t max)
{
	vector_p v = vecAlloc(nval);
	urnNextVectorRange(rhandle,v,min,max);
	memCopy(dest,v->elements,nval * sizeof(vec_t));
	memFree(v);
}

/*
 * Routines for fast uniform distribution of equal-sized particles.
 */

typedef struct cells_s
{
	u32_t ncells;   /* total number of cells */
	u32_t nx, ny, nz; /* number of layers in each dimension */
	u32_t navail;   /* number of available cells */
	vec3_t dmins;   /* lower bound of distribution domain */
	vec3_t deltas;  /* per-cell increments of coords */
	u8_t  *cstat;   /* cstat[i] == 0 if cell #i empty */
}   cells_t, *cells_p;

V_LOCAL cells_p
alloc_cells(vec3_t domain_mins,vec3_t domain_maxs,vec_t particle_size)
{
	u32_t nx, ny, nz, nc;
	cells_p cs;
	vec3_t domain_dim;
	vec3Sub(domain_dim,domain_maxs,domain_mins);
	nx = (u32_t)floor(domain_dim[0] / particle_size);
	ny = (u32_t)floor(domain_dim[1] / particle_size);
	nz = (u32_t)floor(domain_dim[2] / particle_size);
	if(!nx || !ny || !nz)
	{
		conlPrintf(LOG_ERROR,
			"alloc_cells():(%d,%d,%d) domain too small.\n",nx,ny,nz);
		return NULL;
	}
	nc = nx * ny * nz;
	cs = (cells_p)memAlloc(sizeof(cells_t) + nc * sizeof(u8_t));
	memZero(cs,sizeof(cells_t) + nc * sizeof(u8_t));
	cs->cstat = (u8_t*)(cs + 1);
	cs->nx = nx;
	cs->ny = ny;
	cs->nz = nz;
	cs->ncells = cs->navail = nc;
	vec3Set(cs->deltas,
		domain_dim[0]/nx, domain_dim[1]/ny, domain_dim[2]/nz);
	vec3Copy(cs->dmins,domain_mins);
	return cs;
}

V_LOCAL vboolean
insert_next_particle(cells_p cs,vec3_t pos_ret)
{
	u32_t i, j, nextidx;
	u32_t cx, cy, cz;
	if(!cs->navail)
	{
		conlPuts(LOG_ERROR,"insert_next_particle(): domain full.\n");
		return false;
	}
	nextidx = urnNextLong(rhandle,cs->navail);
	for(i=j=0;i<cs->ncells;i++)
	{
		/* count empty cells */
		if(!cs->cstat[i])
		{
			if(j==nextidx)
			{
				/* target cell */
				cs->cstat[i] = 1;
				cz = i / (cs->nx * cs->ny);
				i -= cz * cs->nx * cs->ny;
				cy = i / cs->nx;
				cx = i - cy * cs->nx;
				pos_ret[0] = cs->dmins[0] + cs->deltas[0] * (cx + 0.5);
				pos_ret[1] = cs->dmins[1] + cs->deltas[1] * (cy + 0.5);
				pos_ret[2] = cs->dmins[2] + cs->deltas[2] * (cz + 0.5);
				break;
			}
			else
				j++;
		}
	}
	cs->navail--;
	return true;
}

vboolean
distribute_equal_sized_in_box(
	vec3_t mins,vec3_t maxs, /* domain boundaries */
	u32_t npart, /* number of particles */
	vec_t particle_radius, /* radius of each particle */
	vec_t phase_ret[] /* array to hold computed positions */)
{
	u32_t i;
	vec3_t pos;
	cells_p cells;
	cells = alloc_cells(mins,maxs,particle_radius * 2);
	if(!cells)
		return false;
	for(i=0;i<npart;i++)
	{
		if(!insert_next_particle(cells,pos))
		{
			memFree(cells);
			return false;
		}
		vec3Copy(PS_POS(phase_ret,i),pos);
	}
	memFree(cells);
	return true;
}

/*
 * General-case routines for uniform distrubution in box.
 * The used approach may be VERY slow.
 */
V_LOCAL vboolean
is_point_away(vec3_t check_pos,vec_t min_dist,
	vec_t particle_radius[],vec_t phase[],u32_t num_checks)
{
	u32_t i;
	vec3_t dist;
	for(i=0;i<num_checks;i++)
	{
		vec3Sub(dist,check_pos,PS_POS(phase,i));
		if((vec3Abs(dist) - particle_radius[i]) < min_dist)
			return false;
	}
	return true;
}

V_LOCAL vboolean
is_distribution_degenerate(vec3_t dmins,vec3_t dmaxs,
	vec_t phase[],u32_t npts,vec_t cur_vol_to_domain_vol)
{
	u32_t i;
	vec3_t mins, maxs, dd;
	vec_t v, dv;
	if(npts == 0) /* bbox volume == 0, degenerate */
		return true;
	bboxClear(mins,maxs);
	for(i=0;i<npts;i++)
		bboxAddPoint(mins,maxs,PS_POS(phase,i));
	/* compute volumes */
	vec3Sub(maxs,maxs,mins);
	vec3Sub(dd,dmaxs,dmins);
	v = maxs[0] * maxs[1] * maxs[2];
	dv = dd[0] * dd[1] * dd[2];
	return (cur_vol_to_domain_vol * dv > v) ? true : false;
}

V_LOCAL vboolean distribute_in_box_ex(
	vec3_t mins,vec3_t maxs,
	u32_t npart,
	vec_t particle_radius[],
	vec_t min_dist,
	vec_t phase_ret[],
	u32_t timeout,
	vec_t cur_vol_to_domain_vol,
	vboolean (*accept_extra)(void_p arg,vec3_t pos),
	void_p accept_arg)
{
	u32_t i,ntry,trycount,fntry,starttime;
	vec3_t pos;
	vboolean still_good;
	starttime = timeTickCount();
	for(fntry = 1;;fntry++)
	{
		ntry = trycount = 0;
		for(i=0;i<npart;)
		{
			urnNextVec3RangeV(rhandle,pos,mins,maxs);
			if(accept_extra)
				still_good = (*accept_extra)(accept_arg,pos);
			else
				still_good = true;
			still_good = still_good &&
				is_point_away(pos,min_dist + particle_radius[i],
					particle_radius,phase_ret,i);
			if(still_good)
			{
				/* accept new coords */
				vec3Copy(PS_POS(phase_ret,i),pos);
				vec3Zero(PS_VEL(phase_ret,i));
				i++;
				continue;
			}
			ntry++;
			trycount++;
			if(trycount == 2048)
			{
				trycount = 0;
				if((timeTickCount() - starttime) > timeout)
				{
					conlPrintf(LOG_ERROR,"dist_in_box_ex(): timeout reached "
						"after %d trials.\n",ntry);
					return false;
				}
			}
		}
		/* box filled; check for degeneracy */
		if(!is_distribution_degenerate(mins,maxs,phase_ret,npart,
			cur_vol_to_domain_vol))
			break;
		if((timeTickCount() - starttime) > timeout)
		{
			conlPrintf(LOG_ERROR,"dist_in_box_ex(): timeout reached "
				"after %d degenerate fills.\n",fntry);
			return false;
		}
	}
	return true;
}

vboolean distribute_in_box(
	vec3_t mins,vec3_t maxs, /* domain boundaries */
	u32_t npart, /* number of particles */
	vec_t particle_radius[], /* radiuses of the particles */
	vec_t min_dist, /* minimal distance between _surfaces_ of the particles */
	vec_t phase_ret[], /* array to hold positions */
	u32_t timeout, /* maximal time to execute in milliseconds */
	vec_t cur_vol_to_domain_vol /* 'coverage' coefficient */)
{
	return distribute_in_box_ex(mins,maxs,npart,particle_radius,
		min_dist,phase_ret,timeout,cur_vol_to_domain_vol,NULL,NULL);
}

/*
 * Routines for uniform distrubution in sphere.
 */
V_LOCAL vboolean is_point_inside_sphere(void_p arg,vec3_t pos)
{
	/* arg must point to array with 4 floats */
	vec_p center = (vec_p)arg;
	vec_t radius = center[3];
	vec3_t dr;
	vec3Sub(dr,pos,center);
	return vec3Abs(dr) < radius;
}

vboolean distribute_in_sphere(
	vec3_t center,vec_t radius, /* center and radius of domain */
	u32_t npart,
	vec_t particle_radius[],
	vec_t min_dist,
	vec_t phase_ret[],u32_t timeout,
	vec_t cur_vol_to_domain_vol /* must _not_ be close to 1 */)
{
	vec_t accept_arg[4];
	vec3_t halfsize, mins, maxs;

	vec3Copy(accept_arg,center);
	accept_arg[3] = radius;
	vec3Set(halfsize,radius,radius,radius);
	vec3Sub(mins,center,halfsize);
	vec3Add(maxs,center,halfsize);

	return distribute_in_box_ex(mins,maxs,npart,particle_radius,
		min_dist,phase_ret,timeout,cur_vol_to_domain_vol,
		is_point_inside_sphere,accept_arg);
}

/*
 * Routines for uniform distrubution in z-axial cylinder.
 */
V_LOCAL vboolean is_point_inside_cylinder(void_p arg,vec3_t pos)
{
	/* arg must point to array with 5 floats */
	vec_p lf_center = (vec_p)arg;
	vec_t radius = lf_center[3];
	vec_t height = lf_center[4];
	vec2_t dxdy;
	/* check if pos is between lower and upper faces */
	if(pos[2] < lf_center[2] || pos[2] > (lf_center[2]+height))
		return false;
	/* check for distance from axis */
	vec2Sub(dxdy,pos,lf_center);
	if(vec2Abs(dxdy) > radius)
		return false;
	return true;
}

vboolean distribute_in_cylinder(
	vec3_t lf_center, /* center of the lower base of the cylinder */
	vec_t radius,vec_t height, /* radius and height of cylinder */
	u32_t npart,vec_t particle_radius[],vec_t min_dist,
	vec_t phase_ret[],u32_t timeout,
	vec_t cur_vol_to_domain_vol /* must _not_ be close to 1 */)
{
	vec_t accept_arg[5];
	vec3_t mins, maxs;
	vec3Copy(accept_arg,lf_center);
	accept_arg[3] = radius;
	accept_arg[4] = height;

	vec3Copy(mins,lf_center);
	mins[0] -= radius;
	mins[1] -= radius;

	vec3Copy(maxs,lf_center);
	maxs[0] += radius;
	maxs[1] += radius;
	maxs[2] += height;

	return distribute_in_box_ex(mins,maxs,npart,particle_radius,
		min_dist,phase_ret,timeout,cur_vol_to_domain_vol,
		is_point_inside_cylinder,accept_arg);
}

