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