//#include "pal.h"
#include "palFactory.h"

/*
	Abstract:
		PAL - Physics Abstraction Layer. 
		Implementation File

	Author: 
		Adrian Boeing
	Revision History:
		Version 0.84:19/09/06 GPS, remerged
		Version 0.83:17/02/05 velocimeter update
		Version 0.82:16/02/05 Changed velocimeter to relative coordinates
		Version 0.81:10/06/04 Correction to palBody::SetPosition 
		Version 0.8 : 3/06/04 
	TODO:
		-saferize vertex copyign for terrain heightmap and mesh
		-defines for infninity for joint limts
*/

#ifdef MEMDEBUG
#include <crtdbg.h>
#define new new(_NORMAL_BLOCK,__FILE__, __LINE__)
#endif

void palPhysics::SetFactoryInstance(palFactory *pf) {
	palFactory::SetInstance(pf);
}

#if 0
void paldebug_printmatrix(palMatrix4x4 *pm) {
	for (int i=0;i<16;i++) {
		printf("%f ",pm->_mat[i]);
		if (i%4 == 3) 
			printf("\n");
	}
}
void paldebug_printvector3(palVector3 *pv) {
	printf("%f %f %f\n",pv->x,pv->y,pv->z);
}

//void paldebug_printvector4(palVector4 *pv) {
//	printf("%f %f %f %f\n",pv->x,pv->y,pv->z,pv->w);
//}
#endif

FACTORY_CLASS_IMPLEMENTATION(palMaterials);
FACTORY_CLASS_IMPLEMENTATION(palMaterialUnique);
FACTORY_CLASS_IMPLEMENTATION(palMaterialInteraction);


void palMaterial::SetParameters(Float static_friction, Float kinetic_friction, Float restitution) {
	m_fStatic = static_friction;
	m_fKinetic = kinetic_friction;
	m_fRestitution = restitution;
}

palMaterialUnique::palMaterialUnique() {
}

void palMaterialUnique::Init(STRING name,Float static_friction, Float kinetic_friction, Float restitution) {
	palMaterial::SetParameters(static_friction,kinetic_friction,restitution);
	m_Name=name;
}

palMaterialInteraction::palMaterialInteraction() {
	m_pMaterial1=NULL;
	m_pMaterial2=NULL;
}

void palMaterialInteraction::Init(palMaterialUnique *pM1, palMaterialUnique *pM2, Float static_friction, Float kinetic_friction, Float restitution) {
	palMaterial::SetParameters(static_friction,kinetic_friction,restitution);
	m_pMaterial1 = pM1;
	m_pMaterial2 = pM2;
}

/*
class palMaterials : public palFactoryObject {
public:
	void NewMaterial(STRING name, Float static_friction, Float kinetic_friction, Float restitution); //default values
	void SetMaterialInteraction(STRING name1, STRING name2, Float static_friction, Float kinetic_friction, Float restitution);
protected:
	vector<STRING> m_MaterialNames;
	std_matrix<palMaterial *> m_Materials;

	FACTORY_CLASS(palMaterials,palMaterials,All,1);
};
*/

palMaterials::palMaterials() {

};

int palMaterials::GetIndex(STRING name) {
//	VECTOR<STRING>::iterator obj;
//	obj = std::find(m_MaterialNames.begin(), m_MaterialNames.end(), name);
	for (unsigned int i=0;i<m_MaterialNames.size();i++)
		if (m_MaterialNames[i] == name)
			return i;
	return -1;
}

palMaterialUnique *palMaterials::GetMaterial(STRING name) {
	int pos = GetIndex(name);
	if (pos<0) return NULL;
	palMaterial *pM= m_Materials.Get(pos,pos);
	return dynamic_cast<palMaterialUnique *> (pM);
}

void palMaterials::SetIndex(int posx, int posy, palMaterial *pm) {
	m_Materials.Set(posx,posy,pm);
}

void palMaterials::SetNameIndex(STRING name) {
	m_MaterialNames.push_back(name);
}

void palMaterials::NewMaterial(STRING name, Float static_friction, Float kinetic_friction, Float restitution) {
	if (GetIndex(name)!=-1) {
		SET_WARNING("Can not replace existing materials!");
		return;
	}

	palFactoryObject *pFO=PF->CreateObject("palMaterialUnique");
	palMaterialUnique *pMU = dynamic_cast<palMaterialUnique *>(pFO);
	if (pMU == NULL) {
		SET_ERROR("Could not create material");
		return;
	}
	pMU->Init(name,static_friction,kinetic_friction,restitution);
	//error?
	SetNameIndex(name);

	int size,check;
	m_Materials.GetDimensions(size,check); 
	if (size!=check) {
		SET_ERROR("Material size is non-equal. Might be out of memory");
		return;
	}
	m_Materials.Resize(size+1,size+1);
	//error?
	m_Materials.GetDimensions(size,check); 
	if (size!=check) {
		SET_ERROR("Material size is non-equal. Might be out of memory");
		return;
	}
	int pos = GetIndex(name);
	//m_Materials.Set(pos,pos,pMU);
	SetIndex(pos,pos,pMU);
	int i;
	for (i=0;i<size;i++) {
		SetIndex(i,pos,pMU);
	//	m_Materials.Set(i,pos,pMU);
	}
	for (i=0;i<size;i++) {
		SetIndex(pos,i,pMU);
		//m_Materials.Set(pos,i,pMU);
	}
}

void palMaterials::SetMaterialInteraction(STRING name1, STRING name2, Float static_friction, Float kinetic_friction, Float restitution) {
	if (name1==name2) {
		palMaterial *pm=GetMaterial(name1);
		pm->SetParameters(static_friction,kinetic_friction,restitution);
	} else {
		palFactoryObject *pFO=PF->CreateObject("palMaterialInteraction");
		palMaterialInteraction *pMI = dynamic_cast<palMaterialInteraction *>(pFO);
		pMI->Init(GetMaterial(name1),GetMaterial(name2),static_friction,kinetic_friction,restitution);
		int p1=GetIndex(name1);
		int p2=GetIndex(name2);
		SetIndex(p1,p2,pMI);
		SetIndex(p2,p1,pMI);
		//m_Materials.Set(p1,p2,pMI);
		//m_Materials.Set(p2,p1,pMI);
	}
}

////////////////////////////////////////
void palConvex::Init(Float x, Float y, Float z, const Float *pVertices, int nVertices, Float mass) {
	palBody::SetPosition(x,y,z);

	palFactoryObject *pFO=PF->CreateObject("palConvexGeometry");
	palConvexGeometry *m_pGeom = dynamic_cast<palConvexGeometry *>(pFO); //create the geometry
	m_Geometries.push_back(m_pGeom);
	SetGeometryBody(m_pGeom);
	m_pGeom->Init(m_mLoc,pVertices,nVertices,mass);

	m_Type = PAL_BODY_CONVEX;
}

void palBox::Init(Float x, Float y, Float z, Float width, Float height, Float depth, Float mass) {
	palBody::SetPosition(x,y,z);

	palFactoryObject *pFO=PF->CreateObject("palBoxGeometry");
	palBoxGeometry *m_pGeom = dynamic_cast<palBoxGeometry *>(pFO); //create the geometry
	m_Geometries.push_back(m_pGeom);
	SetGeometryBody(m_pGeom);

//	m_pBoxGeom = dynamic_cast<palBoxGeometry *>(pFO); //create the geometry
	//SetGeometryBody(m_pBoxGeom);
//	m_pBoxGeom->Init(x,y,z,width,height,depth,mass);
	m_pGeom->Init(m_mLoc,width,height,depth,mass);

	m_Type = PAL_BODY_BOX;
}

void palBox::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p=(Float *)param_array;
	printf("generic init of the box now! loc: %f %f %f, dim:%f %f %f\n",pos._41,pos._42,pos._43,p[0],p[1],p[2],p[3]);
	Init(pos._41,pos._42,pos._43,p[0],p[1],p[2],p[3]);
	//SetPosition(pos); 
}

void palSphere::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p=(Float *)param_array;
	Init(pos._41,pos._42,pos._43,p[0],p[1]);
	//SetPosition(pos); 
}

void palCylinder::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p=(Float *)param_array;
	Init(pos._41,pos._42,pos._43,p[0],p[1],p[2]);
	//SetPosition(pos); 
}

void palCompoundBody::GenericInit(palMatrix4x4 &pos, void *param_array) {
	Float *p=(Float *)param_array;
	Init(pos._41,pos._42,pos._43);
	//SetPosition(pos); 
}

/*
void palBox::GenericInit(void *param, ...) {
	Float p[7];
	va_list args;
	va_start( args, param);

	void *ptr;
	char *szParam;

	p[0]=atof( (char *)param );
	for (int i=1;i<7;i++) {
		ptr = va_arg( args, void *);
		szParam = (char *)ptr;
		p[i]=atof(szParam);
	}
	this->Init(p[0],p[1],p[2], p[3],p[4],p[5], p[6]);
}
*/
void palSphere::Init(Float x, Float y, Float z, Float radius, Float mass) {
	palBody::SetPosition(x,y,z);
	
	palFactoryObject *pFO=PF->CreateObject("palSphereGeometry");
	palSphereGeometry *m_pGeom = dynamic_cast<palSphereGeometry *>(pFO); //create the geometry
	m_Geometries.push_back(m_pGeom);
	SetGeometryBody(m_pGeom);

//	SetGeometryBody(m_pSphereGeom);
	//m_pSphereGeom->Init(x,y,z,radius,mass);
	m_pGeom->Init(m_mLoc,radius,mass);

	m_Type = PAL_BODY_SPHERE;
}
/*
void palSphere::GenericInit(void *param, ...) {
	Float p[5];
	va_list args;
	va_start( args, param);

	void *ptr;
	char *szParam;

	p[0]=atof( (char *)param );
	for (int i=1;i<5;i++) {
		ptr = va_arg( args, void *);
		szParam = (char *)ptr;
		p[i]=atof(szParam);
	}
	this->Init(p[0],p[1],p[2], p[3],p[4]);
}
*/
void palCylinder::Init(Float x, Float y, Float z, Float radius, Float length, Float mass) {
	palBody::SetPosition(x,y,z);

	palFactoryObject *pFO=PF->CreateObject("palCylinderGeometry");
	palCylinderGeometry *m_pGeom = dynamic_cast<palCylinderGeometry *>(pFO); //create the geometry
	m_Geometries.push_back(m_pGeom);
	SetGeometryBody(m_pGeom);

	//m_pCylinderGeom = dynamic_cast<palCylinderGeometry *>(pFO); //create the geometry
//	SetGeometryBody(m_pCylinderGeom);
	//m_pCylinderGeom->Init(x,y,z,radius,length,mass);
	m_pGeom->Init(m_mLoc,radius,length,mass);
	
	m_Type = PAL_BODY_CYLINDER;
}
/*
void palCylinder::GenericInit(void *param, ...) {
	Float p[6];
	va_list args;
	va_start( args, param);

	void *ptr;
	char *szParam;

	p[0]=atof( (char *)param );
	for (int i=1;i<6;i++) {
		ptr = va_arg( args, void *);
		szParam = (char *)ptr;
		p[i]=atof(szParam);
	}
	this->Init(p[0],p[1],p[2], p[3],p[4],p[5]);
}
*/

void palCompoundBody::Init(Float x, Float y, Float z) {
	palBody::SetPosition(x,y,z);
	m_Type = PAL_BODY_COMPOUND;
}

palSphereGeometry *palCompoundBody::AddSphere() {
	palFactoryObject *pFO=PF->CreateObject("palSphereGeometry");
	palSphereGeometry *pGeom = dynamic_cast<palSphereGeometry *>(pFO); //create the geometry
	SetGeometryBody(pGeom);
	m_Geometries.push_back(pGeom);
	return pGeom;
}

palBoxGeometry *palCompoundBody::AddBox() {
	palFactoryObject *pFO=PF->CreateObject("palBoxGeometry");
	palBoxGeometry *pGeom = dynamic_cast<palBoxGeometry *>(pFO); //create the geometry
	SetGeometryBody(pGeom);
	m_Geometries.push_back(pGeom);
	return pGeom;
}

palCylinderGeometry *palCompoundBody::AddCylinder() {
	palFactoryObject *pFO=PF->CreateObject("palCylinderGeometry");
	palCylinderGeometry *pGeom = dynamic_cast<palCylinderGeometry *>(pFO); //create the geometry
	SetGeometryBody(pGeom);
	m_Geometries.push_back(pGeom);
	return pGeom;
}

palGeometry *palCompoundBody::AddGeometry(STRING type) {
	palFactoryObject *pFO=PF->CreateObject(type);
	palGeometry *pGeom = dynamic_cast<palGeometry *>(pFO); //create the geometry
	SetGeometryBody(pGeom);
	m_Geometries.push_back(pGeom);
	return pGeom;
}


void palCompoundBody::SumInertia() {
	m_fInertiaXX=0;
	m_fInertiaYY=0;
	m_fInertiaZZ=0;
	m_fMass=0;
	for (unsigned int i=0;i<m_Geometries.size();i++) {
		palVector3 gpos;
		palVector3 pos;
		m_Geometries[i]->GetPosition(gpos);
		pos.x=m_fPosX; pos.y=m_fPosY; pos.z=m_fPosZ;
		palVector3 d;
		vec_sub(&d,&gpos,&pos);
		Float distance = vec_mag(&d);
		m_fInertiaXX+=m_Geometries[i]->m_fInertiaXX + m_Geometries[i]->GetMass() * distance * distance;
		m_fInertiaYY+=m_Geometries[i]->m_fInertiaYY + m_Geometries[i]->GetMass() * distance * distance;
		m_fInertiaZZ+=m_Geometries[i]->m_fInertiaZZ + m_Geometries[i]->GetMass() * distance * distance;
		m_fMass+=m_Geometries[i]->GetMass();
	}
}
/*
void palTriangleMesh::Init(Float x, Float y, Float z, const Float *pVertices, int nVertices, const int *pIndices, int nIndices) {
	palBody::SetPosition(x,y,z);
	m_nVertices=nVertices;
	m_nIndices=nIndices;
	m_pVertices=(float *) pVertices;
	m_pIndices=(int *) pIndices;
}*/

////////////////////////////////////////
////////////////////////////////////////
void palPhysics::Init(Float gravity_x, Float gravity_y, Float gravity_z) {
	m_fGravityX=gravity_x;
	m_fGravityY=gravity_y;
	m_fGravityZ=gravity_z;
}

palPhysics::palPhysics() {
	m_fTime=0;
	m_fLastTimestep = 0;
	m_pMaterials = NULL;
}

void palPhysics::Update(Float timestep) {
	Iterate(timestep);
	m_fTime+=timestep;
	m_fLastTimestep=timestep;
}

palTerrainType palTerrain::GetType() {
	return m_Type;
}

Float palPhysics::GetTime() {
	return m_fTime;
}

Float palPhysics::GetLastTimestep() {
	return m_fLastTimestep;
}

////////////////////////////////////////
void palTerrain::SetPosition(Float x, Float y, Float z) {
	m_fPosX = x;
	m_fPosY = y;
	m_fPosZ = z;
	mat_identity(&m_mLoc);
	m_mLoc._41=x;
	m_mLoc._42=y;
	m_mLoc._43=z;
}

void palTerrain::SetMaterial(palMaterial *material) {
	m_pMaterial = material;
}

palTerrainPlane::palTerrainPlane() {
	m_Type=PAL_TERRAIN_PLANE;
}

void palTerrainPlane::Init(Float x, Float y, Float z, Float min_size) {
	palTerrain::SetPosition(x,y,z);
	m_fSize = min_size;
	m_Type=PAL_TERRAIN_PLANE;
}

Float palTerrainPlane::GetMinimumSize() {
	return m_fSize;
}

palOrientatedTerrainPlane::palOrientatedTerrainPlane() {
	m_Type=PAL_TERRAIN_PLANE; //todo
}


void palOrientatedTerrainPlane::Init(Float x, Float y, Float z, Float nx, Float ny, Float nz,  Float min_size) {
	palTerrain::SetPosition(x,y,z);
	m_fNormX = nx;
	m_fNormY = ny;
	m_fNormZ = nz;
	m_fSize = min_size;
	CalcualteOrientationMatrixFromNormals();
}

Float palOrientatedTerrainPlane::GetMinimumSize() {
	return m_fSize;
}

Float palOrientatedTerrainPlane::CalculateD() {
	palVector3 pos;
	palVector3 norm;
	vec_set(&norm,m_fNormX,m_fNormY,m_fNormZ);
	vec_set(&pos,m_fPosX,m_fPosY,m_fPosZ);
	return -vec_dot(&norm,&pos);

}

void palOrientatedTerrainPlane::CalcualteOrientationMatrixFromNormals() {
	palVector3 pivot;
	palVector3 norm1;
	palVector3 norm2;
	vec_set(&norm1,0,1,0);
	vec_set(&norm2,m_fNormX,m_fNormY,m_fNormZ);
	vec_cross(&pivot,&norm1,&norm2);
	//normalized cross product
	vec_norm(&pivot);
	//theta = arccos(a.b / |a||b|)
	Float dot = vec_dot(&norm1,&norm2);
	Float denom = vec_mag(&norm1) * vec_mag(&norm2);
	Float amount = acos(dot/denom);

	//from  haegarr :
	palVector3 b0,b1,b2; //basis vectors
	const Float sine = sin(amount);
	const Float cosine = cos(amount);
	const Float oneMinusCosine = 1.0f-cosine;
	Float temp;
	temp = oneMinusCosine*pivot.x;
	b0.x = pivot.x*temp+cosine;
	b0.y = pivot.y*temp+sine*pivot.z;
	b0.z = pivot.z*temp-sine*pivot.y;
	temp = oneMinusCosine*pivot.y;
	b1.x = pivot.x*temp-sine*pivot.z;
	b1.y = pivot.y*temp+cosine;
	b1.z = pivot.z*temp+sine*pivot.x;
	temp = oneMinusCosine*pivot.z;
	b2.x = pivot.x*temp+sine*pivot.y;
	b2.y = pivot.y*temp-sine*pivot.x;
	b2.z = pivot.z*temp+cosine;

	mat_identity(&m_mLoc);

	m_mLoc._11 = b0.x;
	m_mLoc._12 = b0.y;
	m_mLoc._13 = b0.z;

	m_mLoc._21 = b1.x;
	m_mLoc._22 = b1.y;
	m_mLoc._23 = b1.z;

	m_mLoc._31 = b2.x;
	m_mLoc._32 = b2.y;
	m_mLoc._33 = b2.z;

	mat_set_translation(&m_mLoc,m_fPosX,m_fPosY,m_fPosZ);
}

palMatrix4x4& palOrientatedTerrainPlane::GetLocationMatrix() {
	return m_mLoc;
}

void palTerrainHeightmap::Init(Float x, Float y, Float z, Float width, Float depth, int terrain_data_width, int terrain_data_depth, const Float *pHeightmap) {
	palTerrain::SetPosition(x,y,z);
	m_fWidth = width;
	m_fDepth = depth;
	m_iDataWidth = terrain_data_width;
	m_iDataDepth = terrain_data_depth;
	m_pHeightmap = new Float[m_iDataWidth * m_iDataDepth];
	memcpy(m_pHeightmap,pHeightmap,sizeof(Float) * m_iDataWidth * m_iDataDepth);
	m_Type = PAL_TERRAIN_HEIGHTMAP;
}

palTerrainHeightmap::palTerrainHeightmap() {
	m_pHeightmap  = NULL;
	m_Type = PAL_TERRAIN_HEIGHTMAP;
}

palTerrainHeightmap::~palTerrainHeightmap() {
	if (m_pHeightmap )
		delete m_pHeightmap  ;
}

const Float *palTerrainHeightmap::GetHeightMap() {
	return m_pHeightmap;
}
Float palTerrainHeightmap::GetWidth() {
	return m_fWidth;
}
Float palTerrainHeightmap::GetDepth() {
	return m_fDepth;
}
int palTerrainHeightmap::GetDataWidth() {
	return m_iDataWidth;
}
int palTerrainHeightmap::GetDataDepth() {
	return m_iDataDepth;
}

palTerrainMesh::palTerrainMesh(){
	m_Type=PAL_TERRAIN_MESH;
}
void palTerrainMesh::Init(Float x, Float y, Float z, const Float *pVertices, int nVertices, const int *pIndices, int nIndices) {
	palTerrain::SetPosition(x,y,z);
	m_nVertices=nVertices;
	m_nIndices=nIndices;
	m_pVertices=(Float *) pVertices;
	m_pIndices=(int *) pIndices;
}


palTerrain::palTerrain() {
	m_pMaterial=NULL;
	m_Type = PAL_TERRAIN_NONE;
}
///////////////////////////////////////
