faststep 0.1.0

use super::TouchEvent;

const SAMPLE_WINDOW_MS: u32 = 180;
const MAX_SAMPLES: usize = 16;

#[derive(Clone, Copy, Debug, Default)]
struct MotionSample {
    y: f32,
    timestamp_ms: u32,
}

#[derive(Clone, Copy, Debug, Default)]
pub(crate) struct VelocityTracker {
    samples: [MotionSample; MAX_SAMPLES],
    len: usize,
    head: usize,
}

impl VelocityTracker {
    pub const fn new() -> Self {
        Self {
            samples: [MotionSample {
                y: 0.0,
                timestamp_ms: 0,
            }; MAX_SAMPLES],
            len: 0,
            head: 0,
        }
    }

    pub fn reset(&mut self, touch: TouchEvent) {
        self.len = 0;
        self.head = 0;
        self.add(touch);
    }

    pub fn add(&mut self, touch: TouchEvent) {
        self.samples[self.head] = MotionSample {
            y: touch.point.y as f32,
            timestamp_ms: touch.timestamp_ms,
        };
        self.head = (self.head + 1) % MAX_SAMPLES;
        if self.len < MAX_SAMPLES {
            self.len += 1;
        }
    }

    pub fn velocity(&self) -> f32 {
        if self.len < 2 {
            return 0.0;
        }

        let newest = self.sample(self.len - 1);
        let mut weighted_velocity = 0.0;
        let mut total_weight = 0.0;
        let mut newer = newest;
        for index in (0..(self.len - 1)).rev() {
            let older = self.sample(index);
            let age_ms = newest.timestamp_ms.saturating_sub(older.timestamp_ms);
            if age_ms > SAMPLE_WINDOW_MS {
                break;
            }

            let dt_ms = newer.timestamp_ms.saturating_sub(older.timestamp_ms);
            if dt_ms == 0 {
                newer = older;
                continue;
            }

            let dy = newer.y - older.y;
            let segment_velocity = -(dy * 1000.0) / dt_ms as f32;
            let weight = 1.0 - (age_ms as f32 / SAMPLE_WINDOW_MS as f32) * 0.55;
            weighted_velocity += segment_velocity * weight;
            total_weight += weight;
            newer = older;
        }

        if total_weight > 0.0 {
            weighted_velocity / total_weight
        } else {
            0.0
        }
    }

    fn sample(&self, index: usize) -> MotionSample {
        let start = (self.head + MAX_SAMPLES - self.len) % MAX_SAMPLES;
        self.samples[(start + index) % MAX_SAMPLES]
    }
}