use super::StmConfig;
use crate::commands::Command;
use crate::commands::operation::{
ChangePatternBank, ConfigPattern, PATTERN_MAX_PER_FRAME, PatternCompression,
WritePatternBuffer, WritePatternCompressed,
};
use crate::datagram::DatagramBuilder;
use crate::error::PayloadError;
use crate::params::{BUFFER_SIZE_MIN, EMISSION_MAX_INDICES};
use crate::value::{Emission, LoopBehavior, PatternBank, TransitionMode};
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub enum PatternStmMode {
#[default]
PhaseIntensityFull,
PhaseFull,
PhaseHalf,
}
impl PatternStmMode {
const fn compression(self) -> Option<PatternCompression> {
match self {
PatternStmMode::PhaseIntensityFull => None,
PatternStmMode::PhaseFull => Some(PatternCompression::PhaseFull),
PatternStmMode::PhaseHalf => Some(PatternCompression::PhaseHalf),
}
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub struct PatternStmOption {
pub bank: PatternBank,
pub mode: PatternStmMode,
pub loop_behavior: LoopBehavior,
pub transition_mode: TransitionMode,
}
#[derive(Clone, Copy, Debug)]
pub struct PatternStm<'a> {
pub config: StmConfig,
pub patterns: &'a [Vec<Vec<Emission>>],
pub option: PatternStmOption,
}
impl<'a> PatternStm<'a> {
#[must_use]
pub fn new(
config: impl Into<StmConfig>,
patterns: &'a [Vec<Vec<Emission>>],
option: PatternStmOption,
) -> Self {
Self {
config: config.into(),
patterns,
option,
}
}
}
impl<'a> Command<'a> for PatternStm<'a> {
fn expand(self, builder: &mut DatagramBuilder<'a>) {
let n = self.patterns.len();
if n < BUFFER_SIZE_MIN {
builder.reject(PayloadError::StmSizeOutOfRange {
size: n,
min: BUFFER_SIZE_MIN,
max: EMISSION_MAX_INDICES,
});
return;
}
let config = self.config.into_sampling_config(n);
let size = n;
let bank = self.option.bank;
match self.option.mode.compression() {
None => {
for (i, pattern) in self.patterns.iter().enumerate() {
builder.push(WritePatternBuffer {
bank,
index: i,
emissions: pattern.as_slice(),
});
}
}
Some(format) => {
let per_frame = format.per_frame();
let mut base = 0;
while base < n {
let count = per_frame.min(n - base);
let mut patterns: [Option<&'a [Vec<Emission>]>; PATTERN_MAX_PER_FRAME] =
[None; PATTERN_MAX_PER_FRAME];
for (g, slot) in patterns.iter_mut().take(count).enumerate() {
*slot = Some(self.patterns[base + g].as_slice());
}
builder.push(WritePatternCompressed {
bank,
index: base,
format,
patterns,
});
base += count;
}
}
}
builder
.push(ConfigPattern {
bank,
config,
size,
loop_behavior: self.option.loop_behavior,
})
.push(ChangePatternBank {
bank,
transition_mode: self.option.transition_mode,
});
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::geometry::Autd3;
use crate::params::EMISSION_SLOT_WORDS;
use crate::protocol::Cmd;
use crate::test_utils::test_geometry_arc;
use crate::value::SamplingConfig;
#[test]
fn pattern_stm_expands_per_index_then_config_change() {
let patterns: Vec<Vec<Vec<Emission>>> = (0..3)
.map(|_| vec![vec![Emission::default(); Autd3::NUM_TRANSDUCERS]])
.collect();
let stm = PatternStm::new(
SamplingConfig::FREQ_4K,
&patterns,
PatternStmOption::default(),
);
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert_eq!(datagrams.len(), 5);
for i in 0..3 {
let f = datagrams.frame(i).unwrap();
assert_eq!(f.datagrams()[0].cmd, Cmd::WritePatternBuffer);
let offset = u32::try_from(i * EMISSION_SLOT_WORDS).unwrap();
assert_eq!(&f.datagrams()[0].payload[2..6], &offset.to_le_bytes());
}
let cfg = datagrams.frame(3).unwrap();
assert_eq!(cfg.datagrams()[0].cmd, Cmd::ConfigPattern);
assert_eq!(cfg.datagrams()[0].payload[1], 1, "RawEmissions data_type");
assert_eq!(
&cfg.datagrams()[0].payload[2..4],
&10u16.to_le_bytes(),
"FREQ_4K divider"
);
assert_eq!(
&cfg.datagrams()[0].payload[4..8],
&3u32.to_le_bytes(),
"size = pattern count"
);
let chg = datagrams.frame(4).unwrap();
assert_eq!(chg.datagrams()[0].cmd, Cmd::ChangePatternBank);
assert_eq!(chg.datagrams()[0].payload[1], 0xFF, "IMMEDIATE");
}
#[test]
fn pattern_stm_rejects_a_single_pattern() {
use crate::error::Error;
let patterns: Vec<Vec<Vec<Emission>>> =
vec![vec![vec![Emission::default(); Autd3::NUM_TRANSDUCERS]]];
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(PatternStm::new(
SamplingConfig::FREQ_4K,
&patterns,
PatternStmOption::default(),
));
assert!(matches!(b.build(), Err(Error::InvalidPayload(_))));
}
fn make_patterns(n: usize) -> Vec<Vec<Vec<Emission>>> {
use crate::value::{Intensity, Phase};
(0..n)
.map(|k| {
let mut e = vec![Emission::default(); Autd3::NUM_TRANSDUCERS];
for (t, em) in e.iter_mut().enumerate() {
em.phase = Phase(u8::try_from((k * 7 + t) % 256).unwrap());
em.intensity = Intensity(0x80);
}
vec![e]
})
.collect()
}
#[test]
fn pattern_stm_phase_full_packs_two_indices_per_frame() {
let patterns = make_patterns(5);
let stm = PatternStm::new(
SamplingConfig::FREQ_4K,
&patterns,
PatternStmOption {
mode: PatternStmMode::PhaseFull,
..Default::default()
},
);
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert_eq!(datagrams.len(), 5);
let expected_counts = [2u8, 2, 1];
let expected_indices = [0u32, 2, 4];
for (f, (&count, &idx)) in expected_counts
.iter()
.zip(expected_indices.iter())
.enumerate()
{
let dg = &datagrams.frame(f).unwrap().datagrams()[0];
assert_eq!(dg.cmd, Cmd::WritePatternCompressed, "frame {f} cmd");
let payload = &dg.payload;
assert_eq!(payload[1], 1, "frame {f} format = PhaseFull");
assert_eq!(payload[2], count, "frame {f} count");
let offset = idx * u32::try_from(EMISSION_SLOT_WORDS).unwrap();
assert_eq!(&payload[4..8], &offset.to_le_bytes(), "frame {f} offset");
let p0 = patterns[idx as usize][0][0].phase.0;
assert_eq!(payload[8], p0, "frame {f} low phase");
}
let cfg = datagrams.frame(3).unwrap();
assert_eq!(cfg.datagrams()[0].cmd, Cmd::ConfigPattern);
assert_eq!(cfg.datagrams()[0].payload[1], 1, "data_type stays Raw");
assert_eq!(
&cfg.datagrams()[0].payload[4..8],
&5u32.to_le_bytes(),
"size = total index count"
);
}
#[test]
fn pattern_stm_loop_behavior_encodes_rep() {
use crate::value::LoopBehavior;
use core::num::NonZeroU16;
let patterns = make_patterns(3);
let stm = PatternStm::new(
SamplingConfig::FREQ_4K,
&patterns,
PatternStmOption {
loop_behavior: LoopBehavior::Finite(NonZeroU16::new(5).unwrap()),
..Default::default()
},
);
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
let cfg = datagrams.frame(3).unwrap();
assert_eq!(cfg.datagrams()[0].cmd, Cmd::ConfigPattern);
assert_eq!(
&cfg.datagrams()[0].payload[12..14],
&4u16.to_le_bytes(),
"rep = loop_count - 1"
);
}
#[test]
fn pattern_stm_phase_half_packs_four_indices_per_frame() {
let patterns = make_patterns(4);
let stm = PatternStm::new(
SamplingConfig::FREQ_4K,
&patterns,
PatternStmOption {
mode: PatternStmMode::PhaseHalf,
..Default::default()
},
);
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert_eq!(datagrams.len(), 3);
let dg = &datagrams.frame(0).unwrap().datagrams()[0];
assert_eq!(dg.cmd, Cmd::WritePatternCompressed);
let payload = &dg.payload;
assert_eq!(payload[1], 2, "format = PhaseHalf");
assert_eq!(payload[2], 4, "count = 4");
let word = u16::from_le_bytes([payload[8], payload[9]]);
let expected = u16::from(patterns[0][0][0].phase.0 >> 4)
| (u16::from(patterns[1][0][0].phase.0 >> 4) << 4)
| (u16::from(patterns[2][0][0].phase.0 >> 4) << 8)
| (u16::from(patterns[3][0][0].phase.0 >> 4) << 12);
assert_eq!(word, expected);
}
}