#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 "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
#include "rbs_skill_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 "moveit/robot_model_loader/robot_model_loader.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 rbs_skill_actions
{

class MoveCartesianActionServer : public rclcpp::Node
{
public:
    using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;

    //explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node) 
    explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node)
    {
        // using namespace std::placeholders;
        // this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
        //     this->get_node_base_interface(),
        //     this->get_node_clock_interface(),
        //     this->get_node_logging_interface(),
        //     this->get_node_waitables_interface(),
        //     "move_topose",
        //     std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
        //     std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
        //     std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
    }

    void init()
    {
        
        action_server_ = rclcpp_action::create_server<MoveitSendPose>(
            node_->get_node_base_interface(),
            node_->get_node_clock_interface(),
            node_->get_node_logging_interface(),
            node_->get_node_waitables_interface(),
            "move_cartesian",
            std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1),
            std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_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.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z,
            goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z, 
            goal->target_pose.orientation.w);
        (void)uuid;

        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(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
        // std::thread(
        //     [this, goal_handle]() {
        //         execute(goal_handle);
        //     }).detach();
    }
    
    void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing action goal");
        const auto goal = goal_handle->get_goal();
        auto result = std::make_shared<MoveitSendPose::Result>();
        
        moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
        move_group_interface.startStateMonitor();

        moveit::core::RobotState start_state(*move_group_interface.getCurrentState());

        std::vector<geometry_msgs::msg::Pose> waypoints;
        auto current_pose = move_group_interface.getCurrentPose();
        //waypoints.push_back(current_pose.pose);
        geometry_msgs::msg::Pose target_pose = goal->target_pose;
        //target_pose.position = goal->target_pose.position;
        waypoints.push_back(target_pose);

        RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]", 
        target_pose.position.x, target_pose.position.y, target_pose.position.z);
        //waypoints.push_back(start_pose.pose);
        moveit_msgs::msg::RobotTrajectory trajectory;
        const double jump_threshold = 0.0;
        const double eef_step = 0.001;
        double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        if(fraction>0)
        {
            RCLCPP_INFO(this->get_logger(), "Planning success");
            moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory);
            if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS)
            {
                goal_handle->succeed(result);
                RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
            }else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE)
            {
                RCLCPP_ERROR(this->get_logger(), "Failure in move:");
            }
            
            //move_group_interface.move();
        }else{
            RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
            goal_handle->abort(result);
        }

        if (goal_handle->is_canceling()) {
            goal_handle->canceled(result);
            RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
            return;
        }
    }
}; // class MoveCartesianActionServer

}// namespace rbs_skill_actions

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions node_options;
    node_options.automatically_declare_parameters_from_overrides(true);
    auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);

    rbs_skill_actions::MoveCartesianActionServer server(node);
    std::thread run_server([&server]() {
        rclcpp::sleep_for(std::chrono::seconds(3));
        server.init();
    });

    rclcpp::spin(node);
    run_server.join();

    return 0;
}