#include "UrdfParser.h"

#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"

UrdfParser::UrdfParser()
:m_parseSDF(false),
m_activeSdfModel(-1)
{
}


UrdfParser::~UrdfParser()
{
    cleanModel(&m_urdf2Model);
    
    for (int i=0;i<m_tmpModels.size();i++)
    {
        cleanModel(m_tmpModels[i]);
    }
    m_sdfModels.clear();
    m_tmpModels.clear();
}

void UrdfParser::cleanModel(UrdfModel* model)
{
    for (int i=0;i<model->m_materials.size();i++)
    {
        UrdfMaterial** matPtr = model->m_materials.getAtIndex(i);
        if (matPtr)
        {
            UrdfMaterial* mat = *matPtr;
            delete mat;
        }
    }
    
    for (int i=0;i<model->m_links.size();i++)
    {
        UrdfLink** linkPtr = model->m_links.getAtIndex(i);
        if (linkPtr)
        {
            UrdfLink* link = *linkPtr;
            delete link;
        }
    }
    
    for (int i=0;i<model->m_joints.size();i++)
    {
        UrdfJoint** jointPtr = model->m_joints.getAtIndex(i);
        if (jointPtr)
        {
            UrdfJoint* joint = *jointPtr;
            delete joint;
        }
    }
}

static bool parseVector4(btVector4& vec4, const std::string& vector_str)
{
	vec4.setZero();
	btArray<std::string> pieces;
	btArray<float> rgba;
	btAlignedObjectArray<std::string> strArray;
	urdfIsAnyOf(" ", strArray);
	urdfStringSplit(pieces, vector_str, strArray);
	for (int i = 0; i < pieces.size(); ++i)
	{
		if (!pieces[i].empty())
		{
			rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
		}
	}
	if (rgba.size() != 4)
	{
		return false;
	}
	vec4.setValue(rgba[0],rgba[1],rgba[2],rgba[3]);
	return true;
}

static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
	vec3.setZero();
	btArray<std::string> pieces;
	btArray<float> rgba;
	btAlignedObjectArray<std::string> strArray;
	urdfIsAnyOf(" ", strArray);
	urdfStringSplit(pieces, vector_str, strArray);
	for (int i = 0; i < pieces.size(); ++i)
	{
		if (!pieces[i].empty())
		{
			rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
		}
	}
	if (rgba.size() < 3)
	{
		logger->reportWarning("Couldn't parse vector3");
		return false;
	}
    if (lastThree) {
        vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
    }
    else
    {
        vec3.setValue(rgba[0],rgba[1],rgba[2]);

    }
	return true;
}

bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger)
{
		
	if (!config->Attribute("name"))
	{
	  logger->reportError("Material must contain a name attribute");
	  return false;
	}

	material.m_name = config->Attribute("name");
		
	// texture
	TiXmlElement *t = config->FirstChildElement("texture");
	if (t)
	{
	  if (t->Attribute("filename"))
	  {
		  material.m_textureFilename = t->Attribute("filename");
	  }
	}

	if (material.m_textureFilename.length()==0)
	{
		//logger->reportWarning("material has no texture file name");
	}
		
	// color
	TiXmlElement *c = config->FirstChildElement("color");
	if (c)
	{
	  if (c->Attribute("rgba")) 
	  {
		  if (!parseVector4(material.m_rgbaColor,c->Attribute("rgba")))
		  {
			  std::string msg = material.m_name+" has no rgba";
			  logger->reportWarning(msg.c_str());
		  }
	  }
	}
	return true;

}

bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false)
{
    tr.setIdentity();
    
    if (parseSDF)
    {
        parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
    }
    else
    {
        const char* xyz_str = xml->Attribute("xyz");
        if (xyz_str)
        {
            parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
        }
    }
    
    if (parseSDF)
    {
        btVector3 rpy;
        if (parseVector3(rpy,std::string(xml->GetText()),logger,true))
        {
            double phi, the, psi;
            double roll = rpy[0];
            double pitch = rpy[1];
            double yaw = rpy[2];
            
            phi = roll / 2.0;
            the = pitch / 2.0;
            psi = yaw / 2.0;
            
            btQuaternion orn(
                             sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
                             cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
                             cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
                             cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
            
            orn.normalize();
            tr.setRotation(orn);
        }
    }
    else
    {
        const char* rpy_str = xml->Attribute("rpy");
        if (rpy_str != NULL)
        {
            btVector3 rpy;
            if (parseVector3(rpy,std::string(rpy_str),logger))
            {
                double phi, the, psi;
                double roll = rpy[0];
                double pitch = rpy[1];
                double yaw = rpy[2];
                
                phi = roll / 2.0;
                the = pitch / 2.0;
                psi = yaw / 2.0;
                
                btQuaternion orn(
                                 sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
                                 cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
                                 cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
                                 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
                
                orn.normalize();
                tr.setRotation(orn);
            }
        }
    }
    return true;
}

bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
	inertia.m_linkLocalFrame.setIdentity();
	inertia.m_mass = 0.f;
	if(m_parseSDF)
	{
		TiXmlElement* pose = config->FirstChildElement("pose");
		if (pose)
		{
			parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
		}
	}

	
		
	// Origin
	TiXmlElement *o = config->FirstChildElement("origin");
	if (o)
	{
	  if (!parseTransform(inertia.m_linkLocalFrame,o,logger))
	  {
		  return false;
	  }
	}
		
	TiXmlElement *mass_xml = config->FirstChildElement("mass");
	if (!mass_xml)
	{
	  logger->reportError("Inertial element must have a mass element");
	  return false;
	}
    if (m_parseSDF)
    {
        inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
    } else
    {
        if (!mass_xml->Attribute("value"))
        {
          logger->reportError("Inertial: mass element must have value attribute");
          return false;
        }

        inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
    }
		
	TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
	if (!inertia_xml)
	{
	  logger->reportError("Inertial element must have inertia element");
	  return false;
	}
    if (m_parseSDF)
    {
        TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
        TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
        TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
        TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
        TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
        TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
        if (ixx && ixy && ixz && iyy && iyz && izz)
        {
            inertia.m_ixx  = urdfLexicalCast<double>(ixx->GetText());
            inertia.m_ixy  = urdfLexicalCast<double>(ixy->GetText());
            inertia.m_ixz  = urdfLexicalCast<double>(ixz->GetText());
            inertia.m_iyy  = urdfLexicalCast<double>(iyy->GetText());
            inertia.m_iyz  = urdfLexicalCast<double>(iyz->GetText());
            inertia.m_izz  = urdfLexicalCast<double>(izz->GetText());
        } else
        {
            logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
            return false;
        }
    } else
    {
        if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
            inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
            inertia_xml->Attribute("izz")))
        {
          logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
          return false;
        }
        inertia.m_ixx  = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
        inertia.m_ixy  = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
        inertia.m_ixz  = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
        inertia.m_iyy  = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
        inertia.m_iyz  = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
        inertia.m_izz  = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
    }
	return true;
    
}

bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* logger)
{
	btAssert(g);
		
	TiXmlElement *shape = g->FirstChildElement();
	if (!shape)
	{
		logger->reportError("Geometry tag contains no child element.");
		return false;
	}
		
	const std::string type_name = shape->ValueTStr().c_str();
	if (type_name == "sphere")
	{
		geom.m_type = URDF_GEOM_SPHERE;
		if (!shape->Attribute("radius"))
		{
			logger->reportError("Sphere shape must have a radius attribute");
			return false;
		} else
		{
			geom.m_sphereRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
		}
	}	
	else if (type_name == "box")
	{
		geom.m_type = URDF_GEOM_BOX;
        if (m_parseSDF)
        {
            TiXmlElement* size = shape->FirstChildElement("size");
            if (0==size)
            {
                logger->reportError("box requires a size child element");
                return false;
            }
            parseVector3(geom.m_boxSize,size->GetText(),logger);
        }
        else
        {
              if (!shape->Attribute("size"))
              {
                  logger->reportError("box requires a size attribute");
                  return false;
              } else
              {
                  parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
              }
        }
	}
	else if (type_name == "cylinder")
	{
		geom.m_type = URDF_GEOM_CYLINDER;
		if (!shape->Attribute("length") ||
			!shape->Attribute("radius"))
	  {
		  logger->reportError("Cylinder shape must have both length and radius attributes");
		  return false;
	  }
		geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
		geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
		
	}
	
  else if (type_name == "mesh")
  {
	  geom.m_type = URDF_GEOM_MESH;
      if (m_parseSDF)
      {
          TiXmlElement* scale = shape->FirstChildElement("scale");
          if (0==scale)
          {
              geom.m_meshScale.setValue(1,1,1);
          }
          else
          {
              parseVector3(geom.m_meshScale,scale->GetText(),logger);
          }
          
          TiXmlElement* filename = shape->FirstChildElement("uri");
          geom.m_meshFileName = filename->GetText();
      }
      else
      {
          if (!shape->Attribute("filename")) {
              logger->reportError("Mesh must contain a filename attribute");
              return false;
          }
          
          geom.m_meshFileName = shape->Attribute("filename");
		  geom.m_meshScale.setValue(1,1,1);

		  if (shape->Attribute("scale"))
          {
              if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
			  {
				  logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
				  std::string scalar_str = shape->Attribute("scale");
				  double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
				  if (scaleFactor)
				  {
					  geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
				  }
			  }
          } else
          {
          }
      }
  }
  else
  {
      if (this->m_parseSDF)
      {
          if (type_name == "plane")
          {
              geom.m_type = URDF_GEOM_PLANE;
             
              TiXmlElement *n = shape->FirstChildElement("normal");
              TiXmlElement *s = shape->FirstChildElement("size");

              if ((0==n)||(0==s))
              {
                  logger->reportError("Plane shape must have both normal and size attributes");
                  return false;
              }
            
              parseVector3(geom.m_planeNormal,n->GetText(),logger);
          }
      } else
      {
          logger->reportError("Unknown geometry type:");
          logger->reportError(type_name.c_str());
          return false;
      }
  }
  
	return true;
}


bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, ErrorLogger* logger)
{

	collision.m_linkLocalFrame.setIdentity();
	
	if(m_parseSDF)
	{
		TiXmlElement* pose = config->FirstChildElement("pose");
		if (pose)
		{
			parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
		}
	}

	// Origin
	TiXmlElement *o = config->FirstChildElement("origin");
	if (o) 
	{
		if (!parseTransform(collision.m_linkLocalFrame, o,logger))
			return false;
	}
 // Geometry
	TiXmlElement *geom = config->FirstChildElement("geometry");
	if (!parseGeometry(collision.m_geometry,geom,logger))
	{
		return false;
	}
	
	
	const char *name_char = config->Attribute("name");
	if (name_char)
		collision.m_name = name_char;
	
	const char *concave_char = config->Attribute("concave");
	if (concave_char)
		collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
	
	return true;
}

bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
	visual.m_linkLocalFrame.setIdentity();
	if(m_parseSDF)
	{
		TiXmlElement* pose = config->FirstChildElement("pose");
		if (pose)
		{
			parseTransform(visual.m_linkLocalFrame, pose,logger,m_parseSDF);
		}
	}

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (o) 
  {
	  if (!parseTransform(visual.m_linkLocalFrame, o,logger))
		  return false;
  }
 // Geometry
	TiXmlElement *geom = config->FirstChildElement("geometry");
	if (!parseGeometry(visual.m_geometry,geom,logger))
	{
		return false;
	}
	
 		
  const char *name_char = config->Attribute("name");
  if (name_char)
	  visual.m_name = name_char;

	visual.m_hasLocalMaterial = false;
	
  // Material
  TiXmlElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
  if (mat)
  {
    if (m_parseSDF)
    {
        UrdfMaterial* matPtr = new UrdfMaterial;
        matPtr->m_name = "mat";
		if (name_char)
			matPtr->m_name = name_char;
        TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
        if (diffuse) {
            std::string diffuseText = diffuse->GetText();
            btVector4 rgba(1,0,0,1);
            parseVector4(rgba,diffuseText);
            matPtr->m_rgbaColor = rgba;
            model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
            visual.m_materialName = matPtr->m_name;
            visual.m_hasLocalMaterial = true;
        }
    } 
    else
      {
          // get material name
          if (!mat->Attribute("name")) 
          {
              logger->reportError("Visual material must contain a name attribute");
              return false;
          }
          visual.m_materialName = mat->Attribute("name");
          
          // try to parse material element in place
          
          TiXmlElement *t = mat->FirstChildElement("texture");
          TiXmlElement *c = mat->FirstChildElement("color");
          if (t||c)
          {
              if (parseMaterial(visual.m_localMaterial, mat,logger))
              {
                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
                  model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
                  visual.m_hasLocalMaterial = true;
              }
          }
      }
  }
  
  return true;
}

bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
	const char* linkName  = config->Attribute("name");
	if (!linkName)
	{
		logger->reportError("Link with no name");
		return false;
	}
	link.m_name = linkName;
    
    if (m_parseSDF) {


        TiXmlElement* pose = config->FirstChildElement("pose");
        if (0==pose)
        {
            link.m_linkTransformInWorld.setIdentity();
        }
        else
        {
            parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
        }
    }

	{
		//optional 'contact' parameters
	 TiXmlElement* ci = config->FirstChildElement("contact");
	  if (ci)
	  {
        
          TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
          if (damping_xml)
          {
              if (m_parseSDF)
              {
                  link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
                  link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
              } else
              {
                  if (!damping_xml->Attribute("value"))
                  {
                      logger->reportError("Link/contact: damping element must have value attribute");
                      return false;
                  }
                  
                  link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
                  link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;

              }
          }
          {
		TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
		if (friction_xml)
		{
			if (m_parseSDF)
			{
				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
			} else
			{
				if (!friction_xml->Attribute("value"))
				{
				  logger->reportError("Link/contact: lateral_friction element must have value attribute");
				  return false;
				}

				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
			}
		}
          }
          
          {
              TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
              if (rolling_xml)
              {
                  if (m_parseSDF)
                  {
                      link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
                  } else
                  {
                      if (!rolling_xml->Attribute("value"))
                      {
                          logger->reportError("Link/contact: rolling friction element must have value attribute");
                          return false;
                      }
                      
                      link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
                      
                  }
              }
          }
          {
              TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
              if (spinning_xml)
              {
                  if (m_parseSDF)
                  {
                      link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->GetText());
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
                  } else
                  {
                      if (!spinning_xml->Attribute("value"))
                      {
                          logger->reportError("Link/contact: spinning friction element must have value attribute");
                          return false;
                      }
                      
                      link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->Attribute("value"));
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
                      
                  }
              }
          }
           {
           
              TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
              if (stiffness_xml)
              {
                  if (m_parseSDF)
                  {
                      link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
                  } else
                  {
                      if (!stiffness_xml->Attribute("value"))
                      {
                          logger->reportError("Link/contact: stiffness element must have value attribute");
                          return false;
                      }
                      
                      link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
                      
                  }
              }
            }
            {
           
              TiXmlElement *damping_xml = ci->FirstChildElement("damping");
              if (damping_xml)
              {
                  if (m_parseSDF)
                  {
                      link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
                  } else
                  {
                      if (!damping_xml->Attribute("value"))
                      {
                          logger->reportError("Link/contact: damping element must have value attribute");
                          return false;
                      }
                      
                      link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
                      
                  }
              }
            }
	  }
	}

  // Inertial (optional)
  TiXmlElement *i = config->FirstChildElement("inertial");
  if (i)
  {
	  if (!parseInertia(link.m_inertia, i,logger))
	  {
		  logger->reportError("Could not parse inertial element for Link:");
		  logger->reportError(link.m_name.c_str());
		  return false;
	  }
  } else
  {
      
      if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
      {
          link.m_inertia.m_mass = 0.f;
          link.m_inertia.m_linkLocalFrame.setIdentity();
          link.m_inertia.m_ixx = 0.f;
          link.m_inertia.m_iyy = 0.f;
          link.m_inertia.m_izz= 0.f;
      } else
      {
          
          logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
          link.m_inertia.m_mass = 1.f;
          link.m_inertia.m_linkLocalFrame.setIdentity();
          link.m_inertia.m_ixx = 1.f;
          link.m_inertia.m_iyy = 1.f;
          link.m_inertia.m_izz= 1.f;
          logger->reportWarning(link.m_name.c_str());
      }
  }

  // Multiple Visuals (optional)
  for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
  {
	  UrdfVisual visual;
	  
	  if (parseVisual(model, visual, vis_xml,logger))
	  {
		  link.m_visualArray.push_back(visual);
	  }
	  else
	  {
		  logger->reportError("Could not parse visual element for Link:");
		  logger->reportError(link.m_name.c_str());
		  return false;
	  }
	  
  }
		
  
  // Multiple Collisions (optional)
  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
  {
	  UrdfCollision col;
	  if (parseCollision(col, col_xml,logger))
	  {      
		  link.m_collisionArray.push_back(col);
	  }
	  else
	  {
		  logger->reportError("Could not parse collision element for Link:");
		  logger->reportError(link.m_name.c_str());
		  return false;
	  }
  }
	return true;
}

bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
	joint.m_lowerLimit = 0.f;
	joint.m_upperLimit = -1.f;
	joint.m_effortLimit = 0.f;
	joint.m_velocityLimit = 0.f;
	joint.m_jointDamping = 0.f;
	joint.m_jointFriction = 0.f;
	
    if (m_parseSDF)
    {
        TiXmlElement *lower_xml = config->FirstChildElement("lower");
        if (lower_xml) {
            joint.m_lowerLimit = urdfLexicalCast<double>(lower_xml->GetText());
        }
        
        TiXmlElement *upper_xml = config->FirstChildElement("upper");
        if (upper_xml) {
            joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
        }
        
        TiXmlElement *effort_xml = config->FirstChildElement("effort");
        if (effort_xml) {
            joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
        }
        
        TiXmlElement *velocity_xml = config->FirstChildElement("velocity");
        if (velocity_xml) {
            joint.m_velocityLimit = urdfLexicalCast<double>(velocity_xml->GetText());
        }
    }
    else
    {
        const char* lower_str = config->Attribute("lower");
        if (lower_str)
        {
            joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
        }
        
        const char* upper_str = config->Attribute("upper");
        if (upper_str)
        {
            joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
        }
        
        
        // Get joint effort limit
        const char* effort_str = config->Attribute("effort");
        if (effort_str)
        {
            joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
        }
        
        // Get joint velocity limit
        const char* velocity_str = config->Attribute("velocity");
        if (velocity_str)
        {
            joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
        }
    }
		
	return true;
}

bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
{
    joint.m_jointDamping = 0;
    joint.m_jointFriction = 0;
    
    if (m_parseSDF) {
        TiXmlElement *damping_xml = config->FirstChildElement("damping");
        if (damping_xml) {
            joint.m_jointDamping = urdfLexicalCast<double>(damping_xml->GetText());
        }
        
        TiXmlElement *friction_xml = config->FirstChildElement("friction");
        if (friction_xml) {
            joint.m_jointFriction = urdfLexicalCast<double>(friction_xml->GetText());
        }
        
        if (damping_xml == NULL && friction_xml == NULL)
        {
            logger->reportError("joint dynamics element specified with no damping and no friction");
            return false;
        }
    }
    else
    {
        // Get joint damping
        const char* damping_str = config->Attribute("damping");
        if (damping_str)
        {
            joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
        }
        
        // Get joint friction
        const char* friction_str = config->Attribute("friction");
        if (friction_str)
        {
            joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
        }
        
        if (damping_str == NULL && friction_str == NULL)
        {
            logger->reportError("joint dynamics element specified with no damping and no friction");
            return false;
        }
    }
    
    return true;
}

bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{
	
		
	// Get Joint Name
	const char *name = config->Attribute("name");
	if (!name)
	{
	  logger->reportError("unnamed joint found");
	  return false;
	}
	joint.m_name = name;
	joint.m_parentLinkToJointTransform.setIdentity();
	
	// Get transform from Parent Link to Joint Frame
	TiXmlElement *origin_xml = config->FirstChildElement("origin");
	if (origin_xml)
	{
	  if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml,logger))
	  {
		  logger->reportError("Malformed parent origin element for joint:");
		  logger->reportError(joint.m_name.c_str());
		  return false;
	  }
  }
		
  // Get Parent Link
  TiXmlElement *parent_xml = config->FirstChildElement("parent");
  if (parent_xml)
  {
      if (m_parseSDF)
      {
          joint.m_parentLinkName = std::string(parent_xml->GetText());
      }
      else
      {
          const char *pname = parent_xml->Attribute("link");
          if (!pname)
          {
              logger->reportError("no parent link name specified for Joint link. this might be the root?");
              logger->reportError(joint.m_name.c_str());
              return false;
          }
          else
          {
              joint.m_parentLinkName = std::string(pname);
          }
      }
  }
 		
  // Get Child Link
  TiXmlElement *child_xml = config->FirstChildElement("child");
  if (child_xml)
  {
      if (m_parseSDF)
      {
          joint.m_childLinkName = std::string(child_xml->GetText());
      }
      else
      {
          const char *pname = child_xml->Attribute("link");
          if (!pname)
          {
              logger->reportError("no child link name specified for Joint link [%s].");
              logger->reportError(joint.m_name.c_str());
              return false;
          }
          else
          {
              joint.m_childLinkName = std::string(pname);
          }
      }
  }
		
  // Get Joint type
  const char* type_char = config->Attribute("type");
  if (!type_char)
  {
	  logger->reportError("joint [%s] has no type, check to see if it's a reference.");
	  logger->reportError(joint.m_name.c_str());
	  return false;
  }
  
  std::string type_str = type_char;
  if (type_str == "planar")
	  joint.m_type = URDFPlanarJoint;
  else if (type_str == "floating")
	  joint.m_type = URDFFloatingJoint;
  else if (type_str == "revolute")
	  joint.m_type = URDFRevoluteJoint;
  else if (type_str == "continuous")
	  joint.m_type = URDFContinuousJoint;
  else if (type_str == "prismatic")
	  joint.m_type = URDFPrismaticJoint;
  else if (type_str == "fixed")
	  joint.m_type = URDFFixedJoint;
  else
  {
	  logger->reportError("Joint ");
	  logger->reportError(joint.m_name.c_str());
	  logger->reportError("has unknown type:");
	  logger->reportError(type_str.c_str());
	  return false;
  }
		
    if (m_parseSDF)
    {
        if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
        {
            // axis
            TiXmlElement *axis_xml = config->FirstChildElement("axis");
            if (!axis_xml){
                logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
                logger->reportWarning(joint.m_name.c_str());
                joint.m_localJointAxis.setValue(1,0,0);
            }
            else{
                TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz");
                if (xyz_xml) {
                    if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger))
                    {
                        logger->reportError("Malformed axis element:");
                        logger->reportError(joint.m_name.c_str());
                        logger->reportError(" for joint:");
                        logger->reportError(xyz_xml->GetText());
                        return false;
                    }
                }
                
                TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
                if (limit_xml)
                {
					if (joint.m_type != URDFContinuousJoint)
					{
						if (!parseJointLimits(joint, limit_xml,logger))
						{
							logger->reportError("Could not parse limit element for joint:");
							logger->reportError(joint.m_name.c_str());
							return false;
						}
					}
                }
                else if (joint.m_type == URDFRevoluteJoint)
                {
                    logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
                    logger->reportError(joint.m_name.c_str());
                    return false;
                }
                else if (joint.m_type == URDFPrismaticJoint)
                {
                    logger->reportError("Joint is of type PRISMATIC without limits");
                    logger->reportError( joint.m_name.c_str());
                    return false;
                }
                
                TiXmlElement *prop_xml = axis_xml->FirstChildElement("dynamics");
                if (prop_xml)
                {
                    if (!parseJointDynamics(joint, prop_xml,logger))
                    {
                        logger->reportError("Could not parse dynamics element for joint:");
                        logger->reportError(joint.m_name.c_str());
                        return false;
                    }
                }
            }
        }
    }
    else
    {
        // Get Joint Axis
        if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
        {
            // axis
            TiXmlElement *axis_xml = config->FirstChildElement("axis");
            if (!axis_xml){
                logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
                logger->reportWarning(joint.m_name.c_str());
                joint.m_localJointAxis.setValue(1,0,0);
            }
            else{
                if (axis_xml->Attribute("xyz"))
                {
                    if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
                    {
                        logger->reportError("Malformed axis element:");
                        logger->reportError(joint.m_name.c_str());
                        logger->reportError(" for joint:");
                        logger->reportError(axis_xml->Attribute("xyz"));
                        return false;
                    }
                }
            }
        }
        
        // Get limit
        TiXmlElement *limit_xml = config->FirstChildElement("limit");
        if (limit_xml)
        {
            if (!parseJointLimits(joint, limit_xml,logger))
            {
                logger->reportError("Could not parse limit element for joint:");
                logger->reportError(joint.m_name.c_str());
                return false;
            }
        }
        else if (joint.m_type == URDFRevoluteJoint)
        {
            logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
            logger->reportError(joint.m_name.c_str());
            return false;
        }
        else if (joint.m_type == URDFPrismaticJoint)
        {
            logger->reportError("Joint is of type PRISMATIC without limits");
            logger->reportError( joint.m_name.c_str());
            return false;
        }
        
        joint.m_jointDamping = 0;
        joint.m_jointFriction = 0;
        
        // Get Dynamics
        TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
        if (prop_xml)
        {
            
            // Get joint damping
            const char* damping_str = prop_xml->Attribute("damping");
            if (damping_str)
            {
                joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
            }
            
            // Get joint friction
            const char* friction_str = prop_xml->Attribute("friction");
            if (friction_str)
            {
                joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
            }
            
            if (damping_str == NULL && friction_str == NULL)
            {
                logger->reportError("joint dynamics element specified with no damping and no friction");
                return false;
            }
        }
    }
	
	return true;
}


bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
{
	// every link has children links and joints, but no parents, so we create a
	// local convenience data structure for keeping child->parent relations
	btHashMap<btHashString,btHashString> parentLinkTree;
	
	// loop through all joints, for every link, assign children links and children joints
	for (int i=0;i<model.m_joints.size();i++)
	{
		UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
		if (jointPtr)
		{
			UrdfJoint* joint = *jointPtr;
			std::string parent_link_name = joint->m_parentLinkName;
			std::string child_link_name = joint->m_childLinkName;
			if (parent_link_name.empty() || child_link_name.empty())
			{
				logger->reportError("parent link or child link is empty for joint");
				logger->reportError(joint->m_name.c_str());
				return false;
			}
			
			UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
			if (!childLinkPtr)
			{
				logger->reportError("Cannot find child link for joint ");
				logger->reportError(joint->m_name.c_str());

				return false;
			}
			UrdfLink* childLink = *childLinkPtr;
			
			UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
			if (!parentLinkPtr)
			{
				logger->reportError("Cannot find parent link for a joint");
				logger->reportError(joint->m_name.c_str());
				return false;
			}
			UrdfLink* parentLink = *parentLinkPtr;
			
			childLink->m_parentLink  = parentLink;
			
			childLink->m_parentJoint = joint;
			parentLink->m_childJoints.push_back(joint);
			parentLink->m_childLinks.push_back(childLink);
			parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
		}
	}

	//search for children that have no parent, those are 'root'
	for (int i=0;i<model.m_links.size();i++)
	{
		UrdfLink** linkPtr = model.m_links.getAtIndex(i);
		btAssert(linkPtr);
		if (linkPtr)
		{
			UrdfLink* link = *linkPtr;
			link->m_linkIndex = i;
			
			if (!link->m_parentLink)
			{
				model.m_rootLinks.push_back(link);
			}
		}
		
	}
	
	if (model.m_rootLinks.size()>1)
	{
		logger->reportWarning("URDF file with multiple root links found");
	}
	
	if (model.m_rootLinks.size()==0)
	{
		logger->reportError("URDF without root link found");
		return false;
	}
	return true;
	
}

bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
	
	TiXmlDocument xml_doc;
	xml_doc.Parse(urdfText);
	if (xml_doc.Error())
	{
		logger->reportError(xml_doc.ErrorDesc());
		xml_doc.ClearError();
		return false;
	}

	TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
	if (!robot_xml)
	{
		logger->reportError("expected a robot element");
		return false;
	}
	
	// Get robot name
	const char *name = robot_xml->Attribute("name");
	if (!name)
	{
		logger->reportError("Expected a name for robot");
		return false;
	}
	m_urdf2Model.m_name = name;
	
	
	
	// Get all Material elements
	for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
	{
		UrdfMaterial* material = new UrdfMaterial;
		
		parseMaterial(*material, material_xml, logger);


		UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
		if (mat)
		{
			logger->reportWarning("Duplicate material");
		} else
		{
			m_urdf2Model.m_materials.insert(material->m_name.c_str(),material);
		}
	}


//	char msg[1024];
//	sprintf(msg,"Num materials=%d", m_model.m_materials.size());
//	logger->printMessage(msg);

	
	for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
	{
		UrdfLink* link = new UrdfLink;
		
		if (parseLink(m_urdf2Model,*link, link_xml,logger))
		{
			if (m_urdf2Model.m_links.find(link->m_name.c_str()))
			{
				logger->reportError("Link name is not unique, link names in the same model have to be unique");
				logger->reportError(link->m_name.c_str());
				return false;
			} else
			{
				//copy model material into link material, if link has no local material
				for (int i=0;i<link->m_visualArray.size();i++)
				{
					UrdfVisual& vis = link->m_visualArray.at(i);
					if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
					{
						UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
						if (mat && *mat)
						{
							vis.m_localMaterial = **mat;
						} else
						{
							//logger->reportError("Cannot find material with name:");
							//logger->reportError(vis.m_materialName.c_str());
						}
					}
				}
				
				m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
			}
		} else
		{
			logger->reportError("failed to parse link");
			delete link;
			return false;
		}
		
	}
	if (m_urdf2Model.m_links.size() == 0)
	{
		logger->reportWarning("No links found in URDF file");
		return false;
	}
	
	// Get all Joint elements
	for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
	{
		UrdfJoint* joint = new UrdfJoint;
		
		if (parseJoint(*joint, joint_xml,logger))
		{
			if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
			{
				logger->reportError("joint '%s' is not unique.");
				logger->reportError(joint->m_name.c_str());
				return false;
			}
			else
			{
				m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
			}
		}
		else
		{
			logger->reportError("joint xml is not initialized correctly");
			return false;
		}
	}

	bool ok(initTreeAndRoot(m_urdf2Model,logger));
	if (!ok)
	{
		return false;
	}
	
	if (forceFixedBase)
	{
		for (int i=0;i<m_urdf2Model.m_rootLinks.size();i++)
		{
			UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
			link->m_inertia.m_mass = 0.0;
			link->m_inertia.m_ixx = 0.0;
			link->m_inertia.m_ixy = 0.0;
			link->m_inertia.m_ixz = 0.0;
			link->m_inertia.m_iyy = 0.0;
			link->m_inertia.m_iyz = 0.0;
			link->m_inertia.m_izz = 0.0;
		}
	}
	
	return true;
}

void UrdfParser::activateModel(int modelIndex)
{
    m_activeSdfModel = modelIndex;
}


bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
    
    TiXmlDocument xml_doc;
    xml_doc.Parse(sdfText);
    if (xml_doc.Error())
    {
        logger->reportError(xml_doc.ErrorDesc());
        xml_doc.ClearError();
        return false;
    }

    TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
    if (!sdf_xml)
    {
        logger->reportError("expected an sdf element");
        return false;
    }
    
	//apparently, SDF doesn't require a "world" element, optional? URDF does.
	TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
	
	TiXmlElement* robot_xml = 0;

	if (!world_xml)
	{
		logger->reportWarning("expected a world element, continuing without it.");
		robot_xml = sdf_xml->FirstChildElement("model");
	} else
	{
		robot_xml = world_xml->FirstChildElement("model");
	}

    // Get all model (robot) elements
    for (; robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
    {
        UrdfModel* localModel = new UrdfModel;
        m_tmpModels.push_back(localModel);
        
		TiXmlElement* stat = robot_xml->FirstChildElement("static");
        if (0!=stat)
        {
			int val = int(atof(stat->GetText()));
			if (val==1)
			{
				localModel->m_overrideFixedBase = true;
			}
		}

        
        // Get robot name
        const char *name = robot_xml->Attribute("name");
        if (!name)
        {
            logger->reportError("Expected a name for robot");
            return false;
        }
        localModel->m_name = name;
        
        TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
        if (0==pose_xml)
        {
            localModel->m_rootTransformInWorld.setIdentity();
        }
        else
        {
            parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
        }

        // Get all Material elements
        for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
        {
            UrdfMaterial* material = new UrdfMaterial;
            
            parseMaterial(*material, material_xml, logger);
            
            
            UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
            if (mat)
            {
                logger->reportWarning("Duplicate material");
            } else
            {
                localModel->m_materials.insert(material->m_name.c_str(),material);
            }
        }
        
        
        //	char msg[1024];
        //	sprintf(msg,"Num materials=%d", m_model.m_materials.size());
        //	logger->printMessage(msg);
        
        
        for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
        {
            UrdfLink* link = new UrdfLink;
            
            if (parseLink(*localModel, *link, link_xml,logger))
            {
                if (localModel->m_links.find(link->m_name.c_str()))
                {
                    logger->reportError("Link name is not unique, link names in the same model have to be unique");
                    logger->reportError(link->m_name.c_str());
                    return false;
                } else
                {
                    //copy model material into link material, if link has no local material
                    for (int i=0;i<link->m_visualArray.size();i++)
                    {
                        UrdfVisual& vis = link->m_visualArray.at(i);
                        if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
                        {
                            UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
                            if (mat && *mat)
                            {
                                vis.m_localMaterial = **mat;
                            } else
                            {
                                //logger->reportError("Cannot find material with name:");
                                //logger->reportError(vis.m_materialName.c_str());
                            }
                        }
                    }
                    
                    localModel->m_links.insert(link->m_name.c_str(),link);
                }
            } else
            {
                logger->reportError("failed to parse link");
                delete link;
                return false;
            }
            
        }
        if (localModel->m_links.size() == 0)
        {
            logger->reportWarning("No links found in URDF file");
            return false;
        }
        
        // Get all Joint elements
        for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
        {
            UrdfJoint* joint = new UrdfJoint;
            
            if (parseJoint(*joint, joint_xml,logger))
            {
                if (localModel->m_joints.find(joint->m_name.c_str()))
                {
                    logger->reportError("joint '%s' is not unique.");
                    logger->reportError(joint->m_name.c_str());
                    return false;
                }
                else
                {
                    localModel->m_joints.insert(joint->m_name.c_str(),joint);
                }
            }
            else
            {
                logger->reportError("joint xml is not initialized correctly");
                return false;
            }
        }
        
        bool ok(initTreeAndRoot(*localModel,logger));
        if (!ok)
        {
            return false;
        }
        m_sdfModels.push_back(localModel);
    }
    
    return true;
}