#pragma once

#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/rotate.hpp"

namespace mg {

    class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
      public:
        RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
          BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }

        static BT::PortsList providedPorts() {
            return providedBasicPorts({
              BT::InputPort<double>("angle"),
              BT::InputPort<double>("tolerance", 0.001, {}),
              BT::InputPort<double>("max_wheel_speed", 3, {}),
              BT::InputPort<double>("max_angular", 3.14, {}),
            });
        }

        bool setGoal(Goal& goal) override {
            auto angle           = getInput<double>("angle");
            auto tolerance       = getInput<double>("tolerance");
            auto max_wheel_speed = getInput<double>("max_wheel_speed");
            auto max_angular     = getInput<double>("max_angular");
            goal.angle           = angle.value();
            goal.tolerance       = tolerance.value_or(goal.tolerance);
            goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
            goal.max_angular     = max_angular.value_or(goal.max_angular);
            return true;
        }

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

        BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
            return (wr.result->error == ActionType::Result::SUCCESS) ? 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;
        }
    };

}