#include <pluginlib/class_list_macros.h>
#include <uwsim/newdevice.h>

SimulatedDeviceConfig::Ptr NewDevice_Factory::processConfig(const xmlpp::Node* node, ConfigFile * config)
{   
  NewDevice_Config * cfg = new NewDevice_Config(getType());
  xmlpp::Node::NodeList list = node->get_children();
  for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter)
  {
    const xmlpp::Node* child = dynamic_cast<const xmlpp::Node*>(*iter);
    if (child->get_name() == "relativeTo")
      config->extractStringChar(child, cfg->relativeTo);
    else if (child->get_name() == "position")
      config->extractPositionOrColor(child, cfg->position);
    else if (child->get_name() == "orientation")
      config->extractOrientation(child, cfg->orientation);
  }
 
  return SimulatedDeviceConfig::Ptr(cfg);
}

bool NewDevice_Factory::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration)
{
  if (iteration > 0)
    return true;
  for (size_t i = 0; i < vehicleChars.simulated_devices.size(); ++i)
    if (vehicleChars.simulated_devices[i]->getType() == this->getType())
    {
      NewDevice_Config * cfg = dynamic_cast<NewDevice_Config *>(vehicleChars.simulated_devices[i].get());
      if (cfg)
      {
        int target=-1;
        for(int j=0;j<auv->urdf->link.size();j++)
        {
          if(auv->urdf->link[j]->getName()==cfg->relativeTo)
          {
            target=j;
          }
        }
        if(target==-1)
	{
          OSG_FATAL << "NewDevice device '" << vehicleChars.simulated_devices[i]->name << "' inside robot '"
            << vehicleChars.name << "' has an unknown relativeTo, discarding..." << std::endl;
        }
        else
        {
          osg::ref_ptr < osg::Transform > vMd = (osg::Transform*) new osg::PositionAttitudeTransform;
          vMd->asPositionAttitudeTransform()->setPosition(osg::Vec3d(cfg->position[0], cfg->position[1], cfg->position[2]));
          vMd->asPositionAttitudeTransform()->setAttitude(
              osg::Quat(cfg->orientation[0], osg::Vec3d(1, 0, 0), cfg->orientation[1], osg::Vec3d(0, 1, 0),
              cfg->orientation[2], osg::Vec3d(0, 0, 1)));
          auv->urdf->link[target]->getParent(0)->getParent(0)->asGroup()->addChild(vMd);
 
          auv->devices->all.push_back(NewDevice::Ptr(new NewDevice(cfg,vMd)));
        }
      }
      else
        OSG_FATAL << "NewDevice device '" << vehicleChars.simulated_devices[i]->name << "' inside robot '"
            << vehicleChars.name << "' has empty cfg, discarding..." << std::endl;
    }
  return true;
}

std::vector<boost::shared_ptr<ROSInterface> > NewDevice_Factory::getInterface(ROSInterfaceInfo & rosInterface, std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile)
{
  std::vector < boost::shared_ptr<ROSInterface> > ifaces;
  for (size_t i = 0; i < iauvFile.size(); ++i)
    for (size_t d = 0; d < iauvFile[i]->devices->all.size(); ++d)
      if (iauvFile[i]->devices->all[d]->getType() == this->getType()
          && iauvFile[i]->devices->all[d]->name == rosInterface.targetName)
      {
        ifaces.push_back(
            boost::shared_ptr < ROSInterface
                > (new NewDevice_ROSPublisher(dynamic_cast<NewDevice*>(iauvFile[i]->devices->all[d].get()),
                                                rosInterface.topic, rosInterface.rate)));
      }
  if (ifaces.size() == 0)
    ROS_WARN("Returning empty ROS interface for device %s...", rosInterface.targetName.c_str());
  return ifaces;
}

NewDevice::NewDevice(NewDevice_Config * cfg,osg::Node *trackNode) :
    SimulatedDevice(cfg)
{
  this->parent=trackNode;
}



void NewDevice_ROSPublisher::createPublisher(ros::NodeHandle &nh)
{
  ROS_INFO("NewDevice_ROSPublisher on topic %s", topic.c_str());
  pub_ = nh.advertise < geometry_msgs::Pose > (topic, 1);
}

void NewDevice_ROSPublisher::publish()
{
  geometry_msgs::Pose msg;
  boost::shared_ptr<osg::Matrix> mat = getWorldCoords(dev->parent);
  msg.position.x= mat->getTrans().x();
  msg.position.y= mat->getTrans().y();
  msg.position.z= mat->getTrans().z();
  msg.orientation.x=mat->getRotate().x();	
  msg.orientation.y=mat->getRotate().y();	
  msg.orientation.z=mat->getRotate().z();	
  msg.orientation.w=mat->getRotate().w();	
  pub_.publish(msg);
}


#if ROS_VERSION_MINIMUM(1, 9, 0)
// new pluginlib API in Groovy and Hydro
PLUGINLIB_EXPORT_CLASS(NewDevice_Factory, uwsim::SimulatedDeviceFactory)
#else
PLUGINLIB_REGISTER_CLASS(NewDevice_Factory, NewDevice_Factory, uwsim::SimulatedDeviceFactory)
#endif