#include <GL/glut.h>


class mass {
public:
	float x,y,z;
	float vx,vy,vz;
	float m, inv_m;
	float Fx, Fy, Fz;

	mass () {};
	mass (float* pos, float* speed, float m);
	inline void resetForces() {Fx=Fy=Fz=0;}
	inline void addForceZ(float force) {Fz+=force;}
	void computePFD();
};


mass::mass(float* pos, float* speed, float m) {
	glPointSize(3.0f);
	this->x=pos[0];this->y=pos[1];this->z=pos[2];
	
	this->vx=speed[0];this->vy=speed[1];this->vz=speed[2];

	this->m=m;this->inv_m=1/m;
}


void mass::computePFD() {
	float dt=1.0f/60.0f;//60 Hz

	float ax=Fx*this->inv_m;float ay=Fy*this->inv_m;float az=Fz*this->inv_m;

	vx=dt*ax+vx;vy=dt*ay+vy;vz=dt*az+vz;

	x=dt*vx+x;y=dt*vy+y;z=dt*vz+z;

	if (z<=0) {
		z=-z;
		vz=-0.7*vz;
	}
}


mass M1;
mass M2;

void display(void) {


	//glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

	glLoadIdentity();
	
	gluLookAt(1.20f,-4.50f,2.30f,0.0f,0.0f,0.0f,276.0f,1035.0f,66825.0f);

#if 1
	glTranslatef(M1.x,M1.y,M1.z);
	glutSolidSphere(0.01,8,4);

	glTranslatef(M2.x,M2.y,M2.z);
	glutSolidSphere(0.01,8,4);
#else
	glBegin(GL_POINTS);
		glVertex3f(M1.x,M1.y,M1.z);
		glVertex3f(M2.x,M2.y,M2.z);
	glEnd();
#endif
	glutSwapBuffers();
}


void idle(void) {

	M1.resetForces();
	M1.addForceZ(-9.8*M1.m);//gravity

	M1.computePFD();

	M2.resetForces();
	M2.addForceZ(-9.8*M2.m);//gravity

	M2.computePFD();

	glutPostRedisplay();

}


void reshapeWindowed(int w, int h) {
	glViewport(0,0,(GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f,(float)w/(float)h,1.0f,20.0f);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

int main(int argc, char* argv[]) {
	glutInit(&argc, argv);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);

	glutInitWindowSize(640,480);
	glutCreateWindow("___ DEBUG ___");
	glutReshapeFunc(reshapeWindowed);


//Registration des fonctions

	glutDisplayFunc (display);
	glutIdleFunc(idle);

	float Pos1[3] = {-1.0f,-1.0f,2.0f};
	float Speed1[3]={1.0f,0.0f,-0.6f};
	M1=mass(Pos1,Speed1,1.0f);

	float Pos2[3] = {-1.0f,1.0f,1.0f};
	float Speed2[3]={1.0f,0.3f,0.6f};
	M2=mass(Pos2,Speed2,1.0f);


	glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

	glutMainLoop();
	return 0;
}

  

  glPushMatrix();glTranslatef(M1.x,M1.y,M1.z);glutSolidSphere(0.01,8,4);glPopMatrix();glPushMatrix();glTranslatef(M2.x,M2.y,M2.z);glutSolidSphere(0.01,8,4);glPopMatrix();