#pragma comment(lib, "SDL.lib")
#pragma comment(lib, "SDLmain.lib")
#pragma comment(lib, "OpenGL32.lib")
#pragma comment(lib, "GLu32.lib")
#pragma comment(lib, "ode.lib")

#include "SDL.h"
#include "ode/ode.h"
#include <windows.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>

#define PHYSICSTEST_FPS 60

SDL_Surface *screen;
dWorldID world;
dBodyID box_bodies[10];
dGeomID box_geoms[10];
dJointGroupID contact_group;
dSpaceID space;
float mouse_position[2];
bool keys[512];

void InitializeGraphics() {
	glShadeModel(GL_SMOOTH);
	glClearColor(0, 0, 0, 0);
	glClearDepth(1);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE);
	glEnable(GL_TEXTURE_2D);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
}
void InitializeViewport(int width, int height) {
	if(!height) height = 1;
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glScalef(2 / (float)width, 2 / (float)height, 0);
	glRotatef(180, 1, 0, 0);
	glTranslatef(GLfloat(width / -2), GLfloat(height / -2), 0);
}
void InitializePysics() {
	world = dWorldCreate();
	dWorldSetERP(world, 1);
	space = dSimpleSpaceCreate(0);
	
	dMass mass;
	dMassSetBoxTotal(&mass, 1, 30, 30, 0);

	srand(time(NULL));
	for(int i = 0; i < 10; i++) {
		box_bodies[i] = dBodyCreate(world);
		dBodySetPosition(box_bodies[i], rand() % 400 + 200, rand() % 300 + 150, 0);
		box_geoms[i] = dCreateBox(space, 30, 30, 10);
		dGeomSetBody(box_geoms[i], box_bodies[i]);
		dBodySetMass(box_bodies[i], &mass);
	}
	contact_group = dJointGroupCreate(0);
}
void NearCallback(void *data, dGeomID first_geom, dGeomID second_geom) {
	dBodyID first_body = dGeomGetBody(first_geom),
		second_body = dGeomGetBody(second_geom);

	dContact contact_info[5];
	int contact_number = dCollide(first_geom, second_geom, 5,
		&contact_info[0].geom, sizeof(dContact));
	for(int i = 0; i < contact_number; i++) {
		contact_info[i].surface.mode = 0;
		contact_info[i].surface.mu = 0.00001;
		dJointID contact_joint = dJointCreateContact(world,
			contact_group,
			&contact_info[i]);
		dJointAttach(contact_joint, first_body, second_body);
	}
}
void Think() {
	if(keys[SDLK_LEFT]) dBodyAddForce(box_bodies[0], -500, 0, 0);
	if(keys[SDLK_RIGHT]) dBodyAddForce(box_bodies[0], 500, 0, 0);
	if(keys[SDLK_UP]) dBodyAddForce(box_bodies[0], 0, -500, 0);
	if(keys[SDLK_DOWN]) dBodyAddForce(box_bodies[0], 0, 500, 0);
	
	dSpaceCollide(space, 0, NearCallback);
	for(int i = 0; i < 10; ++i) {
		float *body_position = (float *)dBodyGetPosition(box_bodies[i]);
		body_position[2] = 0;
		float *body_angular_velocity = (float *)dBodyGetAngularVel(box_bodies[i]);
		body_angular_velocity[0] = 0;
		body_angular_velocity[1] = 0;
	}
	// If we are running into performance problems, change this into dWorldQuickStep
	dWorldStep(world, 0.01);
	dJointGroupEmpty(contact_group);
}
// Given a body and a destination float[16], creates a transform matrix that reflects the bodys position and rotation
void BodyCreateTransform(dBodyID body, float *dest) {
	if(!dest) return;
	float *body_rotation = (float *)dBodyGetRotation(body);
	float *body_position = (float *)dBodyGetPosition(body);
	dest[0] = body_rotation[0];
	dest[1] = body_rotation[4];
	dest[2] = body_rotation[8];
	dest[3] = 0;

	dest[4] = body_rotation[1];
	dest[5] = body_rotation[5];
	dest[6] = body_rotation[9];
	dest[7] = 0;

	dest[8] = body_rotation[2];
	dest[9] = body_rotation[6];
	dest[10] = body_rotation[10];
	dest[11] = 0;

	dest[12] = body_position[0];
	dest[13] = body_position[1];
	dest[14] = 0;
	dest[15] = 1;
}
void Render() {
	glMatrixMode(GL_MODELVIEW);
	glClear(GL_COLOR_BUFFER_BIT |
		GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
	glPushMatrix();

	for(int i = 0; i < 10; i++) {
		if(!i)
			glColor3f(0, 1, 0);
		else
			glColor3f(1, 1, 1);

		glPushMatrix();
		float box_matrix[16];
		BodyCreateTransform(box_bodies[i], box_matrix);
		glMultMatrixf(box_matrix);
		glBegin(GL_QUADS);
			glVertex2i(-15, -15);
			glVertex2i(15, -15);
			glVertex2i(15, 15);
			glVertex2i(-15, 15);
		glEnd();
		glPopMatrix();
	}

	glPopMatrix();
	SDL_GL_SwapBuffers();
}
void Destroy() {
	dJointGroupDestroy(contact_group);
	for(int i = 0; i < 10; ++i) {
		dGeomDestroy(box_geoms[i]);
		dBodyDestroy(box_bodies[i]);
	}
	dWorldDestroy(world);
	dCloseODE();
	SDL_Quit();
}
int main(int argc, char **argv) {
	if(SDL_Init(SDL_INIT_VIDEO) < 0) {
		std::cout << "main: Failed to init SDL video!\n";
		SDL_Quit();
	}

	const SDL_VideoInfo *video_info = SDL_GetVideoInfo();
	if(!video_info) {
		std::cout << "main: Failed to get video info!\n";
		SDL_Quit();
	}

	int video_flags = SDL_OPENGL |
		SDL_GL_DOUBLEBUFFER |
		SDL_HWPALETTE;
	if(video_info->hw_available)
		video_flags |= SDL_HWSURFACE;
	else
		video_flags |= SDL_SWSURFACE;
	if(video_info->blit_hw)
		video_flags |= SDL_HWACCEL;
	SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
	
	screen = SDL_SetVideoMode(800, 600, 32, video_flags);
	if(!screen) {
		std::cout << "main: Failed to set video mode!\n";
		SDL_Quit();
	}
	std::cout << "main: Initialized SDL video!\n";

	InitializeViewport(800, 600);
	InitializeGraphics();
	InitializePysics();
	SDL_ShowCursor(true);
	SDL_WM_SetCaption("PhysicsTest", 0);

	atexit(Destroy);

	bool done = false,
		active = true;
	SDL_Event event;
	for(int i = 0; i < 512; ++i)
		keys[i] = false;

	int last_tick = SDL_GetTicks(),
		current_tick;

	while(!done) {
		while(SDL_PollEvent(&event)) {
			switch(event.type) {
			case SDL_ACTIVEEVENT:
				if(!event.active.gain)
					active = false;
				else
					active = true;
				break;
			case SDL_MOUSEMOTION:
				mouse_position[0] = event.motion.x;
				mouse_position[1] = event.motion.y;
				break;
			case SDL_MOUSEBUTTONDOWN: break;
			case SDL_MOUSEBUTTONUP: break;
			case SDL_KEYDOWN:
				if(event.key.keysym.sym == SDLK_ESCAPE)
					done = true;
				keys[event.key.keysym.sym] = true;
				break;
			case SDL_KEYUP:
				keys[event.key.keysym.sym] = false;
				break;
			case SDL_QUIT: done = true;
			}
		}
		if(active) {
			current_tick = SDL_GetTicks();
			if(current_tick <= last_tick) {
				SDL_Delay(1);
			}
			while(last_tick < current_tick) {
				Think();
				last_tick += 1000 / PHYSICSTEST_FPS;
			}
			Render();
		}
	}
	return 0;
}