File timer.hpp

// Copyright 2025 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.

#pragma once

// Timer handle and the free set_period().

#include <rclcpp/exceptions/exceptions.hpp>
#include <rclcpp/rclcpp.hpp>

#include <rcl/timer.h>

#include <chrono>
#include <cstdint>
#include <memory>
#include <stdexcept>
#include <utility>

#ifdef USE_AGNOCAST_ENABLED

#include <agnocast/agnocast.hpp>

namespace nebula::agnocast_wrapper
{

class Timer
{
public:
  using SharedPtr = std::shared_ptr<Timer>;
  virtual ~Timer() = default;

  virtual void cancel() = 0;
  virtual void reset() = 0;
  virtual bool is_canceled() = 0;
  virtual std::chrono::nanoseconds time_until_trigger() = 0;

private:
  // Private so callers must use the free set_period() function, which also works in the
  // non-agnocast build (where NEBULA_TIMER_PTR is a plain rclcpp::TimerBase, no set_period).
  virtual void set_period(std::chrono::nanoseconds period) = 0;
  friend void set_period(const SharedPtr & timer, std::chrono::nanoseconds period);
};

class AgnocastTimer : public Timer
{
  std::shared_ptr<agnocast::TimerBase> timer_;

public:
  explicit AgnocastTimer(std::shared_ptr<agnocast::TimerBase> timer) : timer_(std::move(timer)) {}

  void cancel() override { timer_->cancel(); }
  void reset() override { timer_->reset(); }
  bool is_canceled() override { return timer_->is_canceled(); }
  std::chrono::nanoseconds time_until_trigger() override { return timer_->time_until_trigger(); }

private:
  void set_period(std::chrono::nanoseconds period) override { timer_->set_period(period); }
};

class ROS2Timer : public Timer
{
  rclcpp::TimerBase::SharedPtr timer_;

public:
  explicit ROS2Timer(rclcpp::TimerBase::SharedPtr timer) : timer_(std::move(timer)) {}

  void cancel() override { timer_->cancel(); }
  void reset() override { timer_->reset(); }
  bool is_canceled() override { return timer_->is_canceled(); }
  std::chrono::nanoseconds time_until_trigger() override { return timer_->time_until_trigger(); }

private:
  // rclcpp::TimerBase does not expose a set_period API; fall back to the rcl C API and
  // convert the rcl_ret_t to an rclcpp::exceptions::RCLError (matching the throw style used
  // by the other rclcpp timer methods such as cancel/reset/time_until_trigger).
  void set_period(std::chrono::nanoseconds period) override
  {
    int64_t old_period = 0;
    const rcl_ret_t ret =
      rcl_timer_exchange_period(timer_->get_timer_handle().get(), period.count(), &old_period);
    if (ret != RCL_RET_OK) {
      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer period");
    }
  }
};

inline void set_period(const Timer::SharedPtr & timer, std::chrono::nanoseconds period)
{
  if (period < std::chrono::nanoseconds::zero()) {
    throw std::invalid_argument{"timer period cannot be negative"};
  }
  if (period == std::chrono::nanoseconds::max()) {
    throw std::invalid_argument{"timer period must be less than std::chrono::nanoseconds::max()"};
  }
  timer->set_period(period);
}

}  // namespace nebula::agnocast_wrapper

#else

namespace nebula::agnocast_wrapper
{

inline void set_period(const rclcpp::TimerBase::SharedPtr & timer, std::chrono::nanoseconds period)
{
  if (period < std::chrono::nanoseconds::zero()) {
    throw std::invalid_argument{"timer period cannot be negative"};
  }
  if (period == std::chrono::nanoseconds::max()) {
    throw std::invalid_argument{"timer period must be less than std::chrono::nanoseconds::max()"};
  }
  int64_t old_period = 0;
  const rcl_ret_t ret =
    rcl_timer_exchange_period(timer->get_timer_handle().get(), period.count(), &old_period);
  if (ret != RCL_RET_OK) {
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer period");
  }
}

}  // namespace nebula::agnocast_wrapper

#endif