#include <stdlib.h>
#include <stdio.h>
#include <gl/glut.h>
#include <math.h>
#include <sys\timeb.h>

#define XWINSIZE 600 
#define YWINSIZE 600 
#define XWINPOS 100
#define YWINPOS 100

#define SPEED 0.15f
/*these must be constants for the mass to work, change at will*/

#define radius1 3.0
#define radius2 1.50

double q=20.0; /*size of cube, multiple of 4 please*/

double mass=3.0*radius1*radius1*radius1;
double mass2=3.0*radius2*radius2*radius2;



/*all of the initial conditions, some will change during program*/

double tint=5.0;
double xi=4.0;
double yi=5.0;
double zi=6.0;
double vxi=-15.0;
double vyi=13.0;
double vzi=-14.0;
double vy, vx, vz;
double yaccel=-10;
double xaccel=0;
double zaccel=0;
double dampy=.95;
double dampx=.95;
double dampz=.95;

double tint2=5.0;
double xi2=14.0;
double yi2=14.0;
double zi2=14.0;
double vxi2=16.0;
double vyi2=20.0;
double vzi2=9.0;
double vy2, vx2, vz2;
double yaccel2=-10;
double xaccel2=0;
double zaccel2=0;

double temp; /*used to swap velocities*/



double pos[3];
double pos2[3];
double prevpos[3]; /*this was the fix for collisions*/
double prevpos2[3];
double floorpos[3];
double floorpos2[3];

double unit[3];
double unit2[3];
double dotprod;
double dotprod2;


typedef struct
{
	float x;
	float y;
	float z;
} vector;

struct camera
{
	vector pos;
	vector view;
	vector up;
};

struct camera Camera;
int frame = 0, time, timebase = 0;

struct camera setcamposition(struct camera Cam,
		float posx, float posy, float posz,
		float viewx, float viewy, float viewz,
		float upx, float upy, float upz)
{
	Cam.pos.x = posx;
	Cam.pos.y = posy;
	Cam.pos.z = posz;
	
	Cam.view.x = viewx;
	Cam.view.y = viewy;
	Cam.view.z = viewz;

	Cam.up.x = upx;
	Cam.up.y = upy;
	Cam.up.z = upz;

	return Cam;
}

struct camera movecamera(struct camera Cam, float speed)
{
	vector vpvec;
	vpvec.x = Cam.view.x - Cam.pos.x;
	vpvec.y = Cam.view.y - Cam.pos.y;
	vpvec.z = Cam.view.z - Cam.pos.z;

	Cam.pos.x = Cam.pos.x + vpvec.x * speed;
	Cam.pos.z = Cam.pos.z + vpvec.z * speed;
	
	Cam.view.x = Cam.view.x + vpvec.x * speed;
	Cam.view.z = Cam.view.z + vpvec.z * speed;

	return Cam;
}

struct camera strafecamera(struct camera Cam, float speed)
{
	vector vpvec;
	vpvec.x = Cam.view.x - Cam.pos.x;
	vpvec.y = Cam.view.y - Cam.pos.y;
	vpvec.z = Cam.view.z - Cam.pos.z;

	Cam.pos.x = Cam.pos.x - vpvec.z * speed;
	Cam.pos.z = Cam.pos.z + vpvec.x * speed;

	Cam.view.x = Cam.view.x - vpvec.z * speed;
	Cam.view.z = Cam.view.z + vpvec.x * speed;

	return Cam;
}

struct camera rotatecamera(struct camera Cam, float speed)
{
	vector vpvec;
	vpvec.x = Cam.view.x - Cam.pos.x;
	vpvec.y = Cam.view.y - Cam.pos.y;
	vpvec.z = Cam.view.z - Cam.pos.z;

	Cam.view.z = (float) (Cam.pos.z + sin(speed) * vpvec.x + cos(speed) * vpvec.z);
	Cam.view.x = (float) (Cam.pos.x + cos(speed) * vpvec.x - sin(speed) * vpvec.z);

	return Cam;
}

struct camera upcamera(struct camera Cam, float speed)
{
	Cam.view.y = Cam.view.y + 2 * speed; 

	if((Cam.view.y - Cam.pos.y) > 8) Cam.view.y = Cam.pos.y + 8;
	if((Cam.view.y - Cam.pos.y) < -8) Cam.view.y = Cam.pos.y - 8;

	return Cam;
}

struct camera flycamera(struct camera Cam, float speed)
{
	Cam.view.y = Cam.view.y + speed;
	Cam.pos.y = Cam.pos.y + speed;

	return Cam;
}

void init()
{
	glShadeModel(GL_SMOOTH);
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glClearDepth(1.0f);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	gluPerspective(45.0f, XWINSIZE/YWINSIZE, 0.1f, 100.0f);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	Camera = setcamposition(Camera, 0, 2.5f, 5, 0, 2.5f, 0, 0, 1, 0);
}


void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	//gluPerspective(45.0f, XWINSIZE/YWINSIZE, 0.1f, 100.0f); 
	gluPerspective(45.0f, w/h, 0.1f, 100.0f);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(Camera.pos.x, Camera.pos.y, Camera.pos.z,
		  Camera.view.x, Camera.view.y, Camera.view.z,
		  Camera.up.x, Camera.up.y, Camera.up.z);
}

void drawgrid()
{
	float i;

	glBegin(GL_LINES);
	for(i = 0; i <= q; i += 4)
	{
		glColor3f(0.1f, 0.1f, 0.1f);
		/*glVertex3f(0, 0, i);
		glVertex3f(q, 0, i);
		glVertex3f(i, 0, 0);
		glVertex3f(i, 0, q);*/
		glVertex3f(0, q, i);
		glVertex3f(q, q, i);
		glVertex3f(i, q, 0);
		glVertex3f(i, q, q);
		glVertex3f(0, i, 0);
		glVertex3f(q, i, 0);
		glVertex3f(i, 0, 0);
		glVertex3f(i, q, 0);
		glVertex3f(0, i, q);
		glVertex3f(q, i, q);
		glVertex3f(i, 0, q);
		glVertex3f(i, q, q);
		glVertex3f(0, 0, i);
		glVertex3f(0, q, i);
		glVertex3f(0, i, 0);
		glVertex3f(0, i, q);
		glVertex3f(q, 0, i);
		glVertex3f(q, q, i);
		glVertex3f(q, i, 0);
		glVertex3f(q, i, q);
	}

	glEnd();
}

void drawviewpoint()
{
	/*glColor3f(0.0, 0.0, 0.0);
        glPointSize(3.0);

	glBegin(GL_POINTS);
		glVertex3f(Camera.view.x, Camera.view.y, Camera.view.z);
	glEnd();	*/
}


void drawfunc()
{
	float x;
	float z;
	float start;
	
	glBegin(GL_LINES);
    for(x = 0; x < q; x += .5)
    for(z = 0; z < q; z += .5)
    {
    glVertex3f(x, (x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2)), z);
    glVertex3f(x+0.5, (x+0.5-q/2)*pow(1.05, -pow(x+0.5-q/2,2)-pow(z+0.5-q/2,2)), z+0.5);
    glVertex3f(x+0.5, (x+0.5-q/2)*pow(1.05, -pow(x+0.5-q/2,2)-pow(z-q/2,2)), z);
    glVertex3f(x, (x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z+0.5-q/2,2)), z+0.5);
    
start = glutGet(GLUT_ELAPSED_TIME);

glColor3f(cos(start/100)*sqrt(sin(x)+sin(z)), sin(start/100)*sqrt(sin(x+3)+sin(z+3)),0);
    
    }
    glEnd();
    }



int floortest()
{
float x;
float z;
for (x = pos[0]-radius1; x <= (pos[0]+radius1); x = x+radius1/13.0)
{
for (z = pos[2]-radius1; z <= (pos[2]+radius1); z = z+radius1/13.0)
{
if((sqrt((pos[0]-x)*(pos[0]-x)+(pos[1]-((x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2))))*(pos[1]-((x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2))))+(pos[2]-z)*(pos[2]-z)))<=radius1)
{
floorpos[0]=x;
floorpos[1]=(x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2));
floorpos[2]=z;
return 1;
}}}
return 0;
}



int floortest2()
{
float x;
float z;
for (x = pos2[0]-radius2; x <= (pos2[0]+radius2); x = x+radius2/13.0)
{
for (z = pos2[2]-radius2; z <= (pos2[2]+radius2); z = z+radius2/13.0)
{
if((sqrt((pos2[0]-x)*(pos2[0]-x)+(pos2[1]-((x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2))))*(pos2[1]-((x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2))))+(pos2[2]-z)*(pos2[2]-z)))<=radius2)
{
floorpos2[0]=x;
floorpos2[1]=(x-q/2)*pow(1.05, -pow(x-q/2,2)-pow(z-q/2,2));
floorpos2[2]=z;
return 1;
}}}
return 0;
}





void display(void)
{
	


double magic;
double k[3];

struct timeb a;
struct timeb c;
double tchange;


struct timeb a2;
struct timeb c2;
double tchange2;

/*time initializer and changer*/
if ((tint==5.0))
{
	ftime(& a);
	tint=a.time+a.millitm/1000.0;
	tchange=tint;
}
else
{
	ftime(& c);
	tchange=c.time+c.millitm/1000.0;
}

if ((tint2==5.0))
{
	ftime(& a2);
	tint2=a2.time+a2.millitm/1000.0;
	tchange2=tint2;
}
else
{
	ftime(& c2);
	tchange2=c2.time+c2.millitm/1000.0;
}


/*kinematics for motion*/
pos[0]=xi;
pos[1]=yi;
pos[2]=zi;
pos[0]=pos[0]+(vxi*(tchange-tint)+xaccel*(tchange-tint)*(tchange-tint)/2.0);
pos[1]=pos[1]+(vyi*(tchange-tint)+yaccel*(tchange-tint)*(tchange-tint)/2.0);
pos[2]=pos[2]+(vzi*(tchange-tint)+zaccel*(tchange-tint)*(tchange-tint)/2.0);

pos2[0]=xi2;
pos2[1]=yi2;
pos2[2]=zi2;
pos2[0]=pos2[0]+(vxi2*(tchange2-tint2)+xaccel2*(tchange2-tint2)*(tchange2-tint2)/2.0);
pos2[1]=pos2[1]+(vyi2*(tchange2-tint2)+yaccel2*(tchange2-tint2)*(tchange2-tint2)/2.0);
pos2[2]=pos2[2]+(vzi2*(tchange2-tint2)+zaccel2*(tchange2-tint2)*(tchange2-tint2)/2.0);


/*collision and kinematics for bounce*/
/*notice the prevpos fix for the new position*/


/*ball collision*/

if(sqrt((pos[0]-pos2[0])*(pos[0]-pos2[0])+(pos[1]-pos2[1])*(pos[1]-pos2[1])+(pos[2]-pos2[2])*(pos[2]-pos2[2]))<=(radius1+radius2))
{
vz2=vzi2+zaccel2*(tchange2-tint2);
vzi2=vz2;
vz=vzi+zaccel*(tchange-tint);
vzi=vz;
vy2=vyi2+yaccel2*(tchange2-tint2);
vyi2=vy2;
vy=vyi+yaccel*(tchange-tint);
vyi=vy;
vx=vxi+xaccel*(tchange-tint);
vxi=vx;
vx2=vxi2+xaccel2*(tchange2-tint2);
vxi2=vx2;





/*old collision
temp=vyi2;
vyi2=dampy*vyi;
vyi=dampy*temp;

temp=vxi2;
vxi2=dampx*vxi;
vxi=dampx*temp;

temp=vzi2;
vzi2=dampz*vzi;
vzi=dampz*temp;
*/


k[0]=(pos[0]-pos2[0])/(radius1+radius2);
k[1]=(pos[1]-pos2[1])/(radius1+radius2);
k[2]=(pos[2]-pos2[2])/(radius1+radius2);


magic=2*(k[0]*(vx-vx2)+k[1]*(vy-vy2)+k[2]*(vz-vz2))/(1/mass+1/mass2);
vxi=-(magic*k[0]/mass-vxi);
vyi=-(magic*k[1]/mass-vyi);
vzi=-(magic*k[2]/mass-vzi);

vxi2=-(magic*k[0]/(-mass2)-vxi2);
vyi2=-(magic*k[1]/(-mass2)-vyi2);
vzi2=-(magic*k[2]/(-mass2)-vzi2);


xi=prevpos[0];
yi=prevpos[1];
xi2=prevpos2[0];
yi2=prevpos2[1];
zi=prevpos[2];
zi2=prevpos2[2];

pos[0]=prevpos[0];
pos[1]=prevpos[1];
pos[2]=prevpos[2];

pos2[0]=prevpos2[0];
pos2[1]=prevpos2[1];
pos2[2]=prevpos2[2];

tint=5.0;
tint2=5.0;

}


else
{



if(floortest()==1)
{
vz=vzi+zaccel*(tchange-tint);
vzi=vz;
vy=vyi+yaccel*(tchange-tint);
vyi=vy;
vx=vxi+xaccel*(tchange-tint);
vxi=vx;




unit[0]=(-pos[0]+floorpos[0])/radius1;
unit[1]=(-pos[1]+floorpos[1])/radius1;
unit[2]=(-pos[2]+floorpos[2])/radius1;

dotprod=(unit[0]*(vx)+unit[1]*(vy)+unit[2]*(vz));
vxi=-dampx*(2*dotprod*unit[0]-vxi);
vyi=-dampy*(2*dotprod*unit[1]-vyi);
vzi=-dampz*(2*dotprod*unit[2]-vzi);


tint=5.0;


zi=prevpos[2];
yi=prevpos[1];
xi=prevpos[0];

pos[0]=prevpos[0];
pos[1]=prevpos[1];
pos[2]=prevpos[2];
}

if(floortest2()==1)
{
vz2=vzi2+zaccel2*(tchange2-tint2);
vzi2=vz2;
vy2=vyi2+yaccel2*(tchange2-tint2);
vyi2=vy2;
vx2=vxi2+xaccel2*(tchange2-tint2);
vxi2=vx2;




unit2[0]=(-pos2[0]+floorpos2[0])/radius2;
unit2[1]=(-pos2[1]+floorpos2[1])/radius2;
unit2[2]=(-pos2[2]+floorpos2[2])/radius2;

dotprod2=(unit2[0]*(vx2)+unit2[1]*(vy2)+unit2[2]*(vz2));
vxi2=-dampx*(2*dotprod2*unit2[0]-vxi2);
vyi2=-dampy*(2*dotprod2*unit2[1]-vyi2);
vzi2=-dampz*(2*dotprod2*unit2[2]-vzi2);


tint2=5.0;


zi2=prevpos2[2];
yi2=prevpos2[1];
xi2=prevpos2[0];

pos2[0]=prevpos2[0];
pos2[1]=prevpos2[1];
pos2[2]=prevpos2[2];
}



if((pos[2]<(0.0+radius1)) || pos[2]>(q-radius1))
{
vz=vzi+zaccel*(tchange-tint);
vzi=-dampz*vz;
vy=vyi+yaccel*(tchange-tint);
vyi=vy;
vx=vxi+xaccel*(tchange-tint);
vxi=vx;
tint=5.0;



zi=prevpos[2];
yi=prevpos[1];
xi=prevpos[0];

pos[0]=prevpos[0];
pos[1]=prevpos[1];
pos[2]=prevpos[2];
}

if(pos2[2]<(0.0+radius2) || pos2[2]>(q-radius2))
{
vz2=vzi2+zaccel2*(tchange2-tint2);
vzi2=-dampz*vz2;
vy2=vyi2+yaccel2*(tchange2-tint2);
vyi2=vy2;
vx2=vxi2+xaccel2*(tchange2-tint2);
vxi2=vx2;
tint2=5.0;


zi2=prevpos2[2];
yi2=prevpos2[1];
xi2=prevpos2[0];

pos2[0]=prevpos2[0];
pos2[1]=prevpos2[1];
pos2[2]=prevpos2[2];

}

if(/*pos[1]<(0.0+radius1) || */pos[1]>(q-radius1))
{
vz=vzi+zaccel*(tchange-tint);
vzi=vz;
vy=vyi+yaccel*(tchange-tint);
vyi=-dampy*vy;
vx=vxi+xaccel*(tchange-tint);
vxi=vx;
tint=5.0;


zi=prevpos[2];
yi=prevpos[1];
xi=prevpos[0];

pos[0]=prevpos[0];
pos[1]=prevpos[1];
pos[2]=prevpos[2];
}

if(/*pos2[1]<(0.0+radius2) || */pos2[1]>(q-radius2))
{
vz2=vzi2+zaccel2*(tchange2-tint2);
vzi2=vz2;
vy2=vyi2+yaccel2*(tchange2-tint2);
vyi2=-dampy*vy2;
vx2=vxi2+xaccel2*(tchange2-tint2);
vxi2=vx2;
tint2=5.0;
zi2=prevpos2[2];
yi2=prevpos2[1];
xi2=prevpos2[0];

pos2[0]=prevpos2[0];
pos2[1]=prevpos2[1];
pos2[2]=prevpos2[2];

}

if(pos[0]<(0.0+radius1) || pos[0]>(q-radius1))
{
vz=vzi+zaccel*(tchange-tint);
vzi=vz;
vy=vyi+yaccel*(tchange-tint);
vyi=vy;
vx=vxi+xaccel*(tchange-tint);
vxi=-dampx*vx;
tint=5.0;


zi=prevpos[2];
yi=prevpos[1];
xi=prevpos[0];

pos[0]=prevpos[0];
pos[1]=prevpos[1];
pos[2]=prevpos[2];
}

if(pos2[0]<(0.0+radius2) || pos2[0]>(q-radius2))
{
vz2=vzi2+zaccel2*(tchange2-tint2);
vzi2=vz2;
vy2=vyi2+yaccel2*(tchange2-tint2);
vyi2=vy2;
vx2=vxi2+xaccel2*(tchange2-tint2);
vxi2=-dampx*vx2;
tint2=5.0;
zi2=prevpos2[2];
yi2=prevpos2[1];
xi2=prevpos2[0];

pos2[0]=prevpos2[0];
pos2[1]=prevpos2[1];
pos2[2]=prevpos2[2];

}

}






prevpos[0]=pos[0];
prevpos[1]=pos[1];
prevpos[2]=pos[2];
prevpos2[0]=pos2[0];
prevpos2[1]=pos2[1];
prevpos2[2]=pos2[2];

	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
	gluLookAt(Camera.pos.x, Camera.pos.y, Camera.pos.z,
		  Camera.view.x, Camera.view.y, Camera.view.z,
		  Camera.up.x, Camera.up.y, Camera.up.z);

	drawgrid();
	drawviewpoint();
	drawfunc();
	/*spheres*/

	glPushMatrix();
	glColor3f(0.0f, 0.2f, 1.0f);
	glTranslatef(pos[0],pos[1],pos[2]);
	glutSolidSphere(radius1, 100.0, 16.0);
	glFlush();
	glPopMatrix();





	glPushMatrix();
	glColor3f(0.750f, 0.0f, 1.0f);
	glTranslatef(pos2[0],pos2[1],pos2[2]);
	glutSolidSphere(radius2, 100.0, 16.0);
	glFlush();
	glPopMatrix();
	
	

/*indicator lines*/
	
	glBegin(GL_LINES);
	glColor3f(0.0f, 0.1f, .5f);
	glVertex3f(pos[0],pos[1],q);
	glVertex3f(pos[0],pos[1],0.0);

	glVertex3f(0.0,pos[1],pos[2]);
	glVertex3f(q,pos[1],pos[2]);

	glVertex3f(pos[0],q,pos[2]);
	glVertex3f(pos[0],0.0,pos[2]);
	
	glColor3f(0.38f,0.0f,0.5f);
	glVertex3f(pos2[0],pos2[1],q);
	glVertex3f(pos2[0],pos2[1],0.0);

	glVertex3f(0.0,pos2[1],pos2[2]);
	glVertex3f(q,pos2[1],pos2[2]);

	glVertex3f(pos2[0],q,pos2[2]);
	glVertex3f(pos2[0],0.0,pos2[2]);
	glEnd();

	glColor3f(0.0,1.0,0.0);
/*cube*/

	glBegin(GL_LINES);
	
	glVertex3f(0,0,0);
	glVertex3f(0,q,0);
	
	glVertex3f(0,q,0);
	glVertex3f(0,q,q);
	
	glVertex3f(0,q,q);
	glVertex3f(0,0,q);
	
	glVertex3f(0,0,q);
	glVertex3f(0,0,0);
	
	


	glVertex3f(q,0,0);
	glVertex3f(q,q,0);
	
	glVertex3f(q,q,0);
	glVertex3f(q,q,q);
	
	glVertex3f(q,q,q);
	glVertex3f(q,0,q);
	
	glVertex3f(q,0,q);
	glVertex3f(q,0,0);
	
	


	glVertex3f(0,0,0);
	glVertex3f(q,0,0);
	
	glVertex3f(0,0,q);
	glVertex3f(q,0,q);
	
	glVertex3f(0,q,q);
	glVertex3f(q,q,q);
	
	glVertex3f(0,q,0);
	glVertex3f(q,q,0);
	
	
	glEnd();


	glFlush();
	glutSwapBuffers();
}

void idle(void)
{
	glutPostRedisplay();
}


void keyboard(unsigned char key, int x, int y)
{
	switch(key)
	{
		case 'i':
			Camera = upcamera(Camera, +SPEED);
			break;
		case 'k':
			Camera = upcamera(Camera, -SPEED);
			break;
		case 'l':
			Camera = rotatecamera(Camera, +SPEED);
			break;
		case 'j': 
			Camera = rotatecamera(Camera, -SPEED);	
			break;
		case 'w':
			Camera = movecamera(Camera, +SPEED);
			break;
		case 's':
			Camera = movecamera(Camera, -SPEED);
			break;
		case 'a':
			Camera = strafecamera(Camera, -SPEED);
			break;
		case 'd':
			Camera = strafecamera(Camera, +SPEED);
			break;
		case 'r':
			Camera = flycamera(Camera, +SPEED);
			break;
		case 'f':
			Camera = flycamera(Camera, -SPEED);
			break;
		case 'q':
			exit(0);
			break;
	}
}

int main(int argc, char** argv)
{
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(XWINSIZE, YWINSIZE);
	glutInitWindowPosition(XWINPOS, YWINPOS);
	glutCreateWindow("Curtis and Jeff Project");

	init();
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	//glutMotionFunc(motion);
	glutIdleFunc(idle);
	//glutMouseFunc(mouse);
	glutKeyboardFunc(keyboard);
	glutMainLoop();

	return 0;
}