cu29_clock/

lib.rs

#![cfg_attr(not(feature = "std"), no_std)]

extern crate alloc;
#[cfg(test)]
extern crate approx;

mod calibration;
#[cfg_attr(target_arch = "aarch64", path = "aarch64.rs")]
#[cfg_attr(all(target_os = "none", target_arch = "arm"), path = "cortexm.rs")]
#[cfg_attr(target_arch = "riscv64", path = "riscv64.rs")]
#[cfg_attr(
    all(feature = "std", target_arch = "wasm32", target_os = "unknown"),
    path = "wasm.rs"
)]
#[cfg_attr(target_arch = "x86_64", path = "x86_64.rs")]
#[cfg_attr(
    not(any(
        target_arch = "x86_64",
        target_arch = "aarch64",
        all(target_os = "none", target_arch = "arm"),
        target_arch = "riscv64",
        all(feature = "std", target_arch = "wasm32", target_os = "unknown")
    )),
    path = "fallback.rs"
)]
mod raw_counter;

pub use raw_counter::*;

#[cfg(feature = "reflect")]
use bevy_reflect::Reflect;
use bincode::BorrowDecode;
use bincode::de::BorrowDecoder;
use bincode::de::Decoder;
use bincode::enc::Encoder;
use bincode::error::{DecodeError, EncodeError};
use bincode::{Decode, Encode};
use core::ops::{Add, Sub};
use serde::{Deserialize, Serialize};

// We use this to be able to support embedded 32bit platforms
use portable_atomic::{AtomicU64, Ordering};

use alloc::format;
use alloc::sync::Arc;
use alloc::vec::Vec;
use core::fmt::{Display, Formatter};
use core::ops::{AddAssign, Div, Mul, SubAssign};

/// High-precision instant in time, represented as nanoseconds since an arbitrary epoch
#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
pub struct CuInstant(u64);

pub type Instant = CuInstant; // Backward compatibility

impl CuInstant {
    pub fn now() -> Self {
        CuInstant(calibration::counter_to_nanos(read_raw_counter))
    }

    pub fn as_nanos(&self) -> u64 {
        self.0
    }
}

impl Sub for CuInstant {
    type Output = CuDuration;

    fn sub(self, other: CuInstant) -> CuDuration {
        CuDuration(self.0.saturating_sub(other.0))
    }
}

impl Sub<CuDuration> for CuInstant {
    type Output = CuInstant;

    fn sub(self, duration: CuDuration) -> CuInstant {
        CuInstant(self.0.saturating_sub(duration.as_nanos()))
    }
}

impl Add<CuDuration> for CuInstant {
    type Output = CuInstant;

    fn add(self, duration: CuDuration) -> CuInstant {
        CuInstant(self.0.saturating_add(duration.as_nanos()))
    }
}

/// For Robot times, the underlying type is a u64 representing nanoseconds.
/// It is always positive to simplify the reasoning on the user side.
#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize, Default)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
pub struct CuDuration(pub u64);

impl CuDuration {
    // Lowest value a CuDuration can have.
    pub const MIN: CuDuration = CuDuration(0u64);
    // Highest value a CuDuration can have reserving the max value for None.
    pub const MAX: CuDuration = CuDuration(NONE_VALUE - 1);

    pub fn max(self, other: CuDuration) -> CuDuration {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuDuration(lhs.max(rhs))
    }

    pub fn min(self, other: CuDuration) -> CuDuration {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuDuration(lhs.min(rhs))
    }

    pub fn as_nanos(&self) -> u64 {
        let Self(nanos) = self;
        *nanos
    }

    pub fn as_micros(&self) -> u64 {
        let Self(nanos) = self;
        nanos / 1_000
    }

    pub fn as_millis(&self) -> u64 {
        let Self(nanos) = self;
        nanos / 1_000_000
    }

    pub fn as_secs(&self) -> u64 {
        let Self(nanos) = self;
        nanos / 1_000_000_000
    }

    pub fn from_nanos(nanos: u64) -> Self {
        CuDuration(nanos)
    }

    pub fn from_micros(micros: u64) -> Self {
        CuDuration(micros * 1_000)
    }

    pub fn from_millis(millis: u64) -> Self {
        CuDuration(millis * 1_000_000)
    }

    pub fn from_secs(secs: u64) -> Self {
        CuDuration(secs * 1_000_000_000)
    }
}

/// Saturating subtraction for time and duration types.
pub trait SaturatingSub {
    fn saturating_sub(self, other: Self) -> Self;
}

impl SaturatingSub for CuDuration {
    fn saturating_sub(self, other: Self) -> Self {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuDuration(lhs.saturating_sub(rhs))
    }
}

/// bridge the API with standard Durations.
#[cfg(feature = "std")]
impl From<std::time::Duration> for CuDuration {
    fn from(duration: std::time::Duration) -> Self {
        CuDuration(duration.as_nanos() as u64)
    }
}

#[cfg(not(feature = "std"))]
impl From<core::time::Duration> for CuDuration {
    fn from(duration: core::time::Duration) -> Self {
        CuDuration(duration.as_nanos() as u64)
    }
}

#[cfg(feature = "std")]
impl From<CuDuration> for std::time::Duration {
    fn from(val: CuDuration) -> Self {
        let CuDuration(nanos) = val;
        std::time::Duration::from_nanos(nanos)
    }
}

impl From<u64> for CuDuration {
    fn from(duration: u64) -> Self {
        CuDuration(duration)
    }
}

impl From<CuDuration> for u64 {
    fn from(val: CuDuration) -> Self {
        let CuDuration(nanos) = val;
        nanos
    }
}

impl Sub for CuDuration {
    type Output = Self;

    fn sub(self, rhs: Self) -> Self::Output {
        let CuDuration(lhs) = self;
        let CuDuration(rhs) = rhs;
        CuDuration(lhs - rhs)
    }
}

impl Add for CuDuration {
    type Output = Self;

    fn add(self, rhs: Self) -> Self::Output {
        let CuDuration(lhs) = self;
        let CuDuration(rhs) = rhs;
        CuDuration(lhs + rhs)
    }
}

impl AddAssign for CuDuration {
    fn add_assign(&mut self, rhs: Self) {
        let CuDuration(lhs) = self;
        let CuDuration(rhs) = rhs;
        *lhs += rhs;
    }
}

impl SubAssign for CuDuration {
    fn sub_assign(&mut self, rhs: Self) {
        let CuDuration(lhs) = self;
        let CuDuration(rhs) = rhs;
        *lhs -= rhs;
    }
}

// a way to divide a duration by a scalar.
// useful to compute averages for example.
impl<T> Div<T> for CuDuration
where
    T: Into<u64>,
{
    type Output = Self;
    fn div(self, rhs: T) -> Self {
        let CuDuration(lhs) = self;
        CuDuration(lhs / rhs.into())
    }
}

// a way to multiply a duration by a scalar.
// useful to compute offsets for example.
// CuDuration * scalar
impl<T> Mul<T> for CuDuration
where
    T: Into<u64>,
{
    type Output = CuDuration;

    fn mul(self, rhs: T) -> CuDuration {
        let CuDuration(lhs) = self;
        CuDuration(lhs * rhs.into())
    }
}

// u64 * CuDuration
impl Mul<CuDuration> for u64 {
    type Output = CuDuration;

    fn mul(self, rhs: CuDuration) -> CuDuration {
        let CuDuration(nanos) = rhs;
        CuDuration(self * nanos)
    }
}

// u32 * CuDuration
impl Mul<CuDuration> for u32 {
    type Output = CuDuration;

    fn mul(self, rhs: CuDuration) -> CuDuration {
        let CuDuration(nanos) = rhs;
        CuDuration(self as u64 * nanos)
    }
}

// i32 * CuDuration
impl Mul<CuDuration> for i32 {
    type Output = CuDuration;

    fn mul(self, rhs: CuDuration) -> CuDuration {
        let CuDuration(nanos) = rhs;
        CuDuration(self as u64 * nanos)
    }
}

impl Encode for CuDuration {
    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
        let CuDuration(nanos) = self;
        nanos.encode(encoder)
    }
}

impl<Context> Decode<Context> for CuDuration {
    fn decode<D: Decoder>(decoder: &mut D) -> Result<Self, DecodeError> {
        Ok(CuDuration(u64::decode(decoder)?))
    }
}

impl<'de, Context> BorrowDecode<'de, Context> for CuDuration {
    fn borrow_decode<D: BorrowDecoder<'de>>(decoder: &mut D) -> Result<Self, DecodeError> {
        Ok(CuDuration(u64::decode(decoder)?))
    }
}

impl Display for CuDuration {
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        let Self(nanos) = *self;
        if nanos >= 86_400_000_000_000 {
            write!(f, "{:.3} d", nanos as f64 / 86_400_000_000_000.0)
        } else if nanos >= 3_600_000_000_000 {
            write!(f, "{:.3} h", nanos as f64 / 3_600_000_000_000.0)
        } else if nanos >= 60_000_000_000 {
            write!(f, "{:.3} m", nanos as f64 / 60_000_000_000.0)
        } else if nanos >= 1_000_000_000 {
            write!(f, "{:.3} s", nanos as f64 / 1_000_000_000.0)
        } else if nanos >= 1_000_000 {
            write!(f, "{:.3} ms", nanos as f64 / 1_000_000.0)
        } else if nanos >= 1_000 {
            write!(f, "{:.3} µs", nanos as f64 / 1_000.0)
        } else {
            write!(f, "{nanos} ns")
        }
    }
}

/// A robot time is a monotonic timestamp in nanoseconds from the robot clock epoch.
#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize, Default)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
#[repr(transparent)]
pub struct CuTime(pub u64);

impl CuTime {
    pub const MIN: CuTime = CuTime(0u64);
    pub const MAX: CuTime = CuTime(NONE_VALUE - 1);

    pub fn max(self, other: CuTime) -> CuTime {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuTime(lhs.max(rhs))
    }

    pub fn min(self, other: CuTime) -> CuTime {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuTime(lhs.min(rhs))
    }

    pub fn as_nanos(&self) -> u64 {
        let Self(nanos) = self;
        *nanos
    }

    pub fn as_micros(&self) -> u64 {
        self.as_nanos() / 1_000
    }

    pub fn as_millis(&self) -> u64 {
        self.as_nanos() / 1_000_000
    }

    pub fn as_secs(&self) -> u64 {
        self.as_nanos() / 1_000_000_000
    }

    pub fn from_nanos(nanos: u64) -> Self {
        CuTime(nanos)
    }

    pub fn from_micros(micros: u64) -> Self {
        CuTime::from(CuDuration::from_micros(micros))
    }

    pub fn from_millis(millis: u64) -> Self {
        CuTime::from(CuDuration::from_millis(millis))
    }

    pub fn from_secs(secs: u64) -> Self {
        CuTime::from(CuDuration::from_secs(secs))
    }
}

impl SaturatingSub for CuTime {
    fn saturating_sub(self, other: Self) -> Self {
        let Self(lhs) = self;
        let Self(rhs) = other;
        CuTime(lhs.saturating_sub(rhs))
    }
}

#[cfg(feature = "std")]
impl From<std::time::Duration> for CuTime {
    fn from(duration: std::time::Duration) -> Self {
        CuTime::from(CuDuration::from(duration))
    }
}

#[cfg(not(feature = "std"))]
impl From<core::time::Duration> for CuTime {
    fn from(duration: core::time::Duration) -> Self {
        CuTime::from(CuDuration::from(duration))
    }
}

#[cfg(feature = "std")]
impl From<CuTime> for std::time::Duration {
    fn from(val: CuTime) -> Self {
        std::time::Duration::from_nanos(val.as_nanos())
    }
}

impl From<u64> for CuTime {
    fn from(time: u64) -> Self {
        CuTime(time)
    }
}

impl From<CuTime> for u64 {
    fn from(val: CuTime) -> Self {
        let CuTime(nanos) = val;
        nanos
    }
}

impl From<CuDuration> for CuTime {
    fn from(duration: CuDuration) -> Self {
        CuTime(duration.as_nanos())
    }
}

impl From<CuTime> for CuDuration {
    fn from(time: CuTime) -> Self {
        CuDuration(time.as_nanos())
    }
}

impl Add for CuTime {
    type Output = Self;

    fn add(self, rhs: Self) -> Self::Output {
        CuTime(self.as_nanos().saturating_add(rhs.as_nanos()))
    }
}

impl Add<CuDuration> for CuTime {
    type Output = Self;

    fn add(self, rhs: CuDuration) -> Self::Output {
        CuTime(self.as_nanos().saturating_add(rhs.as_nanos()))
    }
}

impl AddAssign<CuDuration> for CuTime {
    fn add_assign(&mut self, rhs: CuDuration) {
        *self = *self + rhs;
    }
}

impl AddAssign for CuTime {
    fn add_assign(&mut self, rhs: Self) {
        *self = *self + rhs;
    }
}

impl Sub<CuDuration> for CuTime {
    type Output = Self;

    fn sub(self, rhs: CuDuration) -> Self::Output {
        CuTime(self.as_nanos().saturating_sub(rhs.as_nanos()))
    }
}

impl SubAssign<CuDuration> for CuTime {
    fn sub_assign(&mut self, rhs: CuDuration) {
        *self = *self - rhs;
    }
}

impl SubAssign for CuTime {
    fn sub_assign(&mut self, rhs: Self) {
        *self = CuTime(self.as_nanos().saturating_sub(rhs.as_nanos()));
    }
}

impl Sub for CuTime {
    type Output = CuDuration;

    fn sub(self, rhs: Self) -> Self::Output {
        CuDuration(self.as_nanos().saturating_sub(rhs.as_nanos()))
    }
}

impl<T> Div<T> for CuTime
where
    T: Into<u64>,
{
    type Output = Self;

    fn div(self, rhs: T) -> Self::Output {
        CuTime(self.as_nanos() / rhs.into())
    }
}

impl<T> Mul<T> for CuTime
where
    T: Into<u64>,
{
    type Output = Self;

    fn mul(self, rhs: T) -> Self::Output {
        CuTime(self.as_nanos() * rhs.into())
    }
}

impl Mul<CuTime> for u64 {
    type Output = CuTime;

    fn mul(self, rhs: CuTime) -> Self::Output {
        CuTime(self * rhs.as_nanos())
    }
}

impl Mul<CuTime> for u32 {
    type Output = CuTime;

    fn mul(self, rhs: CuTime) -> Self::Output {
        CuTime(self as u64 * rhs.as_nanos())
    }
}

impl Mul<CuTime> for i32 {
    type Output = CuTime;

    fn mul(self, rhs: CuTime) -> Self::Output {
        CuTime(self as u64 * rhs.as_nanos())
    }
}

impl Encode for CuTime {
    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
        self.as_nanos().encode(encoder)
    }
}

impl<Context> Decode<Context> for CuTime {
    fn decode<D: Decoder>(decoder: &mut D) -> Result<Self, DecodeError> {
        Ok(CuTime(u64::decode(decoder)?))
    }
}

impl<'de, Context> BorrowDecode<'de, Context> for CuTime {
    fn borrow_decode<D: BorrowDecoder<'de>>(decoder: &mut D) -> Result<Self, DecodeError> {
        Ok(CuTime(u64::decode(decoder)?))
    }
}

impl Display for CuTime {
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        CuDuration(self.as_nanos()).fmt(f)
    }
}

/// A busy looping function based on this clock for a duration.
/// Mainly useful for embedded to spinlocking.
#[inline(always)]
pub fn busy_wait_for(duration: CuDuration) {
    busy_wait_until(CuInstant::now() + duration);
}

/// A busy looping function based on this until a specific time.
/// Mainly useful for embedded to spinlocking.
#[inline(always)]
pub fn busy_wait_until(time: CuInstant) {
    while CuInstant::now() < time {
        core::hint::spin_loop();
    }
}

/// Homebrewed `Option<CuDuration>` to avoid using 128bits just to represent an Option.
#[derive(Copy, Clone, Debug, PartialEq, Encode, Decode, Serialize, Deserialize)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
pub struct OptionCuTime(CuTime);

const NONE_VALUE: u64 = 0xFFFFFFFFFFFFFFFF;

impl OptionCuTime {
    pub const NONE_SENTINEL_NANOS: u64 = NONE_VALUE;

    #[inline]
    pub fn is_none(&self) -> bool {
        let Self(CuTime(nanos)) = self;
        *nanos == NONE_VALUE
    }

    #[inline]
    pub const fn none() -> Self {
        OptionCuTime(CuTime(NONE_VALUE))
    }

    #[inline]
    pub fn unwrap(self) -> CuTime {
        if self.is_none() {
            panic!("called `OptionCuTime::unwrap()` on a `None` value");
        }
        self.0
    }
}

impl Display for OptionCuTime {
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        if self.is_none() {
            write!(f, "None")
        } else {
            write!(f, "{}", self.0)
        }
    }
}

impl Default for OptionCuTime {
    fn default() -> Self {
        Self::none()
    }
}

impl From<Option<CuTime>> for OptionCuTime {
    #[inline]
    fn from(duration: Option<CuTime>) -> Self {
        match duration {
            Some(duration) => OptionCuTime(duration),
            None => OptionCuTime(CuTime(NONE_VALUE)),
        }
    }
}

impl From<OptionCuTime> for Option<CuTime> {
    #[inline]
    fn from(val: OptionCuTime) -> Self {
        let OptionCuTime(CuTime(nanos)) = val;
        if nanos == NONE_VALUE {
            None
        } else {
            Some(CuTime(nanos))
        }
    }
}

impl From<CuTime> for OptionCuTime {
    #[inline]
    fn from(val: CuTime) -> Self {
        Some(val).into()
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ClockDebugScalarKind {
    Time,
    OptionalTime,
    Duration,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct ClockDebugScalarRegistration {
    pub type_path: &'static str,
    pub kind: ClockDebugScalarKind,
}

pub fn debug_scalar_registrations() -> Vec<ClockDebugScalarRegistration> {
    alloc::vec![
        ClockDebugScalarRegistration {
            type_path: core::any::type_name::<CuDuration>(),
            kind: ClockDebugScalarKind::Duration,
        },
        ClockDebugScalarRegistration {
            type_path: core::any::type_name::<CuTime>(),
            kind: ClockDebugScalarKind::Time,
        },
        ClockDebugScalarRegistration {
            type_path: core::any::type_name::<OptionCuTime>(),
            kind: ClockDebugScalarKind::OptionalTime,
        },
    ]
}

/// Represents a time range.
#[derive(Copy, Clone, Debug, Encode, Decode, Serialize, Deserialize, PartialEq)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
pub struct CuTimeRange {
    pub start: CuTime,
    pub end: CuTime,
}

/// Builds a time range from a slice of CuTime.
/// This is an O(n) operation.
impl From<&[CuTime]> for CuTimeRange {
    fn from(slice: &[CuTime]) -> Self {
        CuTimeRange {
            start: *slice.iter().min().expect("Empty slice"),
            end: *slice.iter().max().expect("Empty slice"),
        }
    }
}

/// Represents a time range with possible undefined start or end or both.
#[derive(Default, Copy, Clone, Debug, Encode, Decode, Serialize, Deserialize)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
pub struct PartialCuTimeRange {
    pub start: OptionCuTime,
    pub end: OptionCuTime,
}

impl Display for PartialCuTimeRange {
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        let start = if self.start.is_none() {
            "…"
        } else {
            &format!("{}", self.start)
        };
        let end = if self.end.is_none() {
            "…"
        } else {
            &format!("{}", self.end)
        };
        write!(f, "[{start} – {end}]")
    }
}

/// The time of validity of a message can be more than one time but can be a time range of Tovs.
/// For example a sub scan for a lidar, a set of images etc... can have a range of validity.
#[derive(Default, Clone, Debug, PartialEq, Encode, Decode, Serialize, Deserialize, Copy)]
#[cfg_attr(feature = "reflect", derive(Reflect))]
pub enum Tov {
    #[default]
    None,
    Time(CuTime),
    Range(CuTimeRange),
}

impl Display for Tov {
    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
        match self {
            Tov::None => write!(f, "None"),
            Tov::Time(t) => write!(f, "{t}"),
            Tov::Range(r) => write!(f, "[{} – {}]", r.start, r.end),
        }
    }
}

impl From<Option<CuDuration>> for Tov {
    fn from(duration: Option<CuDuration>) -> Self {
        duration.map(CuTime::from).map_or(Tov::None, Tov::Time)
    }
}

impl From<Option<CuTime>> for Tov {
    fn from(time: Option<CuTime>) -> Self {
        time.map_or(Tov::None, Tov::Time)
    }
}

impl From<CuDuration> for Tov {
    fn from(duration: CuDuration) -> Self {
        Tov::Time(duration.into())
    }
}

impl From<CuTime> for Tov {
    fn from(time: CuTime) -> Self {
        Tov::Time(time)
    }
}

/// Internal clock implementation that provides high-precision timing
#[derive(Clone, Debug)]
struct InternalClock {
    // For real clocks, this stores the initialization time
    // For mock clocks, this references the mock state
    mock_state: Option<Arc<AtomicU64>>,
}

// Implements the std version of the RTC clock
#[cfg(all(
    feature = "std",
    not(all(target_arch = "wasm32", target_os = "unknown"))
))]
#[inline(always)]
fn read_rtc_ns() -> u64 {
    std::time::SystemTime::now()
        .duration_since(std::time::UNIX_EPOCH)
        .unwrap()
        .as_nanos() as u64
}

#[cfg(all(feature = "std", target_arch = "wasm32", target_os = "unknown"))]
#[inline(always)]
fn read_rtc_ns() -> u64 {
    read_raw_counter()
}

#[cfg(all(
    feature = "std",
    not(all(target_arch = "wasm32", target_os = "unknown"))
))]
#[inline(always)]
fn sleep_ns(ns: u64) {
    std::thread::sleep(std::time::Duration::from_nanos(ns));
}

#[cfg(all(feature = "std", target_arch = "wasm32", target_os = "unknown"))]
#[inline(always)]
fn sleep_ns(ns: u64) {
    let start = read_raw_counter();
    while read_raw_counter().saturating_sub(start) < ns {
        core::hint::spin_loop();
    }
}

impl InternalClock {
    fn new(
        read_rtc_ns: impl Fn() -> u64 + Send + Sync + 'static,
        sleep_ns: impl Fn(u64) + Send + Sync + 'static,
    ) -> Self {
        initialize();

        // Initialize the frequency calibration
        calibration::calibrate(read_raw_counter, read_rtc_ns, sleep_ns);
        InternalClock { mock_state: None }
    }

    fn mock() -> (Self, Arc<AtomicU64>) {
        let mock_state = Arc::new(AtomicU64::new(0));
        let clock = InternalClock {
            mock_state: Some(Arc::clone(&mock_state)),
        };
        (clock, mock_state)
    }

    fn now(&self) -> CuInstant {
        if let Some(ref mock_state) = self.mock_state {
            CuInstant(mock_state.load(Ordering::Relaxed))
        } else {
            CuInstant::now()
        }
    }

    fn recent(&self) -> CuInstant {
        // For simplicity, we use the same implementation as now()
        // In a more sophisticated implementation, this could use a cached value
        self.now()
    }
}

/// A running Robot clock.
/// The clock is a monotonic clock that starts at an arbitrary reference time.
/// It is clone resilient, ie a clone will be the same clock, even when mocked.
#[derive(Clone, Debug)]
pub struct RobotClock {
    inner: InternalClock,
    ref_time: CuInstant,
}

/// A mock clock that can be controlled by the user.
#[derive(Debug, Clone)]
pub struct RobotClockMock(Arc<AtomicU64>);

impl RobotClockMock {
    pub fn increment(&self, amount: CuDuration) {
        let Self(mock_state) = self;
        mock_state.fetch_add(amount.as_nanos(), Ordering::Relaxed);
    }

    /// Decrements the time by the given amount.
    /// Be careful this breaks the monotonicity of the clock.
    pub fn decrement(&self, amount: CuDuration) {
        let Self(mock_state) = self;
        mock_state.fetch_sub(amount.as_nanos(), Ordering::Relaxed);
    }

    /// Gets the current value of time.
    pub fn value(&self) -> u64 {
        let Self(mock_state) = self;
        mock_state.load(Ordering::Relaxed)
    }

    /// A convenient way to get the current time from the mocking side.
    pub fn now(&self) -> CuTime {
        let Self(mock_state) = self;
        CuTime(mock_state.load(Ordering::Relaxed))
    }

    /// Sets the absolute value of the time.
    pub fn set_value(&self, value: u64) {
        let Self(mock_state) = self;
        mock_state.store(value, Ordering::Relaxed);
    }
}

impl RobotClock {
    /// Creates a RobotClock using now as its reference time.
    /// It will start at 0ns incrementing monotonically.
    /// This uses the std System Time as a reference clock.
    #[cfg(feature = "std")]
    pub fn new() -> Self {
        let clock = InternalClock::new(read_rtc_ns, sleep_ns);
        let ref_time = clock.now();
        RobotClock {
            inner: clock,
            ref_time,
        }
    }

    /// Builds a RobotClock using a reference RTC clock to calibrate with.
    /// This is mandatory to use with the no-std platforms as we have no idea where to find a reference clock.
    pub fn new_with_rtc(
        read_rtc_ns: impl Fn() -> u64 + Send + Sync + 'static,
        sleep_ns: impl Fn(u64) + Send + Sync + 'static,
    ) -> Self {
        let clock = InternalClock::new(read_rtc_ns, sleep_ns);
        let ref_time = clock.now();
        RobotClock {
            inner: clock,
            ref_time,
        }
    }

    /// Builds a monotonic clock starting at the given reference time.
    #[cfg(feature = "std")]
    pub fn from_ref_time(ref_time_ns: u64) -> Self {
        let clock = InternalClock::new(read_rtc_ns, sleep_ns);
        let ref_time = clock.now() - CuDuration(ref_time_ns);
        RobotClock {
            inner: clock,
            ref_time,
        }
    }

    /// Overrides the RTC with a custom implementation, should be the same as the new_with_rtc.
    pub fn from_ref_time_with_rtc(
        read_rtc_ns: fn() -> u64,
        sleep_ns: fn(u64),
        ref_time_ns: u64,
    ) -> Self {
        let clock = InternalClock::new(read_rtc_ns, sleep_ns);
        let ref_time = clock.now() - CuDuration(ref_time_ns);
        RobotClock {
            inner: clock,
            ref_time,
        }
    }

    /// Build a fake clock with a reference time of 0.
    /// The RobotMock interface enables you to control all the clones of the clock given.
    pub fn mock() -> (Self, RobotClockMock) {
        let (clock, mock_state) = InternalClock::mock();
        let ref_time = clock.now();
        (
            RobotClock {
                inner: clock,
                ref_time,
            },
            RobotClockMock(mock_state),
        )
    }

    /// Now returns the time that passed since the reference time, usually the start time.
    /// It is a monotonically increasing value.
    #[inline]
    pub fn now(&self) -> CuTime {
        CuTime::from_nanos((self.inner.now() - self.ref_time).as_nanos())
    }

    /// A less precise but quicker time
    #[inline]
    pub fn recent(&self) -> CuTime {
        CuTime::from_nanos((self.inner.recent() - self.ref_time).as_nanos())
    }
}

/// We cannot build a default RobotClock on no-std because we don't know how to find a reference clock.
/// Use RobotClock::new_with_rtc instead on no-std.
#[cfg(feature = "std")]
impl Default for RobotClock {
    fn default() -> Self {
        Self::new()
    }
}

/// A trait to provide a clock to the runtime.
pub trait ClockProvider {
    fn get_clock(&self) -> RobotClock;
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    #[test]
    fn test_cuduration_comparison_operators() {
        let a = CuDuration(100);
        let b = CuDuration(200);

        assert!(a < b);
        assert!(b > a);
        assert_ne!(a, b);
        assert_eq!(a, CuDuration(100));
    }

    #[test]
    fn test_cuduration_arithmetic_operations() {
        let a = CuDuration(100);
        let b = CuDuration(50);

        assert_eq!(a + b, CuDuration(150));
        assert_eq!(a - b, CuDuration(50));
        assert_eq!(a * 2u32, CuDuration(200));
        assert_eq!(a / 2u32, CuDuration(50));
    }

    #[test]
    fn test_robot_clock_monotonic() {
        let clock = RobotClock::new();
        let t1 = clock.now();
        let t2 = clock.now();
        assert!(t2 >= t1);
    }

    #[test]
    fn test_robot_clock_mock() {
        let (clock, mock) = RobotClock::mock();
        let t1 = clock.now();
        mock.increment(CuDuration::from_millis(100));
        let t2 = clock.now();
        assert!(t2 > t1);
        assert_eq!(t2 - t1, CuDuration(100_000_000)); // 100ms in nanoseconds
    }

    #[test]
    fn test_robot_clock_clone_consistency() {
        let (clock1, mock) = RobotClock::mock();
        let clock2 = clock1.clone();

        mock.set_value(1_000_000_000); // 1 second
        assert_eq!(clock1.now(), clock2.now());
    }

    #[test]
    fn test_from_ref_time() {
        let tolerance_ms = 10f64;
        let clock = RobotClock::from_ref_time(1_000_000_000);
        assert_relative_eq!(
            clock.now().as_millis() as f64,
            CuDuration::from_secs(1).as_millis() as f64,
            epsilon = tolerance_ms
        );
    }

    #[test]
    fn longest_duration() {
        let maxcu = CuDuration(u64::MAX);
        assert_eq!(maxcu.as_nanos(), u64::MAX);
        let s = maxcu.as_secs();
        let y = s / 60 / 60 / 24 / 365;
        assert!(y >= 584); // 584 years of robot uptime, we should be good.
    }

    #[test]
    fn test_some_time_arithmetics() {
        let a: CuDuration = 10.into();
        let b: CuDuration = 20.into();
        let c = a + b;
        assert_eq!(c.0, 30);
        let d = b - a;
        assert_eq!(d.0, 10);
    }

    #[test]
    fn test_build_range_from_slice() {
        let range = CuTimeRange::from(&[20.into(), 10.into(), 30.into()][..]);
        assert_eq!(range.start, 10.into());
        assert_eq!(range.end, 30.into());
    }

    #[test]
    fn test_time_range_operations() {
        // Test creating a time range and checking its properties
        let start = CuTime::from(100u64);
        let end = CuTime::from(200u64);
        let range = CuTimeRange { start, end };

        assert_eq!(range.start, start);
        assert_eq!(range.end, end);

        // Test creating from a slice
        let times = [
            CuTime::from(150u64),
            CuTime::from(120u64),
            CuTime::from(180u64),
        ];
        let range_from_slice = CuTimeRange::from(&times[..]);

        // Range should capture min and max values
        assert_eq!(range_from_slice.start, CuTime::from(120u64));
        assert_eq!(range_from_slice.end, CuTime::from(180u64));
    }

    #[test]
    fn test_partial_time_range() {
        // Test creating a partial time range with defined start/end
        let start = CuTime::from(100u64);
        let end = CuTime::from(200u64);

        let partial_range = PartialCuTimeRange {
            start: OptionCuTime::from(start),
            end: OptionCuTime::from(end),
        };

        // Test converting to Option
        let opt_start: Option<CuTime> = partial_range.start.into();
        let opt_end: Option<CuTime> = partial_range.end.into();

        assert_eq!(opt_start, Some(start));
        assert_eq!(opt_end, Some(end));

        // Test partial range with undefined values
        let partial_undefined = PartialCuTimeRange::default();
        assert!(partial_undefined.start.is_none());
        assert!(partial_undefined.end.is_none());
    }

    #[test]
    fn test_tov_conversions() {
        // Test different Time of Validity (Tov) variants
        let time = CuTime::from(100u64);

        // Test conversion from CuTime
        let tov_time: Tov = time.into();
        assert!(matches!(tov_time, Tov::Time(_)));

        if let Tov::Time(t) = tov_time {
            assert_eq!(t, time);
        }

        // Test conversion from Option<CuTime>
        let some_time = Some(time);
        let tov_some: Tov = some_time.into();
        assert!(matches!(tov_some, Tov::Time(_)));

        let none_time: Option<CuDuration> = None;
        let tov_none: Tov = none_time.into();
        assert!(matches!(tov_none, Tov::None));

        // Test range
        let start = CuTime::from(100u64);
        let end = CuTime::from(200u64);
        let range = CuTimeRange { start, end };
        let tov_range = Tov::Range(range);

        assert!(matches!(tov_range, Tov::Range(_)));
    }

    #[cfg(feature = "std")]
    #[test]
    fn test_cuduration_display() {
        // Test the display implementation for different magnitudes
        let nano = CuDuration(42);
        assert_eq!(nano.to_string(), "42 ns");

        let micro = CuDuration(42_000);
        assert_eq!(micro.to_string(), "42.000 µs");

        let milli = CuDuration(42_000_000);
        assert_eq!(milli.to_string(), "42.000 ms");

        let sec = CuDuration(1_500_000_000);
        assert_eq!(sec.to_string(), "1.500 s");

        let min = CuDuration(90_000_000_000);
        assert_eq!(min.to_string(), "1.500 m");

        let hour = CuDuration(3_600_000_000_000);
        assert_eq!(hour.to_string(), "1.000 h");

        let day = CuDuration(86_400_000_000_000);
        assert_eq!(day.to_string(), "1.000 d");
    }

    #[test]
    fn test_robot_clock_precision() {
        // Test that RobotClock::now() and RobotClock::recent() return different values
        // and that recent() is always <= now()
        let clock = RobotClock::new();

        // We can't guarantee the exact values, but we can check relationships
        let recent = clock.recent();
        let now = clock.now();

        // recent() should be less than or equal to now()
        assert!(recent <= now);

        // Test precision of from_ref_time
        let ref_time_ns = 1_000_000_000; // 1 second
        let clock = RobotClock::from_ref_time(ref_time_ns);

        // Clock should start at approximately ref_time_ns
        let now = clock.now();
        let now_ns: u64 = now.into();

        // Allow reasonable tolerance for clock initialization time
        let tolerance_ns = 50_000_000; // 50ms tolerance
        assert!(now_ns >= ref_time_ns);
        assert!(now_ns < ref_time_ns + tolerance_ns);
    }

    #[test]
    fn test_mock_clock_advanced_operations() {
        // Test more complex operations with the mock clock
        let (clock, mock) = RobotClock::mock();

        // Test initial state
        assert_eq!(clock.now(), CuTime::from(0));

        // Test increment
        mock.increment(CuDuration::from_secs(10));
        assert_eq!(
            clock.now(),
            CuTime::from_nanos(CuDuration::from_secs(10).as_nanos())
        );

        // Test decrement (unusual but supported)
        mock.decrement(CuDuration::from_secs(5));
        assert_eq!(
            clock.now(),
            CuTime::from_nanos(CuDuration::from_secs(5).as_nanos())
        );

        // Test setting absolute value
        mock.set_value(30_000_000_000); // 30 seconds in ns
        assert_eq!(
            clock.now(),
            CuTime::from_nanos(CuDuration::from_secs(30).as_nanos())
        );

        // Test that getting the time from the mock directly works
        assert_eq!(
            mock.now(),
            CuTime::from_nanos(CuDuration::from_secs(30).as_nanos())
        );
        assert_eq!(mock.value(), 30_000_000_000);
    }

    #[test]
    fn test_cuduration_min_max() {
        // Test MIN and MAX constants
        assert_eq!(CuDuration::MIN, CuDuration(0));

        // Test min/max methods
        let a = CuDuration(100);
        let b = CuDuration(200);

        assert_eq!(a.min(b), a);
        assert_eq!(a.max(b), b);
        assert_eq!(b.min(a), a);
        assert_eq!(b.max(a), b);

        // Edge cases
        assert_eq!(a.min(a), a);
        assert_eq!(a.max(a), a);

        // Test with MIN/MAX constants
        assert_eq!(a.min(CuDuration::MIN), CuDuration::MIN);
        assert_eq!(a.max(CuDuration::MAX), CuDuration::MAX);
    }

    #[test]
    fn test_clock_provider_trait() {
        // Test implementing the ClockProvider trait
        struct TestClockProvider {
            clock: RobotClock,
        }

        impl ClockProvider for TestClockProvider {
            fn get_clock(&self) -> RobotClock {
                self.clock.clone()
            }
        }

        // Create a provider with a mock clock
        let (clock, mock) = RobotClock::mock();
        let provider = TestClockProvider { clock };

        // Test that provider returns a clock synchronized with the original
        let provider_clock = provider.get_clock();
        assert_eq!(provider_clock.now(), CuTime::from(0));

        // Advance the mock clock and check that the provider's clock also advances
        mock.increment(CuDuration::from_secs(5));
        assert_eq!(
            provider_clock.now(),
            CuTime::from_nanos(CuDuration::from_secs(5).as_nanos())
        );
    }
}