/*! \file GazeboZoneSpawner.cpp

    \author Johannes Mey
    \date 31.03.20
*/

#include "GazeboZoneSpawner.h"

#include <sdf/sdf.hh>  // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/tf.h>

void GazeboZoneSpawner::spawnVisualPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA color) {
  if (shape.type != shape.BOX) {
    ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type");
    return;
  }

  sdf::SDFPtr sdf(new sdf::SDF());
  sdf::init(sdf);

  auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
  auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material";

  assert(sdf::readFile(sdfFileName, sdf));

  auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
  auto materialUriElement = visualElement->GetElement("material")->GetElement("script")->GetElement("uri");
  auto poseElement = visualElement->GetElement("pose");
  auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");

  materialUriElement->Set(materialFileName);

  tf::Quaternion rot;
  rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  double roll, pitch, yaw;
  tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);

  std::ostringstream poseStream;
  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " "
             << roll << " " << pitch << " " << yaw;
  poseElement->Set(poseStream.str());

  ROS_INFO_STREAM("Set pose in SDF file to '" << poseElement->GetValue()->GetAsString() << "'.");

  std::ostringstream sizeStream;
  sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
  sizeElement->Set(sizeStream.str());

  ROS_INFO_STREAM("Set size in SDF file to '" << sizeElement->GetValue()->GetAsString() << "'.");

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
  gazebo_msgs::SpawnModel srv;
  srv.request.robot_namespace = "box space";
  // srv.request.initial_pose = pose; // not required here
  srv.request.model_name = std::string("box") + poseStream.str() + sizeStream.str();
  srv.request.model_xml = sdf->ToString();
  // srv.request.reference_frame = ; // if left empty, world is used
  if (client.call(srv)) {
    if (srv.response.success) {
      ROS_INFO_STREAM("Spawned box '" << srv.request.model_name << "'. " << srv.response.status_message);
    } else {
      ROS_ERROR_STREAM("Failed to spawn box '" << srv.request.model_name << "'. " << srv.response.status_message);
    }
  } else {
    ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
  }
}

void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA, double mass, bool is_static) {
  if (shape.type != shape.BOX) {
    ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type");
    return;
  }

  sdf::SDFPtr sdf(new sdf::SDF());
  sdf::init(sdf);

  auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/physical_box.sdf";
  auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material";

  assert(sdf::readFile(sdfFileName, sdf));

  auto modelElement = sdf->Root()->GetElement("model");
  auto linkElement = modelElement->GetElement("link");
  auto poseElement = linkElement->GetElement("pose");

  auto visualElement = linkElement->GetElement("visual");
  auto visualMaterialUriElement = visualElement->GetElement("material")->GetElement("script")->GetElement("uri");
  auto visualSizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");

  auto collisionElement = linkElement->GetElement("collision");
  auto collisionSizeElement = collisionElement->GetElement("geometry")->GetElement("box")->GetElement("size");

  tf::Quaternion rot;
  rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  double roll, pitch, yaw;
  tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);

  std::ostringstream poseStream;
  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " "
             << roll << " " << pitch << " " << yaw;
  poseElement->Set(poseStream.str());

  // set the element to static
  modelElement->GetElement("static")->Set(is_static);

  double sx = shape.dimensions[0];
  double sy = shape.dimensions[1];
  double sz = shape.dimensions[2];

  // set the size both for visual and collision objects
  std::ostringstream sizeStream;
  sizeStream << sx << " " << sy << " " << sz;
  visualSizeElement->Set(sizeStream.str());
  collisionSizeElement->Set(sizeStream.str());

  // set inertia matrix
  auto inertialElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("inertial");
  inertialElement->GetElement("mass")->Set(mass);
  inertialElement->GetElement("inertia")->GetElement("ixx")->Set(0.083 * mass * sy*sy + sz*sz);
  inertialElement->GetElement("inertia")->GetElement("iyy")->Set(0.083 * mass * sx*sx + sz*sz);
  inertialElement->GetElement("inertia")->GetElement("izz")->Set(0.083 * mass * sx*sx + sy*sy);

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
  gazebo_msgs::SpawnModel srv;
  srv.request.robot_namespace = "box space";
  // srv.request.initial_pose = pose; // not required here
  srv.request.model_name = name;
  srv.request.model_xml = sdf->ToString();
  // srv.request.reference_frame = ; // if left empty, world is used
  if (client.call(srv)) {
    if (srv.response.success) {
      ROS_INFO_STREAM("Spawned box '" << srv.request.model_name << "'. " << srv.response.status_message);
    } else {
      ROS_ERROR_STREAM("Failed to spawn box '" << srv.request.model_name << "'. " << srv.response.status_message);
    }
  } else {
    ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
  }
}