#pragma once

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

namespace mg {

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

        static BT::PortsList providedPorts() {
            return providedBasicPorts({ BT::InputPort<double>("x_goal"),
                                        BT::InputPort<double>("y_goal"),
                                        BT::InputPort<double>("tolerance"),
                                        BT::InputPort<double>("max_wheel_speed"),
                                        BT::InputPort<double>("max_angular"),
                                        BT::InputPort<double>("max_vel"),
                                        BT::InputPort<double>("pos_mult"),
                                        BT::InputPort<double>("ornt_mult"),
                                        BT::InputPort<double>("t_ornt_mult"),
                                        BT::InputPort<double>("obst_mult_a"),
                                        BT::InputPort<double>("obst_mult_b"),
                                        BT::InputPort<double>("obst_mult_c"),
                                        BT::InputPort<std::string>("IgnoreList", "", {}) });
        }

        bool setGoal(Goal& goal) override {
            auto x_goal          = getInput<double>("x_goal");
            auto y_goal          = getInput<double>("y_goal");
            auto tolerance       = getInput<double>("tolerance");
            auto max_wheel_speed = getInput<double>("max_wheel_speed");
            auto max_angular     = getInput<double>("max_angular");
            auto max_vel         = getInput<double>("max_vel");
            auto pos_mult        = getInput<double>("pos_mult");
            auto ornt_mult       = getInput<double>("ornt_mult");
            auto t_ornt_mult     = getInput<double>("t_ornt_mult");
            auto obst_mult_a     = getInput<double>("obst_mult_a");
            auto obst_mult_b     = getInput<double>("obst_mult_b");
            auto obst_mult_c     = getInput<double>("obst_mult_c");
            goal.x               = x_goal.value();
            goal.y               = y_goal.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);
            goal.max_vel         = max_vel.value_or(goal.max_vel);
            goal.pos_mult        = pos_mult.value_or(goal.pos_mult);
            goal.ornt_mult       = ornt_mult.value_or(goal.ornt_mult);
            goal.t_ornt_mult     = t_ornt_mult.value_or(goal.t_ornt_mult);
            goal.obst_mult_a     = obst_mult_a.value_or(goal.obst_mult_a);
            goal.obst_mult_b     = obst_mult_b.value_or(goal.obst_mult_b);
            goal.obst_mult_c     = obst_mult_c.value_or(goal.obst_mult_c);
            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;
        }
    };

}