#include <jsoncpp/json/json.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <ros/ros.h>
#include <boost/filesystem.hpp>

#include <sdformat-6.0/sdf/sdf.hh>  // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo

static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const std::string APP_DIRECTORY_NAME = ".panda_simulation";


void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
  if (shape.type != shape.BOX) {
    ROS_ERROR("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";

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

  ROS_INFO_NAMED("model-loader", "successfully read SDF file: %s", sdf->ToString().c_str());

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

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

  ROS_INFO_NAMED("model-loader", "successfully changed SDF file, pose is: %s", poseElement->GetValue()->GetAsString().c_str());

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

  ROS_INFO_NAMED("model-loader", "successfully changed SDF file, size is: '%s', should be '%s'", sizeElement->GetValue()->GetAsString().c_str(), sizeStream.str().c_str());

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
  gazebo_msgs::SpawnModel spawnModelService;
  spawnModelService.request.robot_namespace = "box space";
  // spawnModelService.request.initial_pose = pose; // not required here
  spawnModelService.request.model_name = sdf->Root()->GetElement("model")->GetAttribute("name")->GetAsString();
  spawnModelService.request.model_xml = sdf->ToString().c_str();
  // spawnModelService.request.reference_frame = ; // if left empty, world is used
  if (client.call(spawnModelService))
  {
    ROS_INFO("result: %s", spawnModelService.response.success ? "success!" : "failed!");
    ROS_INFO("result: %s", spawnModelService.response.status_message.c_str());
  }
  else
  {
    ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
  }
}

moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
{
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = "world";
  collision_object.id = name;

  const Json::Value dimensions = root["dimensions"];
  ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
  // Define a box to add to the world.
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = dimensions["x"].asDouble();
  primitive.dimensions[1] = dimensions["y"].asDouble();
  primitive.dimensions[2] = dimensions["z"].asDouble();

  const Json::Value position = root["position"];
  ROS_INFO_STREAM("Extracted position: " << position);

  const Json::Value orientation = root["orientation"];
  ROS_INFO_STREAM("Extracted orientation: " << orientation);
  // Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = orientation["w"].asDouble();
  box_pose.orientation.x = orientation["x"].asDouble();
  box_pose.orientation.y = orientation["y"].asDouble();
  box_pose.orientation.z = orientation["z"].asDouble();

  // MoveIt! planning scene expects the center of the object as position.
  // We add half of its dimension to its position
  box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
  box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
  box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  spawnCollisionBox(primitive, box_pose);

  return std::move(collision_object);
}

int main(int argc, char **argv)
{
  namespace fs = boost::filesystem;
  ROS_INFO("RUNNING robot_control_node");

  ros::init(argc, argv, "robot_control_node");

  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);

  ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  ros::WallDuration sleep_t(0.5);
  while (planning_scene_diff_publisher.getNumSubscribers() < 1)
  {
    sleep_t.sleep();
  }
  moveit_msgs::PlanningScene planning_scene;

  // read JSON files from ~/.panda_simulation
  fs::path home(getenv("HOME"));
  if (fs::is_directory(home))
  {
    fs::path app_directory(home);
    app_directory /= APP_DIRECTORY_NAME;

    if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
    {
      ROS_WARN_STREAM(app_directory << " does not exist");

      // Create .panda_simulation directory
      std::string path(getenv("HOME"));
      path += "/.panda_simulation";
      ROS_INFO("Creating %s collision objects directory.", path);
      try
      {
        boost::filesystem::create_directory(path);
      }
      catch (const std::exception&)
      {
        ROS_ERROR(
          "%s directory could not be created."
          "Please create this directory yourself "
          "if you want to specify collision objects.", path.c_str());
        return -1;
      }
    }

    std::vector<moveit_msgs::CollisionObject> collision_objects;
    ROS_INFO_STREAM(app_directory << " is a directory containing:");
    for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
    {
      ROS_INFO_STREAM(entry);

      std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
      if (file_stream)
      {
        Json::Value root;
        file_stream >> root;

        moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
        collision_objects.push_back(collision_object);
      }
      else
      {
        ROS_WARN_STREAM("could not open file " << entry.path());
      }
    }

    // Publish the collision objects to the scene
    for (const auto &collision_object : collision_objects)
    {
      collision_object.header.frame_id = move_group_arm.getPlanningFrame();
      planning_scene.world.collision_objects.push_back(collision_object);
    }

    ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
    planning_scene.is_diff = true;
    planning_scene_diff_publisher.publish(planning_scene);

    ROS_INFO("robot_control_node is ready");
    ros::waitForShutdown();

    return 0;
  }
}