File diagnostic_updater.hpp

// Copyright 2026 TIER IV, 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.

// cspell:ignore hwid

#pragma once

#include "nebula_core_ros/agnocast_wrapper/node.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>

#include <cstdarg>
#include <cstdio>
#include <memory>
#include <string>
#include <variant>

#ifdef USE_AGNOCAST_ENABLED

#include <agnocast/node/diagnostic_updater/diagnostic_updater.hpp>

namespace nebula::agnocast_wrapper
{
namespace diagnostic_updater
{

class Updater
{
public:
  explicit Updater(nebula::agnocast_wrapper::Node * node, double period = 1.0)
  : logger_(node->get_logger()),
    impl_(
      // unique_ptr indirection is required because both ::diagnostic_updater::Updater
      // and ::agnocast::Updater are non-movable (their constructors register internal
      // callbacks that capture `this`). std::variant alternatives must be at least
      // move-constructible, so we hold each impl through unique_ptr to satisfy that
      // constraint while keeping the underlying impl's address stable.
      use_agnocast()
        ? decltype(impl_)(
            std::in_place_type<AgnocastImpl>,
            std::make_unique<::agnocast::Updater>(*node->get_agnocast_node(), period))
        : decltype(impl_)(
            std::in_place_type<RclcppImpl>,
            std::make_unique<::diagnostic_updater::Updater>(node->get_rclcpp_node(), period))),
    verbose_(std::visit([](auto & impl) -> bool & { return impl->verbose_; }, impl_))
  {
  }

  void add(const std::string & name, ::diagnostic_updater::TaskFunction f)
  {
    std::visit([&](auto & impl) { impl->add(name, f); }, impl_);
  }

  void add(::diagnostic_updater::DiagnosticTask & task)
  {
    std::visit([&](auto & impl) { impl->add(task); }, impl_);
  }

  template <class T>
  void add(
    const std::string name, T * c, void (T::*f)(::diagnostic_updater::DiagnosticStatusWrapper &))
  {
    std::visit([&](auto & impl) { impl->add(name, c, f); }, impl_);
  }

  bool removeByName(const std::string name)
  {
    return std::visit([&](auto & impl) { return impl->removeByName(name); }, impl_);
  }

  auto getPeriod() const
  {
    return std::visit([](const auto & impl) { return impl->getPeriod(); }, impl_);
  }

  void setPeriod(rclcpp::Duration period)
  {
    std::visit([&](auto & impl) { impl->setPeriod(period); }, impl_);
  }

  void setPeriod(double period)
  {
    std::visit([&](auto & impl) { impl->setPeriod(period); }, impl_);
  }

  void force_update()
  {
    std::visit([&](auto & impl) { impl->force_update(); }, impl_);
  }

  void broadcast(unsigned char lvl, const std::string msg)
  {
    std::visit([&](auto & impl) { impl->broadcast(lvl, msg); }, impl_);
  }

  void setHardwareID(const std::string & hwid)
  {
    std::visit([&](auto & impl) { impl->setHardwareID(hwid); }, impl_);
  }

  void setHardwareIDf(const char * format, ...)
  {
    va_list va;
    constexpr int kBufferSize = 1000;
    char buff[kBufferSize];
    va_start(va, format);
    const int written = vsnprintf(buff, kBufferSize, format, va);
    va_end(va);
    // On encoding error vsnprintf returns negative and leaves buff unspecified, so the
    // hardware ID is left untouched rather than built from an unterminated buffer.
    if (written < 0) {
      RCLCPP_DEBUG(logger_, "Encoding error in diagnostic_updater::setHardwareIDf.");
      return;
    }
    if (written >= kBufferSize) {
      RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf.");
    }
    setHardwareID(std::string(buff));
  }

  // Non-copyable and non-movable: `verbose_` is a reference bound at construction
  // to impl_'s active alternative's `verbose_` field. References cannot be reseated,
  // so neither copy-assign nor move (which would require rebinding) is meaningful.
  Updater(const Updater &) = delete;
  Updater & operator=(const Updater &) = delete;
  Updater(Updater &&) = delete;
  Updater & operator=(Updater &&) = delete;

private:
  using RclcppImpl = std::unique_ptr<::diagnostic_updater::Updater>;

  using AgnocastImpl = std::unique_ptr<::agnocast::Updater>;

  rclcpp::Logger logger_;
  std::variant<RclcppImpl, AgnocastImpl> impl_;

public:
  bool & verbose_;
};

}  // namespace diagnostic_updater
}  // namespace nebula::agnocast_wrapper

#else  // !USE_AGNOCAST_ENABLED

namespace nebula::agnocast_wrapper
{
namespace diagnostic_updater
{

class Updater
{
public:
  explicit Updater(nebula::agnocast_wrapper::Node * node, double period = 1.0)
  : logger_(node->get_logger()), impl_(node->get_rclcpp_node(), period), verbose_(impl_.verbose_)
  {
  }

  void add(const std::string & name, ::diagnostic_updater::TaskFunction f) { impl_.add(name, f); }

  void add(::diagnostic_updater::DiagnosticTask & task) { impl_.add(task); }

  template <class T>
  void add(
    const std::string name, T * c, void (T::*f)(::diagnostic_updater::DiagnosticStatusWrapper &))
  {
    impl_.add(name, c, f);
  }

  bool removeByName(const std::string name) { return impl_.removeByName(name); }

  auto getPeriod() const { return impl_.getPeriod(); }

  void setPeriod(rclcpp::Duration period) { impl_.setPeriod(period); }

  void setPeriod(double period) { impl_.setPeriod(period); }

  void force_update() { impl_.force_update(); }

  void broadcast(unsigned char lvl, const std::string msg) { impl_.broadcast(lvl, msg); }

  void setHardwareID(const std::string & hwid) { impl_.setHardwareID(hwid); }

  void setHardwareIDf(const char * format, ...)
  {
    va_list va;
    constexpr int kBufferSize = 1000;
    char buff[kBufferSize];
    va_start(va, format);
    const int written = vsnprintf(buff, kBufferSize, format, va);
    va_end(va);
    // On encoding error vsnprintf returns negative and leaves buff unspecified, so the
    // hardware ID is left untouched rather than built from an unterminated buffer.
    if (written < 0) {
      RCLCPP_DEBUG(logger_, "Encoding error in diagnostic_updater::setHardwareIDf.");
      return;
    }
    if (written >= kBufferSize) {
      RCLCPP_DEBUG(logger_, "Really long string in diagnostic_updater::setHardwareIDf.");
    }
    setHardwareID(std::string(buff));
  }

  // Non-copyable and non-movable: matches the Agnocast-build Updater, whose `verbose_` is a
  // reference bound to the underlying impl and cannot be reseated.
  Updater(const Updater &) = delete;
  Updater & operator=(const Updater &) = delete;
  Updater(Updater &&) = delete;
  Updater & operator=(Updater &&) = delete;

private:
  rclcpp::Logger logger_;
  ::diagnostic_updater::Updater impl_;

public:
  bool & verbose_;
};

}  // namespace diagnostic_updater
}  // namespace nebula::agnocast_wrapper

#endif