rigless 0.0.5

// Copyright (c) 2026 Rigless Labs.
// SPDX-License-Identifier: MIT OR Apache-2.0
//! The LIS3MDL, a three-axis magnetometer: a register file on a serial interface whose command byte
//! carries the direction, an increment flag and the address.
//!
//! Written against ST's LIS3MDL datasheet, `DS9463` rev 7, December 2023. Every section, table and
//! figure below is that document's.
//!
//! **Conversions are derived from virtual time.** Nothing is scheduled per conversion. At each select
//! the conversions that fell due since the rate or the mode last changed are counted from the rate the
//! registers give, and if any did the output registers take the field as held. What a driver polling
//! `STATUS_REG` can observe is the same; what it cannot is the field changing between two reads.
//!
//! | Register or section | Behaviour | Source | Test |
//! |---|---|---|---|
//! | The serial protocol | Bit 0 of the first byte is `RW`, "When 0, the data DI(7:0) is written into the device. When 1, the data DO(7:0) from the device is read"; bit 1 is `MS`, "When 0, the address remains unchanged in multiple read/write commands. When 1, the address is autoincremented"; bits 2 to 7 the address; then a byte per register | 5.2, 5.2.1, 5.2.2 | `the_command_byte_carries_the_direction_the_increment_and_the_address` |
//! | The map | Table 15's registers exist; a reserved address "must not be changed" and reads zero, and the write is dropped | 6 | `a_reserved_address_takes_nothing_and_reads_zero`, `a_read_only_register_takes_no_write` |
//! | Defaults | `CTRL_REG1` `00010000`, `CTRL_REG3` `00000011`, `INT_CFG` `11101000`, every other register zero | 6, table 15 | `the_defaults_are_table_15s` |
//! | `WHO_AM_I` | `00111101` | 7.4, table 16 | `who_am_i_is_0x3d` |
//! | Data rate | `DO[2:0]`: 0.625, 1.25, 2.5, 5, 10, 20, 40, 80 Hz; `FAST_ODR` set: 1000, 560, 300, 155 Hz by `OM[1:0]`; `LP` in `CTRL_REG3`: "`DO[2:0]` is set to 0.625 Hz" | 7.5 tables 19, 21; 7.7 table 26 | `continuous_mode_converts_at_the_data_rate` |
//! | Mode | `MD[1:0]`: `00` continuous-conversion, `01` single-conversion, `10` and `11` power-down, the default | 7.7 table 27 | `power_down_converts_nothing`, `single_conversion_mode_converts_once` |
//! | The output | "The value of the magnetic field is expressed as two's complement" at 6842, 3421, 2281 or 1711 LSB/gauss by `FS[1:0]`; the low byte at the lower address unless `BLE`, "data `MSb` at lower address" | 7.11–7.13; 7.6 table 24; 2.1 table 2; 7.8 table 29 | `the_words_are_the_field_at_the_selected_full_scale`, `ble_puts_the_high_byte_first` |
//! | Hard-iron offsets | Sixteen bits two's complement, "act on the magnetic output data value in order to delete the environmental offset": subtracted in counts | 7.1–7.3 | `the_hard_iron_offsets_are_taken_from_the_output` |
//! | `STATUS_REG` | `ZYXDA` and the axis bits at each set; `ZYXOR` and the axis bits when "a new set of data has overwritten the previous set" | 7.10 table 34 | `zyxda_stands_until_the_set_is_read_and_a_second_set_marks_overrun` |
//! | Temperature | "8 LSB/°C", "acquired at each conversion cycle" when `TEMP_EN` is set; two's complement, low byte first | 2.2 table 3; 7.5; 7.14 | `the_temperature_is_eight_counts_a_degree_when_enabled` |
//! | `SOFT_RST` | "Configuration registers and user register reset function": the control registers and the offsets back to table 15 | 7.6 table 23 | `a_soft_reset_restores_the_defaults` |
//! | What it measures | A field in nanotesla on its own three axes, [`Lis3mdl::set_field`], and the die temperature in hundredths of a degree, [`Lis3mdl::set_temperature`] | 3.1 | `the_words_are_the_field_at_the_selected_full_scale` |
//!
//! # What is not, and why
//!
//! | Departure | Reason |
//! |---|---|
//! | When `ZYXDA` clears | Table 34 says when it sets and not when it clears; the model clears the `DA` bits, and the `OR` bits with them, when `OUT_Z_H`, the last register of the set, is read. Every read of the set the driver makes is a six-byte burst that ends there |
//! | `BDU` | A set never changes under a burst here, with or without the bit, because the output registers change only at the select falling; the bit is stored |
//! | The temperature's zero | Table 3 gives the slope and not the count at any temperature; 25 °C is taken as the zero. The driver never reads it |
//! | Single-conversion mode returning to power-down | Table 27 does not say what `MD` reads after the one conversion; the model converts once and leaves `MD` as written |
//! | The operating modes' averaging, `ST`, `FAST_READ`, `REBOOT`, `SIM` | Stored. A filter's averaging is nothing to a field a plant holds; the board wires four wires |
//! | `INT_CFG`, `INT_SRC`, `INT_THS`, the `INT` and `DRDY` pins | Stored and answered as written, `INT_SRC` at zero; no pin is driven and the driver reads neither |
//! | The 10 MHz ceiling on `SPC` | The seam carries words, not a clock |
//! | Reserved registers doing "permanent damage" when written | The write is dropped and nothing is damaged |

use crate::chip::spi::Peer;
use crate::device::DeviceContext;
use crate::time::VirtualNanos;

/// Table 15's register map, by address.
const OFFSET_X_REG_L_M: usize = 0x05;
const OFFSET_Z_REG_H_M: usize = 0x0A;
const WHO_AM_I: usize = 0x0F;
const CTRL_REG1: usize = 0x20;
const CTRL_REG2: usize = 0x21;
const CTRL_REG3: usize = 0x22;
const CTRL_REG4: usize = 0x23;
const CTRL_REG5: usize = 0x24;
const STATUS_REG: usize = 0x27;
const OUT_X_L: usize = 0x28;
const OUT_Z_H: usize = 0x2D;
const TEMP_OUT_L: usize = 0x2E;
const TEMP_OUT_H: usize = 0x2F;
const INT_CFG: usize = 0x30;
const INT_SRC: usize = 0x31;
const INT_THS_H: usize = 0x33;
/// Six address bits, 5.2.
const REGISTERS: usize = 0x40;

/// The bits the model acts on, section 7.
const DEVICE_ID: u8 = 0x3D;
const CTRL_REG1_DEFAULT: u8 = 0x10;
const CTRL_REG3_DEFAULT: u8 = 0x03;
const INT_CFG_DEFAULT: u8 = 0xE8;
const TEMP_EN: u8 = 0x80;
const OM: u8 = 0x60;
const DO: u8 = 0x1C;
const FAST_ODR: u8 = 0x02;
const FS: u8 = 0x60;
const SOFT_RST: u8 = 0x04;
const LP: u8 = 0x20;
const MD: u8 = 0x03;
const CONTINUOUS: u8 = 0b00;
const SINGLE: u8 = 0b01;
const BLE: u8 = 0x02;
const DATA_AVAILABLE: u8 = 0x0F;
const OVERRUN: u8 = 0xF0;
/// The command byte, 5.2: bit 0 on the wire is the byte's top bit.
const READ: u8 = 0x80;
const INCREMENT: u8 = 0x40;
const ADDRESS: u8 = 0x3F;

/// Table 21, in millihertz, by `DO[2:0]`.
const RATE_MILLIHERTZ: [u64; 8] = [625, 1_250, 2_500, 5_000, 10_000, 20_000, 40_000, 80_000];
/// Table 19, in millihertz, by `OM[1:0]` with `FAST_ODR` set.
const FAST_RATE_MILLIHERTZ: [u64; 4] = [1_000_000, 560_000, 300_000, 155_000];
/// Table 2, LSB per gauss, by `FS[1:0]`.
const SENSITIVITY: [i64; 4] = [6_842, 3_421, 2_281, 1_711];
const NANOTESLA_PER_GAUSS: i64 = 100_000;
/// Table 3, LSB per degree, about the zero the page does not give.
const TEMP_LSB_PER_DEGREE: i64 = 8;
const TEMP_ZERO_CENTI: i32 = 2_500;

/// Where a select cycle is.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Phase {
    Command,
    /// The register the next byte reads or writes, and whether the address moves on after it.
    Read(usize, bool),
    Write(usize, bool),
}

/// One LIS3MDL.
#[derive(Debug)]
pub struct Lis3mdl {
    registers: [u8; REGISTERS],
    /// The field it measures, in nanotesla on its own axes.
    nanotesla: [i32; 3],
    centi_celsius: i32,
    /// Where the current rate began: the instant and the conversion count.
    epoch_at: VirtualNanos,
    epoch_count: u64,
    /// The conversion count single-conversion mode stops at.
    single_until: Option<u64>,
    converted: u64,
    phase: Phase,
    selected: bool,
}

impl Default for Lis3mdl {
    fn default() -> Self {
        Self::new()
    }
}

impl Lis3mdl {
    /// A part at power-on: table 15's defaults, powered down, measuring a plausible northern-hemisphere
    /// field at 25 °C.
    #[must_use]
    pub fn new() -> Self {
        let mut part = Self {
            registers: [0; REGISTERS],
            nanotesla: [22_000, 0, -42_000],
            centi_celsius: TEMP_ZERO_CENTI,
            epoch_at: VirtualNanos(0),
            epoch_count: 0,
            single_until: None,
            converted: 0,
            phase: Phase::Command,
            selected: false,
        };
        part.reset(VirtualNanos(0));
        part
    }

    /// The field the part measures from now on, in nanotesla on its own axes.
    pub fn set_field(&mut self, x: i32, y: i32, z: i32) {
        self.nanotesla = [x, y, z];
    }

    /// The die temperature from now on, in hundredths of a degree.
    pub fn set_temperature(&mut self, centi_celsius: i32) {
        self.centi_celsius = centi_celsius;
    }

    /// Table 15's defaults for every register, and the conversion count restarted.
    fn reset(&mut self, now: VirtualNanos) {
        self.registers = [0; REGISTERS];
        self.registers[WHO_AM_I] = DEVICE_ID;
        self.registers[CTRL_REG1] = CTRL_REG1_DEFAULT;
        self.registers[CTRL_REG3] = CTRL_REG3_DEFAULT;
        self.registers[INT_CFG] = INT_CFG_DEFAULT;
        self.single_until = None;
        self.rebase(now);
    }

    /// Tables 19, 21, 26 and 27: the rate conversions come at, in millihertz, or nothing powered down.
    fn rate_millihertz(&self) -> u64 {
        let control1 = self.registers[CTRL_REG1];
        let control3 = self.registers[CTRL_REG3];
        if control3 & MD != CONTINUOUS && control3 & MD != SINGLE {
            0
        } else if control3 & LP != 0 {
            RATE_MILLIHERTZ[0]
        } else if control1 & FAST_ODR != 0 {
            FAST_RATE_MILLIHERTZ[usize::from((control1 & OM) >> 5)]
        } else {
            RATE_MILLIHERTZ[usize::from((control1 & DO) >> 2)]
        }
    }

    fn due(&self, now: VirtualNanos) -> u64 {
        let since = u128::from(now.0.saturating_sub(self.epoch_at.0));
        let taken = since * u128::from(self.rate_millihertz()) / 1_000_000_000_000;
        let due = self.epoch_count + u64::try_from(taken).expect("a run outlasts no count");
        self.single_until.map_or(due, |until| due.min(until))
    }

    /// A change of rate or of mode: the conversions so far stand, the next come at the new rate.
    fn rebase(&mut self, now: VirtualNanos) {
        self.epoch_count = self.due(now);
        self.epoch_at = now;
    }

    /// The conversions that fell due by `now`, if any, landing as one set from the field as held.
    fn catch_up(&mut self, now: VirtualNanos) {
        let due = self.due(now);
        if due <= self.converted {
            return;
        }
        let unread = self.registers[STATUS_REG] & DATA_AVAILABLE != 0 || due - self.converted > 1;
        self.converted = due;
        let scale = SENSITIVITY[usize::from((self.registers[CTRL_REG2] & FS) >> 5)];
        let high_first = self.registers[CTRL_REG4] & BLE != 0;
        for axis in 0..3 {
            let offset = i64::from(i16::from_le_bytes([
                self.registers[OFFSET_X_REG_L_M + 2 * axis],
                self.registers[OFFSET_X_REG_L_M + 2 * axis + 1],
            ]));
            let counts =
                round(i64::from(self.nanotesla[axis]) * scale, NANOTESLA_PER_GAUSS) - offset;
            let bytes = word(counts, high_first);
            self.registers[OUT_X_L + 2 * axis..OUT_X_L + 2 * axis + 2].copy_from_slice(&bytes);
        }
        let temperature = if self.registers[CTRL_REG1] & TEMP_EN != 0 {
            round(
                i64::from(self.centi_celsius - TEMP_ZERO_CENTI) * TEMP_LSB_PER_DEGREE,
                100,
            )
        } else {
            0
        };
        self.registers[TEMP_OUT_L..=TEMP_OUT_H].copy_from_slice(&word(temperature, false));
        if unread {
            self.registers[STATUS_REG] |= OVERRUN;
        }
        self.registers[STATUS_REG] |= DATA_AVAILABLE;
    }

    /// Table 15: whether an address is a register at all.
    fn mapped(register: usize) -> bool {
        matches!(
            register,
            OFFSET_X_REG_L_M..=OFFSET_Z_REG_H_M | WHO_AM_I | CTRL_REG1..=CTRL_REG5 | STATUS_REG..=INT_THS_H
        )
    }

    /// Table 15's "Type" column: the registers a write lands in.
    fn writable(register: usize) -> bool {
        Self::mapped(register) && !matches!(register, WHO_AM_I | STATUS_REG..=TEMP_OUT_H | INT_SRC)
    }

    fn read_register(&mut self, register: usize) -> u8 {
        let value = self.registers[register];
        if register == OUT_Z_H {
            self.registers[STATUS_REG] = 0;
        }
        value
    }

    fn write_register(&mut self, register: usize, byte: u8, now: VirtualNanos) {
        if !Self::writable(register) {
            return;
        }
        match register {
            CTRL_REG1 => {
                self.rebase(now);
                self.registers[CTRL_REG1] = byte;
            }
            CTRL_REG2 if byte & SOFT_RST != 0 => {
                for register in OFFSET_X_REG_L_M..=OFFSET_Z_REG_H_M {
                    self.registers[register] = 0;
                }
                self.rebase(now);
                self.registers[CTRL_REG1] = CTRL_REG1_DEFAULT;
                self.registers[CTRL_REG2] = 0;
                self.registers[CTRL_REG3] = CTRL_REG3_DEFAULT;
                self.registers[CTRL_REG4] = 0;
                self.registers[CTRL_REG5] = 0;
                self.single_until = None;
            }
            CTRL_REG3 => {
                self.rebase(now);
                self.registers[CTRL_REG3] = byte;
                self.single_until = (byte & MD == SINGLE).then_some(self.epoch_count + 1);
            }
            _ => self.registers[register] = byte,
        }
    }
}

/// `numerator / denominator` to the nearest integer, halves away from zero.
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 held to its range, low byte first unless `high_first`.
fn word(counts: i64, high_first: bool) -> [u8; 2] {
    let word = i16::try_from(counts.clamp(i64::from(i16::MIN), i64::from(i16::MAX)))
        .expect("held to sixteen bits");
    if high_first {
        word.to_be_bytes()
    } else {
        word.to_le_bytes()
    }
}

impl Peer for Lis3mdl {
    /// The select falling: the conversions due by now land, then the first byte is the command.
    fn select(&mut self, context: &DeviceContext<'_>) {
        self.selected = true;
        self.phase = Phase::Command;
        self.catch_up(context.now());
    }

    fn deselect(&mut self, _context: &DeviceContext<'_>) {
        self.selected = false;
        self.phase = Phase::Command;
    }

    /// 5.2: the command byte, then a byte per register, the address moving on only under `MS`.
    fn transfer(&mut self, mosi: u16, context: &DeviceContext<'_>) -> u16 {
        if !self.selected {
            return 0;
        }
        let byte = (mosi & 0xFF) as u8;
        match self.phase {
            Phase::Command => {
                let register = usize::from(byte & ADDRESS);
                let increment = byte & INCREMENT != 0;
                self.phase = if byte & READ != 0 {
                    Phase::Read(register, increment)
                } else {
                    Phase::Write(register, increment)
                };
                0
            }
            Phase::Read(register, increment) => {
                let next = if increment {
                    (register + 1) & 0x3F
                } else {
                    register
                };
                self.phase = Phase::Read(next, increment);
                u16::from(self.read_register(register))
            }
            Phase::Write(register, increment) => {
                self.write_register(register, byte, context.now());
                let next = if increment {
                    (register + 1) & 0x3F
                } else {
                    register
                };
                self.phase = Phase::Write(next, increment);
                0
            }
        }
    }
}