#pragma once
#include "../../stdafx.h"

#include "../glLibTime.h"
#include "../glLibMath/glLibMathIntersect.h"

#include "glLibExtPhysicsBody.h"

/*
template <typename type_float> class glLibExtPhysicsWorld {
	private:
		glLibTimer _timer;
		type_float _forces_global[3];
		list<glLibExtPhysicsBody<type_float>*> _bodies;
		bool _running;

	public:
		glLibExtPhysicsWorld() {
			_forces_global[0] = _forces_global[1] = _forces_global[2] = 0;
			_running = false;
		}
		~glLibExtPhysicsWorld() {}

		inline void add_global_force(type_float force[3]) {
			add_global_force(force[0],force[1],force[2]);
		}
		void add_global_force(type_float force_x, type_float force_y, type_float force_z) {
			_forces_global[0] += force_x;
			_forces_global[1] += force_y;
			_forces_global[2] += force_z;
		}

		void add_body(glLibExtPhysicsBody<type_float>* new_body) {
			//TODO: check for errors (i.e., adding already existing body) here
			_bodies.push_back(new_body);
		}
		//TODO: remove_body

		void step(void) {
			if (_running) {
				double time = _timer.get();
				_timer.mark();
				step( (type_float)(time) );
			}
			else {
				_timer.mark();
				step(0);
			}
		}
		void step(type_float dt) {
			_running = true;

			list<glLibExtPhysicsBody<type_float>*>::const_iterator iter;

			for ( iter=_bodies.begin(); iter!=_bodies.end(); ++iter) {
				(*iter)->add_force(_forces_global,NULL,GLLIBEXT_RELATIVE);
			}

			type_float stepped = 0;
			type_float step_amount = dt;
			while (stepped<dt) {			
				for ( iter=_bodies.begin(); iter!=_bodies.end(); ++iter) {
					(*iter)->glLibInternal_move(step_amount);
				}
				if (glLibInternal_collisions()) {
					for ( iter=_bodies.begin(); iter!=_bodies.end(); ++iter) {
						(*iter)->glLibInternal_move(-step_amount);
					}
					step_amount /= 2.0;
				}
				else {
					stepped += step_amount;
					step_amount = dt - stepped;
				}
			}

			glLibMathVec::zero(_forces_global,3);
			for ( iter=_bodies.begin(); iter!=_bodies.end(); ++iter) {
				(*iter)->glLibInternal_zero();
			}
		}

	private:
		bool glLibInternal_collisions(void) {
			throw glLibError("Not implemented!");
			list<glLibExtPhysicsBody<type_float>*>::const_iterator iter1, iter2;
			for ( iter1=_bodies.begin(); iter1!=_bodies.end(); ++iter1) {
				for ( iter2=_bodies.begin(); iter2!=_bodies.end(); ++iter2) {
					if (iter1==iter2) continue;
					glLibInternal_collide((*iter1),(*iter2));
				}
			}
			return false;
		}
		void glLibInternal_collide(glLibExtPhysicsBody<type_float>* b1, glLibExtPhysicsBody<type_float>* b2) {
			map<string,glLibInternal_geometry<type_float>*>::const_iterator iter1, iter2;

			iter1=b1->_meshes.begin();
			for (iter1=b1->_meshes.begin();iter1!=b1->_meshes.end();++iter1) {

				for (iter2=b2->_meshes.begin();iter2!=b2->_meshes.end();++iter2) {
					glLibInternal_geometry<type_float> *mesh1=iter1->second, *mesh2=iter2->second;

					for (unsigned long i=0;i<mesh1->_numof_triangles;++i) {
						for (unsigned long j=0;j<mesh2->_numof_triangles;++j) {
							//type_float tri1[3][3], tri2[3][3], contact1[3], contact2[3], contact3[3];
							
							/*char f = 0;
							for (char k=0;k<3;++k) {
								for (char l=0;l<3;++l) {
									tri1[k][l] = mesh1->_triangles[9*i + (f++)];
									tri2[k][l] = mesh2->_triangles[9*j + (f++)];
								}
							}*//*

							//if (glLibMathIntersect::triangle_triangle(tri1,tri2,contact1,contact2,contact3)) throw glLibError("Yay!");
							
						}
					}
				}
			}
		}
};

*/