#include <stdlib.h>
#include <stdio.h>

#include <GL/glut.h>
#include <GL/glu.h>
#include <GL/glx.h>

#include <math.h>
#include <time.h>
#include <unistd.h>

/* Structures */
struct vec_t {
	GLfloat x;
	GLfloat y;
	GLfloat z;
};

struct node_t {
	struct vec_t motion;
	struct vec_t accl;
	struct vec_t pos;
	struct node_t *next;
	GLfloat mass;
	GLfloat charge;
	int move;
	int accel;
};

/* Globals */
static int winIdMain = 0;
static int viewport[2] = { 640, 480 };
static GLfloat angle[2] = { 45.0 , -45.0 };
static GLfloat depth = 32.0;
static struct node_t *nodes = NULL;

static GLfloat zerolimit = 0.0;
static GLfloat timefactor = 1.0; /* How many reality samples per frame */

static GLfloat G = 0.0;
static GLfloat E0 = 0.0;

enum {
	X = 0,
	Y = 1,
	Z = 2
};

/* List Functions */
void add_node(struct node_t *node) {
	struct node_t *p;
	if(nodes == NULL) {
		nodes = node;
	} else {
		for(p = nodes; p->next != NULL; p = p->next) { ; }
		p->next = node;
	}
	node->next = NULL;
}

/* Maths Functions */

GLfloat inline sqr(GLfloat num) {
	return (num*num);
}

GLfloat tpo(int pwr) {
	GLfloat t = 1.0;
	for(; pwr > 0; pwr -= 1) {
		t *= 10.0;
	}
	return t;
}

int limit_match(GLfloat val1, GLfloat val2, GLfloat lim) {
	if(val1 == val2)
		return 1;
	val1 -= val2;
	if(val1 < 0.0 && val1 > -lim)
		return 1;
	if(val1 > 0.0 && val1 < lim)
		return 1;

	return 0;
}

/* Printing Functions */
void pvec(char *label, struct vec_t *v) {
	fprintf(stderr,"vector:%s, x:%f,y:%f,z:%f\n",label,v->x,v->y,v->z);
}

void pnode(struct node_t *n) {
	fprintf(stderr,"Node : %p, x:%f,y:%f,z:%f,mass:%f\n",n,n->pos.x,n->pos.y,n->pos.z,n->mass);
	fprintf(stderr,"\ta.x:%f,a.y:%f,a.z:%f\n",n->accl.x,n->accl.y,n->accl.z);
	fprintf(stderr,"\tm.x:%f,m.y:%f,m.z:%f\n",n->motion.x,n->motion.y,n->motion.z);
}


/* Vector Ops */
void inline vec_cpy(struct vec_t *dst, struct vec_t *src) {
	dst->x = src->x;
	dst->y = src->y;
	dst->z = src->z;
}

GLfloat inline xdiff(struct node_t *n1, struct node_t *n2) {
	return (n2->pos.x - n1->pos.x);
}

GLfloat inline ydiff(struct node_t *n1, struct node_t *n2) {
	return (n2->pos.y - n1->pos.y);
}

GLfloat inline zdiff(struct node_t *n1, struct node_t *n2) {
	return (n2->pos.z - n1->pos.z);
}

void inline vec_add(struct vec_t *v1, struct vec_t *v2) {
	v1->x += v2->x;
	v1->y += v2->y;
	v1->z += v2->z;
}

void inline vec_sub(struct vec_t *v1, struct vec_t *v2) {
	v1->x -= v2->x;
	v1->y -= v2->y;
	v1->z -= v2->z;
}

void inline vec_mul(struct vec_t *v1, struct vec_t *v2) {
	v1->x *= v2->x;
	v1->y *= v2->y;
	v1->z *= v2->z;
}

void inline vec_add_factor(struct vec_t *v1, struct vec_t *v2, GLfloat fact) {
	v1->x += v2->x * fact;
	v1->y += v2->y * fact;
	v1->z += v2->z * fact;
}

void inline vec_zero(struct vec_t *v) {
	v->x = 0.0;
	v->y = 0.0;
	v->z = 0.0;
}

GLfloat inline vec_len(struct vec_t *v) {
	return (GLfloat) sqrt(sqr(v->x) + sqr(v->y) + sqr(v->z));
}

GLfloat inline finvert(GLfloat *f) {
	*f = (-1.0)*(*f);
}

/* Physics Functions */

void eforce_between(struct node_t *n1, struct node_t *n2,
			struct vec_t *v1) {
	struct vec_t v2;
	GLfloat len; 
	GLfloat force;
	vec_cpy(&v2,&n2->pos);
	vec_sub(&v2,&n1->pos);
	len = vec_len(&v2);
	if(!limit_match(sqr(len),0.0,zerolimit)) {
		force = (n1->charge * n2->charge * (-1.0))/
			(4 * M_PI * E0 * sqr(len));
	} else {
		force = 0.0;
	}
	v1->x = v2.x * force;
	v1->y = v2.y * force;
	v1->z = v2.z * force;
}

void gforce_between(struct node_t *n1, struct node_t *n2, 
			struct vec_t *v1) {
	struct vec_t v2;
	GLfloat len; 
	GLfloat force;
	vec_cpy(&v2,&n2->pos);
	vec_sub(&v2,&n1->pos);
	len = vec_len(&v2);
	if(!limit_match(sqr(len),0.0,zerolimit)) {
		force = (G * n1->mass * n2->mass)/
			(sqr(len));
	} else {
		force = 0.0;
	}
	v1->x = v2.x * force;
	v1->y = v2.y * force;
	v1->z = v2.z * force;
}

void collide(struct node_t *n1, struct node_t *n2) {
	GLfloat v1,v2;
	//struct vec_t mu;
	//struct vec_t mv;

	//fprintf(stderr,"Colliding %p,%p\n",n1,n2);
	//fprintf(stderr,"Before Colliding\n");
	//pnode(n1);
	//pnode(n2);
	/*
	vec_zero(&mu);
	vec_add_factor(&mu,&n1->motion,n1->mass);
	vec_add_factor(&mu,&n2->motion,n2->mass);
	*/
	
	v1 = ((n1->mass * n1->motion.x) 
		+ (2 * n2->mass * n2->motion.x) 
		- (n2->mass * n1->motion.x)
	) / (n1->mass + n2->mass);
	v2 = n1->motion.x - n2->motion.x + v1;
	n1->motion.x = v1;
	n2->motion.x = v2;

	v1 = (	(n1->mass * n1->motion.y) 
		+ (2 * n2->mass * n2->motion.y) 
		- (n2->mass * n1->motion.y)
	) / (n1->mass + n2->mass);
	v2 = n1->motion.y - n2->motion.y + v1;
	n1->motion.y = v1;
	n2->motion.y = v2;

	v1 = (	(n1->mass * n1->motion.z) 
		+ (2 * n2->mass * n2->motion.z) 
		- (n2->mass * n1->motion.z)
	) / (n1->mass + n2->mass);
	v2 = n1->motion.z - n2->motion.z + v1;
	n1->motion.z = v1;
	n2->motion.z = v2;

	
	//fprintf(stderr,"After Colliding\n");
	//pnode(n1);
	//pnode(n2);
	/*
	vec_zero(&mv);
	vec_add_factor(&mv,&n1->motion,n1->mass);
	vec_add_factor(&mv,&n2->motion,n2->mass);
	pvec("mu",&mu);
	pvec("mv",&mv);
	*/
	//sleep(5);
	//exit(0);
}

int collision(struct node_t *n1, struct node_t *n2, GLfloat *dt) {
	GLfloat theta,lambda;
	lambda = ( n2->motion.x * (n1->pos.y - n2->pos.y)
			+ n2->motion.y * (n2->pos.x - n1->pos.x))
		 / (n2->motion.y * n1->motion.x - n2->motion.x * n1->motion.y);
	theta =	(n1->pos.y - n2->pos.y + (n1->motion.y * lambda)) 
		/ n2->motion.y;
	
	if(theta > 0.0 && theta < *dt && limit_match(lambda,theta,0.00001)) {
			//fprintf(stderr,"Collision Detected, %f\n",*dt);
			//fprintf(stderr,"lambda %f, theta %f\n",lambda,theta);
			//pnode(n1);
			//pnode(n2);
			*dt = theta;
			return 1;
	}
	return 0;
}

void forces_on(struct node_t *n, struct vec_t *r) {
	struct vec_t v;
	struct node_t *p;
	vec_zero(r);
	for(p = nodes; p != NULL; p = p->next) {
		if(p != n) {
			if(n->mass != 0.0) {
				gforce_between(n,p,&v);
				vec_add(r,&v);
			}
			if(n->charge != 0.0) {
				eforce_between(n,p,&v);
				vec_add(r,&v);
			}
		}
	}
}

void set_accel(struct node_t *n) {
	struct vec_t v;
	if(n->accel) {
		forces_on(n,&v);
		vec_zero(&n->accl);
		vec_add_factor(&n->accl,&v,(1/n->mass)); // Factor is Division
	}
}

void accelerate(struct node_t *n, GLfloat dt) {
	if(n->move) {
		vec_add_factor(&n->motion,&n->accl,dt);
	}
}

void displace(struct node_t *n, GLfloat dt) {
	vec_add_factor(&n->pos,&n->motion,dt);
}

void global_accel(GLfloat dt) {
	struct node_t *n;
	for(n = nodes; n != NULL; n = n->next) {
		set_accel(n);
		accelerate(n,dt);
	}
}


void global_displace(GLfloat dt) {
	struct node_t *n;
	for(n = nodes; n != NULL; n = n->next) {
		displace(n,dt); 
	}
}

void global_cd(const GLfloat dt) {
	GLfloat d;
	GLfloat cnext,ccnext;
	GLfloat elapsed = 0.0;
	struct node_t *p,*cp,*pp = NULL;
	struct node_t *n,*cn,*pn = NULL;
	//int loop = 0;
	//int collisions;
	
	while(elapsed < dt) {
		//collisions = 0;
		cp = NULL;
		cn = NULL;
		ccnext = 0.0;
		cnext = dt - elapsed;
		for(n = nodes; n != NULL; n = n->next) {
			for(p = n->next; p != NULL; p = p->next) {
				if(p != n) {
					d = dt;
					if(collision(n,p,&d)) {
						if(d < cnext && !limit_match(d,0.0,zerolimit)) {
							if(p != pp && n != pn) {
								ccnext = cnext;
								cnext = d;
								cp = p;
								cn = n;
								//collisions++;
							}
						}
					}
				}
			}
		}
		global_displace(cnext);
		global_accel(cnext);
		elapsed += cnext;
		if(cp != NULL && cn != NULL) {
			collide(cp,cn);
			pp = cp;
			pn = cn;
			global_displace(ccnext - cnext);
			global_accel(ccnext - cnext);
		}
	}
}
		

GLfloat deltatime(struct timeval *bef, struct timeval *aft) {
	GLfloat dt;
	dt = (aft->tv_sec - bef->tv_sec) 
		+ ((aft->tv_usec - bef->tv_usec) / 1000000.0);
	return dt;
}

/* Functions */

void DrawAxis(int type) {
	GLfloat cords[3] = {0.0, 0.0, 0.0};
	cords[type] = (depth/2.0);
	glPushMatrix();
	glBegin(GL_LINES);
		glColor4f(1.0,1.0,1.0,0.8);
		glVertex3f(-cords[X],-cords[Y],-cords[Z]);
		glVertex3f(cords[X],cords[Y],cords[Z]);
	glEnd();
	glPopMatrix();
}

void DrawPoint(struct node_t *n) {
	struct vec_t col;
	GLUquadricObj *sph;
	glPushMatrix();
	/*
	glBegin(GL_POINTS);
		glPointSize(1.0);
		//col.x = sqr(n->motion.x) < 0.75 ? sqr(n->motion.x) + 0.25 : 1.0;
		//col.y = sqr(n->motion.y) < 0.75 ? sqr(n->motion.y) + 0.25 : 1.0;
		//col.z = (col.x + col.y) / 2.0;
		col.x = 1.0;
		col.y = 1.0;
		col.z = 1.0;
		glColor3f(col.x,col.y,col.z);
		glVertex3f(n->pos.x,n->pos.y,n->pos.z);
		//glPointSize(1.0);
	glEnd();
	*/
	col.x = sqr(n->motion.x) < 0.75 ? sqr(n->motion.x) + 0.25 : 1.0;
	col.y = sqr(n->motion.y) < 0.75 ? sqr(n->motion.y) + 0.25 : 1.0;
	col.z = (col.x + col.y) / 2.0;
	glColor3f(col.x,col.y,col.z);
	glTranslatef(n->pos.x,n->pos.y,n->pos.z);
	sph = gluNewQuadric();
	gluSphere(sph,(GLdouble)((n->mass/tpo(10))/2.0),6,6);
	glPopMatrix();

	glPushMatrix();
	glBegin(GL_LINES);
	glVertex3f(n->pos.x,n->pos.y,n->pos.z);
	glVertex3f(n->pos.x + n->accl.x,n->pos.y + n->accl.y,n->pos.z + n->accl.z);
	glEnd();	
	glPopMatrix();

	/*
	glPushMatrix();
	glBegin(GL_LINES);
	glColor4f(1.0,1.0,1.0,0.8);
	glVertex3f(n->pos.x,n->pos.y,n->pos.z);
	glVertex3f(n->pos.x + n->motion.x,n->pos.y + n->motion.y,n->pos.z + n->motion.z);
	glEnd();	
	glPopMatrix();
	*/
}

void draw_points(void) {
	struct node_t *n;
	for(n = nodes; n != NULL; n = n->next) {
		DrawPoint(n);
	}
}

void display(void) {
	glutSetWindow(winIdMain);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	/* Translate View */
	glNormal3f(0.0,0.0,0.0);
	gluPerspective(60.0,((GLfloat)viewport[X]/(GLfloat)viewport[Y]),1.0,depth);
	glTranslatef(0.0,0.0,-(depth/2.0));
	glRotatef(angle[X],0.0,1.0,0.0);
	glRotatef(angle[Y],1.0,0.0,0.0);
	glTranslatef(-0.0,-0.0,-0.0);

	//angle[X] = angle[X] < 360.0 ? angle[X] + 0.1 : 0.0;
	//angle[Y] = angle[Y] < 360.0 ? angle[Y] + 0.01 : 0.0;
	
	/* Models */
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	//DrawAxis(X);
	//DrawAxis(Y);
	//DrawAxis(Z);
	
	draw_points();
	
	glFlush();

	glutSwapBuffers();
}

void simulate(GLfloat dt) {
	int i;
	dt /= timefactor;
	
	for(i = 0; i < timefactor; ++i) {
		global_cd(dt);
	}
}

void idle_loop(void) {
	static struct timeval *last = NULL;
	static struct timeval *this = NULL;
	void *temp;
	int ret;
	GLfloat dt;
	
	if(last == NULL) {
		last = malloc(sizeof(struct timeval));
		this = malloc(sizeof(struct timeval));
		ret = gettimeofday(last,NULL);
		if(ret != 0)
			exit(1);
	}
	ret = gettimeofday(this,NULL);
	if(ret != 0)
		exit(1);
	dt = deltatime(last,this);
	temp = last;
	last = this;
	this = temp;
	
	if(dt > (0.25)) {
		simulate(0.25);
	} else {
		simulate(dt);
	}
	
	//usleep(50000);
	glutPostRedisplay();
}

void keyboard(unsigned char key, int x, int y) {
	key = tolower(key);
	switch (key) {
		case 'q':
			exit(0);
			break;
		default :
			break;
	}
}

void reshape(int w, int h) {
	glViewport(0,0,w,h);
	viewport[X] = w;
	viewport[Y] = h;
	glutPostRedisplay();
}

struct node_t *node_alloc(void) {
	struct node_t *n;
	n = malloc(sizeof(struct node_t));
	vec_zero(&n->motion);
	vec_zero(&n->accl);
	vec_zero(&n->pos);
	n->mass = 1.0;
	n->charge = 0.0;
	n->move = 1;
	n->accel = 1;
	n->next = NULL;
}

void init_nodes(void) {
	struct node_t *n;
	GLfloat i,j,k;
	//n = node_alloc();
	//n->pos.y = -2.0;
	//add_node(n);
	
	//n = node_alloc();
	//n->pos.y = 2.1;
	//add_node(n);

	//n = node_alloc();
	//n->pos.x = 2.0;
	//add_node(n);
	
	for(i = -6.0; i <= 6.0; i += 3.0) {
		for(j = -6.0; j <= 6.0; j += 3.0) {
			for(k = -6.0; k <= 6.0; k += 3.0) {
				n = node_alloc();
				//n->pos.x = ((GLfloat)(3000 - (random() % 6000)))/1000.0;
				n->pos.x = i ;//+ ((GLfloat)(1000 - (random() % 2000)))/10000.0;
				n->pos.y = j ;//+ ((GLfloat)(1000 - (random() % 2000)))/10000.0;
				n->pos.z = k ;
				//n->motion.x = ((GLfloat)(1000 - (random() % 2000)))/10000.0;
				//n->motion.y = ((GLfloat)(1000 - (random() % 2000)))/10000.0;
				n->mass = 1000.0 * 1000.0 * 1000.0;
				add_node(n);
			}
		}
	}

	/*
	n = node_alloc();
	n->pos.x = 0.0;
	n->mass = 1000.0 * 1000.0;
	n->motion.x = -0.05;
	//n->charge = 1.0;
	add_node(n);
	*/
}

void init_physics(void) {
	G = 6.67 * (1.0/(GLfloat)tpo(11));
	E0 = (1.0/(36.0*M_PI)) * (1.0/(GLfloat)tpo(9));
	zerolimit = (1.0/tpo(10));
	fprintf(stderr,"G:%.16f,E0:%.16f\n",G,E0);
}

int main(int argc, char **argv) {
	
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInitWindowSize(viewport[X],viewport[Y]);
	glutInitWindowPosition(0,0);

	winIdMain = glutCreateWindow("GravSim 0.01");

	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_SMOOTH);
	glEnable(GL_POINT_SMOOTH);
	glEnable(GL_POLYGON_SMOOTH);
	glEnable(GL_BLEND);

	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutIdleFunc(idle_loop);
	glutReshapeFunc(reshape);

	init_nodes();
	init_physics();
	glutMainLoop();

	return 0;
}