interpolation.rs

use std::num::Wrapping;

use bincode::{Encode, Decode};

// Time is monotonic time (OS ticks) not clock time.  The difference is precision vs accuracy.
// clock time can have huge variances, be different by over 100ms even when queried within a few ms of each other.  Accurate but not precise.
// OS ticks are very precise over short intervals. But can drift very quickly over longer periods.

// We consume snapshots in sequence order, possibly skipping missing sequences.
// The interpolation state time represents our progression through the server timeline.  Our server timeline is based on received snapshots. We advance through this timeline using local delta time.
// The key is that both client and server are using monotonic time using the same time units.

// We can and will drift over time relative to server time due to the following:
// - monotonic time drift
// - packet loss
// - network latency
// - server lag

//The snapshot buffer is solving for a different problem.  Always having something to interpolate towards even if you have packet loss. We are intentionally adding latency to gain smoothness of motion.

// If our point on the timeline gets too far behind, new snapshots will wrap around and run past our buffer position.  Eventually resulting in a big leap forward in time.
// Note that our timeline is represented by snapshots we have actually received.
// This case is simple we speed up our movement through the timeline based on some threshold.

// If we hit the end of our timeline we no longer have a position to interpolate towards.  This can happen naturally just from monotonic time drift.  Server lag and network latency impact this also.
// Here we can adapt by using larger snapshot buffers, and/or slowing our movement through the server timeline dynamically. 

#[derive(Clone, Copy, Default, Debug, Encode, Decode)]
#[repr(C)]
pub struct Snapshot {
    pub time: f64,
    pub sequence: u16
}

impl Snapshot {
    pub fn new(time: f64, sequence: u16) -> Self {
        Snapshot {
            time,
            sequence
        }
    }
}


#[derive(Clone, Copy, Debug)]
pub struct SnapshotSequence {
    pub sequence: Wrapping<u16>
}

impl SnapshotSequence {
    pub fn new(initial_value: u16) -> Self {
        SnapshotSequence {
            sequence: Wrapping(initial_value)
        }
    }

    pub fn increment(sequence: u16) -> u16 {
        let mut wrapper = Wrapping(sequence);
        wrapper += 1;
        wrapper.0
    }

    pub fn current(&self) -> u16 {
        self.sequence.0
    }

    pub fn next(&mut self) -> u16 {
        self.sequence += 1;
        self.sequence.0
    }
    
}

pub struct SnapshotBuffer {
    pub buffer: Vec<Option<Snapshot>>,
    last_sequence: Option<u16>,
    playout_delay: u16
}

impl SnapshotBuffer {
    pub const BUFFER_LEN: usize = 256;

    pub fn new(playout_delay: u16) -> Self {
        let mut buffer = SnapshotBuffer {
            buffer: Vec::new(),
            last_sequence: None,
            playout_delay
        };

        for _ in 0..Self::BUFFER_LEN {
            buffer.buffer.push(None);
        }
        buffer
    }

    pub fn last(&self) -> Option<&Snapshot> {
        if let Some(last_sequence) = self.last_sequence {
            self.get(last_sequence)
        } else {
            None
        }
    }

    pub fn index(&self, sequence: u16) -> usize {
        (sequence % Self::BUFFER_LEN as u16).into()
    }

    fn get_previous(&self, sequence: u16) -> Option<&Snapshot> {
        let mut prev_seq = Wrapping(sequence);
        prev_seq -= 1;
        let prev_index = self.index(prev_seq.0);
        self.buffer[prev_index].as_ref()
    }
    
    // add a snapshot.  Must have a timestamp later then the snapshot associated with the previous sequence number
    // Since we wrap our buffers it's possible for very old snapshots to arrive and get inserted over newer ones unless we check timestamps
    pub fn push(&mut self, snap: Snapshot) -> Option<usize> {
        if let Some(previous) = self.get_previous(snap.sequence) {
            if snap.time <= previous.time {
                return None;
            }
        }

        let index = self.index(snap.sequence);
        self.buffer[index] = Some(snap);
        self.last_sequence = Some(snap.sequence);
        Some(index)
    }

    pub fn get(&self, sequence: u16) -> Option<&Snapshot> {
        let index = self.index(sequence);
        self.buffer[index].as_ref()
    }

    /// get a starting snapshot that is at least playout_delay sequences ago
    pub fn get_start(&self) -> Option<&Snapshot> {
        if let Some(last_sequence) = self.last_sequence {
            let last = Wrapping(last_sequence);
            let start = last - Wrapping(self.playout_delay);
            let index = self.index(start.0);
            self.buffer[index].as_ref()
        } else {
            None
        }
    }

    // get the next snapshot that is timestamped after this one. Look forward the entire buffer len.
    pub fn get_next(&self, sequence: u16) -> Option<&Snapshot> {
        if let Some(current) = self.get(sequence) {
            let len = Self::BUFFER_LEN as u16;
            for i in 0..len {
                let next = Wrapping(sequence + i + 1);
                let index = self.index(next.0);
                if let Some(snap) = &self.buffer[index] {
                    if snap.time > current.time {
                        return Some(snap);
                    }
                }
            }
        }
        
        return None
    }

}

#[derive(Clone, Copy, Default, Debug)]
#[repr(C)]
pub struct Interpolation {
    pub start: Snapshot,
    pub end: Snapshot,
    pub t: f64,
    pub time: f64
}

pub struct InterpolationState {
    start: Option<Snapshot>,
    end: Option<Snapshot>,
    time: f64,
    buffer: SnapshotBuffer
}


impl InterpolationState {
    pub fn new(buffer: SnapshotBuffer) -> Self {
        InterpolationState {
            start: None,
            end: None,
            time: 0.0,
            buffer
        }
    }

    pub fn has_start(&self) -> bool {
        self.start.is_some()
    }

    pub fn has_end(&self) -> bool {
        self.end.is_some()
    }

    pub fn add_snapshot(&mut self, snap: Snapshot) -> Option<usize> {
        self.buffer.push(snap)
    }

    pub fn interpolate(&mut self, delta_time: f64) -> Option<Interpolation> {

        self.initialize_if_needed();

        if self.start.is_none() && self.end.is_none() {
            return None;
        }

        self.update_end();

        if let Some(start) = &self.start {
            if let Some(end) = &self.end {

                let a = self.time - start.time;
                let b = end.time - start.time;
                let t = f64::clamp(a / b, 0.0, 1.0);
                let result = Interpolation {
                    start: *start,
                    end: *end,
                    t,
                    time: self.time
                };

                self.time += delta_time;

                return Some(result);
            }
        }

        None
        
    }

    // initially we have no start/end and time is not set.
    fn initialize_if_needed(&mut self) {
        if self.start.is_none() && self.end.is_none() {
            if let Some(start) = self.buffer.get_start() {
                if let Some(end) = self.buffer.get_next(start.sequence) {
                    self.time = start.time;
                    self.start = Some(*start);
                    self.end = Some(*end);

                    
                }
            }
        }
    }

    // if we have interpolated to/past the end snapshot, we set the end snapshot as our new start, and try to find a new end in the snapshot buffer.
    fn update_end(&mut self) {
        if self.start.is_some() {
            if let Some(end) = self.end {
                if self.time >= end.time {
                    self.start = Some(end);
                    self.end = None;
                }
            }
        }

        if self.end.is_none() {
            if let Some(start) = self.start {
                if let Some(next) = self.buffer.get_next(start.sequence) {
                    self.end = Some(*next);
                }
            }
            
        }
    }

    
}

#[cfg(test)]
mod tests {
    use crate::interpolation::{SnapshotSequence};

    use super::{SnapshotBuffer, Snapshot, InterpolationState};

    #[test]
    fn interpolate() {
        let buffer = SnapshotBuffer::new(3);
        let mut state = InterpolationState::new(buffer);
        state.add_snapshot(Snapshot::new(1.0, 0));
        state.add_snapshot(Snapshot::new(2.0, 1));
        state.add_snapshot(Snapshot::new(3.0, 2));
        state.add_snapshot(Snapshot::new(4.0, 3));

        let result = state.interpolate(1.02).unwrap();
        assert_eq!(0, result.start.sequence);
        assert_eq!(1, result.end.sequence);

        let result = state.interpolate(1.02).unwrap();
        assert_eq!(1, result.start.sequence);
        assert_eq!(2, result.end.sequence);

        let result = state.interpolate(1.02).unwrap();
        assert_eq!(2, result.start.sequence);
        assert_eq!(3, result.end.sequence);

        let result = state.interpolate(1.02);
        assert!(result.is_none());

        state.add_snapshot(Snapshot::new(5.0, 4));
        let result = state.interpolate(1.02).unwrap();
        assert_eq!(3, result.start.sequence);
        assert_eq!(4, result.end.sequence);

        // skip a sequence
        state.add_snapshot(Snapshot::new(6.0, 6));

        let result = state.interpolate(1.02).unwrap();
        assert_eq!(4, result.start.sequence);
        assert_eq!(6, result.end.sequence);

        let result = state.interpolate(1.02);
        assert!(result.is_none());
        assert!(state.has_start());
        assert!(!state.has_end());
        
    }

    #[test]
    fn snapshot_sequence() {

        let mut wrapping = SnapshotSequence::new(u16::MAX);
        assert_eq!(0, wrapping.next());
        assert_eq!(1, wrapping.next());
    }

    #[test]
    fn push_validation() {
        let mut buffer = SnapshotBuffer::new(2);

        let _index = buffer.push(Snapshot::new(0.0, 0)).unwrap();
        let _index = buffer.push(Snapshot::new(1.0, 1)).unwrap();
        let _index = buffer.push(Snapshot::new(2.0, 2)).unwrap();
        let _index = buffer.push(Snapshot::new(3.0, 3)).unwrap();
        let _index = buffer.push(Snapshot::new(4.0, 4)).unwrap();
        let _index = buffer.push(Snapshot::new(5.0, 5)).unwrap();

        // too old
        assert!(buffer.push(Snapshot::new(4.0, 5)).is_none());
        assert!(buffer.push(Snapshot::new(4.0, 6)).is_none());

        assert!(buffer.push(Snapshot::new(6.0, 6)).is_some());
        assert!(buffer.push(Snapshot::new(7.0, 7)).is_some());


        // wraps at max len
        let _index = buffer.push(Snapshot::new(8.0, SnapshotBuffer::BUFFER_LEN as u16)).unwrap();
        assert_eq!(0, _index);

    }

    #[test]
    fn next_skips_missing_snapshot() {
        let mut buffer = SnapshotBuffer::new(2);

        buffer.push(Snapshot::new(1.0, 0));
        buffer.push(Snapshot::new(3.0, 3));

        let end = buffer.get_next(0).unwrap();
        assert_eq!(3, end.sequence);
    }

    #[test]
    fn get_next_valid_ranges() {
        let mut buffer = SnapshotBuffer::new(2);

        buffer.push(Snapshot::new(1.0, 0));
        let seq = (SnapshotBuffer::BUFFER_LEN - 1) as u16;
        buffer.push(Snapshot::new(3.0, seq));

        let next = buffer.get_next(0).unwrap();
        assert_eq!(255, next.sequence);


        let mut buffer = SnapshotBuffer::new(2);

        buffer.push(Snapshot::new(1.0, 0));
        let seq = SnapshotBuffer::BUFFER_LEN as u16;
        buffer.push(Snapshot::new(3.0, seq));

        let next = buffer.get_next(0);
        assert!(next.is_none());
    }

    #[test]
    fn get_start_enough_history() {
        let mut buffer = SnapshotBuffer::new(2);

        buffer.push(Snapshot::new(1.0, 0));
        buffer.push(Snapshot::new(2.0, 1));

        // not enough history
        let start = buffer.get_start();
        assert!(start.is_none());

        buffer.push(Snapshot::new(3.0, 2));

        let start = buffer.get_start().unwrap();
        assert_eq!(0, start.sequence);

    }

}