1use std::num::NonZeroU16;
2
3use autd3_core::gain::{Intensity, Phase};
4use autd3_driver::datagram::{FixedCompletionSteps, FixedUpdateRate};
5
6use super::{super::params::*, FPGAEmulator};
7
8#[derive(Debug, Clone, Copy)]
9pub struct SilencerEmulator<T> {
10 current: i32,
11 fixed_update_rate_mode: bool,
12 value: u16,
13 current_target: u8,
14 diff_mem: u8,
15 step_rem_mem: u16,
16 _phantom: std::marker::PhantomData<T>,
17}
18
19impl SilencerEmulator<Phase> {
20 #[allow(clippy::collapsible_else_if)]
21 #[must_use]
22 fn update_rate(&mut self, input: u8) -> u16 {
23 if self.fixed_update_rate_mode {
24 self.value
25 } else {
26 let diff = if input < self.current_target {
27 self.current_target - input
28 } else {
29 input - self.current_target
30 };
31 self.current_target = input;
32 let diff = if diff >= 128 {
33 (256 - diff as u16) as u8
34 } else {
35 diff
36 };
37 let (diff, rst) = if diff == 0 {
38 (self.diff_mem, false)
39 } else {
40 self.diff_mem = diff;
41 (diff, true)
42 };
43 let step_quo = ((diff as u16) << 8) / self.value;
44 let step_rem = ((diff as u16) << 8) % self.value;
45 if rst {
46 self.step_rem_mem = step_rem;
47 step_quo
48 } else {
49 if self.step_rem_mem == 0 {
50 step_quo
51 } else {
52 self.step_rem_mem -= 1;
53 step_quo + 1
54 }
55 }
56 }
57 }
58
59 #[allow(clippy::collapsible_else_if)]
60 #[must_use]
61 pub fn apply(&mut self, input: u8) -> u8 {
62 let update_rate = self.update_rate(input) as i32;
63 let step = ((input as i32) << 8) - self.current;
64 let step = if step < 0 {
65 if -32768 <= step { step } else { step + 65536 }
66 } else {
67 if step <= 32768 { step } else { step - 65536 }
68 };
69 if step < 0 {
70 if -update_rate <= step {
71 self.current += step;
72 } else {
73 self.current -= update_rate;
74 }
75 } else {
76 if step <= update_rate {
77 self.current += step;
78 } else {
79 self.current += update_rate;
80 }
81 }
82 (self.current >> 8) as u8
83 }
84}
85
86impl SilencerEmulator<Intensity> {
87 #[allow(clippy::collapsible_else_if)]
88 #[must_use]
89 fn update_rate(&mut self, input: u8) -> u16 {
90 if self.fixed_update_rate_mode {
91 self.value
92 } else {
93 let diff = if input < self.current_target {
94 self.current_target - input
95 } else {
96 input - self.current_target
97 };
98 self.current_target = input;
99 let (diff, rst) = if diff == 0 {
100 (self.diff_mem, false)
101 } else {
102 self.diff_mem = diff;
103 (diff, true)
104 };
105 let step_quo = ((diff as u16) << 8) / self.value;
106 let step_rem = ((diff as u16) << 8) % self.value;
107 if rst {
108 self.step_rem_mem = step_rem;
109 step_quo
110 } else {
111 if self.step_rem_mem == 0 {
112 step_quo
113 } else {
114 self.step_rem_mem -= 1;
115 step_quo + 1
116 }
117 }
118 }
119 }
120
121 #[allow(clippy::collapsible_else_if)]
122 #[must_use]
123 pub fn apply(&mut self, input: u8) -> u8 {
124 let update_rate = self.update_rate(input) as i32;
125 let step = ((input as i32) << 8) - self.current;
126 if step < 0 {
127 if -update_rate <= step {
128 self.current += step;
129 } else {
130 self.current -= update_rate;
131 }
132 } else {
133 if step <= update_rate {
134 self.current += step;
135 } else {
136 self.current += update_rate;
137 }
138 }
139 (self.current >> 8) as u8
140 }
141}
142
143impl FPGAEmulator {
144 #[must_use]
145 pub fn silencer_update_rate(&self) -> FixedUpdateRate {
146 unsafe {
147 FixedUpdateRate {
148 intensity: NonZeroU16::new_unchecked(
149 self.mem
150 .controller_bram
151 .read(ADDR_SILENCER_UPDATE_RATE_INTENSITY),
152 ),
153 phase: NonZeroU16::new_unchecked(
154 self.mem
155 .controller_bram
156 .read(ADDR_SILENCER_UPDATE_RATE_PHASE),
157 ),
158 }
159 }
160 }
161
162 #[must_use]
163 pub fn silencer_completion_steps(&self) -> FixedCompletionSteps {
164 FixedCompletionSteps {
165 intensity: NonZeroU16::new(
166 self.mem
167 .controller_bram
168 .read(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY),
169 )
170 .unwrap(),
171 phase: NonZeroU16::new(
172 self.mem
173 .controller_bram
174 .read(ADDR_SILENCER_COMPLETION_STEPS_PHASE),
175 )
176 .unwrap(),
177 strict_mode: true,
178 }
179 }
180
181 #[must_use]
182 pub fn silencer_fixed_update_rate_mode(&self) -> bool {
183 (self.mem.controller_bram.read(ADDR_SILENCER_FLAG) & SILENCER_FLAG_FIXED_UPDATE_RATE_MODE)
184 == SILENCER_FLAG_FIXED_UPDATE_RATE_MODE
185 }
186
187 #[must_use]
188 pub fn silencer_fixed_completion_steps_mode(&self) -> bool {
189 !self.silencer_fixed_update_rate_mode()
190 }
191
192 #[must_use]
193 pub fn silencer_emulator_phase(&self, initial: u8) -> SilencerEmulator<Phase> {
194 SilencerEmulator {
195 current: (initial as i32) << 8,
196 fixed_update_rate_mode: self.silencer_fixed_update_rate_mode(),
197 value: if self.silencer_fixed_update_rate_mode() {
198 self.silencer_update_rate().phase.get()
199 } else {
200 self.silencer_completion_steps().phase.get()
201 },
202 current_target: initial,
203 diff_mem: 0,
204 step_rem_mem: 0,
205 _phantom: std::marker::PhantomData,
206 }
207 }
208
209 #[must_use]
210 pub fn silencer_emulator_phase_continue_with(
211 &self,
212 prev: SilencerEmulator<Phase>,
213 ) -> SilencerEmulator<Phase> {
214 let SilencerEmulator {
215 current,
216 fixed_update_rate_mode: _fixed_update_rate_mode,
217 value: _value,
218 current_target,
219 diff_mem,
220 step_rem_mem,
221 _phantom,
222 } = prev;
223
224 SilencerEmulator {
225 current,
226 fixed_update_rate_mode: self.silencer_fixed_update_rate_mode(),
227 value: if self.silencer_fixed_update_rate_mode() {
228 self.silencer_update_rate().phase.get()
229 } else {
230 self.silencer_completion_steps().phase.get()
231 },
232 current_target,
233 diff_mem,
234 step_rem_mem,
235 _phantom: std::marker::PhantomData,
236 }
237 }
238
239 #[must_use]
240 pub fn silencer_emulator_intensity(&self, initial: u8) -> SilencerEmulator<Intensity> {
241 SilencerEmulator {
242 current: (initial as i32) << 8,
243 fixed_update_rate_mode: self.silencer_fixed_update_rate_mode(),
244 value: if self.silencer_fixed_update_rate_mode() {
245 self.silencer_update_rate().intensity.get()
246 } else {
247 self.silencer_completion_steps().intensity.get()
248 },
249 current_target: initial,
250 diff_mem: 0,
251 step_rem_mem: 0,
252 _phantom: std::marker::PhantomData,
253 }
254 }
255
256 #[must_use]
257 pub fn silencer_emulator_intensity_continue_with(
258 &self,
259 prev: SilencerEmulator<Intensity>,
260 ) -> SilencerEmulator<Intensity> {
261 let SilencerEmulator {
262 current,
263 fixed_update_rate_mode: _fixed_update_rate_mode,
264 value: _value,
265 current_target,
266 diff_mem,
267 step_rem_mem,
268 _phantom,
269 } = prev;
270
271 SilencerEmulator {
272 current,
273 fixed_update_rate_mode: self.silencer_fixed_update_rate_mode(),
274 value: if self.silencer_fixed_update_rate_mode() {
275 self.silencer_update_rate().intensity.get()
276 } else {
277 self.silencer_completion_steps().intensity.get()
278 },
279 current_target,
280 diff_mem,
281 step_rem_mem,
282 _phantom: std::marker::PhantomData,
283 }
284 }
285}
286
287#[cfg(test)]
288mod tests {
289 use super::*;
290
291 #[rstest::rstest]
292 #[test]
293 #[case([vec![0; 255], vec![1]].concat(), 1, false, 0, vec![1; 256])]
294 #[case([vec![0; 255], vec![1]].concat(), 1, true, 0, vec![1; 256])]
295 #[case([vec![1; 256], vec![0]].concat(), 1, false, 2, vec![0; 257])]
296 #[case([vec![1; 256], vec![0]].concat(), 1, true, 2, vec![0; 257])]
297 #[case([(1..=255).collect::<Vec<_>>(), vec![255]].concat(), 256, false, 0, vec![255; 256])]
298 #[case(vec![255; 256], 256, true, 0, vec![255; 256])]
299 #[case([(0..=254).rev().collect::<Vec<_>>(), vec![0]].concat(), 256, false, 255, vec![0; 256])]
300 #[case(vec![0; 256], 256, true, 255, vec![0; 256])]
301 fn apply_silencer_fixed_update_rate(
302 #[case] expect: Vec<u8>,
303 #[case] value: u16,
304 #[case] phase: bool,
305 #[case] initial: u8,
306 #[case] input: Vec<u8>,
307 ) {
308 let fpga = FPGAEmulator::new(249);
309 fpga.mem
310 .controller_bram
311 .write(ADDR_SILENCER_FLAG, SILENCER_FLAG_FIXED_UPDATE_RATE_MODE);
312 fpga.mem
313 .controller_bram
314 .write(ADDR_SILENCER_UPDATE_RATE_PHASE, value);
315 fpga.mem
316 .controller_bram
317 .write(ADDR_SILENCER_UPDATE_RATE_INTENSITY, value);
318 if phase {
319 let mut silencer = fpga.silencer_emulator_phase(initial);
320 assert_eq!(
321 expect,
322 input
323 .into_iter()
324 .map(|i| silencer.apply(i))
325 .collect::<Vec<_>>()
326 );
327 } else {
328 let mut silencer = fpga.silencer_emulator_intensity(initial);
329 assert_eq!(
330 expect,
331 input
332 .into_iter()
333 .map(|i| silencer.apply(i))
334 .collect::<Vec<_>>()
335 );
336 }
337 }
338
339 #[rstest::rstest]
340 #[test]
341 #[case::intensity_1(vec![21, 33, 45, 57, 69, 80, 92, 104, 116, 128, 128], 10, false, 10, vec![128; 11])]
342 #[case::phase_1(vec![21, 33, 45, 57, 69, 80, 92, 104, 116, 128, 128], 10, true, 10, vec![128; 11])]
343 #[case::intensity_2(vec![25, 51, 76, 102, 127, 153, 178, 204, 229, 255, 255], 10, false, 0, vec![255; 11])]
344 #[case::phase_2(vec![12, 25, 38, 51, 64, 76, 89, 102, 115, 128, 128], 10, true, 0, vec![128; 11])]
345 #[case::intensity_3(vec![25, 51, 76, 102, 127, 153, 178, 204, 229, 255, 255], 10, false, 0, vec![255; 11])]
346 #[case::phase_3(vec![253, 240, 227, 215, 202, 189, 177, 164, 151, 139, 139], 10, true, 10, vec![139; 11])]
347 #[case::intensity_4(vec![25, 51, 76, 102, 127, 153, 178, 204, 229, 255, 255], 10, false, 0, vec![255; 11])]
348 #[case::phase_4(vec![243, 230, 217, 205, 192, 179, 167, 154, 141, 129, 129], 10, true, 0, vec![129; 11])]
349 #[case::intensity_5(vec![25, 51, 76, 102, 127, 153, 178, 204, 229, 255, 255], 10, false, 0, vec![255; 11])]
350 #[case::phase_5(vec![248, 240, 233, 225, 217, 210, 202, 195, 187, 180, 180], 10, true, 0, vec![180; 11])]
351 #[case::intensity_6(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
352 #[case::phase_6(vec![174, 169, 164, 159, 153, 148, 143, 138, 133, 128, 128], 10, true, 180, vec![128; 11])]
353 #[case::intensity_7(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
354 #[case::phase_7(vec![247, 240, 232, 225, 217, 210, 202, 195, 187, 180, 180], 10, true, 255, vec![180; 11])]
355 #[case::intensity_8(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
356 #[case::phase_8(vec![11, 24, 37, 49, 62, 75, 87, 100, 113, 126, 126], 10, true, 255, vec![126; 11])]
357 #[case::intensity_9(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
358 #[case::phase_9(vec![242, 229, 216, 203, 191, 178, 165, 152, 139, 127, 127], 10, true, 255, vec![127; 11])]
359 #[case::intensity_10(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
360 #[case::phase_10(vec![0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 10], 10, true, 255, vec![10; 11])]
361 #[case::intensity_11(vec![254, 253, 252, 251, 250, 249, 248, 247, 246, 245, 245], 10, false, 255, vec![245; 11])]
362 #[case::phase_11(vec![187, 195, 202, 210, 218, 225, 233, 240, 248, 0, 0], 10, true, 180, vec![0; 11])]
363 #[case::intensity_12(vec![0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5], 10, false, 0, vec![5; 11])]
364 #[case::phase_12(vec![0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5], 10, true, 0, vec![5; 11])]
365 fn apply_silencer_fixed_completion_steps(
366 #[case] expect: Vec<u8>,
367 #[case] value: u8,
368 #[case] phase: bool,
369 #[case] initial: u8,
370 #[case] input: Vec<u8>,
371 ) {
372 let fpga = FPGAEmulator::new(249);
373 fpga.mem
374 .controller_bram
375 .write(ADDR_SILENCER_COMPLETION_STEPS_PHASE, value as _);
376 fpga.mem
377 .controller_bram
378 .write(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY, value as _);
379 if phase {
380 let mut silencer = fpga.silencer_emulator_phase(initial);
381 assert_eq!(
382 expect,
383 input
384 .into_iter()
385 .map(|i| silencer.apply(i))
386 .collect::<Vec<_>>()
387 );
388 } else {
389 let mut silencer = fpga.silencer_emulator_intensity(initial);
390 assert_eq!(
391 expect,
392 input
393 .into_iter()
394 .map(|i| silencer.apply(i))
395 .collect::<Vec<_>>()
396 );
397 }
398 }
399
400 #[test]
401 fn silencer_emulator_phase_continue_with() {
402 let fpga = FPGAEmulator::new(249);
403 fpga.mem
404 .controller_bram
405 .write(ADDR_SILENCER_COMPLETION_STEPS_PHASE, 0x01);
406 fpga.mem
407 .controller_bram
408 .write(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY, 0x01);
409
410 let mut silencer = fpga.silencer_emulator_phase(0);
411 _ = silencer.apply(0xFF);
412
413 let SilencerEmulator {
414 current,
415 fixed_update_rate_mode: _,
416 value: _,
417 current_target,
418 diff_mem,
419 step_rem_mem,
420 _phantom,
421 } = silencer;
422
423 fpga.mem
424 .controller_bram
425 .write(ADDR_SILENCER_COMPLETION_STEPS_PHASE, 0x02);
426 fpga.mem
427 .controller_bram
428 .write(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY, 0x02);
429 let silencer = fpga.silencer_emulator_phase_continue_with(silencer);
430
431 assert_eq!(current, silencer.current);
432 assert!(!silencer.fixed_update_rate_mode);
433 assert_eq!(0x02, silencer.value);
434 assert_eq!(current_target, silencer.current_target);
435 assert_eq!(diff_mem, silencer.diff_mem);
436 assert_eq!(step_rem_mem, silencer.step_rem_mem);
437
438 let SilencerEmulator {
439 current,
440 fixed_update_rate_mode: _,
441 value: _,
442 current_target,
443 diff_mem,
444 step_rem_mem,
445 _phantom,
446 } = silencer;
447
448 fpga.mem
449 .controller_bram
450 .write(ADDR_SILENCER_UPDATE_RATE_PHASE, 0x03);
451 fpga.mem
452 .controller_bram
453 .write(ADDR_SILENCER_UPDATE_RATE_INTENSITY, 0x03);
454 fpga.mem.controller_bram.write(
455 ADDR_SILENCER_FLAG,
456 1 << SILENCER_FLAG_BIT_FIXED_UPDATE_RATE_MODE,
457 );
458
459 let silencer = fpga.silencer_emulator_phase_continue_with(silencer);
460
461 assert_eq!(current, silencer.current);
462 assert!(silencer.fixed_update_rate_mode);
463 assert_eq!(0x03, silencer.value);
464 assert_eq!(current_target, silencer.current_target);
465 assert_eq!(diff_mem, silencer.diff_mem);
466 assert_eq!(step_rem_mem, silencer.step_rem_mem);
467 }
468
469 #[test]
470 fn silencer_emulator_intensity_continue_with() {
471 let fpga = FPGAEmulator::new(249);
472 fpga.mem
473 .controller_bram
474 .write(ADDR_SILENCER_COMPLETION_STEPS_PHASE, 0x01);
475 fpga.mem
476 .controller_bram
477 .write(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY, 0x01);
478
479 let mut silencer = fpga.silencer_emulator_intensity(0);
480 _ = silencer.apply(0xFF);
481
482 let SilencerEmulator {
483 current,
484 fixed_update_rate_mode: _,
485 value: _,
486 current_target,
487 diff_mem,
488 step_rem_mem,
489 _phantom,
490 } = silencer;
491
492 fpga.mem
493 .controller_bram
494 .write(ADDR_SILENCER_COMPLETION_STEPS_PHASE, 0x02);
495 fpga.mem
496 .controller_bram
497 .write(ADDR_SILENCER_COMPLETION_STEPS_INTENSITY, 0x02);
498 let silencer = fpga.silencer_emulator_intensity_continue_with(silencer);
499
500 assert_eq!(current, silencer.current);
501 assert!(!silencer.fixed_update_rate_mode);
502 assert_eq!(0x02, silencer.value);
503 assert_eq!(current_target, silencer.current_target);
504 assert_eq!(diff_mem, silencer.diff_mem);
505 assert_eq!(step_rem_mem, silencer.step_rem_mem);
506
507 let SilencerEmulator {
508 current,
509 fixed_update_rate_mode: _,
510 value: _,
511 current_target,
512 diff_mem,
513 step_rem_mem,
514 _phantom,
515 } = silencer;
516
517 fpga.mem
518 .controller_bram
519 .write(ADDR_SILENCER_UPDATE_RATE_PHASE, 0x03);
520 fpga.mem
521 .controller_bram
522 .write(ADDR_SILENCER_UPDATE_RATE_INTENSITY, 0x03);
523 fpga.mem.controller_bram.write(
524 ADDR_SILENCER_FLAG,
525 1 << SILENCER_FLAG_BIT_FIXED_UPDATE_RATE_MODE,
526 );
527
528 let silencer = fpga.silencer_emulator_intensity_continue_with(silencer);
529
530 assert_eq!(current, silencer.current);
531 assert!(silencer.fixed_update_rate_mode);
532 assert_eq!(0x03, silencer.value);
533 assert_eq!(current_target, silencer.current_target);
534 assert_eq!(diff_mem, silencer.diff_mem);
535 assert_eq!(step_rem_mem, silencer.step_rem_mem);
536 }
537}