#include "gz_enviroment/gz_enviroment.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros_gz_bridge/convert.hpp"
#include <memory>
#include <rclcpp/logging.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>

#include <gz/sim/components.hh>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>

namespace gz_enviroment {

GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}

CallbackReturn GzEnviroment::on_init() {
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
  gz::sim::EntityComponentManager ecm;

  const auto modelEntities =
      ecm.EntitiesByComponents(gz::sim::components::Model());

  for (const auto entity : modelEntities) {
    const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
    if (modelName) {
      RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data());
    }
  }
  return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
  // Configuration of parameters
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
  m_gz_node = std::make_shared<gz::transport::Node>();
  m_world_name = getGzWorldName();
  m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
  m_service_spawn = std::string("/world/") + m_world_name + "/create";
  m_config_loader =
      std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2");
  m_follow_frames = m_config_loader->getSceneModelNames();
  // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
  m_transforms = m_config_loader->getTfData("bishop");

  // if (!doGzSpawn())
  // {
  //   return CallbackReturn::ERROR;
  // }

  return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
  // Setting up the subscribers and publishers
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");

  m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
  m_tf2_broadcaster =
      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
  m_tf2_broadcaster_target =
      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());

  return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
  // Clear tf2 broadcasters
  // m_tf2_broadcaster.reset();
  // m_tf2_broadcaster_target.reset();

  // Clear other variables
  m_gz_node.reset();
  m_follow_frames.clear();
  m_topic_name.clear();
  m_service_spawn.clear();
  m_world_name.clear();
  m_config_loader.reset();
  m_transforms = tf2_msgs::msg::TFMessage();
  m_target_places = tf2_msgs::msg::TFMessage();

  return CallbackReturn::SUCCESS;
}

CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
  RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
  // m_tf2_broadcaster.reset();
  // m_tf2_broadcaster_target.reset();
  // m_gz_node.reset();

  return CallbackReturn::SUCCESS;
}

// TODO: Check to do this via EntityComponentManager
void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
  // TODO: Read from config
  // m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
  std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};

  for (const auto &it : pose_v.pose()) {
    if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) ==
        m_follow_frames.end())
      continue;

    geometry_msgs::msg::PoseStamped rpose;
    ros_gz_bridge::convert_gz_to_ros(it, rpose);

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = getNode()->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = it.name();

    t.transform.translation.x = rpose.pose.position.x;
    t.transform.translation.y = rpose.pose.position.y;
    t.transform.translation.z = rpose.pose.position.z;

    t.transform.rotation.x = rpose.pose.orientation.x;
    t.transform.rotation.y = rpose.pose.orientation.y;
    t.transform.rotation.z = rpose.pose.orientation.z;
    t.transform.rotation.w = rpose.pose.orientation.w;

    all_transforms.push_back(t);
  }

  for (auto &place : m_transforms.transforms) {
    all_transforms.push_back(place);
  }

  for (auto &transform : all_transforms) {
    m_tf2_broadcaster->sendTransform(transform);
  }
}


std::string GzEnviroment::getGzWorldName() {
  bool executed{false};
  bool result{false};
  unsigned int timeout{5000};
  std::string service{"/gazebo/worlds"};
  gz::msgs::StringMsg_V worlds_msg;

  while (rclcpp::ok() && !executed) {
    RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
    executed = m_gz_node->Request(service, timeout, worlds_msg, result);
  }

  if (!executed) {
    RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
    return "";
  }

  if (!result || worlds_msg.data().empty()) {
    RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names.");
    return "";
  }
  RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str());
  return worlds_msg.data(0);
}

bool GzEnviroment::doGzSpawn() {
  gz::msgs::EntityFactory req;
  gz::msgs::Boolean rep;
  bool result;
  unsigned int timeout = 5000;
  bool executed;

  auto env_models = m_config_loader->getEnvModels();
  for (auto &model : *env_models) {
    std::string sdf_string =
        createGzModelString(model.model_pose, model.model_inertia, model.mass,
                            model.mesh_path, model.model_name);
    req.set_sdf(sdf_string);
    executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
  }
  return executed;
}

std::string GzEnviroment::createGzModelString(
    const std::vector<double> &pose, const std::vector<double> &inertia,
    const double &mass, const std::string &mesh_filepath,
    const std::string &name) {
  std::string model_template =
      std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
      "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
      std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
      std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
      std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
      "</pose>"
      // + "<inertial>"
      // + "<inertia>"
      // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
      // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
      // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
      // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
      // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
      // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
      // + "</inertia>"
      // + "<mass>" + std::to_string(mass) + "</mass>"
      // + "</inertial>"
      + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
      mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
      "<collision name='collision_" + name + "'>" +
      "<geometry><mesh><uri>file://" + mesh_filepath +
      "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
      "</sdf>";
  return model_template;
}

} // namespace gz_enviroment

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
                       env_interface::EnvInterface);