1use std::collections::VecDeque;
2use std::borrow::Borrow;
4
5type Timestamp = u64;
6type Time = f64; type TimeDelta = f64;
8type TimeDeltaMicros = u64;
9
10type Velocity = f64;
11type Position = f64;
12
13use crate::CONFIG as sconfig;
14
15#[derive(Clone, Copy, Debug, PartialEq)]
54enum Phase {
55 Inactive,
56 Interpolating,
57 Released(Time), }
59
60enum TrackPosition {
61 Top,
62 Bottom,
63}
64
65enum BounceState {
66 Bouncing(TrackPosition),
67 Normal,
68}
69
70#[derive(Clone, Copy, Debug, PartialOrd)]
71struct Event {
72 time: Time, value: f64, }
77
78impl std::fmt::Display for Event {
79 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
80 write!(f, "({}: {})", self.time, self.value)
81 }
82}
83
84impl std::cmp::Ord for Event {
85 fn cmp(&self, other: &Self) -> std::cmp::Ordering {
86 let cmp_time = self.time.partial_cmp(&other.time).expect("NaN in time field of an event");
87
88 match cmp_time {
89 std::cmp::Ordering::Equal => self.value.partial_cmp(&other.value).expect("NaN in value field of an event"),
90 other => other
91 }
92 }
93}
94
95impl std::cmp::PartialEq for Event {
96 fn eq(&self, other: &Self) -> bool {
97 self.value == other.value && self.time == other.time
98 }
99}
100
101impl std::cmp::Eq for Event {}
102
103#[derive(Clone, Copy)]
107struct Sample {
108 time: Time,
109 velocity: Velocity,
110 position: Position,
111}
112
113impl std::fmt::Display for Sample {
114 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
115 write!(f, "({}: {}, {})", self.time, self.position, self.velocity)
116 }
117}
118
119
120pub struct Interpolator {
121 redistributable: bool,
122 events: VecDeque<Event>,
128 samples: VecDeque<Sample>,
129 pan_start_time: Time,
130 current_phase: Phase,
131
132 track_bound_upper: f64,
134 track_bound_lower: f64,
135 track_initial_pos: f64,
136
137 bouncing: BounceState,
138
139 min_tick_period: TimeDelta,
140
141 last_value: f64,
142 flips_same_value: u64,
143 source: crate::Source,
144 }
147
148impl Interpolator {
149 pub fn set_source(&mut self, source: crate::Source) {
150 self.source = source;
152 }
153
154 pub fn print_events(&self) {
155 return;
156
157 println!("Current events are {{{}}}",
158 self.events.iter().fold(
159 String::new(), |s, evt| { s.to_owned() + &evt.to_string()[..] }));
160 }
161
162 pub fn new(redistributable: bool, track_bounds: (f64, f64), initial_position: f64) -> Interpolator {
163 Interpolator {
164 redistributable,
165 events: VecDeque::with_capacity(5),
166 samples: VecDeque::new(),
167 pan_start_time: 0.0,
168 min_tick_period: f64::INFINITY,
169 current_phase: Phase::Inactive,
170 track_bound_lower: track_bounds.0,
171 track_bound_upper: track_bounds.1,
172 track_initial_pos: initial_position,
173 flips_same_value: 0,
174 last_value: 0.0,
175 bouncing: BounceState::Normal,
176 source: crate::Source::Undefined,
177 }
178 }
179
180 pub fn sample(&mut self, time: Time) -> Position {
181 let config = sconfig.read().unwrap();
182
183 self.prevent_coast(time);
184 let last_sample = self.samples.back().map(|&evt| evt).unwrap_or(Sample { time, velocity: 0.0, position: 0.0 });
189
190 let iter = iter_range(last_sample.time + config.SHIFT_WINDOW_MS, time + config.SHIFT_WINDOW_MS, config.TIMESTEP);
195
196 let mut cur_position = last_sample.position;
199 let mut cur_velocity = last_sample.velocity;
200 for (start, end) in iter {
202 let stepped_velocity = self.step_velocity(start, end, cur_position, cur_velocity);
203 let velocity_per_step = (stepped_velocity + cur_velocity) / config.TIMESTEP; let time_delta = end - start;
210
211 let integral = time_delta * velocity_per_step;
212
213
214 cur_position += integral;
215 cur_velocity = stepped_velocity;
216 }
219 self.samples.push_back(Sample { time, velocity: cur_velocity, position: cur_position });
235
236 self.cull();
239 self.check_idle(cur_position, cur_velocity);
241
242 cur_position
254 }
255
256 pub fn cull(&mut self) {
257 let config = sconfig.read().unwrap();
258
259 while self.samples.len() > config.SAMPLE_EXPIRY_COUNT {
260 self.samples.pop_front();
261 }
262 while self.events.len() > config.EVENT_EXPIRY_COUNT {
263 self.events.pop_front();
264 }
265 }
266
267 pub fn signal_fling(&mut self, time: Time) {
268 self.current_phase = Phase::Released(time);
270
271 }
273
274 pub fn signal_interrupt(&mut self, time: Time) {
275 println!("Interrupt at {}", time);
276 self.current_phase = Phase::Inactive;
278 self.flush(time);
279 self.min_tick_period = f64::INFINITY;
280 }
281
282 pub fn signal_pan(&mut self, time: Time, delta: f64) {
283 if time == 0.0 {
285 panic!("can't pass zero timestamps into signal_pan");
286 }
287 self.current_phase = Phase::Interpolating;
289
290 let (prev_val, prev_time) = self.events.back().map(|evt| (evt.value, evt.time)).unwrap_or((self.track_initial_pos, f64::NEG_INFINITY));
291
292 let current_val = prev_val + delta;
293
294 if time - prev_time <= 0.0 {
295 self.samples.retain(|s| s.time < prev_time);
315 self.events.pop_back();
316 } else {
317 self.min_tick_period = time - prev_time;
318 }
319
320 self.events.push_back(Event { value: current_val, time });
321 }
322
323 pub fn animating(&self) -> bool {
324 let r = match self.current_phase {
325 Phase::Inactive => false,
326 _ => true,
327 };
328
329 r
332 }
333
334 pub fn set_geometry(&mut self, min: f64, max: f64) {
335 self.track_bound_upper = max;
336 self.track_bound_lower = min;
337 }
338}
339impl Interpolator {
341 fn flush(&mut self, time: Time) {
344 }
348
349 fn check_idle(&mut self, position: Position, velocity: Velocity) {
350 let config = sconfig.read().unwrap();
351
352 match self.current_phase {
353 Phase::Released(_) => {
354 if position == self.last_value || velocity.abs() < config.MIN_VELOCITY_TO_IDLE {
355 self.flips_same_value += 1;
356 } else {
357 self.flips_same_value = 0;
358 }
359 self.last_value = position;
360
361 if self.flips_same_value > config.FLIPS_TO_IDLE {
362 eprintln!("Goes to idle");
363 println!("check_idle goes to Inactive");
364 self.current_phase = Phase::Inactive;
365 }
366 },
367 Phase::Interpolating => {
368 self.flips_same_value = 0;
369 },
370 Phase::Inactive => {}
371 }
372 }
373 fn prevent_coast(&mut self, time: Time) {
381 let config = sconfig.read().unwrap();
382
383 match self.current_phase {
384 Phase::Interpolating => match self.events.len() {
385 0 => {}
386 _ => {
387 let evt = self.events.back().expect("Events was empty despite len > 0");
388 let delta = (time - evt.time).abs();
389 if delta > self.min_tick_period * config.TICKS_TO_COAST || delta > config.MAX_MS_WITHOUT_ZERO_INJECTION {
390 println!("Clamps to prevent coast. Delta {} evt {} min_tick_period {}", delta, evt, self.min_tick_period);
393 self.signal_interrupt(time);
395 }
396 }
397 },
398 _ => {},
399 }
400 }
401
402 fn interpolate(&self, time: Time) -> Velocity {
403 let config = sconfig.read().unwrap();
404
405 let first_before = self
406 .events
407 .iter()
408 .filter(|evt| evt.time < time)
409 .max_by(|evt_a, evt_b| (evt_a.time as u64).cmp(&(evt_b.time as u64)));
411
412 let first_after = self
413 .events
414 .iter()
415 .filter(|evt| evt.time >= time)
416 .min_by(|evt_a, evt_b| (evt_a.time as u64).cmp(&(evt_b.time as u64)));
417
418 let second_before = match first_before {
419 None => None,
420 Some(first) => {
421 self
422 .events
423 .iter()
424 .filter(|evt| evt.time < first.time)
425 .max_by(|evt_a, evt_b| (evt_a.time as u64).cmp(&(evt_b.time as u64)))
426 }
427 };
428
429 let second_after = match first_after {
430 None => None,
431 Some(first) => {
432 self
433 .events
434 .iter()
435 .filter(|evt| evt.time < first.time)
436 .min_by(|evt_a, evt_b| (evt_a.time as u64).cmp(&(evt_b.time as u64)))
437 }
438 };
439
440 let events: Vec<&Event> = vec![&second_before, &first_before, &first_after]
442 .into_iter()
443 .filter_map(|&evt| evt) .collect();
445
446 let result = match events.len() {
447 0 => {
448 0.0
450 },
451 1 => {
452 let evt = events.first().expect("no elements in size 1 vec");
454 evt.value / config.MAX_MS_WITHOUT_ZERO_INJECTION
457 }
458 2 => Self::interpolate_linear(&events, time),
460 3 => Self::interpolate_linear_averaging(&events, time),
461 4 => Self::interpolate_hermite(&events, time),
462 _ => panic!("Programming error: events len greater than 4"),
463 };
464
465 if events.len() == 1 {
466 }
468
469 if result == 0.0 {
470 }
472
473 result
476 }
477
478 fn outside_bounds(&self, position: Position) -> bool {
479 let config = sconfig.read().unwrap();
480
481 position > self.track_bound_upper || position < self.track_bound_lower
482 }
483
484 fn short_circuit_single_event(&self) -> Position {
485 let config = sconfig.read().unwrap();
486
487 self.events.back().map(|evt| evt.value).unwrap_or(0.0)
488 }
489
490 fn fling_boost(&self, velocity: Velocity) -> Velocity {
491 let config = sconfig.read().unwrap();
492
493 velocity * config.FLING_BOOST_CONSTANT_FACTOR
494 }
495
496 fn handle_overscroll(&self, start: Time, end: Time, position: Position, velocity: Velocity) -> Velocity {
497 let config = sconfig.read().unwrap();
498
499 if self.outside_bounds(position) {
500 if self.source.overscrolls() {
502 let outside_by = if position > self.track_bound_upper {
503 if velocity < 0.0 {
504 return velocity;
505 }
506
507 position - self.track_bound_upper
508 } else {
509 if velocity > 0.0 {
510 return velocity;
511 }
512
513 self.track_bound_lower - position
514 };
515
516 let abs_vel = velocity.abs();
517 let timedelta = end - start;
518 let r_velocity = velocity * (1.0 / (outside_by * config.OVERSCROLL_ELASTICITY_COEFFICIENT));
519
520 if r_velocity.is_nan() {
528 panic!("handle_overscroll tried to return NaN");
529 }
530 r_velocity
531 } else {
532 if velocity < 0.0 && position < self.track_bound_upper {
533 0.0
534 } else if velocity > 0.0 && position > self.track_bound_lower {
535 0.0
536 } else {
537 velocity
538 }
539 }
540 } else {
541 velocity
542 }
543 }
544
545 fn accelerate(&self, velocity: Velocity) -> Velocity {
546 let config = sconfig.read().unwrap();
547 if self.source.accelerates() {
549 (velocity / config.ACCEL_DECEL_DESCRIMINANT).abs().powf(config.ACCELERATION_EXPONENT).copysign(velocity) * config.ACCEL_DECEL_DESCRIMINANT
550 } else {
551 velocity
552 }
553 }
554
555 fn pre_scale(&self, velocity: Velocity) -> Velocity {
556 let config = sconfig.read().unwrap();
557
558 velocity * config.PRE_ACCEL_SCALE_VELOCITY
559 }
560
561 fn post_scale(&self, velocity: Velocity) -> Velocity {
562 let config = sconfig.read().unwrap();
563
564 velocity * config.POST_ACCEL_SCALE_VELOCITY
565 }
566
567 fn decay(&self, start: Time, end: Time, _position: Position, old_velocity: Velocity) -> Velocity {
568 if old_velocity.is_nan() {
569 panic!("given NaN velocity");
570 }
571
572 if old_velocity == 0.0 {
573 return 0.0;
574 }
575
576 let timedelta = end - start;
577 let abs_vel = old_velocity.abs();
579
580 if timedelta < 0.0 {
581 panic!("Negative timedelta passed to decay");
582 }
583
584 let friction_factor = if old_velocity != 0.0 {
586 old_velocity.abs().powf(1.3) / old_velocity.abs()
587 } else {
588 0.0
589 };
590
591 let slope = -0.00009 * friction_factor;
592 let new_vel = abs_vel + slope * timedelta;
593
594 let floored = if new_vel < 0.0 {
595 0.0
596 } else {
597 new_vel.copysign(old_velocity)
598 };
599
600 if floored.abs() > old_velocity.abs() {
603 panic!("Somehow accelerated");
604 }
605
606 if floored.is_nan() {
607 panic!("tried to return NaN velocity. Given {}", old_velocity);
608 }
609
610 floored
611 }
632
633 fn bounce(&mut self, start: Time, end: Time, position: Position, old_velocity: Velocity) -> Velocity {
634 let config = sconfig.read().unwrap();
635
636 if self.outside_bounds(position) {
637 let trackposition = if position > self.track_bound_upper {
638 TrackPosition::Bottom
639 } else {
640 TrackPosition::Top
641 };
642
643 self.bouncing = BounceState::Bouncing(trackposition);
644 }
645
646 match &self.bouncing {
647 BounceState::Normal => old_velocity,
648 BounceState::Bouncing(trackposition) => {
649 if old_velocity.is_nan() {
650 panic!("Given NaN velocity");
651 }
652
653 let displacement = match trackposition {
654 TrackPosition::Top => position - self.track_bound_lower,
655 TrackPosition::Bottom => position - self.track_bound_upper,
656 };
657
658 let force = -displacement * config.OVERSCROLL_SPRING_CONSTANT;
659 let timedelta = end - start;
660 let acceleration = force / config.CONTENT_MASS_VALUE;
661 let velocity = old_velocity + acceleration * timedelta;
662
663 let velocity = velocity * config.BOUNCE_DAMP_FACTOR;
664
665 if velocity.is_nan() {
666 panic!("Velocity was NaN");
667 } else if velocity.is_infinite() {
668 panic!();
669 }
670
671 velocity
672 }
673 }
674 }
675
676 fn sample_velocity(&self, start: Time, end: Time) -> Velocity {
677 let p1 = self.interpolate(start);
678 let p2 = self.interpolate(end);
679 let timedelta = end - start;
680 let vel = (p2 - p1) * timedelta; vel
685 }
686
687 fn step_velocity(&mut self, start: Time, end: Time, position: Position, old_velocity: Velocity) -> Velocity {
689 match self.current_phase {
690 Phase::Released(release_time) if release_time < start && release_time >= end => {
691 let r = self.fling_boost(old_velocity);
692
693 r
694 },
695 Phase::Released(release_time) if release_time < start => {
696 let b = self.bounce(
697 start,
698 end,
699 position,
700 old_velocity);
701 let r = self.decay(
702 start,
703 end,
704 position,
705 b);
706
707 r
712 },
713 Phase::Interpolating | Phase::Released(_) => {
714 self.bouncing = BounceState::Normal;
717 let r = self.post_scale(
718 self.handle_overscroll(
719 start,
720 end,
721 position,
722 self.accelerate(
723 self.pre_scale(
724 self.sample_velocity(start, end)))));
725 r
726 },
727 Phase::Inactive => 0.0
728 }
729 }
730
731 fn set_inactive(&mut self) {
732 println!("set_inactive sets Inactive");
733 self.current_phase = Phase::Inactive;
734 self.bouncing = BounceState::Normal;
735 self.samples.clear();
736 }
737}
738
739impl Interpolator {
741 fn sample_linear(first: &Event, second: &Event, sample: Time) -> f64 {
742 let slope = Self::slope_of(first, second);
743
744 slope * (sample - first.time) + first.value
745 }
746
747 fn slope_of(first: &Event, second: &Event) -> f64 {
748 if first.time == second.time {
750 0.0
751 } else {
752 (first.value - second.value) / ((first.time as i64) - (second.time as i64)) as f64
753 }
754 }
755
756 fn rounds_to_zero(val: f64) -> bool {
757 val.abs() < 0.5
758 }
759
760 fn interpolate_constant(events: &Vec<&Event>, at: Time) -> f64 {
761 let vel = events.first().expect("interpolate_constant given empty events vec").value;
762
763 vel
766 }
767
768 fn interpolate_linear(events: &Vec<&Event>, at: Time) -> f64 {
769 let first = events[0];
771 let second = events[1];
772
773 if first == second {
774 panic!("interpolate_linear given single event");
775 }
776
777 Self::sample_linear(first, second, at)
782 }
783
784 fn interpolate_linear_averaging(events: &Vec<&Event>, at: Time) -> f64 {
785 let first = events[1];
787 let second = events[2];
788 Self::interpolate_linear(&vec![first, second], at)
789 }
802
803 fn interpolate_hermite(events: &Vec<&Event>, at: Time) -> f64 {
804 println!("Interpolating hermite");
805 panic!("not implemented");
806 0.0
807 }
808}
809
810struct TimestampIterator {
811 end: f64,
812 cur: f64,
813 step: f64,
814}
815
816impl Iterator for TimestampIterator {
818 type Item = (f64, f64);
819
820 fn next(&mut self) -> Option<Self::Item> {
821 if self.cur == self.end {
822 None
823 } else if (self.end - self.cur) < self.step {
824 let r = (self.cur, self.end);
825 self.cur = self.end;
826 Some(r)
827 } else {
828 let r = (self.cur, self.cur + self.step);
829 self.cur += self.step;
830 Some(r)
831 }
832 }
833}
834
835fn iter_range(start: f64, end: f64, by: f64) -> TimestampIterator {
836 TimestampIterator {
837 end,
838 cur: start,
839 step: by,
840 }
841}