/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Wim Meeussen */


#include <urdf_interface/link.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <algorithm>

namespace urdf{

boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
{
  boost::shared_ptr<Geometry> geom;
  if (!g) return geom;

  TiXmlElement *shape = g->FirstChildElement();
  if (!shape)
  {
    ROS_ERROR("Geometry tag contains no child element.");
    return geom;
  }

  std::string type_name = shape->ValueStr();
  if (type_name == "sphere")
    geom.reset(new Sphere);
  else if (type_name == "box")
    geom.reset(new Box);
  else if (type_name == "cylinder")
    geom.reset(new Cylinder);
  else if (type_name == "mesh")
    geom.reset(new Mesh);
  else
  {
    ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
    return geom;
  }

  // clear geom object when fails to initialize
  if (!geom->initXml(shape)){
    ROS_ERROR("Geometry failed to parse");
    geom.reset();
  }

  return geom;
}

bool Material::initXml(TiXmlElement *config)
{
  bool has_rgb = false;
  bool has_filename = false;

  this->clear();

  if (!config->Attribute("name"))
  {
    ROS_ERROR("Material must contain a name attribute");
    return false;
  }

  this->name = config->Attribute("name");

  // texture
  TiXmlElement *t = config->FirstChildElement("texture");
  if (t)
  {
    if (t->Attribute("filename"))
    {
      this->texture_filename = t->Attribute("filename");
      has_filename = true;
    }
    else
    {
      ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
    }
  }

  // color
  TiXmlElement *c = config->FirstChildElement("color");
  if (c)
  {
    if (c->Attribute("rgba"))
    {
      if (!this->color.init(c->Attribute("rgba")))
      {
        ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
        this->color.clear();
        return false;
      }
      else
        has_rgb = true;
    }
    else
    {
      ROS_ERROR("Material %s color has no rgba",this->name.c_str());
    }
  }

  return (has_rgb || has_filename);
}

bool Inertial::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)");
    this->origin.clear();
  }
  else
  {
    if (!this->origin.initXml(o))
    {
      ROS_ERROR("Inertial has a malformed origin tag");
      this->origin.clear();
      return false;
    }
  }

  TiXmlElement *mass_xml = config->FirstChildElement("mass");
  if (!mass_xml)
  {
    ROS_ERROR("Inertial element must have mass element");
    return false;
  }
  if (!mass_xml->Attribute("value"))
  {
    ROS_ERROR("Inertial: mass element must have value attributes");
    return false;
  }

  try
  {
    mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value"));
    return false;
  }

  TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
  if (!inertia_xml)
  {
    ROS_ERROR("Inertial element must have inertia element");
    return false;
  }
  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")))
  {
    ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
    return false;
  }
  try
  {
    ixx  = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
    ixy  = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
    ixz  = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
    iyy  = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
    iyz  = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
    izz  = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double",
              inertia_xml->Attribute("ixx"),
              inertia_xml->Attribute("ixy"),
              inertia_xml->Attribute("ixz"),
              inertia_xml->Attribute("iyy"),
              inertia_xml->Attribute("iyz"),
              inertia_xml->Attribute("izz"));
    return false;
  }

  return true;
}

bool Visual::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Visual has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Visual element");
    return false;
  }

  // Material
  TiXmlElement *mat = config->FirstChildElement("material");
  if (!mat)
  {
    ROS_DEBUG("visual element has no material tag.");
  }
  else
  {
    // get material name
    if (!mat->Attribute("name"))
    {
      ROS_ERROR("Visual material must contain a name attribute");
      return false;
    }
    this->material_name = mat->Attribute("name");

    // try to parse material element in place
    this->material.reset(new Material);
    if (!this->material->initXml(mat))
    {
      ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
      this->material.reset();
    }
    else
    {
      ROS_DEBUG("Parsed material element in Visual block.");
    }
  }

  // Group Tag (optional)
  // collision blocks without a group tag are designated to the "default" group
  const char *group_name_char = config->Attribute("group");
  if (!group_name_char)
    group_name = std::string("default");
  else
    group_name = std::string(group_name_char);

  return true;
}

bool Collision::initXml(TiXmlElement* config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for collision element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Collision has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Collision element");
    return false;
  }

  // Group Tag (optional)
  // collision blocks without a group tag are designated to the "default" group
  const char *group_name_char = config->Attribute("group");
  if (!group_name_char)
    group_name = std::string("default");
  else
    group_name = std::string(group_name_char);

  return true;
}

bool Sphere::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = SPHERE;
  if (!c->Attribute("radius"))
  {
    ROS_ERROR("Sphere shape must have a radius attribute");
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Box::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = BOX;
  if (!c->Attribute("size"))
  {
    ROS_ERROR("Box shape has no size attribute");
    return false;
  }
  if (!dim.init(c->Attribute("size")))
  {
    ROS_ERROR("Box shape has malformed size attribute");
    dim.clear();
    return false;
  }
  return true;
}

bool Cylinder::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = CYLINDER;
  if (!c->Attribute("length") ||
      !c->Attribute("radius"))
  {
    ROS_ERROR("Cylinder shape must have both length and radius attributes");
    return false;
  }

  try
  {
    length = boost::lexical_cast<double>(c->Attribute("length"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("length (%s) is not a valid float",c->Attribute("length"));
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Mesh::fileExists(std::string filename)
{
  std::string fullname = filename;
  if (fullname.find("package://") == 0)
  {
    fullname.erase(0, strlen("package://"));
    size_t pos = fullname.find("/");
    if (pos == std::string::npos)
    {
      ROS_FATAL("Could not parse package:// format for [%s]",filename.c_str());
    }

    std::string package = fullname.substr(0, pos);
    fullname.erase(0, pos);
    std::string package_path = ros::package::getPath(package);

    if (package_path.empty())
    {
      ROS_FATAL("%s Package[%s] does not exist",filename.c_str(),package.c_str());
      return false;
    }

    fullname = package_path + fullname;
  }
  std::ifstream fin; fin.open(fullname.c_str(), std::ios::in); fin.close();
  if (fin.fail()) {
    ROS_FATAL("Mesh [%s] does not exist",filename.c_str());
    return false;
  }


  return true;
}

bool Mesh::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = MESH;
  if (!c->Attribute("filename"))
  {
    ROS_ERROR("Mesh must contain a filename attribute");
    return false;
  }

  filename = c->Attribute("filename");

  // check if filename exists, is this really necessary?
  if (!fileExists(filename))
    ROS_WARN("filename referred by mesh [%s] does not appear to exist.",filename.c_str());

  if (c->Attribute("scale"))
  {
    if (!this->scale.init(c->Attribute("scale")))
    {
      ROS_ERROR("Mesh scale was specified, but could not be parsed");
      this->scale.clear();
      return false;
    }
  }
  else
    ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");

  return true;
}


bool Link::initXml(TiXmlElement* config)
{
  this->clear();

  const char *name_char = config->Attribute("name");
  if (!name_char)
  {
    ROS_ERROR("No name given for the link.");
    return false;
  }
  name = std::string(name_char);

  // Inertial (optional)
  TiXmlElement *i = config->FirstChildElement("inertial");
  if (i)
  {
    inertial.reset(new Inertial);
    if (!inertial->initXml(i))
    {
      ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
      return false;
    }
  }

  // Multiple Visuals (optional)
  for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
  {
    boost::shared_ptr<Visual> vis;
    vis.reset(new Visual);

    if (vis->initXml(vis_xml))
    {
      boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss = this->getVisuals(vis->group_name);
      if (!viss)
      {
        // group does not exist, create one and add to map
        viss.reset(new std::vector<boost::shared_ptr<Visual > >);
        // new group name, create vector, add vector to map and add Visual to the vector
        this->visual_groups.insert(make_pair(vis->group_name,viss));
        ROS_DEBUG("successfully added a new visual group name '%s'",vis->group_name.c_str());
      }

      // group exists, add Visual to the vector in the map
      viss->push_back(vis);
      ROS_DEBUG("successfully added a new visual under group name '%s'",vis->group_name.c_str());
    }
    else
    {
      ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
      vis.reset();
      return false;
    }
  }
  // Visual (optional)
  // Assign one single default visual pointer from the visual_groups map
  this->visual.reset();
  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > default_visual = this->getVisuals("default");
  if (!default_visual)
  {
    ROS_DEBUG("No 'default' visual group for Link '%s'", this->name.c_str());
  }
  else if (default_visual->empty())
  {
    ROS_DEBUG("'default' visual group is empty for Link '%s'", this->name.c_str());
  }
  else
  {
    if (default_visual->size() > 1)
      ROS_WARN("'default' visual group has %d visuals for Link '%s', taking the first one as default",(int)default_visual->size(), this->name.c_str());
    this->visual = (*default_visual->begin());
  }



  // Multiple Collisions (optional)
  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
  {
    boost::shared_ptr<Collision> col;
    col.reset(new Collision);

    if (col->initXml(col_xml))
    {
      boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols = this->getCollisions(col->group_name);
      if (!cols)
      {
        // group does not exist, create one and add to map
        cols.reset(new std::vector<boost::shared_ptr<Collision > >);
        // new group name, create vector, add vector to map and add Collision to the vector
        this->collision_groups.insert(make_pair(col->group_name,cols));
        ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str());
      }

      // group exists, add Collision to the vector in the map
      cols->push_back(col);
      ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str());
    }
    else
    {
      ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
      col.reset();
      return false;
    }
  }

  // Collision (optional)
  // Assign one single default collision pointer from the collision_groups map
  this->collision.reset();
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > default_collision = this->getCollisions("default");
  if (!default_collision)
  {
    ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str());
  }
  else if (default_collision->empty())
  {
    ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str());
  }
  else
  {
    if (default_collision->size() > 1)
      ROS_WARN("'default' collision group has %d collisions for Link '%s', taking the first one as default",(int)default_collision->size(), this->name.c_str());
    this->collision = (*default_collision->begin());
  }

  return true;
}

void Link::addVisual(std::string group_name, boost::shared_ptr<Visual> visual)
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > viss = this->getVisuals(group_name);
  if (!viss)
  {
    // group does not exist, create one and add to map
    viss.reset(new std::vector<boost::shared_ptr<Visual > >);
    // new group name, create vector, add vector to map and add Visual to the vector
    this->visual_groups.insert(make_pair(group_name,viss));
    ROS_DEBUG("successfully added a new visual group name '%s'",group_name.c_str());
  }

  // group exists, add Visual to the vector in the map
  std::vector<boost::shared_ptr<Visual > >::iterator vis_it = find(viss->begin(),viss->end(),visual);
  if (vis_it != viss->end())
    ROS_WARN("attempted to add a visual that already exists under group name '%s', skipping.",group_name.c_str());
  else
    viss->push_back(visual);
  ROS_DEBUG("successfully added a new visual under group name '%s'",group_name.c_str());

}

boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > Link::getVisuals(const std::string& group_name) const
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Visual > > > ptr;
  if (this->visual_groups.find(group_name) == this->visual_groups.end())
    ptr.reset();
  else
    ptr = this->visual_groups.find(group_name)->second;
  return ptr;
}


void Link::addCollision(std::string group_name, boost::shared_ptr<Collision> collision)
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > viss = this->getCollisions(group_name);
  if (!viss)
  {
    // group does not exist, create one and add to map
    viss.reset(new std::vector<boost::shared_ptr<Collision > >);
    // new group name, create vector, add vector to map and add Collision to the vector
    this->collision_groups.insert(make_pair(group_name,viss));
    ROS_DEBUG("successfully added a new collision group name '%s'",group_name.c_str());
  }

  // group exists, add Collision to the vector in the map
  std::vector<boost::shared_ptr<Collision > >::iterator vis_it = find(viss->begin(),viss->end(),collision);
  if (vis_it != viss->end())
    ROS_WARN("attempted to add a collision that already exists under group name '%s', skipping.",group_name.c_str());
  else
    viss->push_back(collision);
  ROS_DEBUG("successfully added a new collision under group name '%s'",group_name.c_str());

}

boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > Link::getCollisions(const std::string& group_name) const
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > ptr;
  if (this->collision_groups.find(group_name) == this->collision_groups.end())
    ptr.reset();
  else
    ptr = this->collision_groups.find(group_name)->second;
  return ptr;
}

void Link::setParent(boost::shared_ptr<Link> parent)
{
  this->parent_link_ = parent;
  ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
}

void Link::setParentJoint(boost::shared_ptr<Joint> parent)
{
  this->parent_joint = parent;
  ROS_DEBUG("set parent joint '%s' to Link '%s'",  parent->name.c_str(), this->name.c_str());
}

void Link::addChild(boost::shared_ptr<Link> child)
{
  this->child_links.push_back(child);
  ROS_DEBUG("added child Link '%s' to Link '%s'",  child->name.c_str(), this->name.c_str());
}

void Link::addChildJoint(boost::shared_ptr<Joint> child)
{
  this->child_joints.push_back(child);
  ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}



}