rigless 0.0.5

// Copyright (c) 2026 Rigless Labs.
// SPDX-License-Identifier: MIT OR Apache-2.0
//! What one family of inertial units shares: samples counted from virtual time, a FIFO of whole samples,
//! the words their tables scale a signal to, and the serial protocol of an address byte and a burst.
//!
//! Each part cites its own document in its own file and keeps everything that differs — its map, its
//! reset values, what an empty FIFO answers. Nothing here is a parameter that exists to reconcile two
//! parts; each thing here carries a unit or an invariant both parts hold to the same numbers.

use std::collections::VecDeque;
use std::ops::Range;

use crate::time::VirtualNanos;

/// The FIFO's size on every part of the family.
pub(super) const FIFO_BYTES: usize = 512;
/// The register a burst read stays on.
pub(super) const FIFO_R_W: usize = 0x74;
/// One sample of every field, the fourteen data registers in order.
pub(super) const SAMPLE_BYTES: usize = 14;
/// The bits of `FIFO_EN`, the same on every part: temperature, each gyro axis, the accelerometer.
const TEMP_FIFO_EN: u8 = 0x80;
const GYRO_FIFO_EN: [u8; 3] = [0x40, 0x20, 0x10];
const ACCEL_FIFO_EN: u8 = 0x08;
/// Counts per gravity at the narrowest range, halved for each step of the range.
const ACCEL_LSB_PER_G: i64 = 16_384;
/// Counts per degree per second at the narrowest range, the nominal 2^16 over 500 that every part's table
/// rounds to 131; halved for each step of the range.
const GYRO_LSB_NUMERATOR: i64 = 1 << 16;
const GYRO_DPS_DENOMINATOR: i64 = 500;

/// The signals a unit measures, on its own axes.
#[derive(Debug, Clone, Copy)]
pub(super) struct Signals {
    /// Thousandths of a degree per second.
    pub(super) rate: [i32; 3],
    /// Millionths of a gravity.
    pub(super) acceleration: [i32; 3],
    /// Hundredths of a degree.
    pub(super) centi_celsius: i32,
}

impl Signals {
    /// A still, level unit: no rate, one gravity on Z.
    pub(super) fn still(centi_celsius: i32) -> Self {
        Self {
            rate: [0; 3],
            acceleration: [0, 0, 1_000_000],
            centi_celsius,
        }
    }

    /// The accelerometer's counts on `axis` at range step `step`, less a fifteen-bit offset in steps of
    /// a sixteenth of a count at the narrowest range.
    pub(super) fn accel_counts(&self, axis: usize, step: u32, offset: i64) -> i64 {
        let counts = round(
            i64::from(self.acceleration[axis]) * ACCEL_LSB_PER_G,
            1_000_000 << step,
        );
        counts - ((offset * 16) >> step)
    }

    /// The gyroscope's counts on `axis` at range step `step`, before any offset.
    pub(super) fn gyro_counts(&self, axis: usize, step: u32) -> i64 {
        round(
            i64::from(self.rate[axis]) * GYRO_LSB_NUMERATOR,
            (GYRO_DPS_DENOMINATOR * 1_000) << step,
        )
    }
}

/// A fifteen-bit two's complement offset: `[14:7]` in the high byte, `[6:0]` in bits 7:1 of the low.
pub(super) fn offset15(high: u8, low: u8) -> i64 {
    let fifteen = (u16::from(high) << 7) | u16::from(low >> 1);
    i64::from((fifteen << 1).cast_signed() >> 1)
}

/// `numerator / denominator` to the nearest integer, halves away from zero.
pub(super) fn round(numerator: i64, denominator: i64) -> i64 {
    let half = denominator / 2;
    if numerator >= 0 {
        (numerator + half) / denominator
    } else {
        (numerator - half) / denominator
    }
}

/// A sixteen-bit two's complement word, big-endian, held to its range.
pub(super) fn word(counts: i64) -> [u8; 2] {
    i16::try_from(counts.clamp(i64::from(i16::MIN), i64::from(i16::MAX)))
        .expect("held to sixteen bits")
        .to_be_bytes()
}

/// The fourteen data registers of one sample in order: accelerometer, temperature, gyroscope.
pub(super) fn pack(accel: [i64; 3], temperature: i64, gyro: [i64; 3]) -> [u8; SAMPLE_BYTES] {
    let mut bytes = [0; SAMPLE_BYTES];
    for axis in 0..3 {
        bytes[2 * axis..2 * axis + 2].copy_from_slice(&word(accel[axis]));
        bytes[8 + 2 * axis..10 + 2 * axis].copy_from_slice(&word(gyro[axis]));
    }
    bytes[6..8].copy_from_slice(&word(temperature));
    bytes
}

/// The bytes of a sample `FIFO_EN` writes, in register order.
pub(super) fn fifo_bytes(sample: &[u8; SAMPLE_BYTES], enabled: u8) -> Vec<u8> {
    let mut bytes = Vec::with_capacity(SAMPLE_BYTES);
    if enabled & ACCEL_FIFO_EN != 0 {
        bytes.extend_from_slice(&sample[0..6]);
    }
    if enabled & TEMP_FIFO_EN != 0 {
        bytes.extend_from_slice(&sample[6..8]);
    }
    for axis in 0..3 {
        if enabled & GYRO_FIFO_EN[axis] != 0 {
            bytes.extend_from_slice(&sample[8 + 2 * axis..10 + 2 * axis]);
        }
    }
    bytes
}

/// Samples counted from virtual time rather than scheduled: where the current rate began, as an instant
/// and a sample number, and how many samples have been materialised so far.
#[derive(Debug, Clone, Copy)]
pub(super) struct SampleClock {
    epoch_at: VirtualNanos,
    epoch_sample: u64,
    generated: u64,
}

impl SampleClock {
    pub(super) fn new() -> Self {
        Self {
            epoch_at: VirtualNanos(0),
            epoch_sample: 0,
            generated: 0,
        }
    }

    /// Sampling starts over at `now`, nothing materialised.
    pub(super) fn restart(&mut self, now: VirtualNanos) {
        *self = Self {
            epoch_at: now,
            epoch_sample: 0,
            generated: 0,
        };
    }

    /// The samples taken by `now` at `hz` since the epoch.
    pub(super) fn due(&self, now: VirtualNanos, hz: u64) -> u64 {
        let since = u128::from(now.0.saturating_sub(self.epoch_at.0));
        let taken = since * u128::from(hz) / 1_000_000_000;
        self.epoch_sample + u64::try_from(taken).expect("a run outlasts no sample count")
    }

    /// A change of rate or of running at `now`: the samples so far at the old rate stand, the next come
    /// at whatever rate the next call gives.
    pub(super) fn rebase(&mut self, now: VirtualNanos, hz: u64) {
        self.epoch_sample = self.due(now, hz);
        self.epoch_at = now;
    }

    /// The samples due by `now` at `hz` and not yet materialised, now counted as materialised: their
    /// numbers, empty when none fell due.
    pub(super) fn catch_up(&mut self, now: VirtualNanos, hz: u64) -> Range<u64> {
        let due = self.due(now, hz).max(self.generated);
        let taken = self.generated..due;
        self.generated = due;
        taken
    }
}

/// A FIFO of whole samples, with the count a read of its high byte latches.
#[derive(Debug)]
pub(super) struct Fifo {
    bytes: VecDeque<u8>,
    latched: u16,
}

impl Fifo {
    pub(super) fn new() -> Self {
        Self {
            bytes: VecDeque::with_capacity(FIFO_BYTES),
            latched: 0,
        }
    }

    pub(super) fn clear(&mut self) {
        self.bytes.clear();
    }

    /// `count` copies of `sample` written in; `true` where any did not fit. With `keep_oldest` the copies
    /// past the room stop; without it the oldest samples make room for the newest.
    pub(super) fn push(&mut self, sample: &[u8], count: u64, keep_oldest: bool) -> bool {
        if sample.is_empty() {
            return false;
        }
        let room = (FIFO_BYTES - self.bytes.len()) / sample.len();
        let count = usize::try_from(count).unwrap_or(usize::MAX);
        if keep_oldest {
            for _ in 0..count.min(room) {
                self.bytes.extend(sample);
            }
        } else {
            // The oldest sample goes, not the oldest byte, so a FIFO a driver reads by
            // `count / sample` stays sample-aligned.
            let whole = FIFO_BYTES / sample.len();
            for _ in 0..count.min(whole) {
                while self.bytes.len() + sample.len() > FIFO_BYTES {
                    self.bytes.drain(..sample.len());
                }
                self.bytes.extend(sample);
            }
        }
        count > room
    }

    /// Latches the count for both bytes; the high byte.
    pub(super) fn latch(&mut self) -> u8 {
        self.latched = u16::try_from(self.bytes.len()).expect("the FIFO is 512 bytes");
        (self.latched >> 8) as u8
    }

    /// The low byte of the count as last latched.
    pub(super) fn latched_low(&self) -> u8 {
        (self.latched & 0xFF) as u8
    }

    /// The oldest byte, taken out.
    pub(super) fn pop(&mut self) -> Option<u8> {
        self.bytes.pop_front()
    }
}

/// What one byte on the wire asks of the register file.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub(super) enum Access {
    Read(usize),
    Write(usize, u8),
}

/// Where a select cycle is.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Phase {
    Address,
    Read(usize),
    Write(usize),
}

/// The family's serial protocol: the first byte after the select falls is an address whose top bit is
/// the direction, every byte after it is the next register, and a burst read of the FIFO stays on it.
#[derive(Debug)]
pub(super) struct Serial {
    phase: Phase,
    selected: bool,
}

impl Serial {
    pub(super) fn new() -> Self {
        Self {
            phase: Phase::Address,
            selected: false,
        }
    }

    pub(super) fn select(&mut self) {
        self.selected = true;
        self.phase = Phase::Address;
    }

    pub(super) fn deselect(&mut self) {
        self.selected = false;
        self.phase = Phase::Address;
    }

    /// The access `byte` asks for, or nothing for the address byte or while not selected.
    pub(super) fn step(&mut self, byte: u8) -> Option<Access> {
        if !self.selected {
            return None;
        }
        match self.phase {
            Phase::Address => {
                let register = usize::from(byte & 0x7F);
                self.phase = if byte & 0x80 != 0 {
                    Phase::Read(register)
                } else {
                    Phase::Write(register)
                };
                None
            }
            Phase::Read(register) => {
                self.phase = Phase::Read(if register == FIFO_R_W {
                    FIFO_R_W
                } else {
                    (register + 1) & 0x7F
                });
                Some(Access::Read(register))
            }
            Phase::Write(register) => {
                self.phase = Phase::Write((register + 1) & 0x7F);
                Some(Access::Write(register, byte))
            }
        }
    }
}