// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP_ACTION__SERVER_HPP_
#define RCLCPP_ACTION__SERVER_HPP_

#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "action_msgs/srv/cancel_goal.hpp"
#include "rcl/event_callback.h"
#include "rcl_action/action_server.h"
#include "rosidl_runtime_c/action_type_support_struct.h"
#include "rosidl_typesupport_cpp/action_type_support.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/waitable.hpp"

#include "rclcpp_action/visibility_control.hpp"
#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp_action/types.hpp"

namespace rclcpp_action
{
// Forward declaration
class ServerBaseImpl;

/// A response returned by an action server callback when a goal is requested.
enum class GoalResponse : int8_t
{
  /// The goal is rejected and will not be executed.
  REJECT = 1,
  /// The server accepts the goal, and is going to begin execution immediately.
  ACCEPT_AND_EXECUTE = 2,
  /// The server accepts the goal, and is going to execute it later.
  ACCEPT_AND_DEFER = 3,
};

/// A response returned by an action server callback when a goal has been asked to be canceled.
enum class CancelResponse : int8_t
{
  /// The server will not try to cancel the goal.
  REJECT = 1,
  /// The server has agreed to try to cancel the goal.
  ACCEPT = 2,
};

/// Base Action Server implementation
/// \internal
/**
 * This class should not be used directly by users writing an action server.
 * Instead users should use `rclcpp_action::Server`.
 *
 * Internally, this class is responsible for interfacing with the `rcl_action` API.
 */
class ServerBase : public rclcpp::Waitable
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServerBase)

  /// Enum to identify entities belonging to the action server
  enum class EntityType : std::size_t
  {
    GoalService,
    ResultService,
    CancelService,
    Expired,
  };

  RCLCPP_ACTION_PUBLIC
  virtual ~ServerBase();

  // -------------
  // Waitables API

  /// Return the number of subscriptions used to implement an action server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_subscriptions() override;

  /// Return the number of timers used to implement an action server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_timers() override;

  /// Return the number of service clients used to implement an action server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_clients() override;

  /// Return the number of service servers used to implement an action server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_services() override;

  /// Return the number of guard conditions used to implement an action server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_guard_conditions() override;

  /// Add all entities to a wait set.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  add_to_wait_set(rcl_wait_set_t & wait_set) override;

  /// Return true if any entity belonging to the action server is ready to be executed.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  bool
  is_ready(const rcl_wait_set_t & wait_set) override;

  RCLCPP_ACTION_PUBLIC
  std::shared_ptr<void>
  take_data() override;

  RCLCPP_ACTION_PUBLIC
  std::shared_ptr<void>
  take_data_by_entity_id(size_t id) override;

  /// Act on entities in the wait set which are ready to be acted upon.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  execute(const std::shared_ptr<void> & data) override;

  /// \internal
  /// Set a callback to be called when action server entities have an event
  /**
   * The callback receives a size_t which is the number of messages received
   * since the last time this callback was called.
   * Normally this is 1, but can be > 1 if messages were received before any
   * callback was set.
   *
   * The callback also receives an int identifier argument, which identifies
   * the action server entity which is ready.
   * This implies that the provided callback can use the identifier to behave
   * differently depending on which entity triggered the waitable to become ready.
   *
   * Calling it again will clear any previously set callback.
   *
   * An exception will be thrown if the callback is not callable.
   *
   * This function is thread-safe.
   *
   * If you want more information available in the callback, like the subscription
   * or other information, you may use a lambda with captures or std::bind.
   *
   * \param[in] callback functor to be called when a new message is received.
   */
  RCLCPP_ACTION_PUBLIC
  void
  set_on_ready_callback(std::function<void(size_t, int)> callback) override;

  /// Unset the callback to be called whenever the waitable becomes ready.
  RCLCPP_ACTION_PUBLIC
  void
  clear_on_ready_callback() override;

  /// Returns all timers used by this waitable
  RCLCPP_ACTION_PUBLIC
  std::vector<std::shared_ptr<rclcpp::TimerBase>>
  get_timers() const override;

  // End Waitables API
  // -----------------

  /// Configure action server introspection
  /**
   * \param[in] clock clock to use to generate introspection timestamps
   * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher
   * \param[in] introspection_state the state to set introspection to
   *
   * \throw std::invalid_argument if clock is nullptr
   * \throw rclcpp::exceptions::throw_from_rcl_error if rcl error occurs.
   */
  RCLCPP_ACTION_PUBLIC
  void
  configure_introspection(
    rclcpp::Clock::SharedPtr clock,
    const rclcpp::QoS & qos_service_event_pub,
    rcl_service_introspection_state_t introspection_state);

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_goal_handles();

protected:
  RCLCPP_ACTION_PUBLIC
  ServerBase(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    const std::string & name,
    const rosidl_action_type_support_t * type_support,
    const rcl_action_server_options_t & options);

  // -----------------------------------------------------
  // API for communication between ServerBase and Server<>

  // ServerBase will call this function when a goal request is received.
  // The subclass should convert to the real type and call a user's callback.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  std::pair<GoalResponse, std::shared_ptr<void>>
  call_handle_goal_callback(GoalUUID &, std::shared_ptr<void> request) = 0;

  // ServerBase will determine which goal ids are being cancelled, and then call this function for
  // each goal id.
  // The subclass should look up a goal handle and call the user's callback.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  CancelResponse
  call_handle_cancel_callback(const GoalUUID & uuid) = 0;

  /// Given a goal request message, return the UUID contained within.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  GoalUUID
  get_goal_id_from_goal_request(void * message) = 0;

  /// Create an empty goal request message so it can be taken from a lower layer.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_goal_request() = 0;

  /// Call user callback to inform them a goal has been accepted.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  void
  call_goal_accepted_callback(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
    GoalUUID uuid, std::shared_ptr<void> goal_request_message) = 0;

  /// Given a result request message, return the UUID contained within.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  GoalUUID
  get_goal_id_from_result_request(void * message) = 0;

  /// Create an empty goal request message so it can be taken from a lower layer.
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_result_request() = 0;

  /// Create an empty goal result message so it can be sent as a reply in a lower layer
  /// \internal
  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  publish_status();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  notify_goal_terminal_state();

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg);

  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  publish_feedback(std::shared_ptr<void> feedback_msg);

  // End API for communication between ServerBase and Server<>
  // ---------------------------------------------------------

private:
  /// Handle a request to add a new goal to the server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  execute_goal_request_received(
    rcl_ret_t ret,
    rcl_action_goal_info_t goal_info,
    rmw_request_id_t request_header,
    std::shared_ptr<void> message);

  /// Handle a request to cancel goals on the server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  execute_cancel_request_received(
    rcl_ret_t ret,
    std::shared_ptr<action_msgs::srv::CancelGoal::Request> request,
    rmw_request_id_t request_header);

  /// Handle a request to get the result of an action
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  execute_result_request_received(
    rcl_ret_t ret,
    std::shared_ptr<void> result_request,
    rmw_request_id_t request_header);

  /// Handle a timeout indicating a completed goal should be forgotten by the server
  /// \internal
  RCLCPP_ACTION_PUBLIC
  void
  execute_check_expired_goals();

  /// Private implementation
  /// \internal
  std::unique_ptr<ServerBaseImpl> pimpl_;

  /// Set a std::function callback to be called when the specified entity is ready
  RCLCPP_ACTION_PUBLIC
  void
  set_callback_to_entity(
    EntityType entity_type,
    std::function<void(size_t, int)> callback);

protected:
  // Mutex to protect the callbacks storage.
  std::recursive_mutex listener_mutex_;
  // Storage for std::function callbacks to keep them in scope
  std::unordered_map<EntityType, std::function<void(size_t)>> entity_type_to_on_ready_callback_;

  /// Set a callback to be called when the specified entity is ready
  RCLCPP_ACTION_PUBLIC
  void
  set_on_ready_callback(
    EntityType entity_type,
    rcl_event_callback_t callback,
    const void * user_data);

  bool on_ready_callback_set_{false};
};

/// Action Server
/**
 * This class creates an action server.
 *
 * Create an instance of this server using `rclcpp_action::create_server()`.
 *
 * Internally, this class is responsible for:
 *  - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and
 *  - calling user callbacks.
 */
template<typename ActionT>
class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server)

  /// Signature of a callback that accepts or rejects goal requests.
  using GoalCallback = std::function<GoalResponse(
        const GoalUUID &,
        std::shared_ptr<const typename ActionT::Goal>)>;
  /// Signature of a callback that accepts or rejects requests to cancel a goal.
  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  /// Signature of a callback that is used to notify when the goal has been accepted.
  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;

  /// Construct an action server.
  /**
   * This constructs an action server, but it will not work until it has been added to a node.
   * Use `rclcpp_action::create_server()` to both construct and add to a node.
   *
   * Three callbacks must be provided:
   *  - one to accept or reject goals sent to the server,
   *  - one to accept or reject requests to cancel a goal,
   *  - one to receive a goal handle after a goal has been accepted.
   * All callbacks must be non-blocking.
   * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle`.
   *
   * \param[in] node_base a pointer to the base interface of a node.
   * \param[in] node_clock a pointer to an interface that allows getting a node's clock.
   * \param[in] node_logging a pointer to an interface that allows getting a node's logger.
   * \param[in] name the name of an action.
   *  The same name and type must be used by both the action client and action server to
   *  communicate.
   * \param[in] options Options to pass to the underlying `rcl_action_server_t`.
   * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected.
   * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled.
   *  The return from this callback only indicates if the server will try to cancel a goal.
   *  It does not indicate if the goal was actually canceled.
   * \param[in] handle_accepted a callback that is called to give the user a handle to the goal.
   *  execution.
   */
  Server(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    const std::string & name,
    const rcl_action_server_options_t & options,
    GoalCallback handle_goal,
    CancelCallback handle_cancel,
    AcceptedCallback handle_accepted
  )
  : ServerBase(
      node_base,
      node_clock,
      node_logging,
      name,
      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
      options),
    handle_goal_(handle_goal),
    handle_cancel_(handle_cancel),
    handle_accepted_(handle_accepted)
  {
  }

  virtual ~Server() = default;

protected:
  // -----------------------------------------------------
  // API for communication between ServerBase and Server<>

  /// \internal
  std::pair<GoalResponse, std::shared_ptr<void>>
  call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr<void> message) override
  {
    auto request = std::static_pointer_cast<
      typename ActionT::Impl::SendGoalService::Request>(message);
    auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
    GoalResponse user_response = handle_goal_(uuid, goal);

    auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::Response>();
    ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
      GoalResponse::ACCEPT_AND_DEFER == user_response;
    return std::make_pair(user_response, ros_response);
  }

  /// \internal
  CancelResponse
  call_handle_cancel_callback(const GoalUUID & uuid) override
  {
    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
    {
      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
      auto element = goal_handles_.find(uuid);
      if (element != goal_handles_.end()) {
        goal_handle = element->second.lock();
      }
    }

    CancelResponse resp = CancelResponse::REJECT;
    if (goal_handle) {
      resp = handle_cancel_(goal_handle);
      if (CancelResponse::ACCEPT == resp) {
        try {
          goal_handle->_cancel_goal();
        } catch (const rclcpp::exceptions::RCLError & ex) {
          RCLCPP_DEBUG(
            rclcpp::get_logger("rclcpp_action"),
            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
          return CancelResponse::REJECT;
        }
      }
    }
    return resp;
  }

  /// \internal
  void
  call_goal_accepted_callback(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
    GoalUUID uuid,
    std::shared_ptr<void> goal_request_message) override
  {
    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
    std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();

    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
      [weak_this](const GoalUUID & goal_uuid, std::shared_ptr<void> result_message)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        // Send result message to anyone that asked
        shared_this->publish_result(goal_uuid, result_message);
        // Publish a status message any time a goal handle changes state
        shared_this->publish_status();
        // notify base so it can recalculate the expired goal timer
        shared_this->notify_goal_terminal_state();
        // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
        std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
        shared_this->goal_handles_.erase(goal_uuid);
      };

    std::function<void(const GoalUUID &)> on_executing =
      [weak_this]([[maybe_unused]] const GoalUUID & goal_uuid)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        // Publish a status message any time a goal handle changes state
        shared_this->publish_status();
      };

    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback =
      [weak_this](std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
      };

    auto request = std::static_pointer_cast<
      const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
    auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
    goal_handle.reset(
      new ServerGoalHandle<ActionT>(
        rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
    {
      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
      goal_handles_[uuid] = goal_handle;
    }
    handle_accepted_(goal_handle);
  }

  /// \internal
  GoalUUID
  get_goal_id_from_goal_request(void * message) override
  {
    return
      static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
  }

  /// \internal
  std::shared_ptr<void>
  create_goal_request() override
  {
    return std::shared_ptr<void>(new typename ActionT::Impl::SendGoalService::Request());
  }

  /// \internal
  GoalUUID
  get_goal_id_from_result_request(void * message) override
  {
    return
      static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
  }

  /// \internal
  std::shared_ptr<void>
  create_result_request() override
  {
    return std::shared_ptr<void>(new typename ActionT::Impl::GetResultService::Request());
  }

  /// \internal
  std::shared_ptr<void>
  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
  {
    auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    result->status = status;
    return std::static_pointer_cast<void>(result);
  }

  // End API for communication between ServerBase and Server<>
  // ---------------------------------------------------------

private:
  GoalCallback handle_goal_;
  CancelCallback handle_cancel_;
  AcceptedCallback handle_accepted_;

  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
  /// A map of goal id to goal handle weak pointers.
  /// This is used to provide a goal handle to handle_cancel.
  std::unordered_map<GoalUUID, GoalHandleWeakPtr> goal_handles_;
  std::mutex goal_handles_mutex_;
};
}  // namespace rclcpp_action
#endif  // RCLCPP_ACTION__SERVER_HPP_
