#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/action/moveit_send_joint_states.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 MoveToJointStateActionServer : public rclcpp::Node
{
public:
    using MoveitSendJointStates = robossembler_interfaces::action::MoveitSendJointStates;

    //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
    explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_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(&MoveToPoseActionServer::goal_callback, this, _1, _2),
        //     std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
        //     std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
    }

    void init()
    {
        
        action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
            node_->get_node_base_interface(),
            node_->get_node_clock_interface(),
            node_->get_node_logging_interface(),
            node_->get_node_waitables_interface(),
            "move_to_joint_states",
            std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1),
            std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1)
        );

    }

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

    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;

    rclcpp_action::GoalResponse goal_callback(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const MoveitSendJointStates::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str());
        (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(&MoveToJointStateActionServer::execute, this, _1), 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<MoveitSendJointStates::Result>();

        moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);

        const moveit::core::JointModelGroup* joint_model_group = 
                move_group_interface.getCurrentState()->getJointModelGroup(goal->robot_name);
        moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(10);

        std::vector<double> joint_group_positions;
        current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

        joint_group_positions[0] = goal->joint_value;
        joint_group_positions[1] = goal->joint_value;
        move_group_interface.setJointValueTarget(joint_group_positions);
        move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor);
        move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor);


        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(), "Action goal canceled");
            return;
        }
    
    
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
    }
}; // class MoveToJointStateActionServer

}// namespace robossembler_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_topose", "", node_options);

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

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

    return 0;
}