rigless 0.3.0

// Copyright (c) 2026 Rigless Labs.
// SPDX-License-Identifier: MIT OR Apache-2.0
//! What the ICM-42688-P and the ICM-42605 share: a register file in four banks, samples counted from
//! virtual time into a FIFO of packets, and the serial protocol of an address byte and a burst.
//!
//! Each part cites its own datasheet in its own file and keeps everything that differs, which is
//! `Part`: its identity, its register resets, the output data rates it offers, whether a burst write
//! increments, how its timestamp counts and what its sense registers do with an invalid sample. Nothing
//! here is a parameter that exists to reconcile the two. Where a section is cited without a part, the
//! two datasheets number it alike, and 12.7 is the ICM-42688-P's alone.

use std::collections::VecDeque;

use crate::chip::invensense::{Signals, round, saturate};
use crate::time::VirtualNanos;

// 13: the four user banks the maps list, 0, 1, 2 and 4, of 128 registers each
const BANKS: usize = 4;
const BANK_BYTES: usize = 0x80;
// 9.6: bit 7 of the address byte is the direction and the other seven are the register
const READ: u8 = 0x80;
const ADDRESS: u8 = 0x7F;

// 13.1's registers the model acts on, and the one that picks a bank, which every bank answers
const DEVICE_CONFIG: u8 = 0x11;
const FIFO_CONFIG: u8 = 0x16;
const TEMP_DATA1: u8 = 0x1D;
const ACCEL_DATA_X1: u8 = 0x1F;
const GYRO_DATA_X1: u8 = 0x25;
const GYRO_DATA_Z0: u8 = 0x2A;
const INT_STATUS: u8 = 0x2D;
const FIFO_COUNTH: u8 = 0x2E;
const FIFO_COUNTL: u8 = 0x2F;
const FIFO_DATA: u8 = 0x30;
const SIGNAL_PATH_RESET: u8 = 0x4B;
const INTF_CONFIG0: u8 = 0x4C;
const PWR_MGMT0: u8 = 0x4E;
const GYRO_CONFIG0: u8 = 0x4F;
const ACCEL_CONFIG0: u8 = 0x50;
const TMST_CONFIG: u8 = 0x54;
const FIFO_CONFIG1: u8 = 0x5F;
const REG_BANK_SEL: u8 = 0x76;

const DRIVE_CONFIG: u8 = 0x13;
const INT_CONFIG: u8 = 0x14;
const TEMP_DATA0: u8 = 0x1E;
const ACCEL_DATA_X0: u8 = 0x20;
const ACCEL_DATA_Y1: u8 = 0x21;
const ACCEL_DATA_Y0: u8 = 0x22;
const ACCEL_DATA_Z1: u8 = 0x23;
const ACCEL_DATA_Z0: u8 = 0x24;
const GYRO_DATA_X0: u8 = 0x26;
const GYRO_DATA_Y1: u8 = 0x27;
const GYRO_DATA_Y0: u8 = 0x28;
const GYRO_DATA_Z1: u8 = 0x29;
const TMST_FSYNCH: u8 = 0x2B;
const TMST_FSYNCL: u8 = 0x2C;
const APEX_DATA0: u8 = 0x31;
const APEX_DATA1: u8 = 0x32;
const APEX_DATA2: u8 = 0x33;
const APEX_DATA3: u8 = 0x34;
const APEX_DATA4: u8 = 0x35;
const APEX_DATA5: u8 = 0x36;
const INT_STATUS2: u8 = 0x37;
const INT_STATUS3: u8 = 0x38;
const INTF_CONFIG1: u8 = 0x4D;
const GYRO_CONFIG1: u8 = 0x51;
const GYRO_ACCEL_CONFIG0: u8 = 0x52;
const ACCEL_CONFIG1: u8 = 0x53;
const APEX_CONFIG0: u8 = 0x56;
const SMD_CONFIG: u8 = 0x57;
const FIFO_CONFIG2: u8 = 0x60;
const FIFO_CONFIG3: u8 = 0x61;
const FSYNC_CONFIG: u8 = 0x62;
const INT_CONFIG0: u8 = 0x63;
const INT_CONFIG1: u8 = 0x64;
const INT_SOURCE0: u8 = 0x65;
const INT_SOURCE1: u8 = 0x66;
const INT_SOURCE3: u8 = 0x68;
const INT_SOURCE4: u8 = 0x69;
const FIFO_LOST_PKT0: u8 = 0x6C;
const FIFO_LOST_PKT1: u8 = 0x6D;
const SELF_TEST_CONFIG: u8 = 0x70;
const WHO_AM_I: u8 = 0x75;
const SENSOR_CONFIG0: u8 = 0x03;
const GYRO_CONFIG_STATIC2: u8 = 0x0B;
const GYRO_CONFIG_STATIC3: u8 = 0x0C;
const GYRO_CONFIG_STATIC4: u8 = 0x0D;
const GYRO_CONFIG_STATIC5: u8 = 0x0E;
const GYRO_CONFIG_STATIC6: u8 = 0x0F;
const GYRO_CONFIG_STATIC7: u8 = 0x10;
const GYRO_CONFIG_STATIC8: u8 = 0x11;
const GYRO_CONFIG_STATIC9: u8 = 0x12;
const GYRO_CONFIG_STATIC10: u8 = 0x13;
const XG_ST_DATA: u8 = 0x5F;
const YG_ST_DATA: u8 = 0x60;
const ZG_ST_DATA: u8 = 0x61;
const TMSTVAL0: u8 = 0x62;
const TMSTVAL1: u8 = 0x63;
const TMSTVAL2: u8 = 0x64;
const INTF_CONFIG4: u8 = 0x7A;
const INTF_CONFIG5: u8 = 0x7B;
const INTF_CONFIG6: u8 = 0x7C;
const ACCEL_CONFIG_STATIC2: u8 = 0x03;
const ACCEL_CONFIG_STATIC3: u8 = 0x04;
const ACCEL_CONFIG_STATIC4: u8 = 0x05;
const XA_ST_DATA: u8 = 0x3B;
const YA_ST_DATA: u8 = 0x3C;
const ZA_ST_DATA: u8 = 0x3D;
const APEX_CONFIG1: u8 = 0x40;
const APEX_CONFIG2: u8 = 0x41;
const APEX_CONFIG3: u8 = 0x42;
const APEX_CONFIG4: u8 = 0x43;
const APEX_CONFIG5: u8 = 0x44;
const APEX_CONFIG6: u8 = 0x45;
const APEX_CONFIG7: u8 = 0x46;
const APEX_CONFIG8: u8 = 0x47;
const APEX_CONFIG9: u8 = 0x48;
const ACCEL_WOM_X_THR: u8 = 0x4A;
const ACCEL_WOM_Y_THR: u8 = 0x4B;
const ACCEL_WOM_Z_THR: u8 = 0x4C;
const INT_SOURCE6: u8 = 0x4D;
const INT_SOURCE7: u8 = 0x4E;
const INT_SOURCE8: u8 = 0x4F;
const INT_SOURCE9: u8 = 0x50;
const INT_SOURCE10: u8 = 0x51;
const OFFSET_USER0: u8 = 0x77;
const OFFSET_USER1: u8 = 0x78;
const OFFSET_USER2: u8 = 0x79;
const OFFSET_USER3: u8 = 0x7A;
const OFFSET_USER4: u8 = 0x7B;
const OFFSET_USER5: u8 = 0x7C;
const OFFSET_USER6: u8 = 0x7D;
const OFFSET_USER7: u8 = 0x7E;
const OFFSET_USER8: u8 = 0x7F;

const SOFT_RESET_CONFIG: u8 = 0x01;
const FIFO_MODE: u8 = 0xC0;
const FIFO_MODE_BYPASS: u8 = 0x00;
const FIFO_MODE_STREAM: u8 = 0x40;
const DATA_RDY_INT: u8 = 0x08;
const FIFO_FULL_INT: u8 = 0x02;
const FIFO_FLUSH: u8 = 0x02;
const ABORT_AND_RESET: u8 = 0x08;
const FIFO_HOLD_LAST_DATA_EN: u8 = 0x80;
const FIFO_COUNT_REC: u8 = 0x40;
const FIFO_COUNT_ENDIAN: u8 = 0x20;
const SENSOR_DATA_ENDIAN: u8 = 0x10;
const GYRO_MODE: u8 = 0x0C;
const GYRO_LOW_NOISE: u8 = 0x0C;
const ACCEL_MODE: u8 = 0x03;
const ACCEL_LOW_POWER: u8 = 0x02;
const FS_SEL: u8 = 0xE0;
const FS_SEL_SHIFT: u32 = 5;
const ODR: u8 = 0x0F;
const TMST_RES: u8 = 0x08;
const TMST_DELTA_EN: u8 = 0x04;
const FIFO_HIRES_EN: u8 = 0x10;
const FIFO_GYRO_EN: u8 = 0x02;
const FIFO_ACCEL_EN: u8 = 0x01;
const BANK_SEL: u8 = 0x07;

// 6.2: the header bits a packet carries, the sensors it holds and the timestamp it ends with
const HEADER_ACCEL: u8 = 0x40;
const HEADER_GYRO: u8 = 0x20;
const HEADER_ODR_TIMESTAMP: u8 = 0x08;
const HEADER_20: u8 = 0x10;
// 6.1: the packet sizes of the table in 6.4
const SINGLE_PACKET: usize = 8;
const FULL_PACKET: usize = 16;
const HIRES_PACKET: usize = 20;
// 6.3: the FIFO is 2048 bytes less one packet
const FIFO_BYTES: usize = 2048;
const EMPTY: u8 = 0xFF;

// 4.13: 132.48 counts a degree in the registers and 2.07 in a packet, from 25 °C
const TEMP_ZERO_CENTI: i64 = 2_500;
const TEMP_REGISTER_PER_CENTI: i64 = 13_248;
const TEMP_PACKET_PER_CENTI: i64 = 207;
const TEMP_DIVISOR: i64 = 10_000;
// 14.37 and 14.38: a gyroscope word at full scale 2000 dps is 2^15 counts, and an accelerometer's at 16 g
const GYRO_COUNTS_SHIFT: u32 = 15;
const GYRO_FULL_SCALE_DPS: i64 = 2_000;
const MILLI: i64 = 1_000;
const ACCEL_COUNTS_PER_G_SHIFT: u32 = 11;
const MICRO: i64 = 1_000_000;
// 14.34: a valid sample stops at -32766 unless the last valid sample is held
const LOWEST_VALID: i64 = -32_766;
// 6.1: the 20-bit words, 131 LSB per dps in 19 bits and 8192 LSB per g in 18
const GYRO_HIRES_SHIFT: u32 = 17;
const ACCEL_HIRES_SHIFT: u32 = 13;
const GYRO_HIRES_MAX: i64 = 262_143;
const ACCEL_HIRES_MAX: i64 = 131_071;
const HIRES_INVALID: i32 = -524_288;
const HIRES_LOWEST_VALID: i64 = -524_256;
const HIRES_LOWEST: i64 = -524_288;

// 5.6: the output data rates by code as periods, which are whole nanoseconds at every rate
const ODR_PERIOD_NANOS: [u64; 16] = [
    0,
    31_250,
    62_500,
    125_000,
    250_000,
    500_000,
    1_000_000,
    5_000_000,
    10_000_000,
    20_000_000,
    40_000_000,
    80_000_000,
    160_000_000,
    320_000_000,
    640_000_000,
    2_000_000,
];

#[derive(Debug, Clone, Copy, PartialEq, Eq, serde::Serialize, serde::Deserialize)]
pub(super) enum Part {
    Icm42688p,
    Icm42605,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Sensor {
    Gyro,
    AccelLowNoise,
    AccelLowPower,
}

impl Part {
    fn index(self) -> usize {
        match self {
            Self::Icm42688p => 0,
            Self::Icm42605 => 1,
        }
    }

    // 5.6, 14.37 and 14.38: the period of the rate a code selects, or nothing for a reserved code
    fn odr_period(self, code: u8, sensor: Sensor) -> Option<u64> {
        let code = code & ODR;
        let reserved = code == 0
            || (sensor != Sensor::AccelLowPower && (0x0C..=0x0E).contains(&code))
            || (sensor == Sensor::AccelLowPower && (0x01..=0x06).contains(&code))
            || (self == Self::Icm42605 && (0x01..=0x02).contains(&code));
        (!reserved).then(|| ODR_PERIOD_NANOS[usize::from(code)])
    }

    // 9.6: single writes on the ICM-42688-P, burst writes on the ICM-42605
    fn burst_writes(self) -> bool {
        self == Self::Icm42605
    }

    // 14.34: with the hold bit clear the ICM-42605 takes the invalid code where the other holds
    fn hires(self) -> bool {
        self == Self::Icm42688p
    }

    fn registers_take_invalid_codes(self) -> bool {
        self == Self::Icm42605
    }

    // 12.7: the ICM-42688-P's timestamp counts 30/32 of true microseconds
    fn timestamp_ratio(self) -> (u128, u128) {
        match self {
            Self::Icm42688p => (30, 32),
            Self::Icm42605 => (1, 1),
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Access {
    ReadWrite,
    ReadOnly,
    ReadClear,
    WriteClear,
}

#[derive(Debug)]
struct Row {
    bank: u8,
    address: u8,
    access: Access,
    resets: [u8; 2],
}

const fn row(bank: u8, address: u8, access: Access, reset: u8) -> Row {
    Row {
        bank,
        address,
        access,
        resets: [reset, reset],
    }
}

const fn rw(bank: u8, address: u8, reset: u8) -> Row {
    row(bank, address, Access::ReadWrite, reset)
}

const fn ro(bank: u8, address: u8, reset: u8) -> Row {
    row(bank, address, Access::ReadOnly, reset)
}

const fn rc(bank: u8, address: u8, reset: u8) -> Row {
    row(bank, address, Access::ReadClear, reset)
}

const fn wc(bank: u8, address: u8, reset: u8) -> Row {
    row(bank, address, Access::WriteClear, reset)
}

const fn icm42605_resets(mut register: Row, reset: u8) -> Row {
    register.resets[1] = reset;
    register
}

// 13.1 to 13.4 with the reset values of 14 to 17
const MAP: &[Row] = &[
    rw(0, DEVICE_CONFIG, 0x00),
    rw(0, DRIVE_CONFIG, 0x05),
    rw(0, INT_CONFIG, 0x00),
    rw(0, FIFO_CONFIG, 0x00),
    ro(0, TEMP_DATA1, 0x80),
    ro(0, TEMP_DATA0, 0x00),
    ro(0, ACCEL_DATA_X1, 0x80),
    ro(0, ACCEL_DATA_X0, 0x00),
    ro(0, ACCEL_DATA_Y1, 0x80),
    ro(0, ACCEL_DATA_Y0, 0x00),
    ro(0, ACCEL_DATA_Z1, 0x80),
    ro(0, ACCEL_DATA_Z0, 0x00),
    ro(0, GYRO_DATA_X1, 0x80),
    ro(0, GYRO_DATA_X0, 0x00),
    ro(0, GYRO_DATA_Y1, 0x80),
    ro(0, GYRO_DATA_Y0, 0x00),
    ro(0, GYRO_DATA_Z1, 0x80),
    ro(0, GYRO_DATA_Z0, 0x00),
    ro(0, TMST_FSYNCH, 0x00),
    ro(0, TMST_FSYNCL, 0x00),
    rc(0, INT_STATUS, 0x10),
    ro(0, FIFO_COUNTH, 0x00),
    ro(0, FIFO_COUNTL, 0x00),
    ro(0, FIFO_DATA, 0xFF),
    ro(0, APEX_DATA0, 0x00),
    ro(0, APEX_DATA1, 0x00),
    ro(0, APEX_DATA2, 0x00),
    ro(0, APEX_DATA3, 0x04),
    ro(0, APEX_DATA4, 0x00),
    ro(0, APEX_DATA5, 0x00),
    rc(0, INT_STATUS2, 0x00),
    rc(0, INT_STATUS3, 0x00),
    wc(0, SIGNAL_PATH_RESET, 0x00),
    rw(0, INTF_CONFIG0, 0x30),
    rw(0, INTF_CONFIG1, 0x91),
    rw(0, PWR_MGMT0, 0x00),
    rw(0, GYRO_CONFIG0, 0x06),
    rw(0, ACCEL_CONFIG0, 0x06),
    rw(0, GYRO_CONFIG1, 0x16),
    rw(0, GYRO_ACCEL_CONFIG0, 0x11),
    rw(0, ACCEL_CONFIG1, 0x0D),
    rw(0, TMST_CONFIG, 0x23),
    rw(0, APEX_CONFIG0, 0x82),
    rw(0, SMD_CONFIG, 0x00),
    rw(0, FIFO_CONFIG1, 0x00),
    rw(0, FIFO_CONFIG2, 0x00),
    rw(0, FIFO_CONFIG3, 0x00),
    rw(0, FSYNC_CONFIG, 0x10),
    rw(0, INT_CONFIG0, 0x00),
    rw(0, INT_CONFIG1, 0x10),
    rw(0, INT_SOURCE0, 0x10),
    rw(0, INT_SOURCE1, 0x00),
    rw(0, INT_SOURCE3, 0x00),
    rw(0, INT_SOURCE4, 0x00),
    ro(0, FIFO_LOST_PKT0, 0x00),
    ro(0, FIFO_LOST_PKT1, 0x00),
    rw(0, SELF_TEST_CONFIG, 0x00),
    icm42605_resets(ro(0, WHO_AM_I, 0x47), 0x42),
    icm42605_resets(rw(1, SENSOR_CONFIG0, 0x80), 0x00),
    icm42605_resets(rw(1, GYRO_CONFIG_STATIC2, 0xA0), 0xA8),
    icm42605_resets(rw(1, GYRO_CONFIG_STATIC3, 0x0D), 0x3F),
    icm42605_resets(rw(1, GYRO_CONFIG_STATIC4, 0xAA), 0x80),
    icm42605_resets(rw(1, GYRO_CONFIG_STATIC5, 0x80), 0x3F),
    rw(1, GYRO_CONFIG_STATIC6, 0x00),
    rw(1, GYRO_CONFIG_STATIC7, 0x00),
    rw(1, GYRO_CONFIG_STATIC8, 0x00),
    rw(1, GYRO_CONFIG_STATIC9, 0x00),
    rw(1, GYRO_CONFIG_STATIC10, 0x11),
    rw(1, XG_ST_DATA, 0x00),
    rw(1, YG_ST_DATA, 0x00),
    rw(1, ZG_ST_DATA, 0x00),
    ro(1, TMSTVAL0, 0x00),
    ro(1, TMSTVAL1, 0x00),
    ro(1, TMSTVAL2, 0x00),
    icm42605_resets(rw(1, INTF_CONFIG4, 0x83), 0x03),
    icm42605_resets(rw(1, INTF_CONFIG5, 0x00), 0x20),
    rw(1, INTF_CONFIG6, 0x5F),
    icm42605_resets(rw(2, ACCEL_CONFIG_STATIC2, 0x30), 0x7E),
    icm42605_resets(rw(2, ACCEL_CONFIG_STATIC3, 0x40), 0x80),
    icm42605_resets(rw(2, ACCEL_CONFIG_STATIC4, 0x62), 0x3F),
    rw(2, XA_ST_DATA, 0x00),
    rw(2, YA_ST_DATA, 0x00),
    rw(2, ZA_ST_DATA, 0x00),
    rw(4, APEX_CONFIG1, 0xA2),
    rw(4, APEX_CONFIG2, 0x85),
    rw(4, APEX_CONFIG3, 0x51),
    rw(4, APEX_CONFIG4, 0xA4),
    rw(4, APEX_CONFIG5, 0x8C),
    rw(4, APEX_CONFIG6, 0x5C),
    rw(4, APEX_CONFIG7, 0x45),
    rw(4, APEX_CONFIG8, 0x5B),
    rw(4, APEX_CONFIG9, 0x00),
    rw(4, ACCEL_WOM_X_THR, 0x00),
    rw(4, ACCEL_WOM_Y_THR, 0x00),
    rw(4, ACCEL_WOM_Z_THR, 0x00),
    rw(4, INT_SOURCE6, 0x00),
    rw(4, INT_SOURCE7, 0x00),
    rw(4, INT_SOURCE8, 0x00),
    rw(4, INT_SOURCE9, 0x00),
    rw(4, INT_SOURCE10, 0x00),
    rw(4, OFFSET_USER0, 0x00),
    rw(4, OFFSET_USER1, 0x00),
    rw(4, OFFSET_USER2, 0x00),
    rw(4, OFFSET_USER3, 0x00),
    rw(4, OFFSET_USER4, 0x00),
    rw(4, OFFSET_USER5, 0x00),
    rw(4, OFFSET_USER6, 0x00),
    rw(4, OFFSET_USER7, 0x00),
    rw(4, OFFSET_USER8, 0x00),
];

const fn slot(bank: u8) -> Option<usize> {
    match bank {
        0 => Some(0),
        1 => Some(1),
        2 => Some(2),
        4 => Some(3),
        _ => None,
    }
}

fn find(bank: u8, address: u8) -> Option<&'static Row> {
    MAP.iter()
        .find(|register| register.bank == bank && register.address == address)
}

#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, serde::Serialize, serde::Deserialize)]
struct Words {
    accel: [i16; 3],
    gyro: [i16; 3],
    accel_hires: [i32; 3],
    gyro_hires: [i32; 3],
    temperature: i16,
    packet_temperature: i8,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct Running {
    gyro: Option<u64>,
    accel: Option<u64>,
}

impl Running {
    // 14.6 and 4.13: the faster of the sensors that run sets the pace
    fn period(self) -> Option<u64> {
        [self.gyro, self.accel].into_iter().flatten().min()
    }
}

#[derive(Debug, Clone, Copy, serde::Serialize, serde::Deserialize)]
struct Cadence {
    start: VirtualNanos,
    period: Option<u64>,
    taken: u64,
}

impl Cadence {
    fn at(start: VirtualNanos, period: Option<u64>) -> Self {
        Self {
            start,
            period,
            taken: 0,
        }
    }
}

#[derive(Debug, serde::Serialize, serde::Deserialize)]
struct Fifo {
    bytes: VecDeque<u8>,
    latched: u16,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, serde::Serialize, serde::Deserialize)]
enum Phase {
    Address,
    Read(u8),
    Write(u8),
    Spent,
}

#[derive(Debug, serde::Serialize, serde::Deserialize)]
pub(super) struct Engine {
    part: Part,
    #[serde(with = "crate::snapshot::array")]
    registers: [u8; BANKS * BANK_BYTES],
    bank_select: u8,
    signals: Signals,
    cadence: Cadence,
    fifo: Fifo,
    last_valid: Words,
    sampled: bool,
    phase: Phase,
    selected: bool,
}

impl Engine {
    pub(super) fn new(part: Part) -> Self {
        let mut engine = Self {
            part,
            registers: [0; BANKS * BANK_BYTES],
            bank_select: 0,
            signals: Signals::still(2_500),
            cadence: Cadence::at(VirtualNanos(0), None),
            fifo: Fifo {
                bytes: VecDeque::with_capacity(FIFO_BYTES),
                latched: 0,
            },
            last_valid: Words::default(),
            sampled: false,
            phase: Phase::Address,
            selected: false,
        };
        engine.reset(VirtualNanos(0));
        engine
    }

    pub(super) fn set_rate(&mut self, x: i32, y: i32, z: i32) {
        self.signals.rate = [x, y, z];
    }

    pub(super) fn set_acceleration(&mut self, x: i32, y: i32, z: i32) {
        self.signals.acceleration = [x, y, z];
    }

    pub(super) fn set_temperature(&mut self, centi_celsius: i32) {
        self.signals.centi_celsius = centi_celsius;
    }

    // 14.1: every register at its reset, the FIFO empty and sampling stopped
    fn reset(&mut self, now: VirtualNanos) {
        for register in MAP {
            if let Some(index) = Self::index(register.bank, register.address) {
                self.registers[index] = register.resets[self.part.index()];
            }
        }
        self.bank_select = 0;
        self.fifo.bytes.clear();
        self.fifo.latched = 0;
        self.last_valid = Words::default();
        self.sampled = false;
        self.cadence = Cadence::at(now, None);
    }

    fn index(bank: u8, address: u8) -> Option<usize> {
        Some(slot(bank)? * BANK_BYTES + usize::from(address))
    }

    fn register(&self, address: u8) -> u8 {
        self.registers[usize::from(address)]
    }

    fn bank(&self) -> u8 {
        self.bank_select & BANK_SEL
    }

    fn big_endian_data(&self) -> bool {
        self.register(INTF_CONFIG0) & SENSOR_DATA_ENDIAN != 0
    }

    fn holds_last(&self) -> bool {
        self.register(INTF_CONFIG0) & FIFO_HOLD_LAST_DATA_EN != 0
    }

    // 14.36, 14.37 and 14.38: the sensors that run, each at its rate
    fn running(&self) -> Running {
        let power = self.register(PWR_MGMT0);
        let rate = |on: bool, code: u8, sensor: Sensor| {
            on.then(|| self.part.odr_period(code, sensor)).flatten()
        };
        Running {
            gyro: rate(
                power & GYRO_MODE == GYRO_LOW_NOISE,
                self.register(GYRO_CONFIG0),
                Sensor::Gyro,
            ),
            accel: rate(
                power & ACCEL_MODE >= ACCEL_LOW_POWER,
                self.register(ACCEL_CONFIG0),
                if power & ACCEL_MODE == ACCEL_LOW_POWER {
                    Sensor::AccelLowPower
                } else {
                    Sensor::AccelLowNoise
                },
            ),
        }
    }

    fn retime(&mut self, now: VirtualNanos) {
        let period = self.running().period();
        if period != self.cadence.period {
            self.cadence = Cadence::at(now, period);
        }
    }

    fn measure(&self) -> Words {
        let floor = if self.holds_last() {
            i64::from(i16::MIN)
        } else {
            LOWEST_VALID
        };
        let gyro_step = u32::from((self.register(GYRO_CONFIG0) & FS_SEL) >> FS_SEL_SHIFT);
        let accel_step = match (self.register(ACCEL_CONFIG0) & FS_SEL) >> FS_SEL_SHIFT {
            step @ 0..=3 => u32::from(step),
            _ => 0,
        };
        let gyro = self.signals.rate.map(|rate| {
            saturate(
                round(
                    i64::from(rate) << (GYRO_COUNTS_SHIFT + gyro_step),
                    GYRO_FULL_SCALE_DPS * MILLI,
                )
                .max(floor),
            )
        });
        let accel = self.signals.acceleration.map(|acceleration| {
            saturate(
                round(
                    i64::from(acceleration) << (ACCEL_COUNTS_PER_G_SHIFT + accel_step),
                    MICRO,
                )
                .max(floor),
            )
        });
        let hires_floor = if self.holds_last() {
            HIRES_LOWEST
        } else {
            HIRES_LOWEST_VALID
        };
        let gyro_hires = self.signals.rate.map(|rate| {
            let counts = round(i64::from(rate) << GYRO_HIRES_SHIFT, MICRO)
                .clamp(-GYRO_HIRES_MAX - 1, GYRO_HIRES_MAX);
            hires_word(counts * 2, hires_floor)
        });
        let accel_hires = self.signals.acceleration.map(|acceleration| {
            let counts = round(i64::from(acceleration) << ACCEL_HIRES_SHIFT, MICRO)
                .clamp(-ACCEL_HIRES_MAX - 1, ACCEL_HIRES_MAX);
            hires_word(counts * 4, hires_floor)
        });
        let offset = i64::from(self.signals.centi_celsius) - TEMP_ZERO_CENTI;
        Words {
            accel,
            gyro,
            accel_hires,
            gyro_hires,
            temperature: saturate(round(offset * TEMP_REGISTER_PER_CENTI, TEMP_DIVISOR).max(floor)),
            packet_temperature: i8::try_from(
                round(offset * TEMP_PACKET_PER_CENTI, TEMP_DIVISOR)
                    .clamp(i64::from(i8::MIN), i64::from(i8::MAX)),
            )
            .expect("held to eight bits"),
        }
    }

    // 14.34: a sixteen-bit field as the wire carries it, from its big-endian bytes
    fn ordered(&self, big_endian: [u8; 2]) -> [u8; 2] {
        if self.big_endian_data() {
            big_endian
        } else {
            [big_endian[1], big_endian[0]]
        }
    }

    fn data_bytes(&self, word: i16) -> [u8; 2] {
        self.ordered(word.to_be_bytes())
    }

    fn put_words(&mut self, first: u8, words: &[i16]) {
        for (position, word) in words.iter().enumerate() {
            let index = usize::from(first) + 2 * position;
            let bytes = self.data_bytes(*word);
            self.registers[index..index + 2].copy_from_slice(&bytes);
        }
    }

    // 14.5 to 14.18: the sense registers
    fn show(&mut self, words: &Words, running: Running) {
        let invalid = !self.holds_last() && self.part.registers_take_invalid_codes();
        self.put_words(TEMP_DATA1, &[words.temperature]);
        for (first, valid, measured) in [
            (ACCEL_DATA_X1, running.accel.is_some(), words.accel),
            (GYRO_DATA_X1, running.gyro.is_some(), words.gyro),
        ] {
            if valid {
                self.put_words(first, &measured);
            } else if invalid {
                self.put_words(first, &[i16::MIN; 3]);
            }
        }
    }

    // 6.2 and 14.34: the words a packet carries
    fn packet_words(&mut self, words: &Words, running: Running) -> Words {
        let hold = self.holds_last();
        let mut packet = *words;
        if running.accel.is_some() {
            self.last_valid.accel = words.accel;
            self.last_valid.accel_hires = words.accel_hires;
        } else {
            packet.accel_hires = if hold {
                self.last_valid.accel_hires
            } else {
                [HIRES_INVALID; 3]
            };
            packet.accel = if hold {
                self.last_valid.accel
            } else {
                [i16::MIN; 3]
            };
        }
        if running.gyro.is_some() {
            self.last_valid.gyro = words.gyro;
            self.last_valid.gyro_hires = words.gyro_hires;
        } else {
            packet.gyro_hires = if hold {
                self.last_valid.gyro_hires
            } else {
                [HIRES_INVALID; 3]
            };
            packet.gyro = if hold {
                self.last_valid.gyro
            } else {
                [i16::MIN; 3]
            };
        }
        packet
    }

    // 12.7 and 14.42: the counter of microseconds, or of sixteen of them at `TMST_RES`, at an instant
    fn timestamp(&self, at: VirtualNanos) -> u16 {
        let (numerator, denominator) = self.part.timestamp_ratio();
        let resolution = if self.register(TMST_CONFIG) & TMST_RES != 0 {
            16
        } else {
            1
        };
        let ticks = u128::from(at.0) * numerator / (denominator * 1_000 * resolution);
        u16::try_from(ticks % 0x1_0000).expect("reduced to sixteen bits")
    }

    fn hires(&self) -> bool {
        self.part.hires() && self.register(FIFO_CONFIG1) & FIFO_HIRES_EN != 0
    }

    // 6.4's table: the size of a packet for the sensors `FIFO_CONFIG1` enables, or nothing where it enables none
    fn packet_size(&self) -> Option<usize> {
        let config = self.register(FIFO_CONFIG1);
        if self.hires() {
            return Some(HIRES_PACKET);
        }
        match (config & FIFO_ACCEL_EN != 0, config & FIFO_GYRO_EN != 0) {
            (true, true) => Some(FULL_PACKET),
            (true, false) | (false, true) => Some(SINGLE_PACKET),
            (false, false) => None,
        }
    }

    // 6.1 and 6.2: one packet
    fn packet(&self, words: &Words, at: VirtualNanos, period: u64) -> Vec<u8> {
        let config = self.register(FIFO_CONFIG1);
        let accel = config & FIFO_ACCEL_EN != 0;
        let gyro = config & FIFO_GYRO_EN != 0;
        let stamp = if self.register(TMST_CONFIG) & TMST_DELTA_EN != 0 {
            self.timestamp(VirtualNanos(period))
        } else {
            self.timestamp(at)
        };
        if self.hires() {
            return self.hires_packet(words, stamp);
        }
        let mut header = 0;
        let mut body = Vec::with_capacity(FULL_PACKET);
        if accel {
            header |= HEADER_ACCEL;
            for word in words.accel {
                body.extend(self.data_bytes(word));
            }
        }
        if gyro {
            header |= HEADER_GYRO;
            for word in words.gyro {
                body.extend(self.data_bytes(word));
            }
        }
        body.push(words.packet_temperature.to_be_bytes()[0]);
        if accel && gyro {
            header |= HEADER_ODR_TIMESTAMP;
            body.extend(self.ordered(stamp.to_be_bytes()));
        }
        let mut packet = vec![header];
        packet.extend(body);
        packet
    }

    // 6.1 packet 4
    fn hires_packet(&self, words: &Words, stamp: u16) -> Vec<u8> {
        let mut packet = vec![HEADER_ACCEL | HEADER_GYRO | HEADER_20 | HEADER_ODR_TIMESTAMP];
        let upper = |value: i32| u16::try_from((value >> 4) & 0xFFFF).expect("sixteen bits");
        for value in words.accel_hires.into_iter().chain(words.gyro_hires) {
            packet.extend(self.ordered(upper(value).to_be_bytes()));
        }
        packet.extend(self.data_bytes(words.temperature));
        packet.extend(self.ordered(stamp.to_be_bytes()));
        for axis in 0..3 {
            let nibble = |value: i32| u8::try_from(value & 0xF).expect("a nibble");
            packet.push((nibble(words.accel_hires[axis]) << 4) | nibble(words.gyro_hires[axis]));
        }
        packet
    }

    // 14.4: the samples `first` to `last` go into the FIFO as the mode says
    fn record(&mut self, first: u64, last: u64, period: u64, words: &Words) {
        let Some(size) = self.packet_size() else {
            return;
        };
        let mode = self.register(FIFO_CONFIG) & FIFO_MODE;
        if mode == FIFO_MODE_BYPASS {
            return;
        }
        let capacity = FIFO_BYTES - size;
        if mode == FIFO_MODE_STREAM {
            let partial = self.fifo.bytes.len() % size;
            self.fifo.bytes.drain(..partial);
        }
        let wanted = last - first + 1;
        let fit = if mode == FIFO_MODE_STREAM {
            capacity / size
        } else {
            capacity.saturating_sub(self.fifo.bytes.len()) / size
        };
        let kept = wanted.min(u64::try_from(fit).expect("a count"));
        let from = if mode == FIFO_MODE_STREAM {
            last + 1 - kept
        } else {
            first
        };
        for sample in from..from + kept {
            let at = VirtualNanos(self.cadence.start.0 + sample * period);
            let packet = self.packet(words, at, period);
            while self.fifo.bytes.len() + size > capacity && mode == FIFO_MODE_STREAM {
                self.fifo.bytes.drain(..size);
            }
            self.fifo.bytes.extend(packet);
        }
        if kept < wanted || self.fifo.bytes.len() + size > capacity {
            self.registers[usize::from(INT_STATUS)] |= FIFO_FULL_INT;
        }
    }

    fn catch_up(&mut self, now: VirtualNanos) {
        let Some(period) = self.cadence.period else {
            return;
        };
        let due = now.0.saturating_sub(self.cadence.start.0) / period;
        if due <= self.cadence.taken {
            return;
        }
        let first = self.cadence.taken + 1;
        self.cadence.taken = due;
        let running = self.running();
        let words = self.measure();
        self.sampled = true;
        self.show(&words, running);
        self.registers[usize::from(INT_STATUS)] |= DATA_RDY_INT;
        let packet_words = self.packet_words(&words, running);
        self.record(first, due, period, &packet_words);
    }

    // 14.22 and 14.34: the count in bytes, or in whole packets under `FIFO_COUNT_REC`
    fn count(&self) -> u16 {
        let bytes = self.fifo.bytes.len();
        let count = if self.register(INTF_CONFIG0) & FIFO_COUNT_REC != 0 {
            self.packet_size().map_or(0, |size| bytes / size)
        } else {
            bytes
        };
        u16::try_from(count).expect("a FIFO of 2048 bytes")
    }

    fn read_register(&mut self, address: u8) -> u8 {
        if address == REG_BANK_SEL {
            return self.bank_select;
        }
        let bank = self.bank();
        let Some(register) = find(bank, address) else {
            return 0;
        };
        let Some(index) = Self::index(bank, address) else {
            return 0;
        };
        match (register.access, bank, address) {
            (Access::WriteClear, _, _) => 0,
            (Access::ReadClear, _, _) => std::mem::take(&mut self.registers[index]),
            (_, 0, FIFO_COUNTH) => {
                self.fifo.latched = self.count();
                self.count_byte(true)
            }
            (_, 0, FIFO_COUNTL) => self.count_byte(false),
            (_, 0, FIFO_DATA) => self.fifo.bytes.pop_front().unwrap_or(EMPTY),
            (_, 0, TEMP_DATA1..=GYRO_DATA_Z0) if !self.sampled && self.holds_last() => 0,
            _ => self.registers[index],
        }
    }

    // 14.34: FIFO_COUNTH, the lower address, is the low byte of the count when it is little-endian
    fn count_byte(&self, at_counth: bool) -> u8 {
        let [high, low] = self.fifo.latched.to_be_bytes();
        let big = self.register(INTF_CONFIG0) & FIFO_COUNT_ENDIAN != 0;
        if at_counth == big { high } else { low }
    }

    fn write_register(&mut self, address: u8, byte: u8, now: VirtualNanos) {
        self.catch_up(now);
        if address == REG_BANK_SEL {
            self.bank_select = byte;
            return;
        }
        let bank = self.bank();
        let Some(register) = find(bank, address) else {
            return;
        };
        match register.access {
            Access::ReadOnly | Access::ReadClear => {}
            Access::WriteClear => self.strobe(address, byte, now),
            Access::ReadWrite => {
                let Some(index) = Self::index(bank, address) else {
                    return;
                };
                self.registers[index] = byte;
                if bank == 0 {
                    self.written(address, byte, now);
                }
            }
        }
    }

    // 14.1, 14.36, 14.37 and 14.38: what writing a register of bank 0 does besides keeping it
    fn written(&mut self, address: u8, byte: u8, now: VirtualNanos) {
        match address {
            DEVICE_CONFIG if byte & SOFT_RESET_CONFIG != 0 => self.reset(now),
            PWR_MGMT0 | GYRO_CONFIG0 | ACCEL_CONFIG0 => self.retime(now),
            _ => {}
        }
    }

    // 14.33: the bits of `SIGNAL_PATH_RESET` act when written and keep nothing
    fn strobe(&mut self, address: u8, byte: u8, now: VirtualNanos) {
        if address != SIGNAL_PATH_RESET {
            return;
        }
        if byte & FIFO_FLUSH != 0 {
            self.fifo.bytes.clear();
        }
        if byte & ABORT_AND_RESET != 0 {
            self.cadence = Cadence::at(now, self.running().period());
        }
    }

    fn next(&self, address: u8) -> u8 {
        if self.bank() == 0 && address == FIFO_DATA {
            address
        } else {
            address.wrapping_add(1) & ADDRESS
        }
    }

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

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

    // 9.6: the address byte with its direction bit, then a byte per register, incrementing
    pub(super) fn transfer(&mut self, mosi: u8, now: VirtualNanos) -> u8 {
        if !self.selected {
            return 0;
        }
        match self.phase {
            Phase::Address => {
                let address = mosi & ADDRESS;
                self.phase = if mosi & READ != 0 {
                    Phase::Read(address)
                } else {
                    Phase::Write(address)
                };
                0
            }
            Phase::Read(address) => {
                let value = self.read_register(address);
                self.phase = Phase::Read(self.next(address));
                value
            }
            Phase::Write(address) => {
                self.write_register(address, mosi, now);
                self.phase = if self.part.burst_writes() {
                    Phase::Write(self.next(address))
                } else {
                    Phase::Spent
                };
                0
            }
            Phase::Spent => 0,
        }
    }
}

fn hires_word(value: i64, floor: i64) -> i32 {
    i32::try_from(value.max(floor)).expect("held to twenty bits")
}