use crate::error::{Error, PayloadError};
use crate::geometry::Device;
use crate::params::{EMISSION_MAX_INDICES, EMISSION_SLOT_WORDS};
use crate::protocol::{Cmd, PAYLOAD_BYTES};
use crate::value::{Emission, PatternBank};
use super::{Distribution, Operation};
use autd3_cpu_wire::payload::WritePatternCompressedPayload;
use zerocopy::FromBytes;
use zerocopy::little_endian::U32;
pub const PATTERN_MAX_PER_FRAME: usize = 4;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum PatternCompression {
PhaseFull,
PhaseHalf,
}
impl PatternCompression {
#[must_use]
pub const fn per_frame(self) -> usize {
match self {
PatternCompression::PhaseFull => 2,
PatternCompression::PhaseHalf => 4,
}
}
const fn as_u8(self) -> u8 {
match self {
PatternCompression::PhaseFull => 1,
PatternCompression::PhaseHalf => 2,
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct WritePatternCompressed<'a> {
pub bank: PatternBank,
pub index: usize,
pub format: PatternCompression,
pub patterns: [Option<&'a [Vec<Emission>]>; PATTERN_MAX_PER_FRAME],
}
impl Operation for WritePatternCompressed<'_> {
fn distribution(&self) -> Distribution {
Distribution::PerDevice
}
fn encode(&self, device: &Device, out: &mut [u8; PAYLOAD_BYTES]) -> Result<Cmd, Error> {
let count = self.count();
if count == 0 {
return Err(PayloadError::PatternSizeTooSmall {
size: count,
min: 1,
}
.into());
}
let last_index = self.index + count - 1;
if last_index >= EMISSION_MAX_INDICES {
return Err(PayloadError::PatternIndexOutOfRange {
index: last_index,
max: EMISSION_MAX_INDICES,
}
.into());
}
for pattern in self.patterns.iter().flatten() {
let emissions =
pattern
.get(device.idx())
.ok_or(PayloadError::EmissionsDeviceOutOfRange {
device: device.idx(),
len: pattern.len(),
})?;
if emissions.len() != device.num_transducers() {
return Err(PayloadError::TransducerCountMismatch {
device: device.idx(),
got: emissions.len(),
expected: device.num_transducers(),
}
.into());
}
}
let offset =
u32::try_from(self.index * EMISSION_SLOT_WORDS).expect("bounded by EMISSION_RAM_WORDS");
let (h, rest) = WritePatternCompressedPayload::mut_from_prefix(&mut out[..]).unwrap();
if device.num_transducers() * 2 > rest.len() {
return Err(PayloadError::PatternWriteExceedsCapacity {
device: device.idx(),
len: device.num_transducers() * 2,
capacity: rest.len(),
}
.into());
}
*h = WritePatternCompressedPayload {
bank: self.bank.as_u8(),
format: self.format.as_u8(),
count: u8::try_from(count).expect("count <= PATTERN_MAX_PER_FRAME"),
reserved: 0,
offset: U32::new(offset),
};
rest.chunks_exact_mut(2)
.take(device.num_transducers())
.enumerate()
.for_each(|(t, dst)| {
dst.copy_from_slice(&self.pack_word(device.idx(), t).to_le_bytes());
});
Ok(Cmd::WritePatternCompressed)
}
}
impl WritePatternCompressed<'_> {
fn count(&self) -> usize {
self.patterns
.iter()
.position(Option::is_none)
.unwrap_or(PATTERN_MAX_PER_FRAME)
}
fn pack_word(&self, device: usize, t: usize) -> u16 {
let (shift, hi) = match self.format {
PatternCompression::PhaseFull => (8usize, 0u8),
PatternCompression::PhaseHalf => (4usize, 4u8),
};
self.patterns
.iter()
.enumerate()
.filter_map(|(g, &p)| p.map(|s| (g, s[device][t].phase.0)))
.fold(0u16, |acc, (g, phase)| {
acc | (u16::from(phase >> hi) << (shift * g))
})
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geometry::Autd3;
use crate::test_utils::test_device;
use crate::value::{Intensity, Phase};
const HEADER_BYTES: usize = core::mem::size_of::<WritePatternCompressedPayload>();
#[test]
fn phase_full_packs_two_phases_per_word() {
let mut g0 = vec![Emission::default(); Autd3::NUM_TRANSDUCERS];
let mut g1 = vec![Emission::default(); Autd3::NUM_TRANSDUCERS];
for (i, (a, b)) in g0.iter_mut().zip(g1.iter_mut()).enumerate() {
a.phase = Phase(u8::try_from(i % 256).unwrap());
a.intensity = Intensity(0x12);
b.phase = Phase(u8::try_from((255 - i % 256) % 256).unwrap());
b.intensity = Intensity(0x34);
}
let p0 = [g0];
let p1 = [g1];
let op = WritePatternCompressed {
bank: PatternBank::B0,
index: 4,
format: PatternCompression::PhaseFull,
patterns: [Some(&p0[..]), Some(&p1[..]), None, None],
};
let mut out = [0u8; PAYLOAD_BYTES];
let cmd = op.encode(&test_device(0), &mut out).unwrap();
assert_eq!(cmd, Cmd::WritePatternCompressed);
assert_eq!(out[1], 1, "format = PhaseFull");
assert_eq!(out[2], 2, "count = 2");
let expected_offset = u32::try_from(4 * EMISSION_SLOT_WORDS).unwrap();
assert_eq!(&out[4..8], &expected_offset.to_le_bytes());
for i in 0..Autd3::NUM_TRANSDUCERS {
let word =
u16::from_le_bytes([out[HEADER_BYTES + 2 * i], out[HEADER_BYTES + 2 * i + 1]]);
let expected = u16::from(p0[0][i].phase.0) | (u16::from(p1[0][i].phase.0) << 8);
assert_eq!(word, expected, "t={i}");
}
}
#[test]
fn phase_half_packs_four_nibbles_per_word() {
let mk = |off: u8| {
let mut g = vec![Emission::default(); Autd3::NUM_TRANSDUCERS];
for (i, e) in g.iter_mut().enumerate() {
e.phase = Phase(u8::try_from((i + usize::from(off)) % 256).unwrap());
e.intensity = Intensity(0x55);
}
g
};
let (g0, g1, g2, g3) = (mk(0), mk(16), mk(32), mk(48));
let (p0, p1, p2, p3) = ([g0], [g1], [g2], [g3]);
let op = WritePatternCompressed {
bank: PatternBank::B0,
index: 8,
format: PatternCompression::PhaseHalf,
patterns: [Some(&p0[..]), Some(&p1[..]), Some(&p2[..]), Some(&p3[..])],
};
let mut out = [0u8; PAYLOAD_BYTES];
op.encode(&test_device(0), &mut out).unwrap();
assert_eq!(out[1], 2, "format = PhaseHalf");
assert_eq!(out[2], 4, "count = 4");
for i in 0..Autd3::NUM_TRANSDUCERS {
let word =
u16::from_le_bytes([out[HEADER_BYTES + 2 * i], out[HEADER_BYTES + 2 * i + 1]]);
let expected = u16::from(p0[0][i].phase.0 >> 4)
| (u16::from(p1[0][i].phase.0 >> 4) << 4)
| (u16::from(p2[0][i].phase.0 >> 4) << 8)
| (u16::from(p3[0][i].phase.0 >> 4) << 12);
assert_eq!(word, expected, "t={i}");
}
}
#[test]
fn rejects_last_index_out_of_range() {
let patterns = [vec![Emission::default(); Autd3::NUM_TRANSDUCERS]];
let op = WritePatternCompressed {
bank: PatternBank::B0,
index: EMISSION_MAX_INDICES - 1,
format: PatternCompression::PhaseFull,
patterns: [Some(&patterns[..]), Some(&patterns[..]), None, None],
};
let mut out = [0u8; PAYLOAD_BYTES];
assert!(matches!(
op.encode(&test_device(0), &mut out),
Err(Error::InvalidPayload(_))
));
}
#[test]
fn rejects_device_out_of_range() {
let patterns = [vec![Emission::default(); Autd3::NUM_TRANSDUCERS]];
let op = WritePatternCompressed {
bank: PatternBank::B0,
index: 0,
format: PatternCompression::PhaseFull,
patterns: [Some(&patterns[..]), None, None, None],
};
let mut out = [0u8; PAYLOAD_BYTES];
assert!(op.encode(&test_device(0), &mut out).is_ok());
assert!(matches!(
op.encode(&test_device(1), &mut out),
Err(Error::InvalidPayload(_))
));
}
}