{{comments}}

#pragma once

#include <algorithm>
#include <array>
#include <functional>
#include <mutex>
#include <rclcpp/node.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <set>
#include <sstream>
#include <string>
#include <type_traits>
#include <vector>

#include <fmt/core.h>
#include <fmt/format.h>
#include <fmt/ranges.h>

#include <parameter_traits/parameter_traits.hpp>

{% if user_validation_file|length -%}
#include "{{user_validation_file}}"
{% endif %}

namespace {{namespace}} {

{%- filter indent(width=4) %}
struct Params {
{%- filter indent(width=4) %}
{{struct_content-}}
// for detecting if the parameter struct has been updated
rclcpp::Time __stamp;
{% endfilter -%}
};
{%- endfilter %}

{%- filter indent(width=4) %}
struct StackParams {
{%- filter indent(width=4) %}
{{stack_struct_content-}}
{% endfilter -%}
};
{%- endfilter %}

  class ParamListener{
  public:
    // throws rclcpp::exceptions::InvalidParameterValueException on initialization if invalid parameter are loaded
    ParamListener(rclcpp::Node::SharedPtr node)
    : ParamListener(node->get_node_parameters_interface()) {}

    ParamListener(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
    : ParamListener(node->get_node_parameters_interface()) {}

    ParamListener(const std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface>& parameters_interface){
      parameters_interface_ = parameters_interface;
      declare_params();
      auto update_param_cb = [this](const std::vector<rclcpp::Parameter> &parameters){return this->update(parameters);};
      handle_ = parameters_interface_->add_on_set_parameters_callback(update_param_cb);
      clock_ = rclcpp::Clock();
    }

    Params get_params() const{
      std::lock_guard<std::mutex> lock(mutex_);
      return params_;
    }

    bool is_old(Params const& other) const {
      std::lock_guard<std::mutex> lock(mutex_);
      return params_.__stamp != other.__stamp;
    }

    StackParams get_stack_params() {
      std::lock_guard<std::mutex> lock(mutex_);
      StackParams output;

{%- filter indent(width=6) %}
{{set_stack_params}}
{%- endfilter %}

      return output;
    }

    void refresh_dynamic_parameters() {
      std::lock_guard<std::mutex> lock(mutex_);
      // TODO remove any destroyed dynamic parameters
{%- filter indent(width=6) %}
{{remove_dynamic_parameters}}
{%- endfilter %}
      // declare any new dynamic parameters
      rclcpp::Parameter param;
{%- filter indent(width=6) %}
{{update_declare_dynamic_parameters}}
{%- endfilter %}
    }

    rcl_interfaces::msg::SetParametersResult update(const std::vector<rclcpp::Parameter> &parameters) {
      Params updated_params;
      {
        // Copy params_ so we only update it if all validation succeeds
        std::lock_guard<std::mutex> lock(mutex_);
        updated_params = params_;
      }

      for (const auto &param: parameters) {
{%- filter indent(width=8) %}
{{update_params_set}}
{%- endfilter %}
      }
{% if update_dynamic_parameters|length %}
      // update dynamic parameters
      for (const auto &param: parameters) {
{%- filter indent(width=8) %}
{{update_dynamic_parameters}}
{%- endfilter %}
      }
{%- endif %}
      updated_params.__stamp = clock_.now();
      {
        std::lock_guard<std::mutex> lock(mutex_);
        params_ = updated_params;
      }
      return parameter_traits::OK;
    }

    void declare_params(){
      // declare all parameters and give default values to non-required ones
{%- filter indent(width=6) %}
{{declare_params}}
{%- endfilter %}
      // get parameters and fill struct fields
      rclcpp::Parameter param;

{%- filter indent(width=6) %}
{{declare_params_set}}
{%- endfilter %}
{% if declare_set_dynamic_params|length %}
{% filter indent(width=6) %}
// declare and set all dynamic parameters
{{declare_set_dynamic_params}}
{%- endfilter %}
{%- endif %}

      params_.__stamp = clock_.now();
    }

    private:
      Params params_;
      rclcpp::Clock clock_;
      std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle> handle_;
      std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface_;
      std::mutex mutable mutex_;
  };

} // namespace {{namespace}}
