1#![forbid(unsafe_code)]
2
3use std::time::{Duration, Instant};
39
40#[derive(Debug, Clone)]
46pub struct HoverStabilizerConfig {
47 pub drift_allowance: f32,
50
51 pub detection_threshold: f32,
54
55 pub hysteresis_cells: u16,
59
60 pub decay_rate: f32,
64
65 pub hold_timeout: Duration,
69}
70
71impl Default for HoverStabilizerConfig {
72 fn default() -> Self {
73 Self {
74 drift_allowance: 0.5,
75 detection_threshold: 2.0,
76 hysteresis_cells: 1,
77 decay_rate: 0.1,
78 hold_timeout: Duration::from_millis(500),
79 }
80 }
81}
82
83#[derive(Debug, Clone)]
89struct CandidateTarget {
90 target_id: u64,
92 cusum_score: f32,
94 last_pos: (u16, u16),
96}
97
98#[derive(Debug)]
107pub struct HoverStabilizer {
108 config: HoverStabilizerConfig,
109
110 current_target: Option<u64>,
112
113 current_target_pos: Option<(u16, u16)>,
115
116 last_update: Option<Instant>,
118
119 candidate: Option<CandidateTarget>,
121
122 switches: u64,
124}
125
126impl HoverStabilizer {
127 #[must_use]
129 pub fn new(config: HoverStabilizerConfig) -> Self {
130 Self {
131 config,
132 current_target: None,
133 current_target_pos: None,
134 last_update: None,
135 candidate: None,
136 switches: 0,
137 }
138 }
139
140 pub fn update(
153 &mut self,
154 hit_target: Option<u64>,
155 pos: (u16, u16),
156 now: Instant,
157 ) -> Option<u64> {
158 if let Some(last) = self.last_update
160 && now.duration_since(last) > self.config.hold_timeout
161 {
162 self.reset();
163 }
164 self.last_update = Some(now);
165
166 if self.current_target.is_none() {
168 if hit_target.is_some() {
169 self.current_target = hit_target;
170 self.current_target_pos = Some(pos);
171 self.switches += 1;
172 }
173 return self.current_target;
174 }
175
176 let current = self.current_target.unwrap();
177
178 if hit_target == Some(current) {
180 self.decay_candidate();
181 self.current_target_pos = Some(pos);
182 return self.current_target;
183 }
184
185 let candidate_id = hit_target.unwrap_or(u64::MAX); let distance = self.compute_distance_signal(pos);
190
191 self.update_candidate(candidate_id, distance, pos);
192
193 if let Some(ref cand) = self.candidate
195 && cand.cusum_score >= self.config.detection_threshold
196 && self.past_hysteresis_band(pos)
197 {
198 self.current_target = if candidate_id == u64::MAX {
200 None
201 } else {
202 Some(candidate_id)
203 };
204 self.current_target_pos = Some(pos);
205 self.candidate = None;
206 self.switches += 1;
207 }
208
209 self.current_target
210 }
211
212 #[inline]
214 #[must_use]
215 pub fn current_target(&self) -> Option<u64> {
216 self.current_target
217 }
218
219 pub fn reset(&mut self) {
221 self.current_target = None;
222 self.current_target_pos = None;
223 self.last_update = None;
224 self.candidate = None;
225 }
226
227 #[inline]
229 #[must_use]
230 pub fn switch_count(&self) -> u64 {
231 self.switches
232 }
233
234 #[inline]
236 #[must_use]
237 pub fn config(&self) -> &HoverStabilizerConfig {
238 &self.config
239 }
240
241 pub fn set_config(&mut self, config: HoverStabilizerConfig) {
243 self.config = config;
244 }
245
246 fn compute_distance_signal(&self, pos: (u16, u16)) -> f32 {
255 let Some(target_pos) = self.current_target_pos else {
256 return 1.0; };
258
259 let dx = (pos.0 as i32 - target_pos.0 as i32).abs();
261 let dy = (pos.1 as i32 - target_pos.1 as i32).abs();
262 let manhattan = (dx + dy) as f32;
263
264 let hysteresis = self.config.hysteresis_cells.max(1) as f32;
266
267 (manhattan - hysteresis) / hysteresis
270 }
271
272 fn update_candidate(&mut self, candidate_id: u64, distance_signal: f32, pos: (u16, u16)) {
274 let k = self.config.drift_allowance;
275
276 match &mut self.candidate {
277 Some(cand) if cand.target_id == candidate_id => {
278 cand.cusum_score = (cand.cusum_score + distance_signal - k).max(0.0);
281 cand.last_pos = pos;
282 }
283 _ => {
284 let initial_score = (distance_signal - k).max(0.0);
286 self.candidate = Some(CandidateTarget {
287 target_id: candidate_id,
288 cusum_score: initial_score,
289 last_pos: pos,
290 });
291 }
292 }
293 }
294
295 fn decay_candidate(&mut self) {
297 if let Some(ref mut cand) = self.candidate {
298 cand.cusum_score *= 1.0 - self.config.decay_rate;
299 if cand.cusum_score < 0.01 {
300 self.candidate = None;
301 }
302 }
303 }
304
305 fn past_hysteresis_band(&self, pos: (u16, u16)) -> bool {
307 let Some(target_pos) = self.current_target_pos else {
308 return true; };
310
311 let dx = (pos.0 as i32 - target_pos.0 as i32).unsigned_abs();
312 let dy = (pos.1 as i32 - target_pos.1 as i32).unsigned_abs();
313 let manhattan = dx + dy;
314
315 manhattan > u32::from(self.config.hysteresis_cells)
316 }
317}
318
319#[cfg(test)]
324mod tests {
325 use super::*;
326
327 fn now() -> Instant {
328 Instant::now()
329 }
330
331 fn stabilizer() -> HoverStabilizer {
332 HoverStabilizer::new(HoverStabilizerConfig::default())
333 }
334
335 #[test]
338 fn initial_state_is_none() {
339 let stab = stabilizer();
340 assert!(stab.current_target().is_none());
341 assert_eq!(stab.switch_count(), 0);
342 }
343
344 #[test]
345 fn first_hit_adopted_immediately() {
346 let mut stab = stabilizer();
347 let t = now();
348
349 let target = stab.update(Some(42), (10, 10), t);
350 assert_eq!(target, Some(42));
351 assert_eq!(stab.current_target(), Some(42));
352 assert_eq!(stab.switch_count(), 1);
353 }
354
355 #[test]
356 fn same_target_stays_stable() {
357 let mut stab = stabilizer();
358 let t = now();
359
360 stab.update(Some(42), (10, 10), t);
361 stab.update(Some(42), (10, 11), t);
362 stab.update(Some(42), (11, 10), t);
363
364 assert_eq!(stab.current_target(), Some(42));
365 assert_eq!(stab.switch_count(), 1); }
367
368 #[test]
369 fn jitter_does_not_switch() {
370 let mut stab = stabilizer();
371 let t = now();
372
373 stab.update(Some(42), (10, 10), t);
375
376 for i in 0..10 {
378 let target = if i % 2 == 0 { Some(99) } else { Some(42) };
379 stab.update(target, (10, 10 + (i % 2)), t);
380 }
381
382 assert_eq!(stab.current_target(), Some(42));
384 }
385
386 #[test]
387 fn sustained_crossing_triggers_switch() {
388 let mut stab = stabilizer();
389 let t = now();
390
391 stab.update(Some(42), (10, 10), t);
393
394 for i in 1..=5 {
397 stab.update(Some(99), (10, 10 + i * 2), t);
398 }
399
400 assert_eq!(stab.current_target(), Some(99));
402 assert!(stab.switch_count() >= 2);
403 }
404
405 #[test]
406 fn reset_clears_all_state() {
407 let mut stab = stabilizer();
408 let t = now();
409
410 stab.update(Some(42), (10, 10), t);
411 stab.reset();
412
413 assert!(stab.current_target().is_none());
414 }
415
416 #[test]
419 fn cusum_accumulates_on_consistent_signal() {
420 let mut stab = HoverStabilizer::new(HoverStabilizerConfig {
421 detection_threshold: 3.0,
422 hysteresis_cells: 0, ..Default::default()
424 });
425 let t = now();
426
427 stab.update(Some(42), (10, 10), t);
429
430 stab.update(Some(99), (15, 10), t);
432 stab.update(Some(99), (20, 10), t);
433 stab.update(Some(99), (25, 10), t);
434
435 assert_eq!(stab.current_target(), Some(99));
437 }
438
439 #[test]
440 fn cusum_resets_on_return() {
441 let mut stab = stabilizer();
442 let t = now();
443
444 stab.update(Some(42), (10, 10), t);
445
446 stab.update(Some(99), (12, 10), t);
448
449 stab.update(Some(42), (10, 10), t);
451 stab.update(Some(42), (10, 10), t);
452 stab.update(Some(42), (10, 10), t);
453
454 assert_eq!(stab.current_target(), Some(42));
456 }
457
458 #[test]
461 fn hysteresis_prevents_boundary_oscillation() {
462 let mut stab = HoverStabilizer::new(HoverStabilizerConfig {
463 hysteresis_cells: 2,
464 detection_threshold: 0.5, ..Default::default()
466 });
467 let t = now();
468
469 stab.update(Some(42), (10, 10), t);
470
471 stab.update(Some(99), (11, 10), t);
473 assert_eq!(stab.current_target(), Some(42));
474
475 stab.update(Some(99), (13, 10), t);
477 stab.update(Some(99), (14, 10), t);
478 stab.update(Some(99), (15, 10), t);
479
480 assert_eq!(stab.current_target(), Some(99));
482 }
483
484 #[test]
487 fn timeout_resets_target() {
488 let mut stab = HoverStabilizer::new(HoverStabilizerConfig {
489 hold_timeout: Duration::from_millis(100),
490 ..Default::default()
491 });
492 let t = now();
493
494 stab.update(Some(42), (10, 10), t);
495 assert_eq!(stab.current_target(), Some(42));
496
497 let later = t + Duration::from_millis(200);
499 stab.update(Some(99), (20, 20), later);
500
501 assert_eq!(stab.current_target(), Some(99));
503 }
504
505 #[test]
508 fn transition_to_none_with_evidence() {
509 let mut stab = HoverStabilizer::new(HoverStabilizerConfig {
510 hysteresis_cells: 0,
511 detection_threshold: 1.0,
512 ..Default::default()
513 });
514 let t = now();
515
516 stab.update(Some(42), (10, 10), t);
517
518 for i in 1..=5 {
520 stab.update(None, (10 + i * 3, 10), t);
521 }
522
523 assert!(stab.current_target().is_none());
525 }
526
527 #[test]
530 fn jitter_stability_rate() {
531 let mut stab = stabilizer();
532 let t = now();
533
534 stab.update(Some(42), (10, 10), t);
535
536 let mut stable_count = 0;
538 for i in 0..100 {
539 let target = if i % 2 == 0 { Some(99) } else { Some(42) };
540 stab.update(target, (10, 10), t);
541 if stab.current_target() == Some(42) {
542 stable_count += 1;
543 }
544 }
545
546 assert!(stable_count >= 99, "Stable count: {}", stable_count);
548 }
549
550 #[test]
551 fn crossing_detection_latency() {
552 let mut stab = HoverStabilizer::new(HoverStabilizerConfig {
553 hysteresis_cells: 1,
554 detection_threshold: 1.5,
555 drift_allowance: 0.3,
556 ..Default::default()
557 });
558 let t = now();
559
560 stab.update(Some(42), (10, 10), t);
561
562 let mut frames = 0;
564 for i in 1..=10 {
565 stab.update(Some(99), (10, 10 + i * 2), t);
566 frames += 1;
567 if stab.current_target() == Some(99) {
568 break;
569 }
570 }
571
572 assert!(frames <= 3, "Switch took {} frames", frames);
574 }
575
576 #[test]
579 fn config_getter_and_setter() {
580 let mut stab = stabilizer();
581
582 assert_eq!(stab.config().detection_threshold, 2.0);
583
584 stab.set_config(HoverStabilizerConfig {
585 detection_threshold: 5.0,
586 ..Default::default()
587 });
588
589 assert_eq!(stab.config().detection_threshold, 5.0);
590 }
591
592 #[test]
593 fn default_config_values() {
594 let config = HoverStabilizerConfig::default();
595 assert_eq!(config.drift_allowance, 0.5);
596 assert_eq!(config.detection_threshold, 2.0);
597 assert_eq!(config.hysteresis_cells, 1);
598 assert_eq!(config.decay_rate, 0.1);
599 assert_eq!(config.hold_timeout, Duration::from_millis(500));
600 }
601
602 #[test]
605 fn debug_format() {
606 let stab = stabilizer();
607 let dbg = format!("{:?}", stab);
608 assert!(dbg.contains("HoverStabilizer"));
609 }
610}