#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) 
        {
            RCLCPP_INFO(_node->get_logger(), "Start the node");
        }

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

            pose_des.position.x = pose_[0];
            pose_des.position.y = pose_[1];
            pose_des.position.z = pose_[2];
            pose_des.orientation.x = pose_[3];
            pose_des.orientation.y = pose_[4];
            pose_des.orientation.z = pose_[5];
            pose_des.orientation.w = pose_[6];
            

            RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t 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 = 0.5;
            goal.end_effector_velocity = 0.5;
            
            RCLCPP_INFO(_node->get_logger(), "Goal populated");

            return goal;
        }

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

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

        // geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){

        // }


}; // class MoveToPose

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