#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "robossembler_interfaces/action/gripper_command.hpp"
#include "gazebo_ros_link_attacher/srv/attach.hpp"
#include "sensor_msgs/msg/joint_state.hpp"

using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
double finger_state{0};
namespace robossembler_servers
{
class GripperCmd : public rclcpp::Node
{
public:

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

        this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
                                        ("/rasmt_hand_controller/commands", 10);
        this->attach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("attach");
        this->detach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("detach");

        this->joint_states_ = this->create_subscription<sensor_msgs::msg::JointState>
                                ("/joint_states", 10, std::bind(&robossembler_servers::GripperCmd::get_fingers_state, this, _1));
    }

private:
    rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
    rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr attach_srv_;
    rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr detach_srv_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_;
    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;

    //double finger_state{};

    rclcpp_action::GoalResponse goal_callback(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const GripperCmdAction::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request [%.3f]", goal->value);
        (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(&robossembler_servers::GripperCmd::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 feedback = std::make_shared<GripperCmdAction::Feedback>();
        auto result = std::make_shared<GripperCmdAction::Result>();
        if (goal_handle->is_canceling()) {
            goal_handle->canceled(result);
            RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
            return;
        }
        // Compute finger change for detach or attach
        auto finger_state_change = goal->value - finger_state;
        auto frame_name = goal->frame;
        RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
        if(finger_state_change<0.0)
        {
            send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
        } else if (finger_state_change>0.0)
        {
            send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
        }
        // Send commands to gripper
        std_msgs::msg::Float64MultiArray commands;
        RCLCPP_INFO(this->get_logger(), "Initialize commands");
        commands.data.push_back(goal->value);
        commands.data.push_back(goal->value);
        //commands.data[0] = goal->value;
        //commands.data[1] = goal->value;
        RCLCPP_INFO(this->get_logger(), "Publish commands");
        this->gripper_cmd_publisher_->publish(commands);

        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
    }

    void send_attach_goal(const std::string model1_name, std::string model2_name,
                            std::string model1_link_name, std::string model2_link_name)
    {
        auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
        request->model_name_1 = model1_name;
        request->model_name_2 = model2_name;
        request->link_name_1 = model1_link_name;
        request->link_name_2 = model2_link_name;

        while (!attach_srv_->wait_for_service(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
        }

        auto result = attach_srv_->async_send_request(request);
    }

    void send_detach_goal(const std::string model1_name, std::string model2_name,
                            std::string model1_link_name, std::string model2_link_name)
    {
        auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
        request->model_name_1 = model1_name;
        request->model_name_2 = model2_name;
        request->link_name_1 = model1_link_name;
        request->link_name_2 = model2_link_name;

        while (!detach_srv_->wait_for_service(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
        }

        auto result = detach_srv_->async_send_request(request);
    }

    void get_fingers_state(const sensor_msgs::msg::JointState::SharedPtr msg) const
    {
        finger_state = msg->position.back();
        //RCLCPP_INFO(this->get_logger(), "Current joint position [%.5f]", joint_state);
    }

};
}  // namespace robossembler_servers

RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_servers::GripperCmd)