#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#ifdef DISPLAY
#include <SDL.h>
SDL_Surface *screen;
#endif

typedef struct mole mole;

enum {NV = 4};
struct mole {
	double x[NV], xt[NV], xn[NV], xe[NV], xs[NV];
	double k[6*NV];
	uint32_t c;
};

double dt = 0.01;
enum { nmoles = 50 };
mole moles[nmoles];
enum { SCRW = 800, SCRH = 600 };
double time;

static void
init(void)
{
	mole *m;

	for(m = moles; m < moles + nmoles; m++){
		m->x[0] = rand() / (double) RAND_MAX;
		m->x[1] = rand() / (double) RAND_MAX;
		m->x[2] = 0.5 * (2 * rand() - 1) / (double) RAND_MAX;
		m->x[3] = 0.5 * (2 * rand() - 1) / (double) RAND_MAX;
		m->c = rand();
	}
}

static void
forces(int i)
{
	mole *m, *n;
	double dx, dy, f;

	for(m = moles; m < moles + nmoles; m++){
		m->k[i * NV + 0] = m->xt[2];
		m->k[i * NV + 1] = m->xt[3];
		m->k[i * NV + 2] = 0;
		m->k[i * NV + 3] = -4;
		for(n = moles; n < m; n++){
			dx = m->xt[0] - n->xt[0];
			dy = m->xt[1] - n->xt[1];
			f = 0.001 * pow(dx * dx + dy * dy, -1.5);
			m->k[i * NV + 2] -= f * dx;
			m->k[i * NV + 3] -= f * dy;
			n->k[i * NV + 2] += f * dx;
			n->k[i * NV + 3] += f * dy;
		}
	}
}

static void
rkck(double h)
{
	mole *m;
	int i;

	#define F\
		for(m = moles; m < moles + nmoles; m++)\
			for(i = 0; i < NV; i++)\
				m->xt[i] = m->x[i] + h * 
	#define k(n) m->k[n*NV+i]
	F(0.2*k(0)); forces(1);
	F((3./40)*k(0)+(9./40)*k(1)); forces(2);
	F(0.3*k(0)-0.9*k(1)+1.2*k(2)); forces(3);
	F((-11./54)*k(0)+2.5*k(1)-(70./27.)*k(2)+(35./27.)*k(3)); forces(4);
	F((1631./55296)*k(0)+(175./512)*k(1)+(575./13824)*k(2)+(44275./110592)*k(3)+(253./4096.)*k(4)); forces(5);
	for(m = moles; m < moles + nmoles; m++)
		for(i = 0; i < NV; i++){
			m->xn[i] = m->x[i] + h * ((37./378)*k(0)+(250./621)*k(2)+(125./594)*k(3)+(512./1771)*k(5));
			m->xe[i] = h * ((37./378-2825./27648)*k(0)+(250./621-18575./48384)*k(2)+(125./594-13525./55296)*k(3)-(277./14336)*k(4)+(512./1771-.25)*k(5));
		}
	#undef F
	#undef k
}

static double
rkqs(double h)
{
	mole *m;
	int i;
	double ht, e;
	#define SAFETY 0.9
	#define ERRCON 1.89e-4
	#define eps 1e-5

	for(;;){
		rkck(h);
		e = 0;
		for(m = moles; m < moles + nmoles; m++)
			for(i = 0; i < NV; i++)
				if(m->xe[i] / m->xs[i] > e)
					e = m->xe[i] / m->xs[i];
		e /= eps;
		if(e <= 1.0)
			break;
		ht = SAFETY * h * pow(e, -0.25);
		h = ht <= 0.1*h ? 0.1*h : ht;
	}
	time += h;
	if(e > ERRCON)
		return SAFETY * h * pow(e, -0.2);
	return 5*h;
}

void
ode(double fr)
{
	int i;
	mole *m;
	double t2;

	t2 = time + fr;
	do{
		for(m = moles; m < moles + nmoles; m++)
			for(i = 0; i < NV; i++)
				m->xt[i] = m->x[i];
		forces(0);
		for(m = moles; m < moles + nmoles; m++)
			for(i = 0; i < NV; i++)
				m->xs[i] = fabs(m->x[i]) + fabs(m->k[i]*dt) + 1e-30;
		if(time + dt > t2)
			dt = t2 - time;
		dt = rkqs(dt);
		for(m = moles; m < moles + nmoles; m++){
			for(i = 0; i < NV; i++)
				m->x[i] =  m->xn[i];
			if(m->x[0] < 0 && m->x[2] < 0)
				m->x[2] *= -1;
			if(m->x[1] < 0 && m->x[3] < 0)
				m->x[3] *= -1;
			if(m->x[0] > 1 && m->x[2] > 0)
				m->x[2] *= -1;
			if(m->x[1] > 1 && m->x[3] > 0)
				m->x[3] *= -1;
		}
	}while(time < t2);
}

#ifdef DISPLAY
void
plot(void)
{
	uint32_t *px;
	int x, y;
	mole *m;
	static int nfr, set[SCRW][SCRH];

	SDL_LockSurface(screen);
	px = screen->pixels;
	nfr++;
	for(m = moles; m < moles + nmoles; m++){
		x = (int)(m->x[0] * SCRW) % SCRW;
		y = (int)(m->x[1] * SCRH) % SCRH;
		if(x < 0) x += SCRW;
		if(y < 0) y += SCRH;
		px[y * SCRW + x] = m->c;
		set[x][y] = nfr;
	}
	for(x = 0; x < SCRW; x++)
		for(y = 0; y < SCRH; y++)
			if(set[x][y] <= nfr - 50){
				px[y * SCRW + x] = 0;
				set[x][y] = 0;
			}
	SDL_UnlockSurface(screen);

}
#endif

int
main()
{
#ifdef DISPLAY
        SDL_Event ev;

        if(SDL_Init(SDL_INIT_VIDEO) < 0){
error:
                fprintf(stderr, "error: %s\n", SDL_GetError());
                return 1;
        }

        screen = SDL_SetVideoMode(SCRW, SCRH, 32, SDL_DOUBLEBUF);
        if(screen == NULL)
                goto error;

        init();
        for(;;){
                while(SDL_PollEvent(&ev))
                        switch(ev.type){
                        case SDL_QUIT:
                                SDL_Quit();
                                return 0;
                        }

		ode(0.001);
                plot();
                SDL_Flip(screen);
        }
#else
	init();
	for(;;){
		ode(0.1);
		fprintf(stderr, ".");
		fflush(stderr);
	}
#endif
}