//! The 4D discretization scheme — the recommended vertical and temporal tiering
//! for the Map standard (ported from `joule-spatial3d`).
//!
//! S2 cells give the horizontal axis (`map-index` geometry). This module adds
//! the other two: a logarithmic [`AltBand`] schedule from the Mariana Trench
//! (−11,034 m) to cislunar space, and a logarithmic [`TimeBucket`] schedule from
//! 1-second real-time to 100-year epochs. Together with an S2 cell they form a
//! [`SpatialKey`] — the in-memory analogue of a [`crate::bridge`] `CellKey`.
//!
//! These are the *recommended* scheme; the standard's address space
//! ([`map_schema::CellKey`]) is scheme-agnostic — `AltBand` and `TimeBucket`
//! there are an opaque signed index and a `(start, width)` interval. The
//! [`crate::bridge`] converts this scheme into that wire form. [`Spatial3DIndex`]
//! and [`PositionRingBuffer`] are runtime utilities for the live-entity layer.

use std::collections::BTreeMap;

use crate::cell_id::S2CellId;
use serde::{Deserialize, Serialize};

// ============================================================================
// Altitude Bands
// ============================================================================

/// Logarithmic altitude bands covering -11 km to 400,000 km.
///
/// Band boundaries use powers of 10 with sub-divisions to handle the
/// 7-order-of-magnitude range from seafloor to cislunar space.
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Serialize, Deserialize)]
#[repr(u8)]
pub enum AltBand {
    /// -11,034 m to 0 m (bathymetry, seafloor)
    Bathymetry = 0,
    /// 0 m to 100 m (surface structures, ground level)
    Surface = 1,
    /// 100 m to 1,000 m (tall buildings, low terrain features)
    LowAlt = 2,
    /// 1,000 m to 10,000 m (mountains, low airspace)
    MidAlt = 3,
    /// 10,000 m to 100,000 m (commercial flight to near-space)
    HighAlt = 4,
    /// 100,000 m (100 km) to 2,000 km (LEO)
    Leo = 5,
    /// 2,000 km to 36,000 km (MEO to GEO)
    MeoGeo = 6,
    /// 36,000 km to 400,000 km (beyond GEO, cislunar)
    BeyondGeo = 7,
}

impl AltBand {
    /// Determine the altitude band for a given altitude in meters.
    #[inline]
    pub fn from_altitude_m(alt_m: f64) -> Self {
        if alt_m < 0.0 {
            Self::Bathymetry
        } else if alt_m < 100.0 {
            Self::Surface
        } else if alt_m < 1_000.0 {
            Self::LowAlt
        } else if alt_m < 10_000.0 {
            Self::MidAlt
        } else if alt_m < 100_000.0 {
            Self::HighAlt
        } else if alt_m < 2_000_000.0 {
            Self::Leo
        } else if alt_m < 36_000_000.0 {
            Self::MeoGeo
        } else {
            Self::BeyondGeo
        }
    }

    /// Return the altitude range (min_m, max_m) for this band.
    #[inline]
    pub fn range_m(&self) -> (f64, f64) {
        match self {
            Self::Bathymetry => (-11_034.0, 0.0),
            Self::Surface => (0.0, 100.0),
            Self::LowAlt => (100.0, 1_000.0),
            Self::MidAlt => (1_000.0, 10_000.0),
            Self::HighAlt => (10_000.0, 100_000.0),
            Self::Leo => (100_000.0, 2_000_000.0),
            Self::MeoGeo => (2_000_000.0, 36_000_000.0),
            Self::BeyondGeo => (36_000_000.0, 400_000_000.0),
        }
    }

    /// Check if a given altitude falls within this band.
    #[inline]
    pub fn contains(&self, alt_m: f64) -> bool {
        let (min, max) = self.range_m();
        alt_m >= min && alt_m < max
    }

    /// All bands in order.
    pub const ALL: [AltBand; 8] = [
        Self::Bathymetry,
        Self::Surface,
        Self::LowAlt,
        Self::MidAlt,
        Self::HighAlt,
        Self::Leo,
        Self::MeoGeo,
        Self::BeyondGeo,
    ];

    /// Return bands that overlap a given altitude range.
    pub fn bands_for_range(min_m: f64, max_m: f64) -> Vec<AltBand> {
        Self::ALL
            .iter()
            .copied()
            .filter(|b| {
                let (bmin, bmax) = b.range_m();
                bmin < max_m && bmax > min_m
            })
            .collect()
    }
}

// ============================================================================
// Time Buckets
// ============================================================================

/// Time resolution tiers for temporal partitioning.
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Serialize, Deserialize)]
#[repr(u8)]
pub enum TimeTier {
    /// 1-second buckets — live vehicle/satellite tracking
    Realtime = 0,
    /// 1-hour buckets — weather, traffic, AIS ship positions
    Recent = 1,
    /// 1-day buckets — satellite imagery, daily snapshots
    Daily = 2,
    /// 30-day buckets — historical aerial photography
    Monthly = 3,
    /// 365-day buckets — historical maps, urban change
    Yearly = 4,
    /// 100-year buckets — deep historical maps (pre-1900)
    Epoch = 5,
}

/// A time bucket identifier: tier + bucket start timestamp.
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Serialize, Deserialize)]
pub struct TimeBucket {
    pub tier: TimeTier,
    /// Start of the bucket as Unix timestamp in seconds.
    pub start_secs: i64,
}

impl TimeBucket {
    /// Bucket size in seconds for each tier.
    #[inline]
    pub fn bucket_size_secs(tier: TimeTier) -> i64 {
        match tier {
            TimeTier::Realtime => 1,
            TimeTier::Recent => 3_600,
            TimeTier::Daily => 86_400,
            TimeTier::Monthly => 2_592_000, // 30 days
            TimeTier::Yearly => 31_536_000, // 365 days
            TimeTier::Epoch => 3_153_600_000, // 100 years
        }
    }

    /// Create a time bucket for a given timestamp at a given tier.
    pub fn from_timestamp(timestamp_secs: i64, tier: TimeTier) -> Self {
        let size = Self::bucket_size_secs(tier);
        let start = (timestamp_secs / size) * size;
        Self {
            tier,
            start_secs: start,
        }
    }

    /// End of the bucket (exclusive).
    #[inline]
    pub fn end_secs(&self) -> i64 {
        self.start_secs + Self::bucket_size_secs(self.tier)
    }

    /// Next bucket in sequence.
    #[inline]
    pub fn next(&self) -> Self {
        Self {
            tier: self.tier,
            start_secs: self.end_secs(),
        }
    }

    /// Previous bucket in sequence.
    #[inline]
    pub fn prev(&self) -> Self {
        Self {
            tier: self.tier,
            start_secs: self.start_secs - Self::bucket_size_secs(self.tier),
        }
    }

    /// Iterate all buckets covering a time range.
    pub fn buckets_for_range(start_secs: i64, end_secs: i64, tier: TimeTier) -> Vec<TimeBucket> {
        let first = Self::from_timestamp(start_secs, tier);
        let mut buckets = Vec::new();
        let mut current = first;
        while current.start_secs < end_secs {
            buckets.push(current);
            current = current.next();
        }
        buckets
    }

    /// Choose the appropriate tier for a time range.
    /// Aims for roughly 100-1000 buckets.
    pub fn auto_tier(start_secs: i64, end_secs: i64) -> TimeTier {
        let span = (end_secs - start_secs).max(1);
        if span <= 1_000 {
            TimeTier::Realtime
        } else if span <= 360_000 {
            TimeTier::Recent
        } else if span <= 8_640_000 {
            TimeTier::Daily
        } else if span <= 259_200_000 {
            TimeTier::Monthly
        } else if span <= 3_153_600_000 {
            TimeTier::Yearly
        } else {
            TimeTier::Epoch
        }
    }
}

// ============================================================================
// Space-Time Point
// ============================================================================

/// A point in 4D space-time: lat, lon, altitude, time.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct SpaceTimePoint {
    /// Latitude in degrees (-90 to +90).
    pub lat: f64,
    /// Longitude in degrees (-180 to +180).
    pub lon: f64,
    /// Altitude in meters above WGS84 ellipsoid. Negative for depth.
    pub alt_m: f64,
    /// Unix timestamp in seconds.
    pub time_secs: i64,
}

impl SpaceTimePoint {
    /// Create a new space-time point.
    pub fn new(lat: f64, lon: f64, alt_m: f64, time_secs: i64) -> Self {
        Self {
            lat,
            lon,
            alt_m,
            time_secs,
        }
    }

    /// Altitude band for this point.
    #[inline]
    pub fn alt_band(&self) -> AltBand {
        AltBand::from_altitude_m(self.alt_m)
    }

    /// S2 cell ID at a given level.
    #[inline]
    pub fn s2_cell(&self, level: u8) -> S2CellId {
        let leaf = S2CellId::from_lat_lng(self.lat, self.lon);
        leaf.parent_at_level(level)
    }

    /// Time bucket at a given tier.
    #[inline]
    pub fn time_bucket(&self, tier: TimeTier) -> TimeBucket {
        TimeBucket::from_timestamp(self.time_secs, tier)
    }

    /// Composite spatial key at given S2 level and time tier.
    pub fn spatial_key(&self, s2_level: u8, time_tier: TimeTier) -> SpatialKey {
        SpatialKey {
            s2_cell: self.s2_cell(s2_level),
            alt_band: self.alt_band(),
            time_bucket: self.time_bucket(time_tier),
        }
    }

    /// 3D distance to another point in meters (haversine + altitude diff).
    pub fn distance_3d(&self, other: &SpaceTimePoint) -> f64 {
        let earth_r = 6_371_000.0;
        let lat1 = self.lat.to_radians();
        let lat2 = other.lat.to_radians();
        let dlat = (other.lat - self.lat).to_radians();
        let dlon = (other.lon - self.lon).to_radians();

        let a = (dlat / 2.0).sin().powi(2)
            + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
        let horiz = earth_r * c;
        let vert = other.alt_m - self.alt_m;

        (horiz * horiz + vert * vert).sqrt()
    }
}

// ============================================================================
// Composite Spatial Key
// ============================================================================

/// Composite key for 3D+T spatial indexing: (S2Cell, AltBand, TimeBucket).
///
/// This is the primary index key in JouleDB for all spatial queries.
/// Keys are sorted lexicographically: S2 cell first, then altitude band,
/// then time bucket — enabling efficient range scans over spatial regions.
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Serialize, Deserialize)]
pub struct SpatialKey {
    pub s2_cell: S2CellId,
    pub alt_band: AltBand,
    pub time_bucket: TimeBucket,
}

impl SpatialKey {
    /// Create a new spatial key.
    pub fn new(s2_cell: S2CellId, alt_band: AltBand, time_bucket: TimeBucket) -> Self {
        Self {
            s2_cell,
            alt_band,
            time_bucket,
        }
    }

    /// Encode to a 128-bit value for storage.
    /// Layout: [S2CellId: 64 bits][AltBand: 8 bits][TimeBucket tier: 8 bits][start_secs: 48 bits]
    pub fn encode(&self) -> u128 {
        let s2 = self.s2_cell.0 as u128;
        let alt = (self.alt_band as u8) as u128;
        let tier = (self.time_bucket.tier as u8) as u128;
        // Shift start_secs to unsigned and truncate to 48 bits
        let time = ((self.time_bucket.start_secs as u64) & 0xFFFF_FFFF_FFFF) as u128;
        (s2 << 64) | (alt << 56) | (tier << 48) | time
    }

    /// Decode from a 128-bit value.
    pub fn decode(val: u128) -> Self {
        let s2 = (val >> 64) as u64;
        let alt = ((val >> 56) & 0xFF) as u8;
        let tier = ((val >> 48) & 0xFF) as u8;
        let time_raw = (val & 0xFFFF_FFFF_FFFF) as u64;
        // Sign-extend from 48 bits
        let time_secs = if time_raw & (1 << 47) != 0 {
            (time_raw | 0xFFFF_0000_0000_0000) as i64
        } else {
            time_raw as i64
        };

        let alt_band = match alt {
            0 => AltBand::Bathymetry,
            1 => AltBand::Surface,
            2 => AltBand::LowAlt,
            3 => AltBand::MidAlt,
            4 => AltBand::HighAlt,
            5 => AltBand::Leo,
            6 => AltBand::MeoGeo,
            _ => AltBand::BeyondGeo,
        };

        let time_tier = match tier {
            0 => TimeTier::Realtime,
            1 => TimeTier::Recent,
            2 => TimeTier::Daily,
            3 => TimeTier::Monthly,
            4 => TimeTier::Yearly,
            _ => TimeTier::Epoch,
        };

        Self {
            s2_cell: S2CellId(s2),
            alt_band,
            time_bucket: TimeBucket {
                tier: time_tier,
                start_secs: time_secs,
            },
        }
    }
}

// ============================================================================
// 3D Spatial Index
// ============================================================================

/// Entry in the spatial index.
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct IndexEntry<T> {
    pub point: SpaceTimePoint,
    pub data: T,
}

/// 3D+T spatial index backed by a BTreeMap of SpatialKeys.
///
/// This provides O(log n) insertion and range-query over the composite
/// (S2 cell, altitude band, time bucket) space.
#[derive(Debug)]
pub struct Spatial3DIndex<T> {
    entries: BTreeMap<SpatialKey, Vec<IndexEntry<T>>>,
    s2_level: u8,
    time_tier: TimeTier,
    count: usize,
}

impl<T: Clone> Spatial3DIndex<T> {
    /// Create a new index at the specified S2 level and time tier.
    pub fn new(s2_level: u8, time_tier: TimeTier) -> Self {
        Self {
            entries: BTreeMap::new(),
            s2_level,
            time_tier,
            count: 0,
        }
    }

    /// Insert a point with associated data.
    pub fn insert(&mut self, point: SpaceTimePoint, data: T) {
        let key = point.spatial_key(self.s2_level, self.time_tier);
        self.entries
            .entry(key)
            .or_default()
            .push(IndexEntry { point, data });
        self.count += 1;
    }

    /// Query all entries within a spatial-temporal region.
    ///
    /// Parameters:
    /// - `s2_cells`: set of S2 cell IDs to search (from a covering)
    /// - `alt_bands`: altitude bands to include
    /// - `time_start`/`time_end`: time range in seconds
    pub fn query(
        &self,
        s2_cells: &[S2CellId],
        alt_bands: &[AltBand],
        time_start: i64,
        time_end: i64,
    ) -> Vec<&IndexEntry<T>> {
        let time_buckets = TimeBucket::buckets_for_range(time_start, time_end, self.time_tier);
        let mut results = Vec::new();

        for &cell in s2_cells {
            for &band in alt_bands {
                for &bucket in &time_buckets {
                    let key = SpatialKey::new(cell, band, bucket);
                    if let Some(entries) = self.entries.get(&key) {
                        for entry in entries {
                            // Fine-grained filter: check time is actually in range
                            if entry.point.time_secs >= time_start
                                && entry.point.time_secs < time_end
                            {
                                results.push(entry);
                            }
                        }
                    }
                }
            }
        }

        results
    }

    /// Query all entries within a 3D radius of a point.
    pub fn query_radius(
        &self,
        center: &SpaceTimePoint,
        radius_m: f64,
        time_start: i64,
        time_end: i64,
    ) -> Vec<&IndexEntry<T>> {
        // Determine altitude bands that could be in range
        let alt_min = center.alt_m - radius_m;
        let alt_max = center.alt_m + radius_m;
        let alt_bands = AltBand::bands_for_range(alt_min, alt_max);

        // Use the center cell and its neighbors as a rough spatial filter
        let center_cell = center.s2_cell(self.s2_level);
        let neighbors = center_cell.neighbors();
        let mut cells = vec![center_cell];
        cells.extend_from_slice(&neighbors);

        // Query and filter by actual distance
        self.query(&cells, &alt_bands, time_start, time_end)
            .into_iter()
            .filter(|entry| center.distance_3d(&entry.point) <= radius_m)
            .collect()
    }

    /// Total number of entries.
    #[inline]
    pub fn len(&self) -> usize {
        self.count
    }

    /// Is the index empty?
    #[inline]
    pub fn is_empty(&self) -> bool {
        self.count == 0
    }

    /// Number of distinct spatial keys (buckets).
    #[inline]
    pub fn bucket_count(&self) -> usize {
        self.entries.len()
    }

    /// Remove all entries.
    pub fn clear(&mut self) {
        self.entries.clear();
        self.count = 0;
    }

    /// Estimated memory usage in bytes.
    pub fn memory_usage(&self) -> usize {
        let key_size = std::mem::size_of::<SpatialKey>();
        let entry_overhead = std::mem::size_of::<IndexEntry<T>>();
        // BTreeMap overhead: ~64 bytes per node
        self.entries.len() * (key_size + 64) + self.count * entry_overhead
    }
}

// ============================================================================
// Ring Buffer for Real-Time Entity Tracking
// ============================================================================

/// Fixed-size ring buffer for storing recent positions of a tracked entity.
///
/// Used for satellites, aircraft, ships — stores the last N positions
/// for trail rendering and interpolation.
#[derive(Debug, Clone)]
pub struct PositionRingBuffer {
    positions: Vec<SpaceTimePoint>,
    head: usize,
    len: usize,
    capacity: usize,
}

impl PositionRingBuffer {
    /// Create a new ring buffer with the given capacity.
    pub fn new(capacity: usize) -> Self {
        Self {
            positions: vec![
                SpaceTimePoint::new(0.0, 0.0, 0.0, 0);
                capacity
            ],
            head: 0,
            len: 0,
            capacity,
        }
    }

    /// Push a new position. Overwrites oldest if at capacity.
    pub fn push(&mut self, point: SpaceTimePoint) {
        self.positions[self.head] = point;
        self.head = (self.head + 1) % self.capacity;
        if self.len < self.capacity {
            self.len += 1;
        }
    }

    /// Get the most recent position.
    pub fn latest(&self) -> Option<&SpaceTimePoint> {
        if self.len == 0 {
            return None;
        }
        let idx = if self.head == 0 {
            self.capacity - 1
        } else {
            self.head - 1
        };
        Some(&self.positions[idx])
    }

    /// Iterate positions from oldest to newest.
    pub fn iter(&self) -> impl Iterator<Item = &SpaceTimePoint> {
        let start = if self.len < self.capacity {
            0
        } else {
            self.head
        };
        let buf = &self.positions;
        let cap = self.capacity;
        let len = self.len;
        (0..len).map(move |i| &buf[(start + i) % cap])
    }

    /// Number of positions stored.
    #[inline]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Is the buffer empty?
    #[inline]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Interpolate position at a given time.
    /// Returns None if time is outside the buffer's range.
    pub fn interpolate(&self, time_secs: i64) -> Option<SpaceTimePoint> {
        if self.len < 2 {
            return self.latest().copied();
        }

        let points: Vec<&SpaceTimePoint> = self.iter().collect();

        // Find the two bracketing points
        for window in points.windows(2) {
            let a = window[0];
            let b = window[1];
            if time_secs >= a.time_secs && time_secs <= b.time_secs {
                let span = (b.time_secs - a.time_secs) as f64;
                if span == 0.0 {
                    return Some(*a);
                }
                let t = (time_secs - a.time_secs) as f64 / span;
                return Some(SpaceTimePoint::new(
                    a.lat + t * (b.lat - a.lat),
                    a.lon + t * (b.lon - a.lon),
                    a.alt_m + t * (b.alt_m - a.alt_m),
                    time_secs,
                ));
            }
        }

        // Time is outside range — clamp to nearest
        if time_secs < points[0].time_secs {
            Some(*points[0])
        } else {
            self.latest().copied()
        }
    }
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_alt_band_classification() {
        assert_eq!(AltBand::from_altitude_m(-5000.0), AltBand::Bathymetry);
        assert_eq!(AltBand::from_altitude_m(0.0), AltBand::Surface);
        assert_eq!(AltBand::from_altitude_m(50.0), AltBand::Surface);
        assert_eq!(AltBand::from_altitude_m(500.0), AltBand::LowAlt);
        assert_eq!(AltBand::from_altitude_m(5000.0), AltBand::MidAlt);
        assert_eq!(AltBand::from_altitude_m(35000.0), AltBand::HighAlt);
        assert_eq!(AltBand::from_altitude_m(408_000.0), AltBand::Leo); // ISS
        assert_eq!(AltBand::from_altitude_m(20_200_000.0), AltBand::MeoGeo); // GPS
        assert_eq!(AltBand::from_altitude_m(35_786_000.0), AltBand::MeoGeo); // GEO
        assert_eq!(AltBand::from_altitude_m(384_400_000.0), AltBand::BeyondGeo); // Moon
    }

    #[test]
    fn test_alt_band_range() {
        for band in AltBand::ALL {
            let (min, max) = band.range_m();
            assert!(min < max);
        }
    }

    #[test]
    fn test_alt_bands_for_range() {
        let bands = AltBand::bands_for_range(300_000.0, 600_000.0);
        assert!(bands.contains(&AltBand::Leo));
        assert!(!bands.contains(&AltBand::Surface));
    }

    #[test]
    fn test_time_bucket_from_timestamp() {
        let bucket = TimeBucket::from_timestamp(1711036800, TimeTier::Daily);
        assert_eq!(bucket.start_secs % 86400, 0);
        assert!(bucket.start_secs <= 1711036800);
        assert!(bucket.end_secs() > 1711036800);
    }

    #[test]
    fn test_time_bucket_sequence() {
        let b1 = TimeBucket::from_timestamp(1711036800, TimeTier::Recent);
        let b2 = b1.next();
        let b3 = b2.prev();
        assert_eq!(b1.start_secs, b3.start_secs);
        assert_eq!(b2.start_secs - b1.start_secs, 3600);
    }

    #[test]
    fn test_time_bucket_auto_tier() {
        // 10 minutes -> realtime
        assert_eq!(TimeBucket::auto_tier(0, 600), TimeTier::Realtime);
        // 1 day -> recent (hourly)
        assert_eq!(TimeBucket::auto_tier(0, 86400), TimeTier::Recent);
        // 30 days -> daily
        assert_eq!(TimeBucket::auto_tier(0, 2_592_000), TimeTier::Daily);
        // 1 year -> monthly
        assert_eq!(TimeBucket::auto_tier(0, 31_536_000), TimeTier::Monthly);
    }

    #[test]
    fn test_space_time_point_distance() {
        let a = SpaceTimePoint::new(0.0, 0.0, 0.0, 0);
        let b = SpaceTimePoint::new(0.0, 0.0, 1000.0, 0);
        // Vertical-only distance should be ~1000m
        let d = a.distance_3d(&b);
        assert!((d - 1000.0).abs() < 1.0);
    }

    #[test]
    fn test_spatial_key_encode_decode_roundtrip() {
        let key = SpatialKey {
            s2_cell: S2CellId(0x1234_5678_9ABC_DEF0),
            alt_band: AltBand::Leo,
            time_bucket: TimeBucket {
                tier: TimeTier::Recent,
                start_secs: 1711036800,
            },
        };
        let encoded = key.encode();
        let decoded = SpatialKey::decode(encoded);
        assert_eq!(decoded.s2_cell, key.s2_cell);
        assert_eq!(decoded.alt_band, key.alt_band);
        assert_eq!(decoded.time_bucket.tier, key.time_bucket.tier);
        assert_eq!(decoded.time_bucket.start_secs, key.time_bucket.start_secs);
    }

    #[test]
    fn test_spatial_index_insert_query() {
        let mut index: Spatial3DIndex<String> =
            Spatial3DIndex::new(12, TimeTier::Recent);

        let p1 = SpaceTimePoint::new(27.3364, -82.5307, 0.0, 1711036800);
        let p2 = SpaceTimePoint::new(27.3365, -82.5308, 0.0, 1711036801);

        index.insert(p1, "point_a".to_string());
        index.insert(p2, "point_b".to_string());

        assert_eq!(index.len(), 2);
    }

    #[test]
    fn test_ring_buffer() {
        let mut buf = PositionRingBuffer::new(3);
        assert!(buf.is_empty());

        buf.push(SpaceTimePoint::new(0.0, 0.0, 0.0, 100));
        buf.push(SpaceTimePoint::new(1.0, 1.0, 0.0, 200));
        buf.push(SpaceTimePoint::new(2.0, 2.0, 0.0, 300));
        assert_eq!(buf.len(), 3);

        // Overwrite oldest
        buf.push(SpaceTimePoint::new(3.0, 3.0, 0.0, 400));
        assert_eq!(buf.len(), 3);

        let latest = buf.latest().unwrap();
        assert_eq!(latest.lat, 3.0);
        assert_eq!(latest.time_secs, 400);
    }

    #[test]
    fn test_ring_buffer_interpolation() {
        let mut buf = PositionRingBuffer::new(10);
        buf.push(SpaceTimePoint::new(0.0, 0.0, 0.0, 0));
        buf.push(SpaceTimePoint::new(10.0, 10.0, 1000.0, 100));

        let mid = buf.interpolate(50).unwrap();
        assert!((mid.lat - 5.0).abs() < 0.01);
        assert!((mid.lon - 5.0).abs() < 0.01);
        assert!((mid.alt_m - 500.0).abs() < 0.01);
    }
}