#pragma once

#include "angles/angles.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "tf2/utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "toid_bt/plugin.hpp"
#include "toid_msgs/action/simple_move_coords.hpp"

namespace toid
{

class MovePointNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
{
public:
  MovePointNode(
    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
    PoseFunc pose)
  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
  {
    std::cout << "wtf? " << params.default_port_value << std::endl;
  }

  static BT::PortsList providedPorts()
  {
    return providedBasicPorts({
      BT::InputPort<double>("x"),
      BT::InputPort<double>("y"),
      BT::InputPort<double>("theta"),
      BT::InputPort<double>("max_speed", 0, {}),
      //BT::InputPort<double>("options"),
    });
  }

  bool setGoal(Goal & goal) override
  {
    auto x_goal = getInput<double>("x");
    auto y_goal = getInput<double>("y");
    auto theta = getInput<double>("theta");
    auto max_speed = getInput<double>("max_speed").value();

    goal.x = x_goal.value();
    goal.y = y_goal.value();
    if (!theta.has_value()) {
      geometry_msgs::msg::PoseStamped pose;
      get_pose(pose);
      goal.theta = tf2::getYaw(pose.pose.orientation);
    } else {
      goal.theta = angles::from_degrees(theta.value_or(0));
    }

    goal.max_speed = max_speed;

    return true;
  }

  void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }

  BT::NodeStatus onResultReceived(const WrappedResult & wr) override
  {
    return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
  }

  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
  {
    RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
    return BT::NodeStatus::FAILURE;
  }

  PoseFunc get_pose;
};

}  // namespace toid