// 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__CLIENT_HPP_
#define RCLCPP_ACTION__CLIENT_HPP_

#include <algorithm>
#include <chrono>
#include <functional>
#include <future>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "rcl/event_callback.h"

#include "rclcpp/exceptions.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/waitable.hpp"

#include "rosidl_runtime_c/action_type_support_struct.h"
#include "rosidl_typesupport_cpp/action_type_support.hpp"

#include "rclcpp_action/client_base.hpp"
#include "rclcpp_action/client_goal_handle.hpp"
#include "rclcpp_action/exceptions.hpp"
#include "rclcpp_action/types.hpp"
#include "rclcpp_action/visibility_control.hpp"

namespace rclcpp_action
{
/// Action Client
/**
 * This class creates an action client.
 *
 * To create an instance of an action client use `rclcpp_action::create_client()`.
 *
 * Internally, this class is responsible for:
 *  - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and
 *  - calling user callbacks.
 */
template<typename ActionT>
class Client : public ClientBase
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client<ActionT>)

  using Goal = typename ActionT::Goal;
  using Feedback = typename ActionT::Feedback;
  using GoalHandle = ClientGoalHandle<ActionT>;
  using WrappedResult = typename GoalHandle::WrappedResult;
  using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
  using FeedbackCallback = typename GoalHandle::FeedbackCallback;
  using ResultCallback = typename GoalHandle::ResultCallback;
  using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
  using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
  using CancelCallback = std::function<void (typename CancelResponse::SharedPtr)>;

  /// Options for sending a goal.
  /**
   * This struct is used to pass parameters to the function `async_send_goal`.
   */
  struct SendGoalOptions
  {
    SendGoalOptions()
    : goal_response_callback(nullptr),
      feedback_callback(nullptr),
      result_callback(nullptr)
    {
    }

    /// Function called when the goal is accepted or rejected.
    /**
     * Takes a single argument that is a goal handle shared pointer.
     * If the goal is accepted, then the pointer points to a valid goal handle.
     * If the goal is rejected, then pointer has the value `nullptr`.
     */
    GoalResponseCallback goal_response_callback;

    /// Function called whenever feedback is received for the goal.
    FeedbackCallback feedback_callback;

    /// Function called when the result for the goal is received.
    ResultCallback result_callback;
  };

  /// Construct an action client.
  /**
   * This constructs an action client, but it will not work until it has been added to a node.
   * Use `rclcpp_action::create_client()` to both construct and add to a node.
   *
   * When multiple action clients connect to the same action server, the subscription for receiving
   * feedback messages inside each action client will first receive all feedback messages, and then
   * determine which feedback belongs to itself based on goal ID. When
   * enable_feedback_msg_optimization is set to true, the content filter is used to configure
   * the goal ID for the subscription, which helps avoid the reception of irrelevant feedback
   * messages internally for each action client.
   *
   * If enable_feedback_msg_optimization is set to true, an action client can handle up to 6 goals
   * simultaneously. This optimization takes advantage of the content filter feature. According to
   * the DDS specification, the maximum number of parameters supported by content filter is 100.
   * Configuring one goal ID consumes 16 parameters, so at most, 6 goal IDs can be set
   * simultaneously. If the number of goals exceeds the limit, optimization is automatically
   * disabled. If rmw implementation doesn't support content filter, optimization is also
   * automatically disabled.
   *
   * Even if the RMW implementation supports the content filter feature, different RMW
   * implementations may impose restrictions on the content filter expression. For example,
   * in ConnextDDS, the `contentfilter_property_max_length` setting affects the available length
   * of content filter expressions. When this limit is reached, the optimization will be
   * automatically disabled.
   *
   * \param[in] node_base A pointer to the base interface of a node.
   * \param[in] node_graph A pointer to an interface that allows getting graph information about
   *   a node.
   * \param[in] node_logging A pointer to an interface that allows getting a node's logger.
   * \param[in] action_name The action name.
   * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`.
   * \param[in] enable_feedback_msg_optimization Enable feedback subscription content filter to
   *   optimize the handling of feedback messages.
   */
  Client(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    const std::string & action_name,
    const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(),
    bool enable_feedback_msg_optimization = false
  )
  : ClientBase(
      node_base, node_graph, node_logging, action_name,
      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
      client_options, enable_feedback_msg_optimization)
  {
  }

  /// Send an action goal and asynchronously get the result.
  /**
   * If the goal is accepted by an action server, the returned future is set to a `GoalHandle::SharedPtr`.
   * If the goal is rejected by an action server, then the future is set to a `nullptr`.
   *
   * The goal handle in the future is used to monitor the status of the goal and get the final result.
   *
   * If callbacks were set in @param options, you will receive callbacks, as long as you hold a reference
   * to the shared pointer contained in the returned future, or rclcpp_action::Client is destroyed. Dropping
   * the shared pointer to the goal handle will not cancel the goal. In order to cancel it, you must explicitly
   * call async_cancel_goal.
   *
   * WARNING this method has inconsistent behaviour and a memory leak bug.
   * If you set the result callback in @param options, the handle will be self referencing, and you will receive
   * callbacks even though you do not hold a reference to the shared pointer. In this case, the self reference will
   * be deleted if the result callback was received. If there is no result callback, there will be a memory leak.
   *
   * To prevent the memory leak, you may call stop_callbacks() explicit. This will delete the self reference.
   *
   * \param[in] goal The goal request.
   * \param[in] options Options for sending the goal request. Contains references to callbacks for
   *   the goal response (accepted/rejected), feedback, and the final result.
   * \return A future that completes when the goal has been accepted or rejected.
   *   If the goal is rejected, then the result will be a `nullptr`.
   */
  std::shared_future<typename GoalHandle::SharedPtr>
  async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions())
  {
    // Put promise in the heap to move it around.
    auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
    std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
    using GoalRequest = typename ActionT::Impl::SendGoalService::Request;
    auto goal_request = std::make_shared<GoalRequest>();
    goal_request->goal_id.uuid = this->generate_goal_id();
    goal_request->goal = goal;
    this->send_goal_request(
      std::static_pointer_cast<void>(goal_request),
      [this, goal_request, options, promise](std::shared_ptr<void> response) mutable
      {
        using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
        auto goal_response = std::static_pointer_cast<GoalResponse>(response);
        if (!goal_response->accepted) {
          promise->set_value(nullptr);
          if (options.goal_response_callback) {
            options.goal_response_callback(nullptr);
          }
          return;
        }

        // If feedback message optimization is enabled, add goal id to feedback subscription
        // content filter
        if (enable_feedback_msg_optimization_) {
          std::lock_guard<std::mutex> lock(configure_feedback_sub_content_filter_mutex_);
          if (!configure_feedback_subscription_filter_add_goal_id(goal_request->goal_id.uuid)) {
            RCLCPP_ERROR(
              this->get_logger(),
              "Failed to add goal id to feedback subscription content filter for action client."
              " Disable feedback message optimization.");
            enable_feedback_msg_optimization_ = false;
          }
        }

        GoalInfo goal_info;
        goal_info.goal_id.uuid = goal_request->goal_id.uuid;
        goal_info.stamp = goal_response->stamp;
        // Do not use std::make_shared as friendship cannot be forwarded.
        std::shared_ptr<GoalHandle> goal_handle(
          new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
        {
          std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
          goal_handles_[goal_handle->get_goal_id()] = goal_handle;
        }
        promise->set_value(goal_handle);
        if (options.goal_response_callback) {
          options.goal_response_callback(goal_handle);
        }

        if (options.result_callback) {
          this->make_result_aware(goal_handle);
        }
      });

    // TODO(jacobperron): Encapsulate into it's own function and
    //                    consider exposing an option to disable this cleanup
    // To prevent the list from growing out of control, forget about any goals
    // with no more user references
    {
      std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
      auto goal_handle_it = goal_handles_.begin();
      while (goal_handle_it != goal_handles_.end()) {
        if (!goal_handle_it->second.lock()) {
          RCLCPP_DEBUG(
            this->get_logger(),
            "Dropping weak reference to goal handle during send_goal()");
          goal_handle_it = goal_handles_.erase(goal_handle_it);
        } else {
          ++goal_handle_it;
        }
      }
    }

    return future;
  }

  /// Asynchronously get the result for an active goal.
  /**
   * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
   *   state, or if there was an error requesting the result.
   * \param[in] goal_handle The goal handle for which to get the result.
   * \param[in] result_callback Optional callback that is called when the result is received.
   * \return A future that is set to the goal result when the goal is finished.
   */
  std::shared_future<WrappedResult>
  async_get_result(
    typename GoalHandle::SharedPtr goal_handle,
    ResultCallback result_callback = nullptr)
  {
    std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
      throw exceptions::UnknownGoalHandleError();
    }
    if (goal_handle->is_invalidated()) {
      // This case can happen if there was a failure to send the result request
      // during the goal response callback
      throw goal_handle->invalidate_exception_;
    }
    if (result_callback) {
      // This will override any previously registered callback
      goal_handle->set_result_callback(result_callback);
    }
    this->make_result_aware(goal_handle);
    return goal_handle->async_get_result();
  }

  /// Asynchronously request a goal be canceled.
  /**
   * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a
   *   terminal state.
   * \param[in] goal_handle The goal handle requesting to be canceled.
   * \param[in] cancel_callback Optional callback that is called when the response is received.
   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
   * \return A future to a CancelResponse message that is set when the request has been
   * acknowledged by an action server.
   * See
   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
   * action_msgs/CancelGoal.srv</a>.
   */
  std::shared_future<typename CancelResponse::SharedPtr>
  async_cancel_goal(
    typename GoalHandle::SharedPtr goal_handle,
    CancelCallback cancel_callback = nullptr)
  {
    std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
      throw exceptions::UnknownGoalHandleError();
    }
    auto cancel_request = std::make_shared<CancelRequest>();
    // cancel_request->goal_info.goal_id = goal_handle->get_goal_id();
    cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
    return async_cancel(cancel_request, cancel_callback);
  }

  /// Asynchronously request for all goals to be canceled.
  /**
   * \param[in] cancel_callback Optional callback that is called when the response is received.
   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
   * \return A future to a CancelResponse message that is set when the request has been
   * acknowledged by an action server.
   * See
   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
   * action_msgs/CancelGoal.srv</a>.
   */
  std::shared_future<typename CancelResponse::SharedPtr>
  async_cancel_all_goals(CancelCallback cancel_callback = nullptr)
  {
    auto cancel_request = std::make_shared<CancelRequest>();
    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
    std::fill(
      cancel_request->goal_info.goal_id.uuid.begin(),
      cancel_request->goal_info.goal_id.uuid.end(), 0u);
    return async_cancel(cancel_request, cancel_callback);
  }

  /// Stops the callbacks for the goal in a thread safe way
  /**
   * This will NOT cancel the goal, it will only stop the callbacks.
   *
   * After the call to this function, it is guaranteed that there
   * will be no more callbacks from the goal. This is not guaranteed
   * if multiple threads are involved, and the goal_handle is just
   * dropped.
   *
   * \param[in] goal_handle The goal were the callbacks shall be stopped
   */
  void stop_callbacks(typename GoalHandle::SharedPtr goal_handle)
  {
    goal_handle->set_feedback_callback(typename GoalHandle::FeedbackCallback());
    goal_handle->set_result_callback(typename GoalHandle::ResultCallback());

    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
    const GoalUUID & goal_id = goal_handle->get_goal_id();
    auto it = goal_handles_.find(goal_id);
    if (goal_handles_.end() == it) {
      // someone else already deleted the entry
      // e.g. the result callback
      RCLCPP_DEBUG(
        this->get_logger(),
        "Given goal is unknown. Ignoring...");
      return;
    }
    goal_handles_.erase(it);
  }

  /// Stops the callbacks for the goal in a thread safe way
  /**
   * For futher information see stop_callbacks(typename GoalHandle::SharedPtr goal_handle)
   */
  void stop_callbacks(const GoalUUID & goal_id)
  {
    typename GoalHandle::SharedPtr goal_handle;
    {
      std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
      auto it = goal_handles_.find(goal_id);
      if (goal_handles_.end() == it) {
        // someone else already deleted the entry
        // e.g. the result callback
        RCLCPP_DEBUG(
          this->get_logger(),
          "Given goal is unknown. Ignoring...");
        return;
      }

      goal_handle = it->second.lock();
    }

    if (goal_handle) {
      stop_callbacks(goal_handle);
    }
  }

  /// Asynchronously request all goals at or before a specified time be canceled.
  /**
   * \param[in] stamp The timestamp for the cancel goal request.
   * \param[in] cancel_callback Optional callback that is called when the response is received.
   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
   * \return A future to a CancelResponse message that is set when the request has been
   * acknowledged by an action server.
   * See
   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
   * action_msgs/CancelGoal.srv</a>.
   */
  std::shared_future<typename CancelResponse::SharedPtr>
  async_cancel_goals_before(
    const rclcpp::Time & stamp,
    CancelCallback cancel_callback = nullptr)
  {
    auto cancel_request = std::make_shared<CancelRequest>();
    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
    std::fill(
      cancel_request->goal_info.goal_id.uuid.begin(),
      cancel_request->goal_info.goal_id.uuid.end(), 0u);
    cancel_request->goal_info.stamp = stamp;
    return async_cancel(cancel_request, cancel_callback);
  }

  virtual
  ~Client()
  {
    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
    auto it = goal_handles_.begin();
    while (it != goal_handles_.end()) {
      typename GoalHandle::SharedPtr goal_handle = it->second.lock();
      if (goal_handle) {
        goal_handle->invalidate(exceptions::UnawareGoalHandleError());
      }
      it = goal_handles_.erase(it);
    }
  }

private:
  /// \internal
  std::shared_ptr<void>
  create_goal_response() const override
  {
    using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
    return std::shared_ptr<void>(new GoalResponse());
  }

  /// \internal
  std::shared_ptr<void>
  create_result_response() const override
  {
    using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
    return std::shared_ptr<void>(new GoalResultResponse());
  }

  /// \internal
  std::shared_ptr<void>
  create_cancel_response() const override
  {
    return std::shared_ptr<void>(new CancelResponse());
  }

  /// \internal
  std::shared_ptr<void>
  create_feedback_message() const override
  {
    using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
    return std::shared_ptr<void>(new FeedbackMessage());
  }

  /// \internal
  void
  handle_feedback_message(std::shared_ptr<void> message) override
  {
    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
    using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
    typename FeedbackMessage::SharedPtr feedback_message =
      std::static_pointer_cast<FeedbackMessage>(message);
    const GoalUUID & goal_id = feedback_message->goal_id.uuid;
    if (goal_handles_.count(goal_id) == 0) {
      RCLCPP_DEBUG(
        this->get_logger(),
        "Received feedback for unknown goal. Ignoring...");
      return;
    }
    typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
    // Forget about the goal if there are no more user references
    if (!goal_handle) {
      RCLCPP_DEBUG(
        this->get_logger(),
        "Dropping weak reference to goal handle during feedback callback");
      goal_handles_.erase(goal_id);
      return;
    }
    auto feedback = std::make_shared<Feedback>();
    *feedback = feedback_message->feedback;
    goal_handle->call_feedback_callback(goal_handle, feedback);
  }

  /// \internal
  std::shared_ptr<void>
  create_status_message() const override
  {
    using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
    return std::shared_ptr<void>(new GoalStatusMessage());
  }

  /// \internal
  void
  handle_status_message(std::shared_ptr<void> message) override
  {
    std::lock_guard<std::recursive_mutex> guard(goal_handles_mutex_);
    using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
    auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
    for (const GoalStatus & status : status_message->status_list) {
      const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
      if (goal_handles_.count(goal_id) == 0) {
        RCLCPP_DEBUG(
          this->get_logger(),
          "Received status for unknown goal. Ignoring...");
        continue;
      }
      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock();
      // Forget about the goal if there are no more user references
      if (!goal_handle) {
        RCLCPP_DEBUG(
          this->get_logger(),
          "Dropping weak reference to goal handle during status callback");
        goal_handles_.erase(goal_id);
        continue;
      }
      goal_handle->set_status(status.status);
    }
  }

  /// \internal
  void
  make_result_aware(typename GoalHandle::SharedPtr goal_handle)
  {
    // Avoid making more than one request
    if (goal_handle->set_result_awareness(true)) {
      return;
    }
    using GoalResultRequest = typename ActionT::Impl::GetResultService::Request;
    auto goal_result_request = std::make_shared<GoalResultRequest>();
    goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
    try {
      this->send_result_request(
        std::static_pointer_cast<void>(goal_result_request),
        [goal_handle, this](std::shared_ptr<void> response) mutable
        {
          // Wrap the response in a struct with the fields a user cares about
          WrappedResult wrapped_result;
          using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
          auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
          wrapped_result.result = std::make_shared<typename ActionT::Result>();
          *wrapped_result.result = result_response->result;
          wrapped_result.goal_id = goal_handle->get_goal_id();
          wrapped_result.code = static_cast<ResultCode>(result_response->status);
          goal_handle->set_result(wrapped_result);

          // If feedback message optimization is enabled, remove goal id from feedback subscription
          // content filter
          if (this->enable_feedback_msg_optimization_) {
            std::lock_guard<std::mutex> lock(
              this->configure_feedback_sub_content_filter_mutex_);
            if (!this->configure_feedback_subscription_filter_remove_goal_id(
                goal_handle->get_goal_id()))
            {
              RCLCPP_ERROR(
                this->get_logger(),
                "Failed to remove goal id from feedback subscription content filter for action "
                "client. Disable feedback message optimization.");
              this->enable_feedback_msg_optimization_ = false;
            }
          }
          std::lock_guard<std::recursive_mutex> lock(goal_handles_mutex_);
          goal_handles_.erase(goal_handle->get_goal_id());
        });
    } catch (rclcpp::exceptions::RCLError & ex) {
      // This will cause an exception when the user tries to access the result
      goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message));
    }
  }

  /// \internal
  std::shared_future<typename CancelResponse::SharedPtr>
  async_cancel(
    typename CancelRequest::SharedPtr cancel_request,
    CancelCallback cancel_callback = nullptr)
  {
    // Put promise in the heap to move it around.
    auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
    std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
    this->send_cancel_request(
      std::static_pointer_cast<void>(cancel_request),
      [this, cancel_callback, promise](std::shared_ptr<void> response) mutable
      {
        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);

        // If feedback message optimization is enabled, remove goal id from feedback subscription
        // content filter
        if (this->enable_feedback_msg_optimization_) {
          for (const auto & goal_info : cancel_response->goals_canceling) {
            std::lock_guard<std::mutex> lock(this->configure_feedback_sub_content_filter_mutex_);
            if (!this->configure_feedback_subscription_filter_remove_goal_id(
                goal_info.goal_id.uuid))
            {
              RCLCPP_ERROR(
                this->get_logger(),
                "Failed to remove goal id from feedback subscription content filter for action "
                "client. Disable feedback message optimization.");
              this->enable_feedback_msg_optimization_ = false;
              // When an error occurs, the rcl layer will disable the content filter, so there is
              // no need to continue removing the goal id.
              break;
            }
          }
        }

        promise->set_value(cancel_response);
        if (cancel_callback) {
          cancel_callback(cancel_response);
        }
      });
    return future;
  }

  std::map<GoalUUID, typename GoalHandle::WeakPtr> goal_handles_;
  std::recursive_mutex goal_handles_mutex_;
};
}  // namespace rclcpp_action

#endif  // RCLCPP_ACTION__CLIENT_HPP_
