#include <v_lib.h>
#include "draw.h"
#include "psy.h"
#include "ms_wr.h"
#include "monitor.h"
#include "psys.h"
#include "util.h"
#include "phasesp.h"

#define	PRINT_FLOAT(out,val)	writeVec(out,val,DOT_CHAR)

/*
 * Writes average and standard deviation of "len" samples "X" to "out".
 */
V_LOCAL void
writeAvgAndStdev(vhandle_t out,vec_p X,u32_t len)
{
	vec_t av, std_dev, deviation;
	vec_p ptr, end = X + len;
	av = std_dev = 0;
	for(ptr = X;ptr < end;ptr++)
		av += *ptr;
	av /= (vec_t)len;
	for(ptr = X;ptr < end;ptr++)
	{
		deviation = *ptr - av;
		std_dev += deviation * deviation;
	}
	std_dev = sqrt(std_dev / (vec_t)(len - 1));
	PRINT_FLOAT(out, av);
	fileWrite(out,";",1);
	PRINT_FLOAT(out, std_dev);
}

V_LOCAL void
move_mins_to_start_cols(matrix_p src,u32_t num_nearest)
{
	vec_t temp;
	u32_t r, c, k, idx;
	for(r = 0;r < src->rows;r++)
	{
		for(k = 0;k < num_nearest;k++)
		{
			idx = k;
			for(c = k + 1;c < src->cols;c++)
				if(src->cell[r][c] < src->cell[r][idx])
					idx = c;
			temp = src->cell[r][idx];
			src->cell[r][idx] = src->cell[r][k];
			src->cell[r][k] = temp;
		}
	}
}

/*
 * Writes average and standard deviation of distances (between surfaces)
 * from particles to ps->num_nearest their closest neighbors.
 */
V_LOCAL void
monitorWriteRnStats(particle_system_p ps)
{
	vec_t s;
	u32_t i, j;
	vec_p sums = ps->sums;
	matrix_p dists = ps->mon_dist;
	matCopy(dists, ps->surf_dist);
	/* move ps->num_nearest minimal values to start columns */
	move_mins_to_start_cols(dists, ps->num_nearest);
	for(i = 0;i < ps->num_particles;i++)
	{
		s = 0;
		for(j = 0;j < ps->num_nearest;j++)
			s += dists->cell[i][j];
		s /= ps->num_nearest;
		sums[i] = s;
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

V_LOCAL void
connmatFill(particle_system_p ps)
{
	matrix_p dist = ps->surf_dist;
	u32_t i, j;
	vec_t d1;
	for(i = 0;i < ps->num_particles - 1;i++)
	{
		d1 = ps->percolation_distance[i];
		for(j = i + 1;j < ps->num_particles;j++)
		{
			if(dist->cell[i][j] <= (d1 + ps->percolation_distance[j]))
				ps->connmatrix[i][j] = 1;
			else
				ps->connmatrix[i][j] = 0;
			ps->connmatrix[j][i] = ps->connmatrix[i][j];
		}
	}
}

V_LOCAL void
connmatDumpGraph(particle_system_p ps, vboolean print_isolated)
{
	u32_t i, j, num_particles = ps->num_particles;
	vhandle_t out;
	logFormatString(ps->tmpname,"%s/connmat%04u.dot",ps->outdir,++ps->connmatrix_dump_count);
	out = fileOpenWrite(ps->tmpname);
	filePrintf(out, "graph connmat {\n\tlayout=fdp\n");
	filePrintf(out, "\tnode [shape=ellipse]\n");
	if(print_isolated)
	{
		for(i = 0;i < num_particles;i++)
			filePrintf(out, "\t%u [label=\"P%04u\"]\n", i, i);
	}
	else
	{
		for(i = 0;i < num_particles;i++)
		{
			for(j = 0;j < num_particles;j++)
			{
				if(ps->connmatrix[i][j])
				{
					filePrintf(out, "\t%u [label=\"P%04u\"]\n", i, i);
					break;
				}
			}
		}
	}
	for(i = 0;i < num_particles - 1;i++)
	{
		for(j = i;j < num_particles;j++)
		{
			if(ps->connmatrix[i][j])
				filePrintf(out, "\t%u -- %u\n", i, j);
		}
	}
	filePrintf(out, "}\n");
	fileClose(out);
}

V_LOCAL void
monitorWriteConnmatrix(particle_system_p ps)
{
	connmatDumpGraph(ps, false);
}

/*
 * Writes average and standard deviation of number of neighbors
 * that are closer (from surface) than ps->percolation_distance[i].
 */
V_LOCAL void
monitorWriteNnStats(particle_system_p ps)
{
	u32_t i, j, count;
	vec_p sums = ps->sums;
	for(i = 0;i < ps->num_particles;i++)
	{
		for(count = j = 0;j < ps->num_particles;j++)
		{
			if(ps->connmatrix[i][j])
				count++;
		}
		sums[i] = (vec_t)count;
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

/*
 * Writes average and standard deviation of modulus of velocity.
 */
V_LOCAL void
monitorWriteVelStats(particle_system_p ps)
{
	u32_t i;
	vec_p vel;
	vec_p sums = ps->sums;
	for(i = 0;i < ps->num_particles;i++)
	{
		vel = PS_VEL(ps->cur_phase,i);
		sums[i] = sqrt(vec3Dot(vel, vel));
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

/*
 * Writes average and standard deviation of kinetic energy.
 */
V_LOCAL void
monitorWriteTStats(particle_system_p ps)
{
	u32_t i;
	vec_p vel;
	vec_p sums = ps->sums;
	for(i = 0;i < ps->num_particles;i++)
	{
		vel = PS_VEL(ps->cur_phase,i);
		sums[i] = ps->particle_mass[i] * vec3Dot(vel, vel);
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

#if 0

/*
 * Writes average and standard deviation of potential energy.
 */
V_LOCAL void
monitorWriteUStats(particle_system_p ps)
{
	vec_p sums = ps->sums;
	vec_t dist, energy;
	u32_t i, j;
	for(i = 0;i < ps->num_particles;i++)
	{
		energy = 0;
		for(j = 0;j < ps->num_particles;j++)
		{
			if(i == j)
				continue;
			energy += (ps->binary_interaction[j]->energy)(
				ps->binary_interaction[j], ps->surf_dist->cell[i][j]);
		}
		sums[i] = energy;
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

#endif

/*
 * Writes average and standard deviation of particles' shell intersection area.
 * Uses ps->contact[][] that is calculated for ps->dglue[] in case of
 * tangent_forces == true.
 */
V_LOCAL void
monitorWriteShellIntersectionStats(particle_system_p ps)
{
	u32_t i, j;
	vec_t area;
	vec_p sums = ps->sums;
	for(i=0;i < ps->num_particles;i++)
	{
		area = 0;
		for(j = 0;j < ps->num_particles;j++)
		{
			if(i == j)
				continue;
			area += ps->contact->cell[i][j];
		}
		sums[i] = area / (4 * M_PI * ps->particle_radius[i]); /* FIXME WTF */
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

/*
 * Writes average and standard deviation of one of the coordinates.
 */
V_LOCAL void
monitorWriteCoordStats(particle_system_p ps, u32_t nc)
{
	u32_t i;
	vec_p sums = ps->sums;
	V_ASSERT(nc < 3);
	for(i = 0;i < ps->num_particles;i++)
	{
		sums[i] = PS_POS(ps->cur_phase,i)[nc];
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,ps->num_particles);
}

V_LOCAL void
monitorWriteXStats(particle_system_p ps)
{
	monitorWriteCoordStats(ps, 0);
}

V_LOCAL void
monitorWriteYStats(particle_system_p ps)
{
	monitorWriteCoordStats(ps, 1);
}

V_LOCAL void
monitorWriteZStats(particle_system_p ps)
{
	monitorWriteCoordStats(ps, 2);
}

V_LOCAL vboolean
pt_inside(vec_p pt,vec3_t mins,vec3_t maxs)
{
	int i = 2;
	do {
		if(pt[i] < mins[i] || pt[i] > maxs[i])
			return false;
	} while (i--);
	return true;
}

/*
 * Writes average and standard deviation of number of particles in cuboidal
 * subdomains of cuboidal hull of all particles. Number of subromains are
 * controlled by ps->cuboid_subdiv[0...2].
 */
V_LOCAL void
monitorWriteNSStats(particle_system_p ps)
{
	vec3_t mins, maxs, ext, dim, p1, p2;
	unsigned i, j, k, l, m, count;
	vec_p phase = ps->cur_phase;
	unsigned maxc = ps->cuboid_subdiv[0] * ps->cuboid_subdiv[1] * ps->cuboid_subdiv[2];
	vec_p sums = (maxc > ps->num_particles) ? memAllocVecs(maxc) : ps->sums;
	bboxClear(mins,maxs);
	for(i = 0;i < ps->num_particles;i++)
		bboxAddPoint(mins,maxs,PS_POS(phase,i));
	vec3Sub(ext,maxs,mins);
	dim[0] = ext[0] / ps->cuboid_subdiv[0];
	dim[1] = ext[1] / ps->cuboid_subdiv[1];
	dim[2] = ext[2] / ps->cuboid_subdiv[2];
	count = 0;
	for(i = 0;i < ps->cuboid_subdiv[0];i++)
	{
		for(j = 0;j < ps->cuboid_subdiv[1];j++)
		{
			for(k = 0;k < ps->cuboid_subdiv[2];k++)
			{
				p1[0] = mins[0] + dim[0] * i;
				p1[1] = mins[1] + dim[1] * j;
				p1[2] = mins[2] + dim[2] * k;
				vec3Add(p2,p1,dim);
				m = 0;
				for(l = 0;l < ps->num_particles;l++)
				{
					if(pt_inside(PS_POS(phase,l),p1,p2))
						m++;
				}
				sums[count] = m;
				count++;
			}
		}
	}
	writeAvgAndStdev(ps->kinetics_outfile,sums,maxc);
	if(sums != ps->sums)
		memFree(sums);
}

#ifdef	STATIC_CLUSTERS

/* FIXME */
#define	MAX_PARTICLES	1024

typedef struct iset_s
{
	u32_t num_elems;
	u8_t in[MAX_PARTICLES];
	u32_t list[MAX_PARTICLES]; /* delayed update */
} iset_t, *iset_p;

void isetClear(iset_p set)
{
	memZero(set, sizeof(iset_t));
}

#else /* !STATIC_CLUSTERS */

void isetClear(iset_p set)
{
	set->num_elems = 0;
	memZero(set->in, set->max_elems);
}

#endif /* !STATIC_CLUSTERS */

void isetAdd(iset_p set,u32_t elem)
{
	if(set->in[elem])
		return;
	set->in[elem] = 1;
	set->num_elems++;
}

void isetDel(iset_p set,u32_t elem)
{
	if(!set->in[elem])
		return;
	set->in[elem] = 0;
	set->num_elems--;
}

vboolean isetGet(iset_p set,u32_t *ret_val)
{
	u32_t i;
	if(!set->num_elems)
		return false;
	for(i = 0;!set->in[i];i++);
	*ret_val = i;
	return true;
}
void isetUpdateList(iset_p set)
{
	u32_t i, count;
	for(i = count = 0;count < set->num_elems;i++)
	{
		if(set->in[i])
			set->list[count++] = i;
	}
}

void isetPrintList(iset_p set)
{
	u32_t i;
	if(!set || !set->num_elems)
	{
		conPuts("empty\n");
		return;
	}
	conPrintf("num_elems;%u;elems", set->num_elems);
	for(i = 0;i < set->num_elems;i++)
		conPrintf(";%u", set->list[i]);
	conPuts("\n");
}

#ifdef	STATIC_CLUSTERS

/*
 * FIXME: the code below is not reentrant!
 */

iset_t	isolated;
u32_t	num_clusters;
iset_t	clusters[MAX_PARTICLES];

V_LOCAL void
clustersClear(particle_system_p ps)
{
	u32_t i;
	num_clusters = 0;
	V_ASSERT(ps->num_particles <= MAX_PARTICLES);
	isetClear(&isolated);
	memZero(&clusters, sizeof(clusters));
	for(i = 0;i < ps->num_particles;i++)
		isetAdd(&isolated, i);
	isetUpdateList(&isolated);
}

V_LOCAL void
addParticleToCluster_r(u8_p *connmat,iset_p dst,iset_p src,u32_t particle_no)
{
	iset_t src_copy;
	u32_t i, next_no;
	isetAdd(dst, particle_no);
	isetDel(src, particle_no);
	isetUpdateList(src);
	/* make a copy of src */
	memCopy(&src_copy, src, sizeof(iset_t));
	/* find all particles which are connected to next_no */
	for(i = 0;i < src_copy.num_elems;i++)
	{
		next_no = src_copy.list[i];
		if(connmat[particle_no][next_no])
		{
			/* add particle to current cluster */
			addParticleToCluster_r(connmat,dst,src,next_no);
		}
	}
}

V_LOCAL void
clustersFill(particle_system_p ps)
{
	u32_t next_no;
	for(num_clusters = 0;isetGet(&isolated,&next_no);num_clusters++)
	{
		iset_p cluster = &clusters[num_clusters];
		addParticleToCluster_r(ps->connmatrix,cluster,&isolated,next_no);
		isetUpdateList(cluster);
	}
}

V_LOCAL void
clustersDump(vhandle_t out)
{
	u32_t i, j;
	filePrintf(out,"num_clusters %u\n",num_clusters);
	for(i = 0;i < num_clusters;i++)
	{
		filePrintf(out,"%u;%u", i, clusters[i].num_elems);
		isetUpdateList(&clusters[i]);
		for(j = 0;j < clusters[i].num_elems;j++)
			filePrintf(out,";%u", clusters[i].list[j]);
		filePrintf(out,"\n");
	}
}

#else /* !STATIC_CLUSTERS */

V_LOCAL void
clustersClear(particle_system_p ps)
{
	u32_t i;
	ps->num_clusters = 0;
	isetClear(ps->isolated);
	for(i = 0;i < ps->num_particles;i++)
	{
		isetAdd(ps->isolated, i);
		isetClear(ps->clusters[i]);
	}
	isetUpdateList(ps->isolated);
}

V_LOCAL void
addParticleToCluster_r(u8_p *connmat,iset_p dst,iset_p src,u32_t particle_no)
{
	iset_t src_copy;
	u32_t i, next_no;
	isetAdd(dst, particle_no);
	isetDel(src, particle_no);
	isetUpdateList(src);
	/* make a copy of src */
	memCopy(&src_copy, src, sizeof(iset_t));
	/* find all particles which are connected to next_no */
	for(i = 0;i < src_copy.num_elems;i++)
	{
		next_no = src_copy.list[i];
		if(connmat[particle_no][next_no])
		{
			/* add particle to current cluster */
			addParticleToCluster_r(connmat,dst,src,next_no);
		}
	}
}

V_LOCAL void
clustersFill(particle_system_p ps)
{
	u32_t next_no;
	for(ps->num_clusters = 0;isetGet(ps->isolated,&next_no);ps->num_clusters++)
	{
		iset_p cluster = ps->clusters[ps->num_clusters];
		addParticleToCluster_r(ps->connmatrix,cluster,ps->isolated,next_no);
		isetUpdateList(cluster);
	}
}

V_LOCAL void
clustersDump(vhandle_t out, particle_system_p ps)
{
	u32_t i, j;
	filePrintf(out,"num_clusters %u\n",ps->num_clusters);
	for(i = 0;i < ps->num_clusters;i++)
	{
		filePrintf(out,"%u;%u", i, ps->clusters[i]->num_elems);
		isetUpdateList(ps->clusters[i]);
		for(j = 0;j < ps->clusters[i]->num_elems;j++)
			filePrintf(out,";%u", ps->clusters[i]->list[j]);
		fileWrite(out,"\n",1);
	}
}

#endif	/* !STATIC_CLUSTERS */

V_LOCAL void
monitorClusterStats(particle_system_p ps)
{
	clustersClear(ps);
	clustersFill(ps);
}

V_LOCAL monroutine_t all_monroutines[] = {
	{
		"Writes average and standard deviation of distances (between surfaces) from particles to ps->num_nearest their closest neighbors",
		"distance_to_neighbors",
		"DN",
		monitorWriteRnStats
	},
	{
		"Writes average and standard deviation of number of neighbors that are closer (from surface) than ps->percolation_distance[]",
		"number_of_percolating_neighbors",
		"NN",
		monitorWriteNnStats
	},
	{
		"Writes average and standard deviation of modulus of velocity",
		"velocity_modulus",
		"V",
		monitorWriteVelStats
	},
	{
		"Writes average and standard deviation of kinetic energy",
		"kinetic_energy",
		"T",
		monitorWriteTStats
	},
	{
		"Writes average and standard deviation of particles' shell intersection area; uses ps->contact[][] that is calculated for ps->dglue[] in case of tangent_forces == true",
		"shell_intersection_area",
		"SIA",
		monitorWriteShellIntersectionStats
	},
	{
		"Writes average and standard deviation of X coordinate",
		"x_coordinate",
		"X",
		monitorWriteXStats
	},
	{
		"Writes average and standard deviation of Y coordinate",
		"y_coordinate",
		"Y",
		monitorWriteYStats
	},
	{
		"Writes average and standard deviation of Z coordinate",
		"z_coordinate",
		"Z",
		monitorWriteZStats
	},
	{
		"Writes average and standard deviation of number of particles in cuboidal subdomains of cuboidal hull of all particles. Number of subromains are controlled by ps->cuboid_subdiv[0...2]",
		"number_in_subdomains",
		"NS",
		monitorWriteNSStats
	},
	{
		"Writes connection matrix in graphviz format",
		"write_connmatrix",
		NULL,
		monitorWriteConnmatrix
	},
	{NULL,NULL,NULL,NULL}
};


void listMonitorRoutines()
{
	u32_t i;
	conPuts("Definition of statistic routines:\n\tstatistics <statistics_name>\n");
	conPuts("Valid values for statistics_name are:\n");
	for(i = 0;NULL != all_monroutines[i].name;i++)
		printf("\t\"%s\": %s\n", all_monroutines[i].name, all_monroutines[i].desc);
}

monroutine_entry_t
monitorEntryForName(char *name)
{
	u32_t i;
	for(i = 0;NULL != all_monroutines[i].name;i++)
		if(!strcmp(all_monroutines[i].name, name))
			return all_monroutines[i].entry;
	return NULL;
}

V_LOCAL char_p
monitorDenotationForEntry(void_p entry)
{
	u32_t i;
	void_p ptr;
	for(i = 0;NULL != all_monroutines[i].name;i++)
	{
		FUNC2PTR(ptr,all_monroutines[i].entry);
		if(entry == ptr)
			return all_monroutines[i].denotation;
	}
	return NULL;
}

V_LOCAL void
callMonitors(particle_system_p ps)
{
	char_p denotation;
	void_p monroutine;
	monroutine_entry_t entry;
	u32_t i, num_monroutines = vpsCount(&ps->monroutines);
	if(!num_monroutines)
		return;
	if(LIBV_INVALID_HANDLE != ps->kinetics_outfile)
	{
		PRINT_FLOAT(ps->kinetics_outfile, ps->cur_time);
	}
	for(i = 0;i < num_monroutines;i++)
	{
		monroutine = vpsGet(&ps->monroutines, i);
		denotation = monitorDenotationForEntry(monroutine);
		if(denotation && denotation[0])
			fileWrite(ps->kinetics_outfile,";",1);
		PTR2FUNC(entry,monroutine);
		(entry)(ps);
	}
	if(LIBV_INVALID_HANDLE != ps->kinetics_outfile)
		fileWrite(ps->kinetics_outfile,"\n",1);
}

u32_t ms_skipcount = 100000; /* FIXME */
u32_t global_skipcount = 100000; /* FIXME */
extern vboolean hack_draw; /* FIXME */
extern void ms_save_frame_hack(particle_system_p ps); /* FIXME */

V_LOCAL vboolean
monitorMain(particle_system_p ps)
{
	global_skipcount++;
	if(global_skipcount >= ps->global_skips)
	{
		global_skipcount = 0;
		if(ps->percolation_distance_defined)
			connmatFill(ps);
		callMonitors(ps);
		ms_skipcount++;
		if(ms_skipcount >= ps->ms_draw_skips)
		{
			ms_skipcount = 0;
			hack_draw = true; /* FIXME */
			ms_save_frame_hack(ps);
		}
	}
	return true;
}

V_LOCAL vboolean V_CALL
ode_monitor(void_p arg,vec_t t,vec_t x[])
{
	vec_t rt;
	vboolean retval;
	particle_system_p ps = (particle_system_p)arg;
	ps->cur_time = t;
	ps->cur_phase = x;
	if(ps->monitor && ps->monitoring_context)
		retval = ps->monitor(ps); /* FIXME */
	else
	{
		conlPrintf(LOG_TRACE,"ode_monitor(): time %g\n",t);
		retval = true;
	}
	retval = retval && monitorMain(ps);
	if(retval)
	{
		rt = timerTime(ps->rt_timer);
		if(t >= ps->solution_time_limit)
		{
			conPrintf("ode_monitor(): time limit (%g) reached "
				"(realtime %g).\n",ps->solution_time_limit,rt);
			retval = false;
		}
		else if(rt >= ps->solution_realtime_limit)
		{
			conPrintf("ode_monitor(): realtime limit (%g) reached "
				"(time %g).\n",ps->solution_realtime_limit,t);
			retval = false;
		}
	}
	if(retval)
		return true;
	ps->end_monitoring(ps);
	ps->end_monitoring = NULL;
	ps->monitor = NULL;
	ps->monitoring_context = NULL;
	ps->ode_monitor = NULL;
	ps->ode_monitor_arg = NULL;
	return false;
}

vboolean
monitorInit(particle_system_p ps)
{
	u32_t i, num_monroutines, num_writeroutines;
	vboolean retval;
	char_p denotation;
	if(!ps)
		return false;
	ps->ode_monitor = NULL;
	ps->ode_monitor_arg = NULL;
	ps->monitor = NULL;
	ps->end_monitoring = NULL;
	ps->monitoring_context = NULL;
	if(!ps->nodraw) /* FIXME */
		retval = init_draw_context(ps);
	else if(ps->output_maxscript)
		retval = init_ms_context(ps);
	else
		retval = init_psy_context(ps);
	if(!retval)
		return false;
	ps->ode_monitor = ode_monitor;
	ps->ode_monitor_arg = ps;
	num_monroutines = vpsCount(&ps->monroutines);
	if(!num_monroutines)
		return true;
	for(num_writeroutines = i = 0;i < num_monroutines;i++)
	{
		denotation = monitorDenotationForEntry(vpsGet(&ps->monroutines, i));
		if(denotation && denotation[0])
			num_writeroutines++;
	}
	if(!num_writeroutines)
		return true;
	logFormatString(ps->tmpname,"%s/kinetics.csv",ps->outdir);
	ps->kinetics_outfile = fileOpenWrite(ps->tmpname);
	if(LIBV_INVALID_HANDLE == ps->kinetics_outfile)
		return false;
	/*
	 * Print header to ps->kinetics_outfile
	 */
	filePrintf(ps->kinetics_outfile, "Time");
	for(i = 0;i < num_monroutines;i++)
	{
		denotation = monitorDenotationForEntry(vpsGet(&ps->monroutines, i));
		if(denotation && denotation[0])
			filePrintf(ps->kinetics_outfile, ";%s_av;%s_std", denotation, denotation);
	}
	fileWrite(ps->kinetics_outfile,"\n",1);
	return true;
}

void
monitorShutdown(particle_system_p ps)
{
	if(!ps)
		return;
	if(ps->end_monitoring)
		ps->end_monitoring(ps);
	ps->ode_monitor = NULL;
	ps->ode_monitor_arg = NULL;
	ps->monitor = NULL;
	ps->end_monitoring = NULL;
	ps->monitoring_context = NULL;
	if(LIBV_INVALID_HANDLE != ps->kinetics_outfile)
	{
		fileClose(ps->kinetics_outfile);
		ps->kinetics_outfile = LIBV_INVALID_HANDLE;
	}
}
