#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"

#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"

using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");

class MoveToPose : public BtAction<MoveToPoseAction>
{
    public:
        MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
        : BtAction<MoveToPoseAction>(name, config) 
        {
            //auto params = _node->declare_parameters("Poses")
            RCLCPP_INFO(_node->get_logger(), "Start the node");
            pose_des.position.x = 0.11724797630931184;
            pose_des.position.y = 0.46766635768602544;
            pose_des.position.z = 0.5793862343094913;
            pose_des.orientation.x = 0.9987999001314066;
            pose_des.orientation.y = 0.041553846820940925;
            pose_des.orientation.z = -0.004693314677006583;
            pose_des.orientation.w = -0.025495295825239704;
        }

        MoveToPoseAction::Goal populate_goal() override
        {
            getInput<std::string>("robot_name", robot_name_);
            getInput<std::string>("pose", pose_);

            //auto pose =  _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();

            RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
               pose_des.position.x, pose_des.position.y, pose_des.position.z,
               pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);

            auto goal = MoveToPoseAction::Goal();
            RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
            
            goal.robot_name = robot_name_;
            goal.target_pose = pose_des;
            goal.end_effector_acceleration = 1.0;
            goal.end_effector_velocity = 1.0;
            
            RCLCPP_INFO(_node->get_logger(), "Goal populated");

            return goal;
        }

        static PortsList providedPorts()
        {
            return providedBasicPorts({
                InputPort<std::string>("robot_name"),
                InputPort<std::string>("pose")
            });
        }

    private:
        std::string robot_name_;
        std::string pose_;
        geometry_msgs::msg::Pose pose_des;
        std::map<std::string, geometry_msgs::msg::Pose> Poses;


}; // class MoveToPose

BT_REGISTER_NODES(factory)
{
    factory.registerNodeType<MoveToPose>("MoveToPose");
}