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