#include "robotiq_2f_controllers/robotiq_gripper_command_controller.hpp"


using config_type = controller_interface::interface_configuration_type;

namespace robotiq_2f_controllers
{
RobotiqGripperCommandController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
{
    joint_name_ = auto_declare<std::string>("joint", joint_name_);
    max_position_ = auto_declare<double>("max_gap", max_position_);
    command_interface_types_ =
    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
    state_interface_types_ =
    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);


    if (joint_name_.empty()) {
        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint' parameter.");
        return CallbackReturn::ERROR;
    }
    else{
        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
    }

    return CallbackReturn::SUCCESS;
}

controller_interface::InterfaceConfiguration RobotiqGripperCommandController::command_interface_configuration() const
{
    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");

    controller_interface::InterfaceConfiguration config;
    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

    for (const auto& interface_map_entry : command_interface_map_) {
        const std::string& interface_type = interface_map_entry.first;
        config.names.push_back(joint_name_ + "/" + interface_type);
    }
    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
    return config;
}


controller_interface::InterfaceConfiguration RobotiqGripperCommandController::state_interface_configuration() const
{
    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of state interfaces.");

    controller_interface::InterfaceConfiguration config;
    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

    for (const auto& interface_map_entry : state_interface_map_) {
        const std::string& interface_type = interface_map_entry.first;
        config.names.push_back(joint_name_ + "/" + interface_type);
    }

    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of state interfaces.");
    return config;
}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
{
    action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
        get_node(),
        "~/gripper_command",
        [this](const std::array<unsigned char, 16>& uuid, std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
            return this->handle_goal(uuid, goal);
        },
        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {return this->handle_cancel(goal_handle);},
        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);}
    );
    return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State & )
{
    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
    for (const auto& interface : command_interfaces_) {
        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
    }

    // clear out vectors in case of restart
    joint_position_command_interface_.clear();
    max_velocity_command_interface_.clear();
    max_effort_command_interface_.clear();
    joint_position_state_interface_.clear();

    // assign command interfaces
    for (auto & interface : command_interfaces_)
    {
        command_interface_map_[interface.get_interface_name()]->push_back(interface);
    }

    // assign state interface
        for (auto & interface : state_interfaces_)
    {
        state_interface_map_[interface.get_interface_name()]->push_back(interface);
    }

    RCLCPP_DEBUG(get_node()->get_logger(), "Checking if command interfaces are initialized corretly...");
    if (!command_interface_checker_())
    {
        return CallbackReturn::ERROR;
    }
    RCLCPP_DEBUG(get_node()->get_logger(), "Command interfaces correctly initialized!");

    return CallbackReturn::SUCCESS;
}

controller_interface::return_type RobotiqGripperCommandController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
    // Check if there is an active goal
    if (current_goal_handle_) {

        // Retrieve the current position of the gripper
        try
        {
            if (joint_position_state_interface_.empty())
            {
                throw std::runtime_error("State interface is not correctly initialized");
            }

            current_position_ = joint_position_state_interface_[0].get().get_value();
        } catch (const std::exception& e) {
            RCLCPP_FATAL(get_node()->get_logger(), "Failed to access state interface: %s", e.what());
            return controller_interface::return_type::ERROR;
        }


        // Check if the gripper is stalled
        if (is_stalled(current_position_)) {
            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
            result->final_position = current_position_;
            result->reached_goal = false;
            result->stalled = true;
            current_goal_handle_->abort(result);
            current_goal_handle_.reset();  // Clear the current goal handle
            process_next_goal();  // Process the next goal, if any
        }

        // Check if the gripper has stopped after a cancel request
        if (cancel_requested_ && is_stopped()) {
        auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
        result->final_position = current_position_;
        result->reached_goal = false;
        result->stalled = false;
        current_goal_handle_->canceled(result);  // Notify that the goal was canceled successfully
        cancel_requested_ = false;  // Reset the cancel requested flag
        current_goal_handle_.reset();  // Clear the current goal handle
        process_next_goal();  // Proceed to the next goal if available

        RCLCPP_INFO(get_node()->get_logger(), "Gripper Action was canceled and motion has stopped!");
        }

        // Publish feedback about the current position
        publish_feedback();

        // Check if the target position is reached within a tolerance
        if (std::abs(current_position_ - target_position_) < POSITION_TOLERANCE) {
            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
            result->final_position = current_position_;
            result->reached_goal = true;
            result->stalled = false;
            current_goal_handle_->succeed(result);
            current_goal_handle_.reset();  // Clear the current goal handle
            process_next_goal();  // Process the next goal, if any
            target_position_ = -1.0;
        }
    }

    return controller_interface::return_type::OK;
}


controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State & )
{
    RCLCPP_INFO(get_node()->get_logger(), "Deactivating and releasing interfaces.");
    release_interfaces();  // Ensure this function properly cleans up resources
    joint_position_command_interface_.clear();
    max_velocity_command_interface_.clear();
    max_effort_command_interface_.clear();
    joint_position_state_interface_.clear();
    return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State & )
{
  return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State & )
{
    RCLCPP_ERROR(get_node()->get_logger(), "An error occurred, attempting to reset the controller");

    // Attempt to stop any ongoing gripper motion as a safety precaution
    try {
        stop_motion(joint_position_state_interface_[0].get().get_value());  // Assume stop_motion sets the gripper commands to a safe state
    } catch (const std::exception& e) {
        RCLCPP_FATAL(get_node()->get_logger(), "Failed to stop the gripper during error handling: %s", e.what());
        return controller_interface::CallbackReturn::ERROR;  // Fail if stopping motion fails
    }

    // Log that the controller is attempting to reactivate
    RCLCPP_INFO(get_node()->get_logger(), "Attempting to reactivate the controller");

    // Directly call the on_activate method to reset and reactivate the controller
    return CallbackReturn::SUCCESS;
}

controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State & )
{
  return CallbackReturn::SUCCESS;
}

// Action Handling
rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal)
{
    (void)goal_uuid;  // Suppress unused variable warning
    RCLCPP_INFO(get_node()->get_logger(), "Received goal request with position: %f", goal->desired_position);

    if (!is_valid_goal(goal)) {
        RCLCPP_ERROR(get_node()->get_logger(), "Goal is invalid");
        return rclcpp_action::GoalResponse::REJECT;
    }

    return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}

rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle)
{
    if (current_goal_handle_ == goal_handle) {
        RCLCPP_INFO(get_node()->get_logger(), "Received request to cancel goal");
        stop_motion(joint_position_state_interface_[0].get().get_value());  // Attempt to stop the gripper immediately
        cancel_requested_ = true;  // Set a flag indicating cancellation has been requested
        return rclcpp_action::CancelResponse::ACCEPT;  // Accept the cancellation, pending completion
    }
    return rclcpp_action::CancelResponse::REJECT;  // Reject cancellation if it's not the current goal
}

void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle)
{

    goal_queue_.push(goal_handle);
    if (!current_goal_handle_) {  // Only start a new goal if one isn't already active
        process_next_goal();
    }
}

void RobotiqGripperCommandController::publish_feedback()
{
    if (!current_goal_handle_) {
        RCLCPP_ERROR(get_node()->get_logger(), "Goal handle is null, cannot publish feedback.");
        return;
    }

    if (current_goal_handle_->is_canceling()) {
        RCLCPP_WARN(get_node()->get_logger(), "Goal is being canceled, not publishing feedback.");
        return;
    }

    auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
    feedback->current_position = current_position_;

    try {
        current_goal_handle_->publish_feedback(feedback);
    } catch (const std::exception& e) {
        RCLCPP_FATAL(get_node()->get_logger(), "Failed to publish feedback: %s", e.what());
    }
}

void RobotiqGripperCommandController::process_next_goal()
{
    std::lock_guard<std::mutex> lock(mutex_);
    if (goal_queue_.empty()) {
        current_goal_handle_.reset();
        RCLCPP_DEBUG(get_node()->get_logger(), "No more goals to process.");
        return;
    }
    current_goal_handle_ = goal_queue_.front();
    goal_queue_.pop();
    current_goal_handle_->execute();

    if (current_goal_handle_) {
        auto goal = current_goal_handle_->get_goal();
        if (!goal) {
            RCLCPP_ERROR(get_node()->get_logger(), "Received a null goal in queue, skipping...");
            process_next_goal();  // Recursively handle the next goal
        }
        else {
            try {
                auto goal = current_goal_handle_->get_goal();
                target_position_ = goal->desired_position;
                double desired_max_velocity = goal->max_velocity;
                double desired_max_effort = goal->max_effort;

                if (!command_interface_checker_())
                {
                    throw std::runtime_error("At least one interface is not initialized!");
                }

                joint_position_command_interface_[0].get().set_value(target_position_);
                max_velocity_command_interface_[0].get().set_value(desired_max_velocity);
                max_effort_command_interface_[0].get().set_value(desired_max_effort);
            }
            catch (const std::exception& e) {
                RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what());
            }
        }
    }
}

// Utility
bool RobotiqGripperCommandController::command_interface_checker_()
{
    if (joint_position_command_interface_.empty()) {
        RCLCPP_FATAL(get_node()->get_logger(), "No position command interfaces have been configured.");
        return false;
    } else {
        RCLCPP_DEBUG(get_node()->get_logger(), "Position command interfaces have been successfully initialized.");
    }

    if (max_velocity_command_interface_.empty()) {
        RCLCPP_FATAL(get_node()->get_logger(), "No velocity command interfaces have been configured.");
        return false;
    } else {
        RCLCPP_DEBUG(get_node()->get_logger(), "Velocity command interfaces have been successfully initialized.");
    }

    if (max_effort_command_interface_.empty()) {
        RCLCPP_FATAL(get_node()->get_logger(), "No effort command interfaces have been configured.");
        return false;
    } else {
        RCLCPP_DEBUG(get_node()->get_logger(), "Effort command interfaces have been successfully initialized.");
    }

    if (joint_position_state_interface_.empty()) {
        RCLCPP_FATAL(get_node()->get_logger(), "No position state interfaces have been configured.");
        return false;
    } else {
        RCLCPP_DEBUG(get_node()->get_logger(), "Position stateinterfaces have been successfully initialized.");
    }

    return true;
}

bool RobotiqGripperCommandController::is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
    // Add your validation logic here
    return goal->desired_position >= 0 && goal->desired_position <= max_position_;
}

bool RobotiqGripperCommandController::is_stalled(double current_position)
{
    std::lock_guard<std::mutex> lock(mutex_);
    if (std::abs(current_position - last_position_) < POSITION_TOLERANCE) {
        stall_counter_++;
    } else {
        stall_counter_ = 0;  // Reset the stall counter if there is movement
    }
    last_position_ = current_position;

    return stall_counter_ > MAX_STALL_COUNT;
}

bool RobotiqGripperCommandController::stop_motion(double current_position)
{
    joint_position_command_interface_[0].get().set_value(current_position);
    max_velocity_command_interface_[0].get().set_value(MIN_SPEED);
    max_effort_command_interface_[0].get().set_value(MIN_EFFORT);

    return true;
}

bool RobotiqGripperCommandController::is_stopped()
{
    double current_position = joint_position_state_interface_[0].get().get_value();
    bool stopped = std::abs(current_position - last_position_) < POSITION_TOLERANCE;
    last_position_ = current_position;
    return stopped;
}


}

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqGripperCommandController, controller_interface::ControllerInterface)

