use super::StmConfig;
use crate::Velocity;
use crate::commands::operation::{ChangePatternBank, ConfigFociStm, WriteFociStmFused};
use crate::commands::{Command, WriteFociBuffer};
use crate::datagram::DatagramBuilder;
use crate::value::{ControlPoints, LoopBehavior, PatternBank, TransitionMode};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct FociStmOption {
pub bank: PatternBank,
pub sound_speed: Velocity,
pub loop_behavior: LoopBehavior,
pub transition_mode: TransitionMode,
}
impl Default for FociStmOption {
fn default() -> Self {
Self {
bank: PatternBank::B0,
sound_speed: Velocity::from_m_s(340.0),
loop_behavior: LoopBehavior::Infinite,
transition_mode: TransitionMode::Immediate,
}
}
}
#[derive(Clone, Copy, Debug)]
pub struct FociStm<'a, const N: usize> {
pub config: StmConfig,
pub points: &'a [ControlPoints<N>],
pub option: FociStmOption,
}
impl<'a, const N: usize> FociStm<'a, N> {
#[must_use]
pub fn new(
config: impl Into<StmConfig>,
points: &'a [ControlPoints<N>],
option: FociStmOption,
) -> Self {
Self {
config: config.into(),
points,
option,
}
}
}
impl<'a, const N: usize> Command<'a> for FociStm<'a, N> {
fn expand(self, builder: &mut DatagramBuilder<'a>) {
let n = self.points.len();
let config = self.config.into_sampling_config(n);
let size = n;
let num_foci = u8::try_from(N).unwrap_or(u8::MAX);
let bank = self.option.bank;
if WriteFociStmFused::<N>::fits_single_frame(n) {
builder.push(WriteFociStmFused {
bank,
points: self.points,
config,
sound_speed: self.option.sound_speed,
loop_behavior: self.option.loop_behavior,
transition_mode: self.option.transition_mode,
});
return;
}
builder
.push(WriteFociBuffer {
bank,
index_offset: 0,
points: self.points,
})
.push(ConfigFociStm {
bank,
config,
size,
num_foci,
sound_speed: self.option.sound_speed,
loop_behavior: self.option.loop_behavior,
})
.push(ChangePatternBank {
bank,
transition_mode: self.option.transition_mode,
});
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::commands::operation::MAX_FOCI_PER_FRAME;
use crate::geometry::Point3;
use crate::protocol::Cmd;
use crate::test_utils::test_geometry_arc;
use crate::value::{Intensity, Phase, SamplingConfig};
use core::num::NonZeroU16;
use crate::value::{ControlPoint, Focus};
use crate::commands::operation::{
PATTERN_FUSED_HEADER_BYTES as FH, PATTERN_FUSED_MAX_FOCI_PER_FRAME,
};
fn fused_payload<const N: usize>(stm: FociStm<'_, N>) -> [u8; crate::protocol::PAYLOAD_BYTES] {
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert_eq!(datagrams.len(), 1, "short FociStm fuses into 1 frame");
let f = datagrams.frame(0).unwrap();
assert_eq!(f.datagrams()[0].cmd, Cmd::WritePatternFused);
f.datagrams()[0].payload
}
#[test]
fn foci_stm_expands_to_a_single_fused_frame() {
let points = [
ControlPoints::new(
[ControlPoint::new(Point3::new(0.0, 0.0, 150.0), Phase::ZERO)],
Intensity(0xAA),
),
ControlPoints::new(
[ControlPoint::new(Point3::new(0.0, 0.0, 200.0), Phase::ZERO)],
Intensity(0xBB),
),
];
let payload = fused_payload(FociStm::new(
SamplingConfig::FREQ_4K,
&points,
FociStmOption::default(),
));
assert_eq!(payload[1], 0, "Foci emission_type");
assert_eq!(&payload[2..4], &10u16.to_le_bytes(), "FREQ_4K divider");
assert_eq!(&payload[4..8], &2u32.to_le_bytes(), "size = sample count");
assert_eq!(payload[8], 1, "num_foci = N");
assert_eq!(payload[9], 0xFF, "IMMEDIATE");
assert_eq!(&payload[10..12], &21760u16.to_le_bytes(), "340 m/s * 64");
let expected = Focus {
x: 0,
y: 0,
z: 6000,
intensity_or_offset: 0xAA,
}
.encode()
.unwrap();
let first = u64::from_le_bytes(payload[FH..FH + 8].try_into().unwrap());
assert_eq!(first, expected);
}
#[test]
fn foci_stm_first_focus_carries_intensity_rest_phase_offset() {
let points = [
ControlPoints::new(
[
ControlPoint::new(Point3::new(1.0, 0.0, 0.0), Phase(0x11)),
ControlPoint::new(Point3::new(-1.0, 0.0, 0.0), Phase(0x22)),
],
Intensity(0x80),
),
ControlPoints::new(
[
ControlPoint::new(Point3::new(2.0, 0.0, 0.0), Phase(0x33)),
ControlPoint::new(Point3::new(-2.0, 0.0, 0.0), Phase(0x44)),
],
Intensity(0x40),
),
];
let payload = fused_payload(FociStm::new(
SamplingConfig::new(NonZeroU16::MIN),
&points,
FociStmOption::default(),
));
let f0 = u64::from_le_bytes(payload[FH..FH + 8].try_into().unwrap());
let f1 = u64::from_le_bytes(payload[FH + 8..FH + 16].try_into().unwrap());
assert_eq!((f0 >> 54) & 0xFF, 0x80, "first focus = intensity");
assert_eq!(
(f1 >> 54) & 0xFF,
0x11,
"second focus = phase offset relative to the first"
);
assert_eq!(f0 & 0x3_FFFF, 40);
assert_eq!(f1 & 0x3_FFFF, 0x3_FFD8, "-40 in 18-bit two's complement");
assert_eq!(payload[8], 2, "num_foci = 2");
}
#[test]
fn foci_stm_auto_splits_write_frames() {
let points: Vec<ControlPoints<1>> = (0..MAX_FOCI_PER_FRAME + 5)
.map(|i| ControlPoints::from(Point3::new(0.0, 0.0, i as f32 * 0.1)))
.collect();
let stm = FociStm::new(SamplingConfig::FREQ_4K, &points, FociStmOption::default());
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert_eq!(datagrams.len(), 4);
assert_eq!(
datagrams.frame(0).unwrap().datagrams()[0].cmd,
Cmd::WritePatternBuffer
);
assert_eq!(
datagrams.frame(1).unwrap().datagrams()[0].cmd,
Cmd::WritePatternBuffer
);
assert_eq!(
datagrams.frame(2).unwrap().datagrams()[0].cmd,
Cmd::ConfigPattern
);
let size = u32::try_from(MAX_FOCI_PER_FRAME + 5).unwrap();
assert_eq!(
&datagrams.frame(2).unwrap().datagrams()[0].payload[4..8],
&size.to_le_bytes()
);
}
#[test]
#[allow(clippy::needless_update)]
fn foci_stm_option_default_via_spread_stays_non_breaking() {
let option = FociStmOption {
sound_speed: Velocity::from_m_s(350.0),
..Default::default()
};
assert_eq!(option.sound_speed, Velocity::from_m_s(350.0));
}
#[test]
fn foci_stm_bank_comes_from_option() {
let points = [
ControlPoints::from(Point3::new(0.0, 0.0, 1.0)),
ControlPoints::from(Point3::new(0.0, 0.0, 2.0)),
];
let payload = fused_payload(FociStm::new(
SamplingConfig::FREQ_4K,
&points,
FociStmOption {
bank: PatternBank::B1,
..Default::default()
},
));
assert_eq!(payload[0], 1, "bank B1");
}
#[test]
fn foci_stm_loop_behavior_encodes_rep() {
use crate::value::LoopBehavior;
let points = [
ControlPoints::from(Point3::new(0.0, 0.0, 1.0)),
ControlPoints::from(Point3::new(0.0, 0.0, 2.0)),
];
let payload = fused_payload(FociStm::new(
SamplingConfig::FREQ_4K,
&points,
FociStmOption::default(),
));
assert_eq!(
&payload[12..14],
&0xFFFFu16.to_le_bytes(),
"default = infinite"
);
let payload = fused_payload(FociStm::new(
SamplingConfig::FREQ_4K,
&points,
FociStmOption {
loop_behavior: LoopBehavior::ONCE,
transition_mode: crate::value::TransitionMode::SyncIdx,
..Default::default()
},
));
assert_eq!(&payload[12..14], &0u16.to_le_bytes(), "ONCE = rep 0");
}
#[test]
fn foci_stm_transition_mode_encodes_into_fused_frame() {
use crate::value::{GpioIn, TransitionMode};
let points = [
ControlPoints::from(Point3::new(0.0, 0.0, 1.0)),
ControlPoints::from(Point3::new(0.0, 0.0, 2.0)),
];
let payload = fused_payload(FociStm::new(
SamplingConfig::FREQ_4K,
&points,
FociStmOption {
transition_mode: TransitionMode::Gpio(GpioIn::I1),
..Default::default()
},
));
assert_eq!(payload[9], 0x02, "GPIO");
assert_eq!(&payload[16..24], &1u64.to_le_bytes());
}
#[test]
fn foci_stm_frequency_is_per_loop_over_all_points() {
use crate::units::Hz;
let points: Vec<ControlPoints<1>> = (0..4)
.map(|i| ControlPoints::from(Point3::new(0.0, 0.0, i as f32)))
.collect();
let payload = fused_payload(FociStm::new(1000.0 * Hz, &points, FociStmOption::default()));
assert_eq!(&payload[2..4], &10u16.to_le_bytes());
}
#[test]
fn long_foci_stm_falls_back_to_the_multi_frame_path() {
let points: Vec<ControlPoints<1>> = (0..=PATTERN_FUSED_MAX_FOCI_PER_FRAME)
.map(|i| ControlPoints::from(Point3::new(0.0, 0.0, i as f32 * 0.1)))
.collect();
let stm = FociStm::new(SamplingConfig::FREQ_4K, &points, FociStmOption::default());
let mut b = DatagramBuilder::new(test_geometry_arc(1));
b.push(stm);
let datagrams = b.build().unwrap();
assert!(datagrams.len() >= 3, "falls back to write+config+change");
assert_eq!(
datagrams.frame(0).unwrap().datagrams()[0].cmd,
Cmd::WritePatternBuffer
);
let last = datagrams.len() - 1;
assert_eq!(
datagrams.frame(last).unwrap().datagrams()[0].cmd,
Cmd::ChangePatternBank
);
let size = u32::try_from(points.len()).unwrap();
assert_eq!(
&datagrams.frame(last - 1).unwrap().datagrams()[0].payload[4..8],
&size.to_le_bytes()
);
}
}