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