//! DWT-backed monotonic microsecond clock for STM32F722.

use cortex_m::peripheral::{DCB, DWT};
use kaptan_time::{MonotonicClock, Timestamp};

/// A DWT cycle counter extended into monotonic microseconds.
///
/// `now` must be called at least once during every 32-bit cycle-counter wrap
/// interval. At the Kakute reset clock this is approximately 268 seconds; the
/// board periodic service calls it much more frequently.
pub struct DwtMicrosecondClock {
    cycles_per_microsecond: u32,
    previous_cycles: u32,
    elapsed_cycles: u64,
}

/// Invalid fixed clock configuration for the DWT timestamp source.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum DwtClockStartError {
    /// A zero-megahertz core clock cannot produce microsecond timestamps.
    ZeroFrequency,
}

impl DwtMicrosecondClock {
    /// Enables the DWT cycle counter for a fixed integer-MHz core clock.
    ///
    /// `MEGAHERTZ` must match the board's core clock. This function is called
    /// once during board initialization before interrupts are enabled.
    pub fn start<const MEGAHERTZ: u32>(
        dcb: &mut DCB,
        dwt: &mut DWT,
    ) -> Result<Self, DwtClockStartError> {
        if MEGAHERTZ == 0 {
            return Err(DwtClockStartError::ZeroFrequency);
        }

        dcb.enable_trace();
        DWT::unlock();
        dwt.set_cycle_count(0);
        dwt.enable_cycle_counter();

        Ok(Self {
            cycles_per_microsecond: MEGAHERTZ,
            previous_cycles: 0,
            elapsed_cycles: 0,
        })
    }

    /// Captures the current monotonic microsecond timestamp.
    pub fn now(&mut self) -> Timestamp {
        self.advance_to(DWT::cycle_count())
    }

    /// Creates another independent reader for the same already-running DWT
    /// counter. Each reader must be serviced often enough to observe wraps.
    pub const fn fork(&self) -> Self {
        Self {
            cycles_per_microsecond: self.cycles_per_microsecond,
            previous_cycles: self.previous_cycles,
            elapsed_cycles: self.elapsed_cycles,
        }
    }

    fn advance_to(&mut self, current_cycles: u32) -> Timestamp {
        let elapsed = current_cycles.wrapping_sub(self.previous_cycles);
        self.previous_cycles = current_cycles;
        self.elapsed_cycles = self.elapsed_cycles.saturating_add(u64::from(elapsed));

        Timestamp::from_microseconds(self.elapsed_cycles / u64::from(self.cycles_per_microsecond))
    }
}

impl MonotonicClock for DwtMicrosecondClock {
    fn now(&mut self) -> Timestamp {
        Self::now(self)
    }
}

#[cfg(test)]
mod tests {
    use super::DwtMicrosecondClock;

    fn clock(previous_cycles: u32) -> DwtMicrosecondClock {
        DwtMicrosecondClock {
            cycles_per_microsecond: 16,
            previous_cycles,
            elapsed_cycles: 0,
        }
    }

    #[test]
    fn converts_cycles_to_microseconds() {
        let mut clock = clock(0);

        assert_eq!(clock.advance_to(160).as_microseconds(), 10);
    }

    #[test]
    fn extends_a_single_cycle_counter_wrap() {
        let mut clock = clock(u32::MAX - 15);

        assert_eq!(clock.advance_to(16).as_microseconds(), 2);
    }
}