/*
	Compile with gcc planets.c `sdl-config --cflags --libs`
	-DENERGY prints energy and angular momentum at each step to verify 
	The timestep dt at the top can be adjusted
*/

#define dt 0.00003

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#define DISPLAY

#ifdef DISPLAY
#include <SDL.h>
SDL_Surface *screen;
#define rgb(r, g, b) SDL_MapRGB(screen->format, r, g, b)
#else
#define rgb(r, g, b) 0
#endif

typedef struct Planet Planet;
struct Planet {
	uint32_t c;
	double m;
	double x[6], k[4][6], a[3];
	double X[3], Y[3];
	double l;
};

int npl;
Planet pl[10];
#define mu 39.42290456
#define deg (M_PI/180)

enum { SCRW = 800, SCRH = 600 } ;

void
planet(uint32_t c, double m, double a, double e, double I, double L, double lp, double la)
{
	Planet *p;
	double M, l, E, Et, n, x, y, vx, vy;

	p = &pl[npl++];
	p->c = c;
	p->m = m;
	lp *= deg; la *= deg; L *= deg; I *= deg;
	if(a != 0){
		l = lp - la;
		M = L - lp;
		M -= sqrt(mu / (a * a * a)) * (2000 - 1977.6);
		E = M;
		n = E;
		do{
			E = n;
			n = E - (E - e * sin(E) - M) / (1 - e * cos(E));
		}while(fabs(E - n) >= 1e-10);
		x = a * (cos(E) - e);
		y = a * sqrt(1 - e * e) * sin(E);
		Et = sqrt(mu / (a * a * a)) / (1 - e * cos(E));
		vx = -a * sin(E) * Et;
		vy = a * sqrt(1 - e * e) * cos(E) * Et;
		p->X[0] = cos(l) * cos(la) - sin(l) * sin(la) * cos(I);
		p->X[1] = cos(l) * sin(la) + sin(l) * cos(la) * cos(I);
		p->X[2] = sin(l) * sin(I);
		p->Y[0] = -sin(l) * cos(la) - cos(l) * sin(la) * cos(I);
		p->Y[1] = -sin(l) * sin(la) + cos(l) * cos(la) * cos(I);
		p->Y[2] = cos(l) * sin(I);
		p->x[0] = p->X[0] * x + p->Y[0] * y;
		p->x[1] = p->X[1] * x + p->Y[1] * y;
		p->x[2] = p->X[2] * x + p->Y[2] * y;
		p->x[3] = p->X[0] * vx + p->Y[0] * vy;
		p->x[4] = p->X[1] * vx + p->Y[1] * vy;
		p->x[5] = p->X[2] * vx + p->Y[2] * vy;
	}
}

void
ship(void)
{
	Planet *p, *q;

	p = &pl[npl++];
	q = &pl[3];
	p->c = rgb(255, 255, 255);
	p->x[3] = q->x[3] - 0.05;
	p->x[4] = q->x[4] + 2.295;
	p->x[5] = q->x[5] - 0.30;
	p->x[0] = q->x[0] + p->x[3] * 0.001;
	p->x[1] = q->x[1] + p->x[4] * 0.001;
	p->x[2] = q->x[2] + p->x[5] * 0.001;
	p->m = 1e-10;
}


void
init(void)
{
	planet(rgb(255, 255, 0), 332946, 0, 0, 0, 0, 0, 0);
	planet(rgb(0, 255, 0), 0.3825, 0.38709927, 0.20563593, 7.00497902, 252.25032350, 77.45779628, 48.33076593);
	planet(rgb(128, 255, 0), 0.815, 0.72333566, 0.00677672, 3.39467605, 181.97909950, 131.60246718, 76.67984255);
	planet(rgb(0, 0, 255), 1, 1.00000261, 0.01671123, -0.00001531, 100.46457166, 102.93768193, 0.0);
	planet(rgb(255, 0, 0), 0.107, 1.52371034, 0.09339410, 1.84969142, -4.55343205, -23.94362959, 49.55953891);
	planet(rgb(255, 0, 255), 318, 5.20288700, 0.04838624, 1.30439695, 34.39644051, 14.72847983, 100.47390909);
	planet(rgb(255, 255, 0), 95, 9.53667594, 0.05386179, 2.48599187, 49.95424423, 92.59887831, 113.66242448);
	planet(rgb(255, 128, 128), 14, 19.18916464, 0.04725744, 0.77263783, 313.23810451, 170.95427630, 74.01692503);
	planet(rgb(128, 255, 255), 17, 30.06992276, 0.00859048, 1.77004347, -55.12002969, 44.96476227, 131.78422574);
	ship();
}

void
forces(int i)
{
	double fp;
	Planet *p, *q;
	double fx, fy, fz, dx, dy, dz, r;
	
	if(i == 3)
		fp = dt;
	else if(i != 0)
		fp = dt/2;
	else
		fp = 0;
	for(p = pl; p < pl + npl; p++){
		p->k[i][0] = p->x[3];
		p->k[i][1] = p->x[4];
		p->k[i][2] = p->x[5];
		p->k[i][3] = p->k[i][4] = p->k[i][5] = 0;
		for(q = pl; q < p; q++){
			dx = p->x[0] - q->x[0];
			dy = p->x[1] - q->x[1];
			dz = p->x[2] - q->x[2];
			if(i != 0){
				dx += (p->k[i-1][0] - q->k[i-1][0]) * fp;
				dy += (p->k[i-1][1] - q->k[i-1][1]) * fp;
				dz += (p->k[i-1][2] - q->k[i-1][2]) * fp;
			}
			r = dx * dx + dy * dy + dz * dz;
			r = 1.184e-4 * pow(r, -1.5);
			fx = dx * r;
			fy = dy * r;
			fz = dz * r;
			p->k[i][3] -= fx * q->m;
			p->k[i][4] -= fy * q->m;
			p->k[i][5] -= fz * q->m;
			q->k[i][3] += fx * p->m;
			q->k[i][4] += fy * p->m;
			q->k[i][5] += fz * p->m;
		}
	}
}

void
energy(void)
{
	double E, Lx, Ly, Lz, dx, dy, dz;
	Planet *p, *q;

	E = 0;
	Lx = Ly = Lz = 0;
	for(p = pl; p < pl + npl; p++){
		E += (p->x[3] * p->x[3] + p->x[4] * p->x[4] + p->x[5] * p->x[5]) * p->m / 2;
		Lx += p->m * (p->x[1] * p->x[5] - p->x[2] * p->x[4]);
		Ly += p->m * (p->x[2] * p->x[3] - p->x[0] * p->x[5]);
		Lz += p->m * (p->x[0] * p->x[4] - p->x[1] * p->x[3]);
		for(q = pl; q < p; q++){
			dx = p->x[0] - q->x[0];
			dy = p->x[1] - q->x[1];
			dz = p->x[2] - q->x[2];
			E -= 1.184e-4 * p->m * q->m * pow(dx * dx + dy * dy + dz * dz, -0.5);
		}
	}
	printf("%g (%g %g %g)\n", E, Lx, Ly, Lz);
}

void
move(void)
{
	Planet *p;
	int i;

	for(p = pl; p < pl + npl; p++){
		for(i = 0; i < 4; i++)
			forces(i);
		for(i = 0; i < 6; i++)
			p->x[i] += (p->k[0][i] + 2 * (p->k[1][i] + p->k[2][i]) + p->k[3][i]) * dt/6;
	}
}

void
plot(void)
{
	uint32_t *px;
	int x, y;
	Planet *p;

#ifdef DISPLAY
	SDL_LockSurface(screen);
	px = screen->pixels;
	for(p = pl; p < pl + npl; p++){
		x = (p->x[0]/60 + 0.5) * SCRW;
		y = (-p->x[1]/45 + 0.5) * SCRH;
		if(x < 0 || y < 0 || x >= SCRW || y >= SCRH)
			continue;
		px[y * SCRW + x] = p->c;
	}
	SDL_UnlockSurface(screen);
#endif
}

int
main(int argc, char **argv)
{
	int i;

#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;
#endif

	init();
	for(;;){
#ifdef DISPLAY
		plot();
		SDL_Flip(screen);
		while(SDL_PollEvent(&ev))
			switch(ev.type){
			case SDL_QUIT:
				SDL_Quit();
				return 0;
			}
#endif

		for(i = 0; i < 1000; i++)
			move();
#ifdef ENERGY
		energy();
#endif
	}
}