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