#include "toid_bt/bt_executor.hpp"

#include "behaviortree_cpp/xml_parsing.h"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "rclcpp/rclcpp.hpp"
#include "toid_bt/plugins/empty_srv_action.hpp"
#include "toid_bt/plugins/end_calib_action.hpp"
#include "toid_bt/plugins/in_position_decorator.hpp"
#include "toid_bt/plugins/move_coords_action.hpp"
#include "toid_bt/plugins/pull_pin_action.hpp"
#include "toid_bt/plugins/rotate_action.hpp"
#include "toid_bt/plugins/rotate_towards_action.hpp"
#include "toid_bt/plugins/send_text_action.hpp"
#include "toid_bt/plugins/set_parameter_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp"
#include "toid_bt/plugins/translate_x_action.hpp"

namespace toid
{
TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts)
: BT::TreeExecutionServer(std::make_shared<rclcpp::Node>("toid_bt", opts))
{
  /* 
  executeRegistration();
  std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
  */
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  nav2_util::declare_parameter_if_not_declared(
    node(), "base_frame", rclcpp::ParameterValue("base_footprint"));
  node()->get_parameter("base_frame", base_frame_);

  nav2_util::declare_parameter_if_not_declared(
    node(), "global_frame", rclcpp::ParameterValue("map"));
  node()->get_parameter("global_frame", global_frame_);
  using namespace std::placeholders;

  acorn_pose_sub_ = node()->create_subscription<geometry_msgs::msg::PoseStamped>(
    "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1));

  on_rotate_sub_ = node()->create_subscription<std_msgs::msg::String>(
    "/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1));
}

void TreeExecutor::onTreeCreated(BT::Tree & tree)
{
  logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
}

void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
{
  FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; };
  PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
  PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) {
    this->acorn_position(pose);
  };

  rclcpp::Node::SharedPtr nh = node();
  factory.registerNodeType<toid::MovePointNode>(
    "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);

  factory.registerNodeType<toid::MovePointNode>(
    "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);

  factory.registerNodeType<toid::RotateNode>(
    "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);

  factory.registerNodeType<toid::RotateNode>(
    "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);

  factory.registerNodeType<toid::RotateTowardsNode>(
    "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);

  factory.registerNodeType<toid::TranslateXNode>(
    "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);

  factory.registerNodeType<toid::EndCalibNode>(
    "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);

  factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));

  factory.registerNodeType<toid::SetParameterNode>(
    "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));

  factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));

  factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));

  BT::RosNodeParams service_params1(nh, "/sequence1");
  service_params1.server_timeout = std::chrono::seconds(15);
  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);

  BT::RosNodeParams service_params2(nh, "/sequence2");
  service_params2.server_timeout = std::chrono::seconds(20);
  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);

  BT::RosNodeParams service_params3(nh, "/sequence3");
  service_params3.server_timeout = std::chrono::seconds(15);
  factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);

  factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);

  factory.registerNodeType<toid::InPositionNode>("InPose", get_acorn_pose);

  std::cout << describeCustomNodes() << std::endl;
}

void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)
{
  nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_);
}

void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; }

void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
{
  auto node = this->node();
  try {
    acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint");
  } catch (const tf2::TransformException & e) {
    RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout");
  }
}

void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg)
{
  this->to_flip_ = msg->data;
}

std::optional<std::string> TreeExecutor::onTreeExecutionCompleted(
  BT::NodeStatus status, bool was_cancelled)
{
  (void)status;
  logger_cout_.reset();

  if (was_cancelled) {
    std::cout << "Behavior tree was cancled" << std::endl;
  }

  return std::nullopt;
}

std::string TreeExecutor::describeCustomNodes() { return BT::writeTreeNodesModelXML(factory()); }

}  // namespace toid

int main(const int argc, const char * const * argv)
{
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  auto bt_exec_server = std::make_shared<toid::TreeExecutor>(options);

  rclcpp::executors::MultiThreadedExecutor executor(
    rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));

  executor.add_node(bt_exec_server->node());
  executor.spin();
  executor.remove_node(bt_exec_server->node());

  rclcpp::shutdown();
  return 0;
}