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