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

#include "../glLibMath/glLibMathVec.h"
#include "glLibExtPhysicsWorld.h"

/*
template <typename type_float> class glLibInternal_geometry {
	template <class type_float2> friend class glLibExtPhysicsWorld;

	private:
		unsigned long _numof_triangles;
		type_float* _vertices; //Array of floats (each 9 is a triangle).  Probably better to be a 3D array, but this will be faster
		type_float* _normals;

	public:
		glLibInternal_geometry(glLibInternal_mesh** meshes, int numof_meshes) {
			throw glLibError("Not implemented!");
			char vpf;// = meshes[index_mesh]->verts_per_face;
			/*

			_numof_triangles = 0;
			for (int i=0;i<numof_meshes;++i) {
				if (vpf == 3) {
					_numof_triangles += meshes[i]->numof_faces;
				}
				else if (vpf == 4) {
					_numof_triangles += meshes[i]->numof_faces * 2;
				}
				else throw glLibError("Not implemented!");
			}*//*

			_vertices = new type_float[9*_numof_triangles];
			_normals = new type_float[3*_numof_triangles];

			unsigned long index_float_vertex = 0;
			unsigned long index_float_normal = 0;
			for (int index_mesh=0;index_mesh<numof_meshes;++index_mesh) {
				#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
				if (!meshes[index_mesh]->has_normals) throw glLibError("Geometry must have normal data!");
				#endif

				if (vpf==3) {
					for (unsigned long index_vertex=0;index_vertex<meshes[index_mesh]->numof_faces*3;++index_vertex) {
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[3*index_vertex  ];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[3*index_vertex+1];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[3*index_vertex+2];
					}
				}
				else { //4
					for (unsigned long index_vertex=0;index_vertex<meshes[index_mesh]->numof_faces*4;++index_vertex) {
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex  ];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex+1];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex+2];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex  ];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex+2];
						_vertices[index_float_vertex++] = meshes[index_mesh]->array_vertices[4*index_vertex+3];
					}
				}

				//Normal data
				for (unsigned long index_face=0;index_face<meshes[index_mesh]->numof_faces;++index_face) {
					type_float normal[3] = {0,0,0};
					for (char index_vertex=0;index_vertex<vpf;++index_vertex) {
						normal[0] += meshes[index_mesh]->array_normals[9*index_face+vpf*index_vertex  ];
						normal[1] += meshes[index_mesh]->array_normals[9*index_face+vpf*index_vertex+2];
						normal[2] += meshes[index_mesh]->array_normals[9*index_face+vpf*index_vertex+3];
					}
					_vertices[index_float_normal++] = normal[0] / vpf;
					_vertices[index_float_normal++] = normal[1] / vpf;
					_vertices[index_float_normal++] = normal[2] / vpf;
				}
			}
		}
		~glLibInternal_geometry(void) {
			delete [] _vertices;
			delete [] _normals;
		}
};
template <typename type_float> class glLibExtPhysicsBody {
	template <class type_float> friend class glLibExtPhysicsWorld;
	private:
		map<string,glLibInternal_geometry<type_float>*> _meshes;

		//0 = asleep (collision only)
		//1 = alive (collision and movement)
		char _state;
		type_float _forces[3];
		type_float _torque[3];
		glLibMathMatrix<type_float,char> _orientation;
		type_float _moment_of_inertia;

	public:
		type_float position[3];
		type_float velocity[3];
		type_float velocity_angular[3];
		type_float rotation[3];
		bool locked;
		type_float scale[3];
		type_float mass;

	public:
		glLibExtPhysicsBody() : _orientation(4,4,GLLIB_MATRIX_IDENTITY) {
			position[0] = position[1] = position[2] = 0;
			velocity[0] = velocity[1] = velocity[2] = 0;
			glLibMathVec::zero(velocity_angular,3);
			_forces[0] = _forces[1] = _forces[2] = 0;
			_torque[0] = _torque[1] = _torque[2] = 0;
			glLibMathVec::zero(rotation,3);

			locked = false;
			scale[0] = scale[1] = scale[2] = 1;
			_state = 0;

			mass = 1;
			_moment_of_inertia = 1;
		}
		~glLibExtPhysicsBody() {
			map<string,glLibInternal_geometry<type_float>*>::iterator iter;
			for (iter=_meshes.begin();iter!=_meshes.end();++iter) {
				delete (iter->second);
			}
		}

		void add_meshes(string name, glLibObject* object) {
			#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
			if (object->numof_meshes==0) throw glLibError("Object contains no meshes!");
			if (object->meshes[0]->array_vertices==NULL) throw glLibError("Object does not have array data built!");
			if (_meshes.find(name)!=_meshes.end()) throw glLibError("This name is already being used for a mesh!");
			#endif
			_meshes[name] = new glLibInternal_geometry<type_float>(object->meshes,object->numof_meshes);
		}
		//TODO: remove_meshes

		//TODO: apply_transform(glLibMathMatrix<type_float>* matrix);
		void apply_transform(void) {
			glTranslated(position[0],position[1],position[2]);
			/*const GLdouble mat[16] = {
				_orientation.values[0][0],_orientation.values[0][1],_orientation.values[0][2],_orientation.values[0][3],
				_orientation.values[1][0],_orientation.values[1][1],_orientation.values[1][2],_orientation.values[1][3],
				_orientation.values[2][0],_orientation.values[2][1],_orientation.values[2][2],_orientation.values[2][3],
				_orientation.values[3][0],_orientation.values[3][1],_orientation.values[3][2],_orientation.values[3][3]
			};
			glMultMatrixd(mat);*//*
			type_float l = glLibMathVec::length(rotation,3);
			glRotatef(l,rotation[0],rotation[1],rotation[2]);
			glScaled(scale[0],scale[1],scale[2]);
		}

		inline void add_force(type_float force[3], type_float location[3]=NULL, glLibExtFlagPhysics flag=GLLIBEXT_WORLD) {
			if (location!=NULL) add_force(force[0],force[1],force[2],location[0],location[1],location[2],flag);
			else add_force(force[0],force[1],force[2],0,0,0,flag);
		}
		void add_force(
			type_float force_x, type_float force_y, type_float force_z,
			type_float x=0, type_float y=0, type_float z=0,
			glLibExtFlagPhysics flag=GLLIBEXT_WORLD)
		{
			_forces[0] += force_x;
			_forces[1] += force_y;
			_forces[2] += force_z;

			add_torque(x,y,z,flag);
		}

		//TODO: add_torque(displacement,force)
		inline void add_torque(type_float torque[3], glLibExtFlagPhysics flag=GLLIBEXT_WORLD) {
			add_force(torque[0],torque[1],torque[2],flag);
		}
		void add_torque(type_float torque_x, type_float torque_y, type_float torque_z, glLibExtFlagPhysics flag=GLLIBEXT_WORLD) {
			if (flag==GLLIBEXT_WORLD) throw glLibError("Not implemented!");
			else if (flag==GLLIBEXT_RELATIVE) {
				_torque[0] += torque_x;
				_torque[1] += torque_y;
				_torque[2] += torque_z;
			}
			#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
			else throw glLibError("Flag type not recognized!  Should be GLLIBEXT_RELATIVE or GLLIBEXT_WORLD!");
			#endif
		}

	private:
		void glLibInternal_move(type_float dt) {
			if (locked) return;

			if (position[1]<0) {
				add_force(0,1,0, 1,0,1, GLLIBEXT_RELATIVE);
			}

			//f = m*a, a = f / m
			type_float a[3], temp[3];
			glLibMathVec::scale((type_float)(1.0/mass),_forces,a,3);

			//integral a    = v(t) =        a*t  + v0
			//integral v(t) = x(t) = (a/2)*(t^2) + v0*t + x0

			//Change velocity
			glLibMathVec::scale((type_float)(dt*dt*0.5),a,temp,3);
			glLibMathVec::add(temp,position,position,3);
			//printf("    Nonlinear term = (%2.3f,%2.3f,%2.3f)\n",temp[0],temp[1],temp[2]);
			glLibMathVec::scale(dt,velocity,temp,3);
			//printf("    Linear term = (%2.3f,%2.3f,%2.3f)\n",temp[0],temp[1],temp[2]);
			glLibMathVec::add(temp,position,position,3);

			//Change velocity
			glLibMathVec::scale(dt,a,temp,3);
			glLibMathVec::add(temp,velocity,velocity,3);

			//Get the angular acceleration
			//http://en.wikipedia.org/wiki/Angular_acceleration
			glLibMathVec::scale((type_float)(1.0/_moment_of_inertia),_torque,temp,3);
			glLibMathVec::add(temp,velocity_angular,velocity_angular,3);

			//Add angular velocity
			glLibMathVec::add(velocity_angular,rotation,rotation,3);
		}
		void glLibInternal_zero(void) {
			//Reset everything
			glLibMathVec::zero(_forces,3);
			glLibMathVec::zero(_torque,3);
		}
};*/