// Copyright (c) 2024 PAL Robotics S.L. All rights reserved.
//
// 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 {{ id | upper }}__NODE_{{ id | upper }}_HPP_
#define {{ id | upper }}__NODE_{{ id | upper }}_HPP_

#include <memory>
#include <string>

#include "diagnostic_msgs/msg/diagnostic_array.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// This is an example of a custom skill action message generated by rpk.
// We use it to create our API to start/stop the skill
#include "sample_skill_msgs/action/skill_control.hpp"

namespace {{id}}
{
/** \brief Implementation of {{id}}.
  *
  * This is the main class for the skill. It is a ROS2 node that uses the
  * lifecycle feature of ROS2 to manage its states.
  *
  * This basic skill template does not perform any particular task, and
  * should be used as a starting point to implement your own skills.
  *
  * Don't forget to update accordingly the manifest of the skill, included in
  * package.xml, to describe what your skill is actually doing.
  */
class Node{{Id}} : public rclcpp_lifecycle::LifecycleNode
{
public:
  using LifecycleCallbackReturn =
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
  using Skill = sample_skill_msgs::action::SkillControl;
  using SkillGoal = std::shared_ptr<const Skill::Goal>;
  using SkillGoalHandle = std::shared_ptr<rclcpp_action::ServerGoalHandle<Skill>>;

  /** \brief Construct the skill node. */
  explicit Node{{Id}}(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
  /** \brief Destruct the skill node. */
  ~Node{{Id}}();

  ////////////////////////////////////////////////////////////////////////////////////////////////
  // Lifecycle transitions callbacks

  /** \brief Configure the skill node.
   *
   * You usually want to do the following in this transition:
   * - Read ROS parameters (if any)
   * - Create ROS action clients and servers
   * - Create ROS publishers and subscribers
   * - Start publishing diagnostic information
   *
   * While the skill is configured, but not activated, it should not
   * perform any actions that are not required for configuration, such as
   * effectively processing data or calling external services.
   * For instance, incoming goals on an action server should be rejected.
   */
  LifecycleCallbackReturn on_configure(const rclcpp_lifecycle::State &);

  /** \brief Activate the skill node.
   *
   * You usually want to do the following in this transition:
   * - Create and start any timers performing periodic tasks
   * - Start processing data, and accepting action goals, if any
   */
  LifecycleCallbackReturn on_activate(const rclcpp_lifecycle::State &);

  /** \brief Deactivate the skill node.
   *
   * You usually want to do the following in this transition:
   * - Stop any timer started in the "on_activate" transition
   * - Stop processing data and reject incoming action goals
   */
  LifecycleCallbackReturn on_deactivate(const rclcpp_lifecycle::State &);

  /** \brief Cleanup the configuration for the skill node.
   *
   * You usually want to do the following in this transition:
   * - Destroy ROS action clients and servers
   * - Destroy ROS publishers and subscribers
   * - Stop publishing diagnostic information
   */
  LifecycleCallbackReturn on_cleanup(const rclcpp_lifecycle::State &);

  /** \brief Shutdown the skill node.
   *
   * This makes the node transition to the "finalized" state,
   * a functionally dead end state which is not expected to be exited.
   */
  LifecycleCallbackReturn on_shutdown(const rclcpp_lifecycle::State &);

private:
  // Internal implementation for the "cleanup" transition
  void CleanupImpl();
  // Internal implementation for the "deactivate" transition
  void DeactivateImpl();
  // Mock a skill functionality returning a random name.
  std::string RunSkill();
  // Accept the incoming goal, if appropriate.
  rclcpp_action::GoalResponse OnSkillRequestGoal(const rclcpp_action::GoalUUID &, SkillGoal);
  // Process an accepted incoming goal.
  void OnSkillRequestAccepted(const SkillGoalHandle goal_handle);
  // Periodically publish diagnostic information.
  void PublishDiagnostics();
  // Main logic cycle of the node.
  void Run();

  int nb_requests_;

  rclcpp::OnShutdownCallbackHandle on_shutdown_cb_handle_;
  rclcpp_action::Server<Skill>::SharedPtr skill_server_;
  std::shared_ptr<rclcpp::TimerBase> run_timer_;
  std::shared_ptr<rclcpp::TimerBase> diagnostics_timer_;
  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
};

}  // namespace {{id}}

#endif  // {{ id | upper }}__NODE_{{ id | upper }}_HPP_

