/*
 * Simulation of particle's interaction.
 * Version 0.1.
 * #include <GPL-disclaimer>
 */

/*
 * LibV contains all usefull stuff I’m using here; someday (when it matures a bit)
 * I will put it on ftp://gasa.penza.com.ru/pub/freeware/libv
 * If your, reader, REALLY want to compile this, email me :)
 */
#include <v_lib.h>
#include "ljsetup.h"
#include "ljutil.h"

/*
 * Uncomment next line to enable anti-aliased lines.
 * Note: AA not works properly under current XFree86 DRI
 * drivers (at least, on my Radeon QD :). With NV card
 * all is Ok on both platforms (Win32 and Linux)
#define	AA_ENABLED
*/

/*
 * -----------------------------------------------------------------------
 *              Part 1: model of system being investigated
 * -----------------------------------------------------------------------
 */

typedef struct particle_s
{
	vec3_t	pos;	/* position of particle */
	vec3_t	vel;	/* velocity of particle */
	vec_t	radius;	/* radius of particle */
	vec_t	_6_pi_eta_r;	/* viscous resistance */
	vec_t	mass;	/* gravital mass: 4/3 * Pi * radius^3 * density */
	vec_t	rm;		/* reciprocal inertial mass (1/mass) */
}	particle_t;

#define		NUM_OBJECTS		15
#define		MAX_OBJECTS		1000

u32_t	FPS				 = 10;

/*
 * Deep of minima on the potential curve
 */
vec_t	U0			= 1e-19;
/*
 * Radius and density of the particles
 */
vec_t	OBJ_R		= 1e-6;
vec_t	density 	= 2500;
/*
 * Distance of equilibrium (measured between particles' surfaces)
 */
vec_t	r0			= 2e-7;
/*
 * Viscousity
 */
vec_t	ETA			= 1;
/*
 * Maximal distance from origin in initial state
 */
vec_t				POS_MAX	= 2e-5;
/*
 * Properties of the potential barrier
 */
vec_t		potential_barrier_width		= 1e-7;
vec_t		potential_barrier_height	= 1e-6;



/*
 * Static array shared between 'solver' and 'user'
 */
particle_t	obj[MAX_OBJECTS];
vec_t		simulation_time;
spinlock_t	obj_array_lock = SPINLOCK_UNLOCKED;


#define F_F(r,v)		(6 * M_PI * ETA * r * v)	/* Friction force */

/*
 * We force particles to be close to each other by introducing
 * portential barrier. If particle falls out of domain, we add extra
 * force directed such way that particle moves towards domain's center.
 *
 * There are a number of ways to do this.
 *  	1. The most intuitive 'parabolic' potential which has explicit
 *		physical analogy (elastic force):
 *			U = 0, r <= r_b;
 *			U = \frac{k}{2} * (r-r_b)^2, r > r_b;
 *			\bold{F} = 0, r <= r_b;
 *			\bold{F} = - k * \bold{r} * (1-\frac{r_b}{r}), r > r_b,
 *		where:
 *			\bold{F} - force, acting on particle;
 *			k - elasticity constant;
 *			\bold{r}, r - radius-vector and it modulus, respectively;
 *			r_b - radius of potential barrier.
 *		2. More restrictive 'exponential' potential:
 *			U = 0, r <= r_b;
 *			U = k * (cosh(r-r_b)-1), r > r_b;
 *			\bold{F} = 0, r <= r_b;
 *			\bold{F} = - k * \frac{\bold{r}}{r} * sinh(r-r_b), r > r_b.
 *		3. The above cases are axial-symmetric. We can also use rectangular
 *	potential function:
 *			\bold{F} = 0, r is inside box;
 *			\bold{F} = k * \bold{n} * len_from_wall,
 *		where:
 *			\bold{n} - vector drawn from outside box (perpendicular to wall);
 *			len_from_wall - distance from wall.
 */

vboolean strafe_parabolic(vec3_t frc,vec3_t pos)
{
	vec_t r = vec3Abs(pos);
	if(r < potential_barrier_width)
		return false;
	vec3Scale(frc,pos,
		potential_barrier_height * (potential_barrier_width - r));
	return true;
}

V_LOCAL vec_t hyperbolic_sine(vec_t x)
{
	return (vec_t)0.5 * (vec_t)(exp(x) - exp(-x));
}

vboolean strafe_sinh(vec3_t frc,vec3_t pos)
{
	vec_t r = vec3Abs(pos);
	if(r < potential_barrier_width)
		return false;
	vec3Scale(frc,pos,potential_barrier_height 
		* hyperbolic_sine(potential_barrier_width - r) / r);
	return true;
}

vboolean strafe_rectangular(vec3_t frc,vec3_t pos)
{
	int i;
	vboolean outside = false;
	for(i=0;i<3;i++)
	{
		if(pos[i] < -potential_barrier_width)
		{
			frc[i] = (-potential_barrier_width - pos[i]) * potential_barrier_height; /* positive */
		} else if(pos[i] > potential_barrier_width)
		{
			frc[i] = (potential_barrier_width - pos[i]) * potential_barrier_height; /* negative */
		} else
		{
			frc[i] = 0;
			continue;
		}
		outside = true;
	}
	return outside;
}

vboolean (*strafe_force)(vec3_t frc,vec3_t pos) = strafe_parabolic;

/*
 * For given distance, returns force modulus.
 * Assumes that particles' interaction can be described by
 * Lennard-Jones potential:
 * U=U_0((\frac{r_0}{r})^{12}-2(\frac{r_0}{r})^{6}),
 * where:
 *  U_0 - modulus of energy at minimum;
 *  r_0 - position of minimum;
 *  r - current position.
 * Force calculated as: \frac{d}{dr}(U(r))|_{r=dist}
 * Input:
 *  dist - distance to use in force(distance).
 */
static vec_t
LJ_modulus(vec_t dist)
{
	vec_t rd = (vec_t)1.0 / dist; /* reciprocal distance */
	vec_t r0r = r0 * rd;
	vec_t p6;
	/*
	 * First derivative of Lennard - Jones potential:
	 * d/dr(U0*((r0/r)^12 - 2*(r0/r)^6))
	 */
	p6 = r0r * r0r * r0r;
	p6 *= p6;
	return 12 * U0 * rd * p6 * (1 - p6);
}

/*
 * Calcalates force acting from all particles to particle #index.
 * \bold{F} = \sum_i\frac{\bold{r}_i * LJ_modulus(|\bold{r}_i|-(s1+s2))}{|\bold{r}_i|},
 * where:
 *  \bold{r}_i - vector drawn from particle #i to partile #index;
 *  s1, s2 - sizes of #i and #index, respectively.
 */
void LJ_force(vec3_t force_ret,int index,vec3_t pos,vec3_t other_pos[])
{
	int i;
	vec3_t dr,df;
	vec_t r, ar, f;
	vec3Zero(force_ret);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		if(i == index)
			continue;
		vec3Sub(dr,other_pos[i],pos);
		ar = vec3Abs(dr);
		r = ar - (obj[index].radius + obj[i].radius);
		if(r < 0)
			r = 1e-15;
		f = LJ_modulus(r);
		vec3Scale(df,dr,f / ar);
		vec3Add(force_ret,force_ret,df);
	}
	if(strafe_force(df,pos))
		vec3Add(force_ret,force_ret,df);
}

/*
 * -----------------------------------------------------------------------
 * Part 2: intial conditons
 */
#if 0
V_LOCAL void regular_pos(vec3_t pos_ret)
{
	/*
	static int count = 0;
	vec_t phase = M_2PI * (vec_t)(count++) / (vec_t)(NUM_OBJECTS);
	pos_ret[0] = 2 * cos(phase);
	pos_ret[1] = 2 * sin(phase);
	pos_ret[2] = frand(MIN_Z,MAX_Z);
	*/
}
#endif

/*
 * Returns 'true' if 'pos' is away to 'min_dist' from objects' 0..n-1 surfaces
 */
vboolean is_away(vec3_t pos,vec_t min_dist,u32_t n)
{
	u32_t i;
	vec_t d;
	vec3_t dist;
	for(i=0;i<n;i++)
	{
		vec3Sub(dist,obj[i].pos,pos);
		d = vec3Abs(dist) - obj[i].radius - obj[n-1].radius;
		if(d < min_dist)
			return false;
	}
	return true;
}
 
/*
 * Routine to call from solver_init()
 */
void problem_initial_value(void)
{
	int i, count;
	vec3_t mins, maxs;
	do
	{
	    for(i=0;i<NUM_OBJECTS;i++)
    	{
			obj[i].radius = OBJ_R;
			count = 0;
    		do
    		{
				vec3Rand(obj[i].pos,-POS_MAX,POS_MAX);
				/* obj[i].pos[2] = 0; */
				if(++count > 1000)
					break;
			}	while(!is_away(obj[i].pos,2 * r0,i));
			vec3Set(obj[i].vel,0,0,0);
		}
		/* if(count < 1000) */
		/* Compute bounding box */
		bboxClear(mins,maxs);
		for(i=0;i<NUM_OBJECTS;i++)
			bboxAddPoint(mins,maxs,obj[i].pos);
		vec3Sub(maxs,maxs,mins);
	} while(vec3Abs(maxs) < 1e-18);
	/* setup gravital and reciprocal inertial masses */
    for(i=0;i<NUM_OBJECTS;i++)
    {
		obj[i].mass = 4.0 / 3.0 * M_PI * obj[i].radius * obj[i].radius * obj[i].radius * density;
		obj[i].rm = 1.0 / obj[i].mass;
	}
}

/*
 * Routine to call from ODE solver
 */
void problem_right_side(vec3_t pos_deriv,vec3_t vel_deriv,vec3_t pos,vec3_t all_pos[],vec3_t vel,int index)
{
	vec_t fc;
	vec3_t tmp;
	vec3_t frc;
	LJ_force(frc,index,pos,all_pos);
	fc = F_F(obj[index].radius,vec3Abs(vel));
	vec3Scale(tmp,vel,fc);
	vec3Sub(tmp,frc,tmp);
	vec3Scale(vel_deriv,tmp,obj[index].rm);
	vec3Copy(pos_deriv,vel);
}

/*
 * -----------------------------------------------------------------------
 * Part 3: ODE solver
 */
typedef struct solver_setup_s
{
	vec_t	position_error_limit;
	vec_t	velocity_error_limit;
	vec_t	delta_time_limit;
}	solver_setup_t, *solver_setup_p;

static solver_setup_t defsetup = {POS_ERRLIM, VEL_ERRLIM, MAX_DELTAT};

/*
 * Local copy of the phase
 */
vec3_t	mid_pos[MAX_OBJECTS];
vec3_t	mid_vel[MAX_OBJECTS];

vec3_t	end_pos[MAX_OBJECTS];
vec3_t	end_vel[MAX_OBJECTS];

vec3_t	tmp_pos[MAX_OBJECTS];
vec3_t	tmp_vel[MAX_OBJECTS];

#ifdef	HAVE_INT64
u64_t	cycles = 0;
#else	/* !HAVE_INT64 */
u32_t 	cycles = 0;
#endif	/* !HAVE_INT64 */

vec_t	last_time_diff;	/* last value of step in time */
u32_t	last_step_count;  /* times 'last_time_diff' used without change */

/*
 * Copies global phase from obj[] array into local arrays
 */
void solver_retrieve_phase(vec3_t dest_pos[],vec3_t dest_vel[])
{
	int i;
	spin_lock(&obj_array_lock);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec3Copy(dest_pos[i],obj[i].pos);
		vec3Copy(dest_vel[i],obj[i].vel);
	}
	spin_unlock(&obj_array_lock);
}

/*
 * Copies phase from local arrays to global store obj[].
 */
void solver_store_phase(vec3_t src_pos[],vec3_t src_vel[])
{
	int i;
	spin_lock(&obj_array_lock);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec3Copy(obj[i].pos,src_pos[i]);
		vec3Copy(obj[i].vel,src_vel[i]);
	}
	spin_unlock(&obj_array_lock);
}

/*
 * Copies phase from 'src' arrays info 'dest' arrays.
 */
void solver_copy_phase(vec3_t dest_pos[],vec3_t dest_vel[],
	vec3_t src_pos[],vec3_t src_vel[])
{
	memCopy(dest_pos,src_pos,NUM_OBJECTS * sizeof(vec3_t));
	memCopy(dest_vel,src_vel,NUM_OBJECTS * sizeof(vec3_t));
}

/*
 * Solver core (NR: 'algorithm' routine).
 */
V_LOCAL void 
solver_delta(
	vec3_t delta_r_ret, /* OUT: difference of radius-vector */
	vec3_t delta_v_ret, /* OUT: difference of velocity */
	vec3_t all_pos[],   /* IN: positions of all particles */
	vec3_t vel,         /* IN: velocity of this particle */
	vec_t timediff,     /* IN: step in time domain */
	int index           /* IN: index of particle in 'obj' array */)
{
#ifdef	USE_EULER_DELTA
	/*
	 * Euler delta:
	 */
	vec3_t pd,vd,pos;
	vec3Copy(pos,all_pos[index]);
	problem_right_side(pd,vd,pos,all_pos,vel,index);
	vec3Scale(delta_v,vd,timediff);
	vec3Scale(delta_p,pd,timediff);
#else /* !USE_EULER_DELTA */
	/*
	 * TODO: Implement and test second-order Runge-Kutta
	 */
	/*
	 * Fourth order Runge-Kutta
	 */
	vec3_t dp,dv,pos;
	static vec_t one_by_three = (vec_t)1.0 / (vec_t)3.0;
	vec3_t k1[2], k2[2], k3[2], k4[2], tp, tv;
	vec_t halfstep = 0.5 * timediff;
	vec3Copy(pos,all_pos[index]);
	/* first trial */
	problem_right_side(k1[0],k1[1],pos,all_pos,vel,index);
	vec3Scale(k1[0],k1[0],halfstep);
	vec3Scale(k1[1],k1[1],halfstep);
	vec3Add(tp,pos,k1[0]);
	vec3Add(tv,vel,k1[1]);
	/* second trial */
	problem_right_side(k2[0],k2[1],tp,all_pos,tv,index);
	vec3Scale(k2[0],k2[0],halfstep);
	vec3Scale(k2[1],k2[1],halfstep);
	vec3Add(tp,pos,k2[0]);
	vec3Add(tv,vel,k2[1]);
	/* third trial */
	problem_right_side(k3[0],k3[1],tp,all_pos,tv,index);
	vec3Scale(k3[0],k3[0],timediff);
	vec3Scale(k3[1],k3[1],timediff);
	vec3Add(tp,pos,k3[0]);
	vec3Add(tv,vel,k3[1]);
	/* fourth trial */
	problem_right_side(k4[0],k4[1],tp,all_pos,tv,index);
	vec3Scale(k4[0],k4[0],timediff);
	vec3Scale(k4[1],k4[1],timediff);
	/* join all data */
	vec3Scale(k2[0],k2[0],2);
	vec3Scale(k2[1],k2[1],2);
	vec3Scale(k4[0],k4[0],0.5);
	vec3Scale(k4[1],k4[1],0.5);
	vec3Add(dp,k1[0],k2[0]);
	vec3Add(dp,dp,k3[0]);
	vec3Add(dp,dp,k4[0]);
	vec3Add(dv,k1[1],k2[1]);
	vec3Add(dv,dv,k3[1]);
	vec3Add(dv,dv,k4[1]);
	vec3Scale(delta_r_ret,dp,one_by_three);
	vec3Scale(delta_v_ret,dv,one_by_three);
#endif /* !USE_EULER_DELTA */
}

/*
 * solver_update_phase(): calls solver_delta() for all particles.
 * Uses local arrays 'pos' and 'vel'.
 */
void solver_update_phase(vec3_t pos[],vec3_t vel[],vec_t timediff)
{
	int i;
	vec3_t dv,dp;
	for(i=0;i<NUM_OBJECTS;i++)
	{
		solver_delta(dp,dv,pos,vel[i],timediff,i);
		vec3Add(vel[i],vel[i],dv);
		vec3Add(pos[i],pos[i],dp);
	}
}

void lerp_vectors(vec3_t dest_lerp[],vec3_t lerp_with[],vec_t midpoint)
{
	int i;
	vec3_t v;
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec3Sub(v,lerp_with[i],dest_lerp[i]);
		vec3Scale(v,v,midpoint);
		vec3Add(dest_lerp[i],dest_lerp[i],v);
	}
}

vboolean step_too_large(vec3_t pos1[],vec3_t vel1[],vec3_t pos2[],vec3_t vel2[])
{
	int i;
	vec3_t diff;
	vec_t re = defsetup.position_error_limit;
	vec_t ve = defsetup.velocity_error_limit;
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec3Sub(diff,pos1[i],pos2[i]);
#if 0
		/* faster, but may cause overflow */
		if(vec3Dot(diff,diff) > re * re)
			return true;
		vec3Sub(diff,vel1[i],vel2[i]);
		if(vec3Dot(diff,diff) > ve * ve)
			return true;
#endif
		if(vec3Abs(diff) > re)
			return true;
		vec3Sub(diff,vel1[i],vel2[i]);
		if(vec3Abs(diff) > ve)
			return true;
	}
	return false;
}

void solver_step(vec_t time_diff)
{
	vec_t half_diff;
	solver_retrieve_phase(mid_pos,mid_vel);
	solver_copy_phase(end_pos,end_vel,mid_pos,mid_vel);
	/* First trial: full step */
	solver_update_phase(end_pos,end_vel,time_diff);
	for(;;)
	{
		cycles++;
		half_diff = time_diff * 0.5;
		solver_update_phase(mid_pos,mid_vel,half_diff);
		solver_copy_phase(tmp_pos,tmp_vel,mid_pos,mid_vel);
		solver_update_phase(mid_pos,mid_vel,half_diff);
		if(!step_too_large(end_pos,end_vel,mid_pos,mid_vel))
			break;
		solver_retrieve_phase(mid_pos,mid_vel);
		solver_copy_phase(end_pos,end_vel,tmp_pos,tmp_vel);
		time_diff = half_diff;
	}
#define	LERP_BACKPOS	(vec_t)(-1)
	lerp_vectors(mid_pos,end_pos,LERP_BACKPOS);
	lerp_vectors(mid_vel,end_vel,LERP_BACKPOS);
	solver_store_phase(mid_pos,mid_vel);
	
	simulation_time += time_diff;
	if(IsZero(last_time_diff - time_diff))
		last_step_count++;
	else
		last_step_count = 0;
	last_time_diff = time_diff;
}

void solver_init()
{
	problem_initial_value();
	last_time_diff = defsetup.delta_time_limit;
}

/*
 * Scheduling routine
 */
void solver_solve(
	vec_t max_tdelta, /* Upper limit in simulation time */
	u32_t min_rtdelta,u32_t max_rtdelta) /* Limist in real time (in milliseconds) */
{
	u32_t rt_start, rt, rt_delay, solve_count;
	vec_t t_start, t_delta;
	t_start = simulation_time;
	rt_start = timeTickCount();
	solve_count = 0;
	for(;;)
	{
		rt = timeTickCount() - rt_start;
		if(rt >= max_rtdelta)
			break;
		solver_step(last_time_diff);
		solve_count++;
		if(solve_count == 10)
		{
			if(last_time_diff < defsetup.delta_time_limit && last_step_count > 5)
				last_time_diff *= 2;
			solve_count = 0;
		}
		t_delta = simulation_time - t_start;
		if(t_delta > max_tdelta)
			break;
	}
	if(rt < min_rtdelta)
	{
		rt_delay = min_rtdelta - rt;
		timeDelay(rt_delay);
	}
}

void solver_status()
{
}

/*
 * -----------------------------------------------------------------------
 * Part 4: drawing stuff
 */
typedef struct drawstate_s
{
	GLfloat rotx, rotz;
	GLfloat zoom, vdx, vdy, vdz;
	vboolean inner_grid, outer_grid, ortho;
	int lod;
}	drawstate_t, *drawstate_p;

V_LOCAL drawstate_t drawstate;

V_LOCAL void draw_circle(GLfloat r,int segs)
{
	int i;
	GLfloat phi = 0;
	GLfloat dphi = (GLfloat)(M_2PI / segs);
	GLfloat x,y;
	glBegin(GL_LINE_LOOP);
	for(i=0;i<segs;i++,phi+=dphi)
	{
		x = r * (GLfloat)cos(phi);
		y = r * (GLfloat)sin(phi);
		glVertex2f(x,y);
	}
	glEnd();
}

V_LOCAL void draw_3circles(GLfloat r,int segs)
{
	draw_circle(r,segs);
	glRotatef(90,1,0,0);
	draw_circle(r,segs);
	glRotatef(90,0,1,0);
	draw_circle(r,segs);
}

#define	INNER_SEGS		16
#define	OUTER_SEGS		32

V_LOCAL void draw_particle(int lod,vec3_t pos,vec_t radius,vec_t r_minU)
{
	glPushMatrix();
	glTranslated(pos[0],pos[1],pos[2]);
	if(lod > 0)
	{
		glColor4f(1,1,1,0.65f);
		draw_3circles((GLfloat)radius,INNER_SEGS);
	}
	if(lod > 1)
	{
		glColor4f(0,1,1,0.5f);
		draw_3circles((GLfloat)r_minU,OUTER_SEGS);
	}
	glPopMatrix();
}

V_LOCAL void draw_xOy_grid(vec3_t origin,vec_t width,vec_t height,u32_t num_lines)
{
	u32_t i;
	vec3_t p, dp;
	vec3Copy(p,origin);
	p[0] -= (vec_t)0.5 * width;
	p[1] -= (vec_t)0.5 * height;
	vec3Set(dp,width,height,0);
	vec3Scale(dp,dp,(vec_t)1 / (num_lines-1));
	glBegin(GL_LINES);
		/* grid with lines along Oy direction */
		for(i=0;i<num_lines;i++)
		{
			VGL_VERTEX3(p[0]+i*dp[0],p[1],p[2]);
			VGL_VERTEX3(p[0]+i*dp[0],p[1]+height,p[2]);
		}
		/* grid with lines along Ox direction */
		for(i=0;i<num_lines;i++)
		{
			VGL_VERTEX3(p[0],p[1]+i*dp[1],p[2]);
			VGL_VERTEX3(p[0]+width,p[1]+i*dp[1],p[2]);
		}
	glEnd(); /* GL_LINES */
}

V_LOCAL void draw_2_xOy_grids(vec3_t origin,vec3_t center,vec_t width,vec_t height,u32_t major_lines,u32_t minor_lines)
{
	vec_t dw, dh;
	vec3_t neworg;
	if(minor_lines < 2 || major_lines < 2)
		return;
	dw = width / (major_lines - 1);
	dh = height / (major_lines - 1);
	vec3Copy(neworg,center);
	neworg[0] -= (vec_t)fmod(center[0] - origin[0],dw);
	neworg[1] -= (vec_t)fmod(center[1] - origin[1],dh);
#ifdef	AA_ENABLED
	glLineWidth(1.5f);
#endif	/* AA_ENABLED */
	draw_xOy_grid(neworg,width,height,minor_lines);
#ifdef	AA_ENABLED
	glLineWidth(3);
#endif	/* AA_ENABLED */
	draw_xOy_grid(neworg,width,height,major_lines);
}

V_LOCAL void begin_overlay(int char_width, int char_height)
{
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glOrtho(0,char_width,0,char_height,-1,1);
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();
	VGL_TRANSLATE(0,char_height-1,0);
}

V_LOCAL void end_overlay(void)
{
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}

vec_t scale_base = 3;
vec_t view_scale, scale_exp, scale_mnt;
int seed = -1;

V_LOCAL void draw_status()
{
	solver_status();
	begin_overlay(40,30);
	vglPrintf(
		"Seed %d\n"
		"Cycle %u\n"
		"Time %4.3g\n"
		"Last step %g\n"
		"Last step used %d times\n"
		"View scale %g * %1.3g * %g\n",seed,
		cycles,simulation_time,last_time_diff,last_step_count,
		view_scale,scale_mnt,drawstate.zoom);
	end_overlay();
}

V_LOCAL void draw_reset_state(void)
{
	drawstate.rotx = drawstate.rotz = 0;
	drawstate.zoom = 1;
	drawstate.vdx = drawstate.vdy = drawstate.vdz = 0;
	drawstate.inner_grid = drawstate.outer_grid = true;
	drawstate.ortho = true;
	drawstate.lod = 1;
}

V_LOCAL void mnt_and_exp(vec_t *mnt_ret,vec_t *exp_ret,vec_t x,vec_t base)
{
	vec_t sg;
	double ex, y;
	if(fabs(x) < 1e-50)
	{
		/* hack */
		if(mnt_ret)
			*mnt_ret = 1;
		if(exp_ret)
			*exp_ret = -50;
		return;
	}
	else
		if(x < 0)
		{
			sg = -1;
			x = -x;
		}
		else
			sg = 1;
	y = log10(x) / log10(base);
	ex = floor(y);
	if(exp_ret)
		*exp_ret = (vec_t)ex;
	if(mnt_ret)
		*mnt_ret = sg * (vec_t)pow(base,y - ex);
}

V_LOCAL void draw(void)
{
	int i;
	vec3_t mins, maxs, origin, center;
	glClear(GL_COLOR_BUFFER_BIT);
	glPushMatrix();
	
    glRotatef(drawstate.rotx,1,0,0);
    glRotatef(drawstate.rotz,0,0,1);
	/*
	 * Setup view transform from current positions
	 */
	bboxClear(mins,maxs);
	for(i=0;i<NUM_OBJECTS;i++)
		bboxAddPoint(mins,maxs,obj[i].pos);
	vec3Zero(origin);
	vec3Sub(center,maxs,mins);
	view_scale = vec3Abs(center);
	if(view_scale != 0.0)
	{
		view_scale = 1.0 / view_scale;
	}
	else
		view_scale = 1e5;
	/*
	 * Round view_scale to nearest power of 'scale_base'
	 */
	mnt_and_exp(&scale_mnt,&scale_exp,view_scale,scale_base);
	view_scale = pow(scale_base,scale_exp);
	vec3Add(center,mins,maxs);
	center[2] = 0;
	vec3Scale(center,center,0.5f);
	VGL_SCALE(scale_mnt,scale_mnt,scale_mnt);
	VGL_TRANSLATE(-center[0]+drawstate.vdx,-center[1]+drawstate.vdy,-center[2]+drawstate.vdz);
	glScalef(drawstate.zoom,drawstate.zoom,drawstate.zoom);
	/*
	 * Draw coordsys-aligned grid on xOy plane
	 */
	vec3Set(origin,0,0,0);
	if(drawstate.outer_grid)
	{
		glColor4f(.5f,1.0f,0,0.3f);
		draw_2_xOy_grids(origin,center,8,8,3,9);
	}
	if(drawstate.inner_grid)
	{
		glColor4f(.5f,1.0f,0,0.6f);
		draw_2_xOy_grids(origin,center,2,2,3,9);
	}
	VGL_SCALE(view_scale,view_scale,view_scale);
	/* Origin */
	glColor4f(1,1,1,0.7f);
	glPointSize(6);
	glBegin(GL_POINTS);
		VGL_VERTEX3V(origin);
	glEnd();
	/* Particles */
	if(drawstate.lod > 0)
	{
#ifdef	AA_ENABLED
		glLineWidth(1.5f);
#endif	/* AA_ENABLED */
		for(i=0;i<NUM_OBJECTS;i++)
			draw_particle(drawstate.lod,obj[i].pos,obj[i].radius,obj[i].radius+r0);
	}
	else
	{
		glColor4f(1,0,1,0.8f);
		glBegin(GL_POINTS);
		for(i=0;i<NUM_OBJECTS;i++)
			VGL_VERTEX3V(obj[i].pos);
		glEnd();
	}
	/* Ticks from particles to zero plane */
	if(drawstate.inner_grid || drawstate.outer_grid)
	{
#ifdef	AA_ENABLED
		glLineWidth(1.7f);
#endif	/* AA_ENABLED */
		glColor4f(1,0,0,0.8f);
		glBegin(GL_LINES);
			for(i=0;i<NUM_OBJECTS;i++)
			{
				VGL_VERTEX3V(obj[i].pos);
				glVertex3f((GLfloat)obj[i].pos[0],(GLfloat)obj[i].pos[1],0);
			}
		glEnd();
	}
	/* Draw status */
#ifdef	AA_ENABLED
	glLineWidth(1.3f);
#endif	/* AA_ENABLED */
	glColor4f(1,0.95f,0.93f,0.8f);
	draw_status();
	/* Done */
	glPopMatrix();
	vglSwapBuffers();
}

void ogl_setup_view(u32_t view_width,u32_t view_height,vboolean ortho,vboolean clip)
{
	GLfloat aspect;
	if(view_width && view_height)
	{
		glViewport(0,0,view_width,view_height);
		if(clip)
		{
			glScissor(0,0,view_width,view_height);
			glEnable(GL_SCISSOR_TEST);
		}
		else
			glDisable(GL_SCISSOR_TEST);
		aspect = (GLfloat)view_width / (GLfloat)view_height;
	}
	else
		aspect =  4.0f / 3.0f;
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	if(ortho)
		glOrtho(-aspect,aspect,-1,1,1,50);
	else
		glFrustum(-aspect,aspect,-1,1,1.5f,50);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0,0,-4);
}

vboolean ogl_setup(u32_t view_width,u32_t view_height,vboolean fullscreen)
{
	char *title=
		"Initial value problem for LJ-particles (RK4 + Runge corrector)";
	if(fullscreen)
		vglCreateFullscreenWindow(title,view_width,view_height,0,0);
	else
		vglCreateWindow(title,0,0,view_width,view_height);
	if(vglGetCurrentWindow()==LIBV_INVALID_HANDLE)
		return false;
	vglLoadDefaultFont();
	draw_reset_state();
	ogl_setup_view(view_width,view_height,drawstate.ortho,false);
	/* Current matrix mode is MODELVIEW */
#ifdef	AA_ENABLED
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
	glEnable(GL_LINE_SMOOTH);
	glEnable(GL_POINT_SMOOTH);
#endif	/* AA_ENABLED */
	return true;
}

void ogl_shutdown(void)
{
	vglDestroyWindow(vglGetCurrentWindow());
}

void draw_handle_keydown(u32_t keycode)
{
	switch(keycode)
	{
		case key_up:
			drawstate.rotx += 10;
			break;
		case key_down:
			drawstate.rotx -= 10;
			break;
		case key_left:
			drawstate.rotz += 10;
			break;
		case key_right:
			drawstate.rotz -= 10;
			break;
		case key_numpad_4:
			drawstate.vdx += 1;
			break;
		case key_numpad_6:
			drawstate.vdx -= 1;
			break;
		case key_numpad_2:
			drawstate.vdy += 1;
			break;
		case key_numpad_8:
			drawstate.vdy -= 1;
			break;
		case key_numpad_7:
			drawstate.vdz += 1;
			break;
		case key_numpad_9:
			drawstate.vdz -= 1;
			break;
		case key_grey_minus:
			drawstate.zoom /= (GLfloat)1.1;
			break;
		case key_grey_plus:
			drawstate.zoom *= (GLfloat)1.1;
			break;
		case key_I:
			drawstate.inner_grid = !drawstate.inner_grid;
			break;
		case key_O:
			drawstate.outer_grid = !drawstate.outer_grid;
			break;
		case key_D:
			drawstate.lod = (drawstate.lod + 1) % 3;
			break;
		case key_P:
			drawstate.ortho = !drawstate.ortho;
			ogl_setup_view(0,0,drawstate.ortho,false);
			break;
		case key_space:
			draw_reset_state();
			ogl_setup_view(0,0,drawstate.ortho,false);
			break;
		case key_numpad_5:
			drawstate.zoom *= 0.1f;
			break;
		default:
			break;
	}
}

void draw_handle_keyup(u32_t keycode)
{
	switch(keycode)
	{
		case key_numpad_5:
			drawstate.zoom *= 10;
			break;
		default:
			break;
	}
}

/*
 * -----------------------------------------------------------------------
 * Part 5: UI
 */
vboolean ui_handle_event(input_record_t *in)
{
	switch(in->event)
	{
		case ev_key_down:
			if(in->first_code == key_escape)
				return false;
			draw_handle_keydown(in->first_code);
			break;
		case ev_key_up:
			draw_handle_keyup(in->first_code);
			break;
		default:
			break;
	}
	return true;
}

void ui_help(void)
{
	conPuts(
		"Interactive mode keys:\n"
		"Escape - exit;\n"
		"Cursor keys - rotation;\n"
		"Numpad keys (4/6;2/8;7/9) - shift in object space;\n"
		"Numpad +/- - zoom in/out;\n"
		"Numpad 5 - temporary zoom out;\n"
		"'I','O' - toggle inner/outer grids;\n"
		"'D' - toggle particle level of detail.\n");
}

#define solver_max_dt	3e-2

void ui_main_loop(void)
{
	input_record_t in;
	u32_t delay_ms = 1000 / FPS;
	ui_help();
	for(;;)
	{
		/*
		 * Process all UI events
		 */
		while(vglPending())
		{
			vglNextEvent(&in);
			if(!ui_handle_event(&in))
				goto good_bye;
		}
		/*
		 * Execute solver
		 */
		solver_solve(solver_max_dt,delay_ms,delay_ms);
		/*
		To use 50% of CPU:
		solver_solve(solver_max_dt,delay_ms,delay_ms >> 1);
		*/
		/*
		 * Draw
		 */
		draw();
	}
good_bye:;
}

/*
 * main()
 */

vboolean nodraw = false;
vboolean noburn = false;
vec_t	timelimit = 1e6;
u32_t	rtlimit = (3600 * 1000);	/* 1 hour */

void cmd_help(void)
{
	puts(
		"Command-line switches:\n"
		"nodraw\n"
		"noburn\n"
		"rtlimit <int arg>\n"
		"timelimit <float arg>\n"
		"rectangular\n"
		"sinh\n"
		"strafemul <float arg>\n"
		"eta <float arg>\n"
		"u0 <float arg>\n"
		"out <string arg>\n"
		"seed <int arg>\n"
		"uhole <float arg>\n"
		"r0 <floar arg>\n");
}

char *outfile = NULL;

vboolean cmd_parsecommandline(int argc,char *argv[])
{
	int i;
	for(i=1;i<argc;i++)
	{
		char *cmd = argv[i];
		if(!strcmp(cmd,"nodraw"))
		{
			nodraw = true;
			puts("nodraw: true");
		}
		else if(!strcmp(cmd,"out"))
		{
			outfile = argv[++i];
			printf("out: '%s'\n",outfile);
		}
		else if(!strcmp(cmd,"seed"))
		{
			seed = atoi(argv[++i]);
			printf("seed: '%d'\n",seed);
		}
		else if(!strcmp(cmd,"noburn"))
		{
			noburn = true;
			puts("noburn: true");
		}
		else if(!strcmp(cmd,"timelimit"))
		{
			timelimit = atof(argv[++i]);
			printf("timelimit %g\n",timelimit);
		}
		else if(!strcmp(cmd,"fps"))
		{
			FPS = atoi(argv[++i]);
			printf("FPS %u\n",FPS);
		}
		else if(!strcmp(cmd,"rtlimit"))
		{
			rtlimit = 1000 * atoi(argv[++i]);
			printf("rtlimit %g\n",timelimit);
		}
		else if(!strcmp(cmd,"rectangular"))
		{
			strafe_force = strafe_rectangular;
			puts("rectangular strafe");
		}
		else if(!strcmp(cmd,"sinh"))
		{
			strafe_force = strafe_sinh;
			puts("sinh strafe");
		}
		else if(!strcmp(cmd,"strafemul"))
		{
			potential_barrier_height = atof(argv[++i]);
			printf("potential_barrier_height %g\n",potential_barrier_height);
		}
		else if(!strcmp(cmd,"eta"))
		{
			ETA = atof(argv[++i]);
			printf("ETA %g\n",ETA);
		}
		else if(!strcmp(cmd,"u0"))
		{
			U0 = atof(argv[++i]);
			printf("U0 %g\n",U0);
		}
		else if(!strcmp(cmd,"uhole"))
		{
			potential_barrier_width = atof(argv[++i]);
			printf("potential_barrier_width %g\n",potential_barrier_width);
		}
		else if(!strcmp(cmd,"r0"))
		{
			r0 = atof(argv[++i]);
			printf("r0 %g\n",r0);
		}
		else
		{
			printf("Unrecognized command: %s\n",cmd);
			return false;
		}
	}
	return true;
}

vhandle_t out;

void begin_framedesc()
{
	u32_t i;
	out = fileOpenWrite(outfile ? outfile : "out.psy");
	filePrintf(out,"title \"(noname)\"\n");
	filePrintf(out,"particles %d\n",NUM_OBJECTS);
	filePrintf(out,"fps\t%u\n",FPS);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		filePrintf(out,"particle %u\n{\n",i);
		filePrintf(out,"\tghost\t0\t%g\n\tghost\t1\t%g\n",
			obj[i].radius,obj[i].radius+r0);
		filePrintf(out,"}\n");
	}
}

u32_t total_frames = 0;

void end_framedesc()
{
	filePrintf(out,"frames %u\n",total_frames);
	fileClose(out);
}

void save_frame(u32_t framenum)
{
	int i;
	if(total_frames < framenum)
		total_frames = framenum;
	filePrintf(out,"frame %u\n{\n",framenum);
	for(i=0;i<NUM_OBJECTS;i++)
	{
		vec_t *p = obj[i].pos;
		vec_t *v = obj[i].vel;
		filePrintf(out,"\t{%g,%g,%g},{%g,%g,%g}%s\n",
			p[0],p[1],p[2],
			v[0],v[1],v[2],
			(i < (NUM_OBJECTS-1)) ? "," : "");
		fileControl(out,pctl_flush,NULL);
	}
	filePrintf(out,"}\n");
}

void produce_frames(void)
{
	u32_t fr = 0, rt, starttime = timeTickCount();
	conPrintf("Timelimit %g\n",timelimit);
	conPrintf("RTlimit %u\n",rtlimit);
	for(;;)
	{
		/*
		 * 1. Update solution for exact 1/FPS time.
		 * 2. Don't wait.
		 * 3. Use max 'rtlimit' ms for solving process.
		 */
		solver_solve((vec_t)1.0 / FPS,0,rtlimit);
		rt = timeTickCount() - starttime;
		conPrintf("Frame %u ready: time %4.3g, realtime %u\n",fr,simulation_time,rt);
		save_frame(fr++);
		if(simulation_time > timelimit)
		{
			conPuts("timelimit hit: exiting.\n");
			break;
		}
		if(rt >= rtlimit)
		{
			conPuts("rtlimit hit: exiting.\n");
			break;
		}
	}
}

int main(int argc,char *argv[])
{
	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);

	cmd_help();

	if(!cmd_parsecommandline(argc,argv))
		return -1;

	ljutil_init();
	solver_init();
	/* fsInit(); */
	if(nodraw)
	{
		begin_framedesc();
		produce_frames();
		end_framedesc();
	}
	else
	{
		if(!ogl_setup(640,480,false))
			return -1;
		ui_main_loop();
		ogl_shutdown();
	}
	logStopConsoleLog();
	return 0;
}

