#include "palXmlFactory.h"
#include <stdarg.h>


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

	Version
		0.1 : 14/09/04 - Added impulse actuator
*/

MAP<STRING,palFactoryObject *> g_NameMap;

//extracts float values from a string
int ExtractValues(Float *dest, const char *sz) {
	int count;
	char seps[]   = " ,\t\n";	
	char *token;
	token = strtok( const_cast<char *>(sz), seps );
	count = 0;
	while( token != NULL )
	{
		dest[count] = atof(token);
		printf("read in value %f\n",atof(token));
		count++;
		token = strtok( NULL, seps );
	}
	return count;
}

void palXMLFactory::palPutObject(STRING name, palFactoryObject *ptr) 
{
	printf("inserting %s into %d\n",name.c_str(),g_NameMap);
		g_NameMap.insert(std::make_pair(name,ptr));
}


int palXMLFactory::Read(TiXmlElement* parent_element, char *name, int num, char *label_dest, ...) {
	int i;
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;

	node = parent_element->FirstChildElement(name);	
	if (!node) return -1;
	posdimElement = node->ToElement();
	assert(posdimElement);

	va_list args;
	va_start( args, label_dest);
	char * p[32];
	p[0]=label_dest;
	for (i=1;i<num;i++) {
		p[i]=va_arg( args, char *);
	}
	for (i=0;i<num;i+=2) {

		const char *szp=posdimElement->Attribute(p[i]);
		printf("read:%s\n",szp);
		if (szp == NULL) return -1;
		memcpy(p[i+1],szp,strlen(szp)+1);
	}

	return 0;
}

int GetMaterial_Terrain(palTerrain *pbody, TiXmlElement* element) {
	const char *szMatName;
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;
	node = element->FirstChildElement("Material");
	if (node) {
		posdimElement = node->ToElement();
		assert(posdimElement);
		szMatName = posdimElement->Attribute("name");
		palMaterials *pm = PF->CreateMaterials(); //should recieve last set
		pbody->SetMaterial(pm->GetMaterial(szMatName));
	}
	return 0;
}


int palXMLFactory::LoadTerrain(TiXmlElement* terrainElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;

	const char *szName;
	char szX[256];
	char szY[256];
	char szZ[256];
	
	//get a plane?
	node = terrainElement->FirstChildElement("Plane");	
	if (node) {
		
		const char *szSize;
		TiXmlElement* planeElement = node->ToElement();
		szName = planeElement->Attribute("name");

		//get position
		Read(planeElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		
		//get size
		node = planeElement->FirstChildElement("Dimensions");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szSize=posdimElement->Attribute("min_size");

		//pal stuff:
		palTerrainPlane *pt = PF->CreateTerrainPlane();
		pt->Init(atof(szX),atof(szY),atof(szZ),atof(szSize));

		//get material
		GetMaterial_Terrain(pt,planeElement);
		palPutObject(szName,pt);
		m_Terrain.push_back(pt);
	}

	//get a heightmap?
	node = terrainElement->FirstChildElement("Heightmap");	
	if (node) {
		TiXmlElement* planeElement = node->ToElement();
		szName = planeElement->Attribute("name");

		//get position
		Read(planeElement,"Position",6,"x",szX,"y",szY,"z",szZ);

		//get dimensions
		node = planeElement->FirstChildElement("Dimensions");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char *szWidth;
		const char *szDepth;
		const char *szDWidth;
		const char *szDDepth;
		szWidth=posdimElement->Attribute("width");
		szDepth=posdimElement->Attribute("depth");
		szDWidth=posdimElement->Attribute("datawidth");
		szDDepth=posdimElement->Attribute("datadepth");
	
		int dd,dw;
		dw=atoi(szDWidth);
		dd=atoi(szDDepth);

		Float *data_ptr = new Float[dd*dw];
		node = planeElement->FirstChildElement("Heights");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		node = posdimElement->FirstChild();
		char *sz =const_cast<char *>( node->Value()); //to be honest, i dont know if this is safe.
		printf("string: %s\n",sz);
		Float val;
		char seps[]   = " ,\t\n";
		char *token;
		token = strtok( sz, seps );
		for (int i=0;i<dd*dw;i++) {
			if (token == NULL) {
				printf("error loading terrain\n");
				return -1;
			}
			sscanf(token,"%f",&val);
			data_ptr[i] = val;
			//printf("i(%d):%f\n",i,val);
			token = strtok( NULL, seps );
		}

		//pal stuff:
		palTerrainHeightmap *pt = PF->CreateTerrainHeightmap();
		pt->Init(atof(szX),atof(szY),atof(szZ),atof(szWidth),atof(szDepth),dw,dd,data_ptr);

		//get material
		GetMaterial_Terrain(pt,planeElement);
		palPutObject(szName,pt);
		m_Terrain.push_back(pt);

		delete [] data_ptr;
	}
	return 0;
}

int GetMaterial(palBody *pbody, TiXmlElement* element) {
	const char *szMatName;
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;
	node = element->FirstChildElement("Material");
	if (node) {
		posdimElement = node->ToElement();
		assert(posdimElement);
		szMatName = posdimElement->Attribute("name");
		palMaterials *pm = PF->CreateMaterials(); //should recieve last set
		pbody->SetMaterial(pm->GetMaterial(szMatName));
	}
	return 0;
}

int palXMLFactory::LoadPrimitives(TiXmlElement* primitiveElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;
	
	const char *szType;
	const char *szName;
	const char *szX;
	const char *szY;
	const char *szZ;
	const char *szMass;
	palMatrix4x4 pos;
	Float temp[256];

	node = primitiveElement->FirstChildElement("Body");
	TiXmlElement* bodyElement = node->ToElement();
	do {
		bodyElement = node->ToElement();
		if (node) {
			szType = bodyElement->Attribute("type");
			szName = bodyElement->Attribute("name");

			//get position
			node = bodyElement->FirstChildElement("Position");	
			posdimElement = node->ToElement();
			assert(posdimElement);
			szX=posdimElement->Attribute("x");
			szY=posdimElement->Attribute("y");
			szZ=posdimElement->Attribute("z");
			mat_identity(&pos);
			mat_translate(&pos,atof(szX),atof(szY),atof(szZ));

			//get parameters
			node = bodyElement->FirstChildElement("Parameters");
			if (node) {
				posdimElement = node->ToElement();
				assert(posdimElement);
				node = posdimElement->FirstChild();
				printf("param ele:%s\n",node->Value());
				ExtractValues(temp, node->Value() );
			}

			//construct the object
			palFactoryObject *pFO = NULL;
			palBody *pb = NULL;
			pFO=PF->CreateObject(szType);
			if (!pFO) {
				printf("%s is not a factory object\n",szType);
				return -1;
			} 
			pb = dynamic_cast<palBody *>(pFO);
			if (!pb) {
				printf("%s is not a body\n",szType);
				return -1;
			}

			//is this a compound object?
			palCompoundBody *pCB = NULL;
			pCB = dynamic_cast<palCompoundBody *>(pb);
			if (pCB) {
				printf("compound body\n");
				//we can init it already
				pCB->Init(atof(szX),atof(szY),atof(szZ));
				//get geometries
				node = bodyElement->FirstChildElement("Geometry");	
				while (node) {
					TiXmlElement* geomElement = node->ToElement();
					szType = geomElement->Attribute("type");
					printf("geom type:%s\n",szType);

					//get position
					node = bodyElement->FirstChildElement("Position");	
					if (node) {
						posdimElement = node->ToElement();
						assert(posdimElement);
						szX=posdimElement->Attribute("x");
						szY=posdimElement->Attribute("y");
						szZ=posdimElement->Attribute("z");
						mat_identity(&pos);
						mat_translate(&pos,Float(atof(szX)),Float(atof(szY)),Float(atof(szZ)));
					}
					//get parameters
					node = geomElement->FirstChildElement("Parameters");	
					posdimElement = node->ToElement();
					node = posdimElement->FirstChild();
					//printf("param ele:%s\n",node->Value());
					palGeometry *pG=pCB->AddGeometry(szType);
					if (!pG) {
						printf("could not create geometry:%s\n",szType);
						return -1;
					}
					ExtractValues(temp, node->Value() );
					pG->GenericInit(pos,temp);
					node=geomElement->NextSiblingElement("Geometry");
				} //end while (node)
				pCB->Finalize();
			} else { //end if pCB
				//just a normal body
				printf("initing generic body %s\n",szName);
				pb->GenericInit(pos,temp);
				palPutObject(szName,pb);
				/*
				palSphere *ps;
				switch (pb->m_Type) {
				case PAL_BODY_CYLINDER:
					m_Cylinder.push_back(dynamic_cast<palCylinder *>(pb) );
					break;
				case PAL_BODY_BOX:
					m_Box.push_back( dynamic_cast<palBox *>(pb) );
					break;
				case PAL_BODY_SPHERE:
					ps = dynamic_cast<palSphere *>(pb);
					if (!ps) {
						printf("sphere types mismatch! with %s:%s\n",szType,szName);
						return -1;
					}
					m_Sphere.push_back( ps );
					break;
			 	default:
					printf("WARNING: unkown body type %s (%s)\n",szType,szName);
					break;
				}
				*/
			} //end if pCB

			GetMaterial(pb,bodyElement);

			m_Bodies.push_back(pb);
			printf("inserted body %d\n",pb);
			node=bodyElement->NextSiblingElement("Body");
		} //end if node
	} while (node);

	//boxes?
	node = primitiveElement->FirstChildElement("Box");
	TiXmlElement* boxElement = node->ToElement();
	do {
	boxElement = node->ToElement();
	if (node) {
		szName = boxElement->Attribute("name");
		const char *szWidth;
		const char *szHeight;
		const char *szDepth;
	
		//get position
		node = boxElement->FirstChildElement("Position");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szX=posdimElement->Attribute("x");
		szY=posdimElement->Attribute("y");
		szZ=posdimElement->Attribute("z");
		//Read(boxElement,"Position",6,"x",szX,"y",szY,"z",szZ);

		//get size
		node = boxElement->FirstChildElement("Dimensions");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szWidth =posdimElement->Attribute("width");
		szHeight=posdimElement->Attribute("height");
		szDepth =posdimElement->Attribute("depth");

/*		printf(" attributes:\n");
		TiXmlAttribute *atrib;
		atrib=posdimElement->FirstAttribute();
		printf("%s\n",atrib->Name());;
		do {
		atrib=atrib->Next();
		printf("%s\n",atrib->Name());;
		}
		while (atrib!=posdimElement->LastAttribute() ) ;
*/		
		//get mass
		node = boxElement->FirstChildElement("Mass");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szMass=posdimElement->Attribute("mass");

		//pal stuff:
		palBox *pb=PF->CreateBox();
		printf("creating a box (%s): %s %s %s : %s %s %s : %s\n",szName,szX,szY,szZ,szWidth,szHeight,szDepth,szMass);
		pb->Init(atof(szX),atof(szY),atof(szZ),atof(szWidth),atof(szHeight),atof(szDepth),atof(szMass) );
		if (!pb)	{
			printf("could not create box!\n");
			return -1;
		}
		GetMaterial(pb,boxElement);
		m_Bodies.push_back(pb);
		//m_NameMap.insert(std::make_pair(szName,pb));
		palPutObject(szName,pb);
		node=boxElement->NextSiblingElement("Box");
	}
	} while (node);

	//spheres?
	node = primitiveElement->FirstChildElement("Sphere");
	//assert(node);
	TiXmlElement* sphereElement = node->ToElement();
	do {
	sphereElement = node->ToElement();
	if (node) {
		szName = sphereElement->Attribute("name");
		const char *szRadius;
		//get position
		node = sphereElement->FirstChildElement("Position");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szX=posdimElement->Attribute("x");
		szY=posdimElement->Attribute("y");
		szZ=posdimElement->Attribute("z");

		//get size
		node = sphereElement->FirstChildElement("Dimensions");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szRadius=posdimElement->Attribute("radius");
	
		//get mass
		node = sphereElement->FirstChildElement("Mass");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szMass=posdimElement->Attribute("mass");

		printf("creating a sphere (%s): %s %s %s : %s : %s\n",szName,szX,szY,szZ,szRadius,szMass);
		palSphere *ps = PF->CreateSphere();
		if (!ps)	{
			printf("could not create shere!\n");
			return -1;
		}
		ps->Init(atof(szX),atof(szY),atof(szZ),atof(szRadius),atof(szMass) );
		GetMaterial(ps,sphereElement);
		m_Bodies.push_back(ps);
//		m_NameMap.insert(std::make_pair(szName,ps));
		palPutObject(szName,ps);
		node=sphereElement->NextSiblingElement("Sphere");
	}
	} while(node);

	//cylinders?
	node = primitiveElement->FirstChildElement("Cylinder");
	//assert(node);
	TiXmlElement* cylElement = node->ToElement();
	do {
	cylElement = node->ToElement();
	if (node) {
		szName = cylElement->Attribute("name");
		const char *szRadius;
		const char *szHeight;
		//get position
		node = cylElement->FirstChildElement("Position");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szX=posdimElement->Attribute("x");
		szY=posdimElement->Attribute("y");
		szZ=posdimElement->Attribute("z");

		//get size
		node = cylElement->FirstChildElement("Dimensions");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szRadius=posdimElement->Attribute("radius");
		szHeight=posdimElement->Attribute("height");
	
		//get mass
		node = cylElement->FirstChildElement("Mass");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		szMass=posdimElement->Attribute("mass");

		printf("creating a cylinder (%s): %s %s %s : %s %s : %s\n",szName,szX,szY,szZ,szRadius,szHeight,szMass);
		palCylinder *pc = PF->CreateCylinder();
		if (!pc)	{
			printf("could not create cylinder!\n");
			return -1;
		}
		pc->Init(atof(szX),atof(szY),atof(szZ),atof(szRadius),atof(szHeight),atof(szMass) );
		GetMaterial(pc,cylElement);
		m_Bodies.push_back(pc);
		palPutObject(szName,pc);
		//m_NameMap.insert(std::make_pair(szName,pc));
		node=cylElement->NextSiblingElement("Cylinder");
	}
	} while(node); //end cylinders

	return 0;
}
/*
int GetConnection(TiXmlElement* lElement, palBody **pb0, palBody **pb1) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;

		//get connection
		node = lElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody0 = posdimElement->Attribute("connects");
		const char* szBody1 = posdimElement->Attribute("with");
		*pb0 = palGetObject<palBody>(szBody0);
		*pb1 = palGetObject<palBody>(szBody1);
		if (!*pb0) {
			printf("item %s is not a body\n",szBody0);
			return -1;
		}
		if (!*pb1) {
			printf("item %s is not a body\n",szBody1);
			return -1;
		}
		printf("%s connects to %s\n",szBody0,szBody1);
		return 0;
}*/

int palXMLFactory::LoadLinks(TiXmlElement* linkElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;

	const char *szName;
	const char *szType;
	char szAxisX[256];
	char szAxisY[256];
	char szAxisZ[256];

	char szX[256];
	char szY[256];
	char szZ[256];

//	palBody *pb0=NULL;
//	palBody *pb1=NULL;

	node = linkElement->FirstChildElement("Link");
	while (node) {
		float param[6];

		TiXmlElement* lElement = node->ToElement();
		szType = lElement->Attribute("type");
		szName = lElement->Attribute("name");
		
		Read(lElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		printf("got pos\n");
		Read(lElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		printf("got axis\n");
		
//		GetConnection(lElement,&pb0,&pb1);
		node = lElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody0 = posdimElement->Attribute("connects");
		const char* szBody1 = posdimElement->Attribute("with");
		palBody *pb0 = palGetObject<palBody>(szBody0);
		palBody *pb1 = palGetObject<palBody>(szBody1);

		
		palLink *pl = dynamic_cast<palLink *>(PF->CreateObject(szType));
		if (!pl) {
			printf("Error creating link %s %s!\n",szType,szName);
		}
		
		param[0]=atof(szX);
		param[1]=atof(szY);
		param[2]=atof(szZ);
		param[3]=atof(szAxisX);
		param[4]=atof(szAxisY);
		param[5]=atof(szAxisZ);
		
		pl->GenericInit(pb0,pb1,param);
		palPutObject(szName,pl);
		m_Links.push_back(pl);
		
		node=lElement->NextSiblingElement("Link");
	}
			
	//spherical links?
	node = linkElement->FirstChildElement("SphericalLink");
	while (node) {
		TiXmlElement* slElement = node->ToElement();
	
		szName = slElement->Attribute("name");
		//get position
		Read(slElement,"Position",6,"x",szX,"y",szY,"z",szZ);

		//get connection
		node = slElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody0 = posdimElement->Attribute("connects");
		const char* szBody1 = posdimElement->Attribute("with");
		palBody *pb0 = palGetObject<palBody>(szBody0);
		palBody *pb1 = palGetObject<palBody>(szBody1);

		palSphericalLink *psl=PF->CreateSphericalLink();
		if (!psl) {
			printf("could not create link %s\n",szName);
			return -1;
		}
		psl->Init(pb0,pb1,atof(szX),atof(szY),atof(szZ));
		palPutObject(szName,psl);
		m_Links.push_back(psl);

		node=slElement->NextSiblingElement("SphericalLink");
	} 

	//revolute links?
	node = linkElement->FirstChildElement("RevoluteLink");
	while (node) {
		TiXmlElement* rlElement = node->ToElement();
	
		szName = rlElement->Attribute("name");
		//get position
		Read(rlElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		//get axis
		Read(rlElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		//get connection
		node = rlElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody0 = posdimElement->Attribute("connects");
		const char* szBody1 = posdimElement->Attribute("with");
		palBody *pb0 = palGetObject<palBody>(szBody0);
		palBody *pb1 = palGetObject<palBody>(szBody1);

		
		palRevoluteLink *prl=PF->CreateRevoluteLink();
		if (!prl) {
			printf("could not create link %s\n",szName);
			return -1;
		}

		prl->Init(pb0,pb1,atof(szX),atof(szY),atof(szZ),atof(szAxisX),atof(szAxisY),atof(szAxisZ));
		palPutObject(szName,prl);
		m_Links.push_back(prl);

		node=rlElement->NextSiblingElement("RevoluteLink");	
	} 

	return 0;
}


int palXMLFactory::LoadXML(char *filename) {
	TiXmlDocument doc( filename );
	bool loadOkay = doc.LoadFile();
	
	if ( !loadOkay )
	{
		printf( "Could not load test file. Error='%s'. Exiting.\n", doc.ErrorDesc() );
		exit( 1 );
	}

	printf( "** Script read from disk: ** \n\n" );
	doc.Print( stdout );

	TiXmlNode* node = 0;
	TiXmlElement* physicsElement = 0;

	node = doc.FirstChild( "Physics" );
	if (!node) {
		printf("Could not find the physics node\n");
		exit(1);
	}

	const char *szName;
	
//	const char *temp;
	//get the physics element
	physicsElement = node->ToElement();
	assert(physicsElement);

	TiXmlElement* pElement = 0;
	TiXmlElement* materialsElement = 0;
	TiXmlElement* terrainElement = 0;
	TiXmlElement* primitiveElement = 0;
	TiXmlElement* linkElement = 0;
	TiXmlElement* sensorElement = 0;
	TiXmlElement* actuatorElement = 0;
	TiXmlElement* posdimElement = 0;
	
	//get the engine
	node = physicsElement->FirstChildElement("Engine");	
	if (node) {
		posdimElement = node->ToElement();
		assert(posdimElement);
		PF->SelectEngine(posdimElement->Attribute("engine"));
/*		printf("loading engine:%s\n",posdimElement->Attribute("engine"));
		printf("!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
		PF->DisplayContents();
		printf("sinfo size:%d [%d]\n",PF->sInfo().size(),PF->sInfo().begin());
		for (int x=0;x<PF->sInfo().size();x++) {
			printf("%s\n",PF->sInfo()[x].mUniqueName.c_str());
		}
		printf("!!!!!!!!!!!!!!!!!!!!!!!!!!\n");*/
	}
	//get the gravity
	const char *szX;
	const char *szY;
	const char *szZ;

	palPhysics *pp = NULL;
	node = physicsElement->FirstChildElement("Gravity");	
	if (node) {
	posdimElement = node->ToElement();
	assert(posdimElement);
	szX=posdimElement->Attribute("x");
	szY=posdimElement->Attribute("y");
	szZ=posdimElement->Attribute("z");
	//printf("x,y,z:%s %s %s\n",szX,szY,szZ);
	pp=PF->CreatePhysics();
	if (!pp) {
		printf("phyiscs implementation not found\n");
		return -1;
	}
	pp->Init(atof(szX),atof(szY),atof(szZ));
//	m_NameMap.insert(std::make_pair("Physics",pp));
	palPutObject("Physics",pp);
	}
	

	//get any specifics for newton
	node = physicsElement->FirstChildElement("Newton");	
	if ( node ) {
		palNewtonPhysics *pnp = dynamic_cast<palNewtonPhysics *>(pp);
		if (pnp) {
			pElement = node->ToElement();
			assert(pElement);
			node = pElement->FirstChildElement("Water");
			if (node) {
				posdimElement = node->ToElement();
				assert(posdimElement);
				szX=posdimElement->Attribute("density");
				szY=posdimElement->Attribute("linear_viscosity");
				szZ=posdimElement->Attribute("angular_viscosity");
				pnp->InitWater(atof(szX),atof(szY),atof(szZ));
			}
		}
	}

	node = physicsElement->FirstChildElement("Materials");	
	if (node) {
		materialsElement = node->ToElement();
		assert(materialsElement);
		LoadMaterials(materialsElement);
	}

	//get the terrain
	node = physicsElement->FirstChildElement("Terrain");	
	if (node) {
		terrainElement = node->ToElement();
		assert( terrainElement );
		LoadTerrain(terrainElement);	
	}
	
	//get primitives?
	node = physicsElement->FirstChildElement("Primitives");	
	if (node) {
	assert( node );
	primitiveElement = node->ToElement();
	assert( primitiveElement  );
	
	LoadPrimitives(primitiveElement);
	}
	/*
	palCylinder *pc = PF->CreateCylinder();
	pc->Init(0,8,0,0.5f,1.0f,1.0f);
	g_Cylinder.push_back(pc);
	*/
	////end primitives

	node = physicsElement->FirstChildElement("Links");	
	if (node) {
	linkElement= node->ToElement();
	assert( linkElement  );
		LoadLinks(linkElement);
	}

	node = physicsElement->FirstChildElement("Sensors");	
	if (node) {
		printf("processing Sensors\n");
		sensorElement= node->ToElement();
		assert( sensorElement  );
		LoadSensors(sensorElement);
	} //end sensors

	node = physicsElement->FirstChildElement("Actuators");	
	if (node) {
		printf("processing Actuators\n");
		actuatorElement= node->ToElement();
		assert( actuatorElement  );
		LoadActuators(actuatorElement);
	} //end sensors

	return 0;
}

int palXMLFactory::LoadMaterials(TiXmlElement* materialsElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;
	const char *szName;

	char szStatic[256];
	char szKinetic[256];
	char szRestitution[256];

	palMaterials *pm = PF->CreateMaterials();
	{
		node = materialsElement->FirstChildElement("Material");
		while (node) {
			TiXmlElement* mElement = node->ToElement();
			szName = mElement->Attribute("name");
			Read(mElement,"Properties",6,"static_friction",szStatic,"kinetic_friction",szKinetic,"restitution",szRestitution);
			pm->NewMaterial(szName,atof(szStatic),atof(szKinetic),atof(szRestitution));
			node=mElement->NextSiblingElement("Material");
		}
	}
	{
		node = materialsElement->FirstChildElement("MaterialInteraction");
		while (node) {
			TiXmlElement* mElement = node->ToElement();
			//szName = mElement->Attribute("name");
			Read(mElement,"Properties",6,"static_friction",szStatic,"kinetic_friction",szKinetic,"restitution",szRestitution);

			//get connection
			node = mElement->FirstChildElement("Connection");	
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char* szMat0 = posdimElement->Attribute("connects");
			const char* szMat1 = posdimElement->Attribute("with");
			pm->SetMaterialInteraction(szMat0,szMat1,atof(szStatic),atof(szKinetic),atof(szRestitution));
			node=mElement->NextSiblingElement("MaterialInteraction");
		}
	}
	return 0;
}

int palXMLFactory::LoadActuators(TiXmlElement* actuatorElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;
	const char *szName;
//virtual void Init(palBody *pbody, Float px, Float py, Float pz, Float axis_x, Float axis_y, Float axis_z);

	char szAxisX[256];
	char szAxisY[256];
	char szAxisZ[256];

	char szX[256];
	char szY[256];
	char szZ[256];

	{
	node = actuatorElement->FirstChildElement("ForceActuator");
	while (node) {
		TiXmlElement* faElement = node->ToElement();
		szName = faElement->Attribute("name");

		Read(faElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		Read(faElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);

		//get connection
		node = faElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {printf("item %s is not a body\n",szBody);	return -1;}
				
		palFactoryObject *pfo = PF->CreateObject("palForceActuator");
		palForceActuator *pfa = dynamic_cast<palForceActuator *>(pfo);
		if (!pfa) {printf("force actuator unsuported\n"); return -1;}
		pfa->Init(pb,atof(szX),atof(szY),atof(szZ),atof(szAxisX),atof(szAxisY),atof(szAxisZ));

		//get initial force
		//<InitialForce force="1.0"></InitialForce>
		node = faElement->FirstChildElement("InitialForce");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *force = posdimElement->Attribute("force");
			pfa->SetForce(atof(force));
		}


		palPutObject(szName,pfa);
		m_Actuators.push_back(pfa);
		node=faElement->NextSiblingElement("ForceActuator");
	}
	}
	{
	node = actuatorElement->FirstChildElement("ImpulseActuator");
	while (node) {
		TiXmlElement* faElement = node->ToElement();
		szName = faElement->Attribute("name");

		Read(faElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		Read(faElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);

		//get connection
		node = faElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {printf("item %s is not a body\n",szBody);	return -1;}
				
		palFactoryObject *pfo = PF->CreateObject("palImpulseActuator");
		palImpulseActuator *pfa = dynamic_cast<palImpulseActuator *>(pfo);
		if (!pfa) {printf("impulse actuator unsuported\n"); return -1;}
		pfa->Init(pb,atof(szX),atof(szY),atof(szZ),atof(szAxisX),atof(szAxisY),atof(szAxisZ));

		//get initial force
		//<InitialForce force="1.0"></InitialForce>
		node = faElement->FirstChildElement("InitialImpulse");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *force = posdimElement->Attribute("impulse");
			pfa->SetImpulse(atof(force));
		}

		palPutObject(szName,pfa);
		m_Actuators.push_back(pfa);
		node=faElement->NextSiblingElement("ImpulseActuator");
	}
	}


	{
	node = actuatorElement->FirstChildElement("Propeller");
	while (node) {
		TiXmlElement* faElement = node->ToElement();
		szName = faElement->Attribute("name");
		Read(faElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		Read(faElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);

		//get connection
		node = faElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {printf("item %s is not a body\n",szBody);	return -1;}


		//alpha lumped parameter
		node = faElement->FirstChildElement("Alpha");	
		Float al;
		al=0;
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *force = posdimElement->Attribute("lumped");
			al = atof(force);
		}

		palPropeller *pprop = dynamic_cast<palPropeller *> (PF->CreateObject("palPropeller"));
		if (!pprop) {printf("propeller actuator unsuported\n"); return -1;}
		pprop->Init(pb,atof(szX),atof(szY),atof(szZ),atof(szAxisX),atof(szAxisY),atof(szAxisZ),al);
		palPutObject(szName,pprop);
		m_Actuators.push_back(pprop);
		node=faElement->NextSiblingElement("Propeller");
	}
	}

	/*
	<LiquidDrag name="waterdrag0">
				<Area front="0.25"></Area>
				<DragCoefficient dc="0.4"></DragCoefficient>
				<Density>0.99829</Density>
	</LiquidDrag
	*/

	{
	node = actuatorElement->FirstChildElement("LiquidDrag");
	while (node) {
		TiXmlElement* faElement = node->ToElement();
		szName = faElement->Attribute("name");
		//get connection
		node = faElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {printf("item %s is not a body\n",szBody);	return -1;}

		//parameters
		Float area,dc,density;
		area=0;
		dc=0;
		density=0.99829;
		node = faElement->FirstChildElement("Area");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("front");
			area = atof(tsz);
		}
		node = faElement->FirstChildElement("DragCoeffecient");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("CD");
			dc = atof(tsz);
		}
		node = faElement->FirstChildElement("Density");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("density");
			density = atof(tsz);
		}
		printf("creating liquid drag!\n");
		palLiquidDrag *pld = dynamic_cast<palLiquidDrag *> (PF->CreateObject("palLiquidDrag"));
		if (!pld) {printf("liquid drag actuator unsuported\n"); return -1;}
		pld->Init(pb,area,dc,density);
		palPutObject(szName,pld);
		m_Actuators.push_back(pld);
	
		node=faElement->NextSiblingElement("LiquidDrag");
	}
	}

	{
	node = actuatorElement->FirstChildElement("Hydrofoil");
	while (node) {
		TiXmlElement* faElement = node->ToElement();
		szName = faElement->Attribute("name");
		Read(faElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		Read(faElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		char szlAxisX[256];
		char szlAxisY[256];
		char szlAxisZ[256];
		Read(faElement,"LiftAxis",6,"x",szlAxisX,"y",szlAxisY,"z",szlAxisZ);

		//get connection
		node = faElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {printf("item %s is not a body\n",szBody);	return -1;}

		//parameters
		Float area,la,lb,lc,density;
		area=0;
		la=0;lb=0;lc=0;
		density=0.99829;
		node = faElement->FirstChildElement("Area");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("front");
			area = atof(tsz);
		}
		node = faElement->FirstChildElement("LiftCoeffecient");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("a");
			la = atof(tsz);
			tsz= posdimElement->Attribute("b");
			lb = atof(tsz);
			tsz= posdimElement->Attribute("c");
			lc = atof(tsz);
		}
		node = faElement->FirstChildElement("Density");	
		if (node) {
			posdimElement = node->ToElement();
			assert(posdimElement);
			const char *tsz= posdimElement->Attribute("density");
			density = atof(tsz);
		}


		palHydrofoil *pld = dynamic_cast<palHydrofoil *> (PF->CreateObject("palHydrofoil"));
		if (!pld) {printf("hydrofoil actuator unsuported\n"); return -1;}
		pld->Init(pb,
				atof(szX),atof(szY),atof(szZ),
				atof(szAxisX),atof(szAxisY),atof(szAxisZ),
				atof(szlAxisX),atof(szlAxisY),atof(szlAxisZ),
				area,la,lb,lc,density);
		palPutObject(szName,pld);
		m_Actuators.push_back(pld);
	
		node=faElement->NextSiblingElement("Hydrofoil");


	}
	}

	


}

int palXMLFactory::LoadSensors(TiXmlElement* sensorElement) {
	TiXmlNode* node = 0;
	TiXmlElement* posdimElement = 0;

	const char *szName;

	char szAxisX[256];
	char szAxisY[256];
	char szAxisZ[256];
{
	node = sensorElement->FirstChildElement("Inclinometer");
	TiXmlElement* incElement = node->ToElement();
	do {
		incElement = node->ToElement();
		if (node) {
		szName = incElement->Attribute("name");

		Read(incElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		//get connection
		node = incElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		printf("creating a inclinometer (%s) : %s %s %s : %s\n",szName,szAxisX,szAxisY,szAxisZ,szBody);
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {
			printf("item %s is not a body\n",szBody);
			return -1;
		}
		
		palInclinometerSensor *pi=PF->CreateInclinometerSensor();
		if (!pi) {
			printf("could not create inclinometer %s\n",szName);
			return -1;
		}
		pi->Init(pb,atof(szAxisX),atof(szAxisY),atof(szAxisZ));
		palPutObject(szName,pi);
		node=incElement->NextSiblingElement("Inclinometer");
		}
	} while(node);
}
{
	node = sensorElement->FirstChildElement("Gyroscope");
	TiXmlElement* gyroElement = node->ToElement();
	do {
		gyroElement = node->ToElement();
		if (node) {
		szName = gyroElement->Attribute("name");

		Read(gyroElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		//get connection
		node = gyroElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		printf("creating a Gyroscope (%s) : %s %s %s : %s\n",szName,szAxisX,szAxisY,szAxisZ,szBody);
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {
			printf("item %s is not a body\n",szBody);
			return -1;
		}
		
	
		palFactoryObject *pfo=PF->CreateObject("palGyroscopeSensor");
		palGyroscopeSensor *pg=dynamic_cast<palGyroscopeSensor *>(pfo);

		if (!pg) {
			printf("could not create gyroscope %s\n",szName);
			return -1;
		}
		pg->Init(pb,atof(szAxisX),atof(szAxisY),atof(szAxisZ));
		palPutObject(szName,pg);
		node=gyroElement->NextSiblingElement("Gyroscope");
		}
	} while(node);
}
{
	node = sensorElement->FirstChildElement("Contact");
	TiXmlElement* cElement = node->ToElement();
	do {
		cElement = node->ToElement();
		if (node) {
		szName = cElement->Attribute("name");

		//get connection
		node = cElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		printf("creating a psd (%s) : %s\n",szName,szBody);
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {
			printf("item %s is not a body\n",szBody);
			return -1;
		}
			
		palContactSensor *pc = PF->CreateContactSensor();
		if (!pc) {
			printf("could not create Contact %s\n",szName);
			return -1;
		}

		pc->Init(pb);
		palPutObject(szName,pc);
		node=cElement->NextSiblingElement("Contact");
		}
	} while(node);
}
{
	node = sensorElement->FirstChildElement("PSD");
	TiXmlElement* psdElement = node->ToElement();
	do {
		psdElement = node->ToElement();
		if (node) {
		szName = psdElement->Attribute("name");

		char szX[256];
		char szY[256];
		char szZ[256];
		char szR[256];

		Read(psdElement,"Position",6,"x",szX,"y",szY,"z",szZ);
		Read(psdElement,"Axis",6,"x",szAxisX,"y",szAxisY,"z",szAxisZ);
		Read(psdElement,"Range",2,"range",szR);
		//get connection
		node = psdElement->FirstChildElement("Connection");	
		posdimElement = node->ToElement();
		assert(posdimElement);
		const char* szBody = posdimElement->Attribute("connects");
		printf("creating a psd (%s) : %s %s %s : %s\n",szName,szAxisX,szAxisY,szAxisZ,szBody);
		palBody *pb = palGetObject<palBody>(szBody);
		if (!pb) {
			printf("item %s is not a body\n",szBody);
			return -1;
		}
		
	
		//palFactoryObject *pfo=PF->CreateObject("palGyroscopeSensor");
		//palGyroscopeSensor *pg=dynamic_cast<palGyroscopeSensor *>(pfo);
		palPSDSensor *ps = PF->CreatePSDSensor();
		if (!ps) {
			printf("could not create psd %s\n",szName);
			return -1;
		}

		ps->Init(pb,atof(szX),atof(szY),atof(szZ),atof(szAxisX),atof(szAxisY),atof(szAxisZ),atof(szR));
		palPutObject(szName,ps);
		node=psdElement->NextSiblingElement("PSD");
		}
	} while(node);
}

	return 0;
}
