#include <functional>
#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"

// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
#include "robossembler_interfaces/action/moveit_send_pose.hpp"

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform.hpp>

// moveit libs
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_interface/planning_interface.h>
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
//#include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"

namespace robossembler_actions
{

class MoveToPoseActionServer : public rclcpp::Node
{
public:
    using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;

    explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 
    : Node("move_topose_action_server", options)
    {
        using namespace std::placeholders;
        this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
            this,
            "MoveToPoseActionServer",
            std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
            std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
            std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
    }

private:
    rclcpp::Node::SharedPtr node_;
    rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;

    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
    rclcpp_action::GoalResponse goal_callback(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const MoveitSendPose::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", 
            goal->robot_name, goal->target_pose.pose.position.x, goal->target_pose.pose.position.y, goal->target_pose.pose.position.z,
            goal->target_pose.pose.orientation.x, goal->target_pose.pose.orientation.y, goal->target_pose.pose.orientation.z, 
            goal->target_pose.pose.orientation.w);
        (void)uuid;
        if (false) {
            return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    
    rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received cancel request");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    
    void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        using namespace std::placeholders;
        std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach();
    }
    
    void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        const auto goal = goal_handle->get_goal();
        auto result = std::make_shared<MoveitSendPose::Result>();

        moveit::planning_interface::MoveGroupInterface::Options move_group_options(goal->robot_name, "robot_description");
        moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
        moveit::core::RobotState start_state(*move_group_interface.getCurrentState());

        move_group_interface.setStartState(start_state);
        move_group_interface.setPoseTarget(goal->target_pose.pose);
        move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
        move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);

        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        if(success)
        {
            RCLCPP_INFO(this->get_logger(), "Planning success");
            move_group_interface.execute(plan);
            move_group_interface.move();
        }else{
            RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
        }

        if (goal_handle->is_canceling()) {
            goal_handle->canceled(result);
            RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
            return;
        }
    
    
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
    }
}; // class MoveToPoseActionServer

}// namespace robossembler_actions

RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)