#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/move_coords_action.hpp"
#include "toid_bt/plugins/rotate_action.hpp"
#include "toid_bt/plugins/rotate_towards_action.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_);
}

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

void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
{
  PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); };
  rclcpp::Node::SharedPtr nh = node();
  factory.registerNodeType<toid::MovePointNode>(
    "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);

  factory.registerNodeType<toid::RotateNode>(
    "RotateSimple", BT::RosNodeParams(nh, "/rotate"), 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::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));

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

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

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

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;
}