#include <v_lib.h>
#include "psdl.h"
#include "util.h"
#include "psys.h"
#include "ctrl.h"
#include "rh_side.h"
#include "monitor.h"
#include "phasesp.h"

/*
#define	DEBUG_INPUT	"test.psdl"
*/
#define	LOG_LEVEL	LOG_INFO

/*
 * this is a hack
 */
#define	NUM_NEAREST		4
#define	N_DIST			2e-6

#define	SUBDIVX	3
#define	SUBDIVY	3
#define	SUBDIVZ	3

vhandle_t out_av_pt;
vhandle_t out_av_nb;
vhandle_t out_num_pt;
vhandle_t out_h_coeff;
vhandle_t out_av_and_dev;

void hack_init(void)
{
	unsigned maxc = SUBDIVX * SUBDIVY * SUBDIVZ;
	out_av_pt = fileOpenWrite("av_pt.txt");
	out_av_nb = fileOpenWrite("av_nb.txt");
	out_num_pt = fileOpenWrite("num_pt.txt");
	out_h_coeff = fileOpenWrite("h_coeff.txt");
	out_av_and_dev = fileOpenWrite("av_and_dev.txt");
	filePuts(out_av_and_dev,"# Поля файла:");
	filePuts(out_av_and_dev,"# 1 - Время");
	filePuts(out_av_and_dev,"# 2 - Среднее для X");
	filePuts(out_av_and_dev,"# 3 - Среднее квадртичное отклонение для X");
	filePrintf(out_av_and_dev,"# (x_i - для i-й частицы - среднее расстояние до %u ближайших соседей)\n",NUM_NEAREST);
	filePuts(out_av_and_dev,"# 4 - Среднее для Y");
	filePuts(out_av_and_dev,"# 5 - Среднее квадртичное отклонение для Y");
	filePrintf(out_av_and_dev,"# (y_j - для j-го ближайшего соседа (j=1..%u) - среднее расстояние по числу частиц)\n",NUM_NEAREST);
	filePuts(out_av_and_dev,"# 6 - Среднее для Z");
	filePuts(out_av_and_dev,"# 7 - Среднее квадртичное отклонение для Z");
	filePrintf(out_av_and_dev,"# (z_i - для i-й частицы - число частиц, расстояние до которых от данной не более %g)\n",N_DIST);
	filePuts(out_av_and_dev,"# 8 - Среднее для U");
	filePuts(out_av_and_dev,"# 9 - Среднее квадртичное отклонение для U");
	filePrintf(out_av_and_dev,"# (u_k - для k-й ячейки (k=1..%u) - число частиц)\n",maxc);
}

void hack_done(void)
{
	fileClose(out_av_pt);
	fileClose(out_av_nb);
	fileClose(out_num_pt);
	fileClose(out_h_coeff);
	fileClose(out_av_and_dev);
}

void print_float(vhandle_t out,vec_t val)
{
	char str[256], *ptr;
	sprintf(str,"%g",val);
	for(ptr=str;*ptr;ptr++)
		if(*ptr == '.')
			*ptr = ',';
	fileWrite(out,str,ptr-str);
}

void av_and_std_dev(vector_p X,u32_t len)
{
	u32_t i;
	vec_t av, std_dev, d;
	av = 0;
	for(i=0;i<len;i++)
		av += X->elements[i];
	av /= len;
	std_dev = 0;
	for(i=0;i<len;i++)
	{
		d = X->elements[i] - av;
		std_dev += d * d;
	}
	std_dev = sqrt(std_dev / (vec_t)(len-1));
	print_float(out_av_and_dev,av);
	fileWrite(out_av_and_dev,"\t",1);
	print_float(out_av_and_dev,std_dev);
	fileWrite(out_av_and_dev,"\t",1);
}

void find_mins(matrix_p src)
{
	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;
		}
	}
}

void av_nearset_dists(particle_system_p ps)
{
	u32_t i, j, count;
	vec3_t delta;
	vec_t s;
	matrix_p dists;
	vector_p sums;
	dists = matAlloc(ps->num_particles,ps->num_particles);
	sums = vecAlloc(ps->num_particles);
	/* compute all distances */
	for(i=0;i<ps->num_particles;i++)
	{
		dists->cell[i][i] = MAXFLOAT;
		for(j=i+1;j<ps->num_particles;j++)
		{
			vec3Sub(delta,PS_POS(ps->cur_phase,i),PS_POS(ps->cur_phase,j));
			dists->cell[i][j] = dists->cell[j][i] = 
				vec3Abs(delta) - ps->particle_radius[i] - ps->particle_radius[j];
		}
	}
	/* move 1st NUM_NEAREST minimals to 1st cols */
	find_mins(dists);
	/* compute average by particles */
	for(i=0;i<ps->num_particles;i++)
	{
		s = 0;
		for(j=0;j<NUM_NEAREST;j++)
			s += dists->cell[i][j];
		s /= NUM_NEAREST;
		sums->elements[i] = s;
		print_float(out_av_pt,s);
		if(i != (ps->num_particles-1))
			fileWrite(out_av_pt,"\t",1);
		else
			fileWrite(out_av_pt,"\n",1);
	}
	av_and_std_dev(sums,ps->num_particles);
	/* and by neighbours */
	for(j=0;j<NUM_NEAREST;j++)
	{
		s = 0;
		for(i=0;i<ps->num_particles;i++)
			s += dists->cell[i][j];
		s /= ps->num_particles;
		sums->elements[j] = s;
		print_float(out_av_nb,s);
		if(j != (NUM_NEAREST-1))
			fileWrite(out_av_nb,"\t",1);
		else
			fileWrite(out_av_nb,"\n",1);
	}
	av_and_std_dev(sums,NUM_NEAREST);
	/* compute number of particles that are closer than N_DIST */
	for(i=0;i<ps->num_particles;i++)
	{
		count = 0;
		for(j=0;j<ps->num_particles;j++)
			if(dists->cell[i][j] < N_DIST)
				count++;
		sums->elements[i] = count;
		if(i != (ps->num_particles-1))
			filePrintf(out_num_pt,"%u\t",count);
		else
			filePrintf(out_num_pt,"%u\n",count);
	}
	av_and_std_dev(sums,ps->num_particles);
	memFree(dists);
	memFree(sums);
}

vboolean pt_inside(vec_p pt,vec3_t mins,vec3_t maxs)
{
	int i;
	for(i=0;i<3;i++)
		if(pt[i] < mins[i] || pt[i] > maxs[i])
			return false;
	return true;
}

void h_coeff(particle_system_p ps)
{
	vector_p sums;
	vec_p phase = ps->cur_phase;
	unsigned i, j, k, l, m, count;
	unsigned maxc = SUBDIVX * SUBDIVY * SUBDIVZ;
	vec3_t mins, maxs, ext, dim, p1, p2;
	sums = vecAlloc(maxc);
	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] / SUBDIVX;
	dim[1] = ext[1] / SUBDIVY;
	dim[2] = ext[2] / SUBDIVZ;
	count = 0;
	for(i=0;i<SUBDIVX;i++)
		for(j=0;j<SUBDIVY;j++)
			for(k=0;k<SUBDIVZ;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->elements[count] = m;
				count++;
				if(count < maxc)
					filePrintf(out_h_coeff,"%u\t",m);
				else
					filePrintf(out_h_coeff,"%u\n",m);
				
			}
	av_and_std_dev(sums,maxc);
	memFree(sums);
}

#define	DELTA_T		1
vec_t last_time = -DELTA_T;

#define SKIPS	10
int scount = SKIPS;
extern vboolean hack_draw;
extern void ms_save_frame_hack(particle_system_p ps);

vboolean monitor_extra(particle_system_p ps)
{
	if((ps->cur_time - last_time) < DELTA_T)
		return true;
	last_time = ps->cur_time;
	print_float(out_av_and_dev,last_time);
	fileWrite(out_av_and_dev,"\t",1);
	av_nearset_dists(ps);
	h_coeff(ps);
	fileWrite(out_av_and_dev,"\n",1);
	printf("Time: %g\n",last_time);
	scount++;
	if(scount >= SKIPS)
	{
		scount = 0;
		hack_draw = true;
		ms_save_frame_hack(ps);
	}
	return true;
}

/*
 * end of hack
 */

void
run_simulation(particle_system_p ps)
{
	ps->ode_solver = odeCreateSolver(ps->num_particles * PHASESPACE_DIM,
		problem_right_side,false);
	if(!ps->ode_solver)
		return;
	if(!init_monitoring(ps))
	{
		odeDestroySolver(ps->ode_solver);
		return;
	}
	odeSetMonitor(ps->ode_solver,ps->ode_monitor,
		ps->ode_monitor_arg,(vec_t)1.0 / ps->saves_per_second,true);
	odeSetTimeout(ps->ode_solver,ps->solution_RK_timeout);
	hack_init();
	timerStart(ps->rt_timer);
	odeIntegrate(ps->ode_solver,ps->phase,0,ps->solution_time_limit,
		ps->solution_RK_tolerance,1e-4,1e-14,ps);
	timerStop(ps->rt_timer);
	hack_done();
	shutdown_monitoring(ps);
	odeDestroySolver(ps->ode_solver);
}

void help_main(char *self)
{
	puts("Particle system simulation program ver. 0.2");
	printf("Usage: %s <scriptfile>, where scriptfile is an PSDL script.\n",
		self);
	puts("For brief description of PSDL, try --help-PSDL.");
	puts("For brief description of controllers, try --help-controllers.");
}

V_LOCAL char *psdl_syntax=
	"# comment\n"
	"random_seed <integer random_seed>\n"
	"number_of_particles <integer>\n"
	"particle_radius <uniform float_min float_max>\n"
	"particle_density <uniform float_min float_max>\n"
	"particle_minimal_distance <float minimal_distance>\n"
	"particle_distribution_domain <sphere <vec3 pos> <float radius>> \\\n"
	"\t| <box <vec3 pos> <vec3 extents>> \\\n"
	"\t| <cylinder <vec3 lower_base_center> <float radius <float height>>\n"
	"boundary_plane <vec3 plane_normal> <float dist> <float_controller signed_scalar_f_for_given_distance>\n"
	"boundary_sphere <vec3 center> <float radius> <float_controller signed_scalar_f_for_given_distance>\n"
	"binary_interaction <float_controller unsigned_scalar_f_for_given_distance>\n"
	"environmental_interaction <viscous <ETA> <vec3_controller ENV_V>>\n"
	"unary_force <vec3_controller force>\n"
	"unary_accel <vec3_controller accel>\n"
	"saves_per_second <integer saves_per_second>\n"
	"solution_time_limit <float time_limit>\n"
	"solution_realtime_limit <float realtime_limit>\n"
	"solution_RK_tolerance <float tolerance>\n"
	"solution_RK_timeout <float timeout>\n"
	"[output_psy | output_maxscript]\n\n";

void help_PSDL(void)
{
	fwrite(psdl_syntax,strlen(psdl_syntax),1,stdout);
}

#ifndef DEBUG_INPUT
int main(int argc, char *argv[])
{
#else	/* DEBUG_INPUT */
int main(void)
{
	int argc=2;
	char *argv[]={"main",DEBUG_INPUT};
#endif	/* DEBUG_INPUT */
	particle_system_p ps;

	logStartConsoleLog();
	logUpperLevel(LOG_LEVEL);

	if(argc != 2)
	{
		help_main(argv[0]);
		return -1;
	}
	if(!strcasecmp(argv[1],"--help"))
	{
		help_main(argv[0]);
		return 0;
	}
	if(!strcasecmp(argv[1],"--help-PSDL"))
	{
		help_PSDL();
		return 0;
	}
	if(!strcasecmp(argv[1],"--help-controllers"))
	{
		help_controllers();
		return 0;
	}
	util_init();
	ps = parse_psdl_script(argv[1]);
	if(ps == NULL)
	{
		util_shutdown();
		conlPrintf(LOG_ERROR,"main(): error parsing '%s'\n",argv[1]);
		return -1;
	}
	memSetOpaque(10);
	run_simulation(ps);
	free_particle_system(ps);
	util_shutdown();
#ifdef	DEBUG_INPUT	
	memPrintStats();
	memPrintMemStats();
	memPrintObjStats();
#endif	/* DEBUG_INPUT */
	return 0;
}
