1use std::mem::size_of;
2
3use crate::{
4 defined::METER,
5 error::AUTDDriverError,
6 firmware::{
7 fpga::{
8 FOCI_STM_BUF_SIZE_MAX, FOCI_STM_FOCI_NUM_MAX, LoopBehavior, STM_BUF_SIZE_MIN, STMFocus,
9 SamplingConfig, Segment, TRANSITION_MODE_NONE, TransitionMode,
10 },
11 operation::{Operation, TypeTag, write_to_tx},
12 },
13 geometry::Device,
14};
15
16use zerocopy::{Immutable, IntoBytes};
17
18use super::ControlPoints;
19
20#[derive(Clone, Copy, PartialEq, Debug, IntoBytes, Immutable)]
21#[repr(C)]
22pub struct FociSTMControlFlags(u8);
23
24bitflags::bitflags! {
25 impl FociSTMControlFlags : u8 {
26 const NONE = 0;
27 const BEGIN = 1 << 0;
28 const END = 1 << 1;
29 const TRANSITION = 1 << 2;
30 }
31}
32
33#[repr(C, align(2))]
34#[derive(PartialEq, Debug, IntoBytes, Immutable)]
35struct FociSTMHead {
36 tag: TypeTag,
37 flag: FociSTMControlFlags,
38 send_num: u8,
39 segment: u8,
40 transition_mode: u8,
41 num_foci: u8,
42 sound_speed: u16,
43 freq_div: u16,
44 rep: u16,
45 __: [u8; 4],
46 transition_value: u64,
47}
48
49#[repr(C, align(2))]
50#[derive(IntoBytes, Immutable)]
51struct FociSTMSubseq {
52 tag: TypeTag,
53 flag: FociSTMControlFlags,
54 send_num: u8,
55 segment: u8,
56}
57
58pub trait FociSTMIterator<const N: usize>: Send + Sync {
62 fn next(&mut self) -> ControlPoints<N>;
64}
65
66pub struct FociSTMOp<const N: usize, Iterator: FociSTMIterator<N>> {
67 iter: Iterator,
68 size: usize,
69 sent: usize,
70 config: SamplingConfig,
71 loop_behavior: LoopBehavior,
72 segment: Segment,
73 transition_mode: Option<TransitionMode>,
74}
75
76impl<const N: usize, Iterator: FociSTMIterator<N>> FociSTMOp<N, Iterator> {
77 pub(crate) const fn new(
78 iter: Iterator,
79 size: usize,
80 config: SamplingConfig,
81 loop_behavior: LoopBehavior,
82 segment: Segment,
83 transition_mode: Option<TransitionMode>,
84 ) -> Self {
85 Self {
86 iter,
87 size,
88 sent: 0,
89 config,
90 loop_behavior,
91 segment,
92 transition_mode,
93 }
94 }
95}
96
97impl<const N: usize, Iterator: FociSTMIterator<N>> Operation for FociSTMOp<N, Iterator> {
98 type Error = AUTDDriverError;
99
100 fn pack(&mut self, device: &Device, tx: &mut [u8]) -> Result<usize, AUTDDriverError> {
101 if N == 0 || N > FOCI_STM_FOCI_NUM_MAX {
102 return Err(AUTDDriverError::FociSTMNumFociOutOfRange(N));
103 }
104 let total_foci = self.size * N;
105 if !(STM_BUF_SIZE_MIN..=FOCI_STM_BUF_SIZE_MAX).contains(&total_foci) {
106 return Err(AUTDDriverError::FociSTMTotalSizeOutOfRange(total_foci));
107 }
108
109 let is_first = self.sent == 0;
110
111 let send_num = {
112 let offset = if is_first {
113 size_of::<FociSTMHead>()
114 } else {
115 size_of::<FociSTMSubseq>()
116 };
117
118 let max_send_bytes = tx.len() - offset;
119 let max_send_num = max_send_bytes / (size_of::<STMFocus>() * N);
120 let send_num = (self.size - self.sent).min(max_send_num);
121
122 let mut idx = offset;
123 (0..send_num).try_for_each(|_| {
124 let p = self.iter.next();
125 let p = p.transform(device.inv());
126 write_to_tx(
127 &mut tx[idx..],
128 STMFocus::create(&p[0].point, p.intensity.0)?,
129 );
130 idx += size_of::<STMFocus>();
131 (1..N).try_for_each(|i| {
132 write_to_tx(
133 &mut tx[idx..],
134 STMFocus::create(&p[i].point, (p[i].phase_offset - p[0].phase_offset).0)?,
135 );
136 idx += size_of::<STMFocus>();
137 Result::<_, AUTDDriverError>::Ok(())
138 })
139 })?;
140
141 send_num
142 };
143
144 self.sent += send_num;
145
146 let flag = if self.size == self.sent {
147 FociSTMControlFlags::END
148 | if self.transition_mode.is_some() {
149 FociSTMControlFlags::TRANSITION
150 } else {
151 FociSTMControlFlags::NONE
152 }
153 } else {
154 FociSTMControlFlags::NONE
155 };
156 if is_first {
157 write_to_tx(
158 tx,
159 FociSTMHead {
160 tag: TypeTag::FociSTM,
161 flag: flag | FociSTMControlFlags::BEGIN,
162 segment: self.segment as _,
163 transition_mode: self
164 .transition_mode
165 .map(|m| m.mode())
166 .unwrap_or(TRANSITION_MODE_NONE),
167 transition_value: self.transition_mode.map(TransitionMode::value).unwrap_or(0),
168 send_num: send_num as _,
169 num_foci: N as u8,
170 freq_div: self.config.division()?,
171 sound_speed: (device.sound_speed / METER * 64.0).round() as u16,
172 rep: self.loop_behavior.rep(),
173 __: [0; 4],
174 },
175 );
176 Ok(size_of::<FociSTMHead>() + size_of::<STMFocus>() * send_num * N)
177 } else {
178 write_to_tx(
179 tx,
180 FociSTMSubseq {
181 tag: TypeTag::FociSTM,
182 flag,
183 segment: self.segment as _,
184 send_num: send_num as _,
185 },
186 );
187 Ok(size_of::<FociSTMSubseq>() + size_of::<STMFocus>() * send_num * N)
188 }
189 }
190
191 fn required_size(&self, _: &Device) -> usize {
192 if self.sent == 0 {
193 size_of::<FociSTMHead>() + size_of::<STMFocus>() * N
194 } else {
195 size_of::<FociSTMSubseq>() + size_of::<STMFocus>() * N
196 }
197 }
198
199 fn is_done(&self) -> bool {
200 self.size == self.sent
201 }
202}
203
204#[cfg(test)]
205mod tests {
206 use std::{
207 collections::VecDeque,
208 mem::{offset_of, size_of},
209 num::NonZeroU16,
210 };
211
212 use autd3_core::gain::EmitIntensity;
213 use rand::prelude::*;
214
215 use super::*;
216 use crate::{
217 defined::mm,
218 ethercat::DcSysTime,
219 firmware::{
220 fpga::{FOCI_STM_FIXED_NUM_UNIT, FOCI_STM_FIXED_NUM_UPPER_X},
221 operation::{ControlPoint, tests::create_device},
222 },
223 geometry::Point3,
224 };
225
226 const NUM_TRANS_IN_UNIT: u8 = 249;
227
228 struct TestIterator<const N: usize> {
229 points: VecDeque<ControlPoints<N>>,
230 }
231
232 impl<const N: usize> FociSTMIterator<N> for TestIterator<N> {
233 fn next(&mut self) -> ControlPoints<N> {
234 self.points.pop_front().unwrap()
235 }
236 }
237
238 #[test]
239 fn test() {
240 const FOCI_STM_SIZE: usize = 100;
241 const FRAME_SIZE: usize = size_of::<FociSTMHead>() + size_of::<STMFocus>() * FOCI_STM_SIZE;
242
243 let device = create_device(NUM_TRANS_IN_UNIT);
244
245 let mut tx = vec![0x00u8; FRAME_SIZE];
246
247 let mut rng = rand::rng();
248
249 let points: VecDeque<ControlPoints<1>> = (0..FOCI_STM_SIZE)
250 .map(|_| ControlPoints {
251 points: [ControlPoint::from(Point3::new(
252 rng.random_range(-500.0 * mm..500.0 * mm),
253 rng.random_range(-500.0 * mm..500.0 * mm),
254 rng.random_range(0.0 * mm..500.0 * mm),
255 ))],
256 intensity: EmitIntensity(rng.random::<u8>()),
257 })
258 .collect();
259 let rep = 0xFFFF;
260 let segment = Segment::S0;
261 let freq_div = rng.random_range(0x0001..=0xFFFF);
262 let transition_value = 0x0123456789ABCDEF;
263 let transition_mode = TransitionMode::SysTime(
264 DcSysTime::from_utc(
265 time::macros::datetime!(2000-01-01 0:00 UTC)
266 + std::time::Duration::from_nanos(transition_value),
267 )
268 .unwrap(),
269 );
270
271 let mut op = FociSTMOp::new(
272 TestIterator {
273 points: points.clone(),
274 },
275 FOCI_STM_SIZE,
276 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
277 LoopBehavior::Infinite,
278 segment,
279 Some(transition_mode),
280 );
281
282 assert_eq!(
283 op.required_size(&device),
284 size_of::<FociSTMHead>() + size_of::<STMFocus>()
285 );
286
287 assert_eq!(op.sent, 0);
288
289 assert_eq!(op.pack(&device, &mut tx), Ok(FRAME_SIZE));
290
291 assert_eq!(op.sent, FOCI_STM_SIZE);
292
293 assert_eq!(TypeTag::FociSTM as u8, tx[0]);
294 assert_eq!(
295 (FociSTMControlFlags::BEGIN
296 | FociSTMControlFlags::END
297 | FociSTMControlFlags::TRANSITION)
298 .bits(),
299 tx[1]
300 );
301 assert_eq!(
302 ((FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>()) as u8,
303 tx[2]
304 );
305 assert_eq!(segment as u8, tx[3]);
306 assert_eq!(transition_mode.mode(), tx[4]);
307 assert_eq!(1, tx[5]);
308 let sound_speed = (device.sound_speed / METER * 64.0).round() as u16;
309 assert_eq!(sound_speed as u8, tx[6]);
310 assert_eq!((sound_speed >> 8) as u8, tx[7]);
311 assert_eq!(freq_div as u8, tx[8]);
312 assert_eq!((freq_div >> 8) as u8, tx[9]);
313 assert_eq!(rep as u8, tx[10]);
314 assert_eq!((rep >> 8) as u8, tx[11]);
315 assert_eq!(transition_value as u8, tx[16]);
316 assert_eq!((transition_value >> 8) as u8, tx[17]);
317 assert_eq!((transition_value >> 16) as u8, tx[18]);
318 assert_eq!((transition_value >> 24) as u8, tx[19]);
319 assert_eq!((transition_value >> 32) as u8, tx[20]);
320 assert_eq!((transition_value >> 40) as u8, tx[21]);
321 assert_eq!((transition_value >> 48) as u8, tx[22]);
322 assert_eq!((transition_value >> 56) as u8, tx[23]);
323 tx[size_of::<FociSTMHead>()..]
324 .chunks(size_of::<STMFocus>())
325 .zip(points.iter())
326 .for_each(|(d, p)| {
327 assert_eq!(
328 d,
329 STMFocus::create(&p[0].point, p.intensity.0)
330 .unwrap()
331 .as_bytes()
332 );
333 });
334 }
335
336 #[test]
337 fn test_foci() {
338 const FOCI_STM_SIZE: usize = 10;
339 const N: usize = 8;
340 const FRAME_SIZE: usize =
341 size_of::<FociSTMHead>() + size_of::<STMFocus>() * FOCI_STM_SIZE * N;
342
343 let device = create_device(NUM_TRANS_IN_UNIT);
344
345 let mut tx = vec![0x00u8; FRAME_SIZE];
346
347 let mut rng = rand::rng();
348
349 let points: VecDeque<ControlPoints<N>> = (0..FOCI_STM_SIZE)
350 .map(|_| ControlPoints {
351 points: [0; N].map(|_| {
352 ControlPoint::from(Point3::new(
353 rng.random_range(-500.0 * mm..500.0 * mm),
354 rng.random_range(-500.0 * mm..500.0 * mm),
355 rng.random_range(0.0 * mm..500.0 * mm),
356 ))
357 }),
358 intensity: EmitIntensity(rng.random::<u8>()),
359 })
360 .collect();
361 let rep = 0xFFFF;
362 let segment = Segment::S0;
363 let freq_div = rng.random_range(0x0001..=0xFFFF);
364 let transition_value = 0x0123456789ABCDEF;
365 let transition_mode = TransitionMode::SysTime(
366 DcSysTime::from_utc(
367 time::macros::datetime!(2000-01-01 0:00 UTC)
368 + std::time::Duration::from_nanos(transition_value),
369 )
370 .unwrap(),
371 );
372
373 let mut op = FociSTMOp::new(
374 TestIterator {
375 points: points.clone(),
376 },
377 FOCI_STM_SIZE,
378 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
379 LoopBehavior::Infinite,
380 segment,
381 Some(transition_mode),
382 );
383
384 assert_eq!(
385 op.required_size(&device),
386 size_of::<FociSTMHead>() + size_of::<STMFocus>() * N
387 );
388
389 assert_eq!(op.sent, 0);
390
391 assert_eq!(op.pack(&device, &mut tx), Ok(FRAME_SIZE));
392
393 assert_eq!(op.sent, FOCI_STM_SIZE);
394
395 assert_eq!(TypeTag::FociSTM as u8, tx[0]);
396 assert_eq!(
397 (FociSTMControlFlags::BEGIN
398 | FociSTMControlFlags::END
399 | FociSTMControlFlags::TRANSITION)
400 .bits(),
401 tx[1]
402 );
403 assert_eq!(FOCI_STM_SIZE as u8, tx[2]);
404 assert_eq!(segment as u8, tx[3]);
405 assert_eq!(transition_mode.mode(), tx[4]);
406 assert_eq!(N as u8, tx[5]);
407 let sound_speed = (device.sound_speed / METER * 64.0).round() as u16;
408 assert_eq!(sound_speed as u8, tx[6]);
409 assert_eq!((sound_speed >> 8) as u8, tx[7]);
410 assert_eq!(freq_div as u8, tx[8]);
411 assert_eq!((freq_div >> 8) as u8, tx[9]);
412 assert_eq!(rep as u8, tx[10]);
413 assert_eq!((rep >> 8) as u8, tx[11]);
414 assert_eq!(transition_value as u8, tx[16]);
415 assert_eq!((transition_value >> 8) as u8, tx[17]);
416 assert_eq!((transition_value >> 16) as u8, tx[18]);
417 assert_eq!((transition_value >> 24) as u8, tx[19]);
418 assert_eq!((transition_value >> 32) as u8, tx[20]);
419 assert_eq!((transition_value >> 40) as u8, tx[21]);
420 assert_eq!((transition_value >> 48) as u8, tx[22]);
421 assert_eq!((transition_value >> 56) as u8, tx[23]);
422 tx[size_of::<FociSTMHead>()..]
423 .chunks(size_of::<STMFocus>() * N)
424 .zip(points.iter())
425 .for_each(|(d, p)| {
426 let base_offset = p[0].phase_offset;
427 (0..N).for_each(|i| {
428 let mut buf = [0x00u8; 8];
429 buf.copy_from_slice(
430 STMFocus::create(
431 &p[i].point,
432 if i == 0 {
433 p.intensity.0
434 } else {
435 (p[i].phase_offset - base_offset).0
436 },
437 )
438 .unwrap()
439 .as_bytes(),
440 );
441 assert_eq!(
442 d[i * size_of::<STMFocus>()..i * size_of::<STMFocus>() + 8],
443 buf
444 );
445 });
446 });
447 }
448
449 #[test]
450 fn test_div() {
451 const FRAME_SIZE: usize = 32;
452 const FOCI_STM_SIZE: usize = 7;
453
454 let device = create_device(NUM_TRANS_IN_UNIT);
455
456 let mut tx = vec![0x00u8; FRAME_SIZE];
457
458 let mut rng = rand::rng();
459
460 let points: VecDeque<ControlPoints<1>> = (0..FOCI_STM_SIZE)
461 .map(|_| ControlPoints {
462 points: [ControlPoint::from(Point3::new(
463 rng.random_range(-500.0 * mm..500.0 * mm),
464 rng.random_range(-500.0 * mm..500.0 * mm),
465 rng.random_range(0.0 * mm..500.0 * mm),
466 ))],
467 intensity: EmitIntensity(rng.random::<u8>()),
468 })
469 .collect();
470 let freq_div = rng.random_range(0x0001..0xFFFF);
471 let rep = rng.random_range(0x0001..=0xFFFF);
472 let segment = Segment::S1;
473
474 let mut op = FociSTMOp::new(
475 TestIterator {
476 points: points.clone(),
477 },
478 FOCI_STM_SIZE,
479 SamplingConfig::new(NonZeroU16::new(freq_div).unwrap()),
480 LoopBehavior::Finite(NonZeroU16::new(rep + 1).unwrap()),
481 segment,
482 None,
483 );
484
485 {
487 assert_eq!(
488 op.required_size(&device),
489 size_of::<FociSTMHead>() + size_of::<STMFocus>()
490 );
491
492 assert_eq!(op.sent, 0);
493
494 assert_eq!(
495 op.pack(&device, &mut tx),
496 Ok(size_of::<FociSTMHead>()
497 + (FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>()
498 * size_of::<STMFocus>())
499 );
500
501 assert_eq!(op.sent, 1);
502
503 assert_eq!(TypeTag::FociSTM as u8, tx[0]);
504 assert_eq!(FociSTMControlFlags::BEGIN.bits(), tx[1]);
505 assert_eq!(
506 ((FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>()) as u8,
507 tx[2]
508 );
509 assert_eq!(segment as u8, tx[3]);
510 assert_eq!(1, tx[5]);
511 let sound_speed = (device.sound_speed / METER * 64.0).round() as u16;
512 assert_eq!(sound_speed as u8, tx[6]);
513 assert_eq!((sound_speed >> 8) as u8, tx[7]);
514 assert_eq!(freq_div as u8, tx[8]);
515 assert_eq!((freq_div >> 8) as u8, tx[9]);
516 assert_eq!(rep as u8, tx[10]);
517 assert_eq!((rep >> 8) as u8, tx[11]);
518
519 tx[size_of::<FociSTMHead>()..]
520 .chunks(size_of::<STMFocus>())
521 .zip(
522 points
523 .iter()
524 .take((FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>()),
525 )
526 .for_each(|(d, p)| {
527 assert_eq!(
528 d,
529 STMFocus::create(&p[0].point, p.intensity.0)
530 .unwrap()
531 .as_bytes()
532 );
533 });
534 }
535
536 {
538 assert_eq!(
539 op.required_size(&device),
540 size_of::<FociSTMSubseq>() + size_of::<STMFocus>()
541 );
542
543 assert_eq!(
544 op.pack(&device, &mut tx),
545 Ok(size_of::<FociSTMSubseq>()
546 + (FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()
547 * size_of::<STMFocus>())
548 );
549
550 assert_eq!(op.sent, 4);
551
552 assert_eq!(TypeTag::FociSTM as u8, tx[0]);
553 assert_eq!(1, tx[offset_of!(FociSTMHead, segment)]);
554 assert_eq!(
555 ((FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()) as u8,
556 tx[offset_of!(FociSTMHead, send_num)],
557 );
558 tx[size_of::<FociSTMSubseq>()..]
559 .chunks(size_of::<STMFocus>())
560 .zip(
561 points
562 .iter()
563 .skip((FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>())
564 .take((FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()),
565 )
566 .for_each(|(d, p)| {
567 assert_eq!(
568 d,
569 STMFocus::create(&p[0].point, p.intensity.0)
570 .unwrap()
571 .as_bytes()
572 );
573 });
574 }
575
576 {
578 assert_eq!(
579 op.required_size(&device),
580 size_of::<FociSTMSubseq>() + size_of::<STMFocus>()
581 );
582
583 assert_eq!(
584 op.pack(&device, &mut tx[0..(&device.idx() + 1) * FRAME_SIZE]),
585 Ok(size_of::<FociSTMSubseq>()
586 + (FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()
587 * size_of::<STMFocus>())
588 );
589
590 assert_eq!(op.sent, FOCI_STM_SIZE);
591
592 assert_eq!(TypeTag::FociSTM as u8, tx[0]);
593 assert_eq!(
594 FociSTMControlFlags::END.bits(),
595 tx[offset_of!(FociSTMHead, flag)]
596 );
597 assert_eq!(
598 ((FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()) as u8,
599 tx[offset_of!(FociSTMHead, send_num)],
600 );
601 tx[size_of::<FociSTMSubseq>()..]
602 .chunks(size_of::<STMFocus>())
603 .zip(
604 points
605 .iter()
606 .skip(
607 (FRAME_SIZE - size_of::<FociSTMHead>()) / size_of::<STMFocus>()
608 + (FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>(),
609 )
610 .take((FRAME_SIZE - size_of::<FociSTMSubseq>()) / size_of::<STMFocus>()),
611 )
612 .for_each(|(d, p)| {
613 assert_eq!(
614 d,
615 STMFocus::create(&p[0].point, p.intensity.0)
616 .unwrap()
617 .as_bytes()
618 );
619 })
620 }
621 }
622
623 #[test]
624 fn test_point_out_of_range() {
625 const FOCI_STM_SIZE: usize = 100;
626 const FRAME_SIZE: usize = 16 + 8 * FOCI_STM_SIZE;
627
628 let device = create_device(NUM_TRANS_IN_UNIT);
629
630 let mut tx = vec![0x00u8; FRAME_SIZE];
631
632 let x = FOCI_STM_FIXED_NUM_UNIT * (FOCI_STM_FIXED_NUM_UPPER_X as f32 + 1.);
633
634 let mut op = FociSTMOp::new(
635 TestIterator {
636 points: (0..FOCI_STM_SIZE)
637 .map(|_| ControlPoint::from(Point3::new(x, x, x)).into())
638 .collect::<VecDeque<_>>(),
639 },
640 FOCI_STM_SIZE,
641 SamplingConfig::FREQ_40K,
642 LoopBehavior::Infinite,
643 Segment::S0,
644 Some(TransitionMode::SyncIdx),
645 );
646
647 assert_eq!(
648 op.pack(&device, &mut tx),
649 Err(AUTDDriverError::FociSTMPointOutOfRange(x, x, x))
650 );
651 }
652
653 #[rstest::rstest]
654 #[test]
655 #[case(Err(AUTDDriverError::FociSTMTotalSizeOutOfRange(0)), 0)]
656 #[case(Err(AUTDDriverError::FociSTMTotalSizeOutOfRange(1)), 1)]
657 #[case(Ok(()), 2)]
658 #[case(Ok(()), FOCI_STM_BUF_SIZE_MAX)]
659 #[case(Err(AUTDDriverError::FociSTMTotalSizeOutOfRange(FOCI_STM_BUF_SIZE_MAX+1)), FOCI_STM_BUF_SIZE_MAX+1)]
660 fn test_buffer_out_of_range(#[case] expected: Result<(), AUTDDriverError>, #[case] n: usize) {
661 let device = create_device(NUM_TRANS_IN_UNIT);
662
663 let mut op = FociSTMOp::new(
664 TestIterator {
665 points: (0..n)
666 .map(|_| ControlPoint::from(Point3::origin()).into())
667 .collect::<VecDeque<_>>(),
668 },
669 n,
670 SamplingConfig::FREQ_40K,
671 LoopBehavior::Infinite,
672 Segment::S0,
673 Some(TransitionMode::SyncIdx),
674 );
675
676 let mut tx = vec![0x00u8; size_of::<FociSTMHead>() + n * size_of::<STMFocus>()];
677
678 assert_eq!(op.pack(&device, &mut tx).map(|_| ()), expected);
679 }
680
681 #[test]
682 fn test_foci_out_of_range() {
683 let device = create_device(NUM_TRANS_IN_UNIT);
684
685 {
686 let mut op = FociSTMOp::new(
687 TestIterator {
688 points: (0..2)
689 .map(|_| ControlPoints::<0>::default())
690 .collect::<VecDeque<_>>(),
691 },
692 2,
693 SamplingConfig::FREQ_40K,
694 LoopBehavior::Infinite,
695 Segment::S0,
696 Some(TransitionMode::SyncIdx),
697 );
698 let mut tx = vec![0x00u8; size_of::<FociSTMHead>()];
699 assert_eq!(
700 Err(AUTDDriverError::FociSTMNumFociOutOfRange(0)),
701 op.pack(&device, &mut tx).map(|_| ())
702 );
703 }
704
705 {
706 let mut op = FociSTMOp::new(
707 TestIterator {
708 points: (0..2)
709 .map(|_| [0; 1].map(|_| ControlPoint::from(Point3::origin())).into())
710 .collect::<VecDeque<_>>(),
711 },
712 2,
713 SamplingConfig::FREQ_40K,
714 LoopBehavior::Infinite,
715 Segment::S0,
716 Some(TransitionMode::SyncIdx),
717 );
718 let mut tx = vec![0x00u8; size_of::<FociSTMHead>() + 2 * size_of::<STMFocus>()];
719 assert_eq!(Ok(()), op.pack(&device, &mut tx).map(|_| ()));
720 }
721
722 {
723 let mut op = FociSTMOp::new(
724 TestIterator {
725 points: (0..2)
726 .map(|_| {
727 [0; FOCI_STM_FOCI_NUM_MAX]
728 .map(|_| ControlPoint::from(Point3::origin()))
729 .into()
730 })
731 .collect::<VecDeque<_>>(),
732 },
733 2,
734 SamplingConfig::FREQ_40K,
735 LoopBehavior::Infinite,
736 Segment::S0,
737 Some(TransitionMode::SyncIdx),
738 );
739 let mut tx = vec![
740 0x00u8;
741 size_of::<FociSTMHead>()
742 + 2 * FOCI_STM_FOCI_NUM_MAX * size_of::<STMFocus>()
743 ];
744 assert_eq!(Ok(()), op.pack(&device, &mut tx).map(|_| ()));
745 }
746
747 {
748 let mut op = FociSTMOp::new(
749 TestIterator {
750 points: (0..2)
751 .map(|_| {
752 [0; FOCI_STM_FOCI_NUM_MAX + 1]
753 .map(|_| ControlPoint::from(Point3::origin()))
754 .into()
755 })
756 .collect::<VecDeque<_>>(),
757 },
758 2,
759 SamplingConfig::FREQ_40K,
760 LoopBehavior::Infinite,
761 Segment::S0,
762 Some(TransitionMode::SyncIdx),
763 );
764 let mut tx = vec![
765 0x00u8;
766 size_of::<FociSTMHead>()
767 + 2 * (FOCI_STM_FOCI_NUM_MAX + 1) * size_of::<STMFocus>()
768 ];
769 assert_eq!(
770 Err(AUTDDriverError::FociSTMNumFociOutOfRange(
771 FOCI_STM_FOCI_NUM_MAX + 1
772 )),
773 op.pack(&device, &mut tx).map(|_| ())
774 );
775 }
776 }
777}