Data structure for geolocation history

geo_db.rs

use anyhow::{Context, Result};
use chrono::{DateTime, FixedOffset};
use std::{cmp::Ordering, collections::HashMap};

pub type LatLng = (f32, f32);
type LatLngDelta = (u8, u8);
type Minutes = u32;

pub fn distance_meters(start: LatLng, end: LatLng) -> f32 {
    let r = 6371000.;

    let d_lat = (end.0 - start.0).to_radians();
    let d_lon = (end.1 - start.1).to_radians();
    let lat1 = (start.0).to_radians();
    let lat2 = (end.0).to_radians();

    let a = ((d_lat / 2.0).sin()) * ((d_lat / 2.0).sin())
        + ((d_lon / 2.0).sin()) * ((d_lon / 2.0).sin()) * (lat1.cos()) * (lat2.cos());
    let c = 2.0 * ((a.sqrt()).atan2((1.0 - a).sqrt()));

    r * c
}

#[derive(Default)]
pub struct GeoDB {
    beginning_timestamp: Minutes,
    max_delta_lat: f32,
    high_precision: Vec<(Minutes, LatLng)>,
    low_precision: HashMap<Minutes, Vec<LatLngDelta>>,
}

#[derive(Default)]
pub struct Builder {
    db: GeoDB,
    max_error_meters: f32,
    last_high_pos: LatLng,
    last_high_minutes: Minutes,
    last_low_minutes: Minutes,
}

impl Builder {
    pub fn new() -> Self {
        let db = GeoDB {
            max_delta_lat: 0.006,
            ..Default::default()
        };
        Self {
            db,
            max_error_meters: 5.,
            ..Default::default()
        }
    }

    pub fn set_max_delta_lat(&mut self, max_delta_lat: f32) {
        self.db.max_delta_lat = max_delta_lat;
    }

    pub fn set_max_error(&mut self, max_error_meters: f32) {
        self.max_error_meters = max_error_meters;
    }

    pub fn build(mut self) -> GeoDB {
        self.db.high_precision.shrink_to_fit();
        for v in self.db.low_precision.values_mut() {
            v.shrink_to_fit();
        }
        self.db
    }

    fn add_high_precision(&mut self, minutes: Minutes, pos: LatLng) {
        self.db.high_precision.push((minutes, pos));
        self.last_high_minutes = minutes;
        self.last_high_pos = pos;
    }

    pub fn add(&mut self, datetime: DateTime<FixedOffset>, pos: LatLng) -> Result<bool> {
        let first_datapoint = self.db.beginning_timestamp == 0;
        if first_datapoint {
            self.db.beginning_timestamp = datetime.timestamp() as Minutes;
        }

        let minutes = self.db.minutes_since_beginning(datetime);

        let last_low_minutes = self.last_low_minutes;
        let last_high_minutes = self.last_high_minutes;
        let last_minutes = last_low_minutes.max(last_high_minutes);

        if !first_datapoint {
            match minutes.cmp(&last_minutes) {
                Ordering::Equal => {
                    return Ok(false);
                }
                Ordering::Less => {
                    return Err(anyhow!("tried to insert date in wrong order"));
                }
                _ => {}
            }
        }

        let (lat, lng) = pos;

        if self.db.high_precision.is_empty() {
            self.add_high_precision(minutes, pos);
            return Ok(true);
        }

        let delta_lat = lat - self.last_high_pos.0;
        let delta_lng = lng - self.last_high_pos.1;

        let delta_lat8 = ((2.0 * delta_lat * (255. - 128.) / self.db.max_delta_lat) + 128.) as u8;
        let delta_lng8 = ((2.0 * delta_lng * (255. - 128.) / self.db.max_delta_lng()) + 128.) as u8;
        let delta = (delta_lat8, delta_lng8);

        let to_store = self.db.pos_from_delta(self.last_high_pos, delta);

        let error = distance_meters(pos, to_store);
        if error > self.max_error_meters {
            self.add_high_precision(minutes, pos);
            return Ok(true);
        }

        self.db
            .low_precision
            .entry(self.last_high_minutes)
            .or_insert_with(Vec::new);

        let points = self.db.low_precision.get_mut(&self.last_high_minutes).unwrap();

        let delta_minutes = minutes - last_high_minutes - (points.len() as Minutes);
        if delta_minutes > 4 {
            self.add_high_precision(minutes, pos);
            return Ok(true);
        }

        if delta_minutes > 0 {
            let last_delta8 = if points.is_empty() {
                (128, 128)
            } else {
                *points.last().unwrap()
            };
            for _ in 0..delta_minutes - 1 {
                points.push(last_delta8);
            }
        }

        self.last_low_minutes = minutes;
        points.push(delta);

        Ok(true)
    }
}

impl GeoDB {
    fn minutes_since_beginning(&self, datetime: DateTime<FixedOffset>) -> Minutes {
        (datetime.timestamp() as Minutes - self.beginning_timestamp) / 60
    }

    fn max_delta_lng(&self) -> f32 {
        self.max_delta_lat * 2.
    }

    fn pos_from_delta(&self, high: LatLng, delta: LatLngDelta) -> LatLng {
        let delta_lat = self.max_delta_lat * 0.5 / (255. - 128.) * (delta.0 as f32 - 128.);
        let lat = high.0 + delta_lat;

        let delta_lng = self.max_delta_lng() * 0.5 / (255. - 128.) * (delta.1 as f32 - 128.);
        let lng = high.1 + delta_lng;

        (lat, lng)
    }

    fn low_precision_point_count(&self) -> usize {
        self.low_precision.values().fold(0, |sum, item| sum + item.len())
    }

    pub fn pos(&self, timestamp: DateTime<FixedOffset>) -> Result<LatLng> {
        let minutes = self.minutes_since_beginning(timestamp);

        let search_res = self
            .high_precision
            .binary_search_by(|entry| entry.0.cmp(&minutes));

        let high_index = match search_res {
            Ok(i) => i,
            Err(i) => i - 1,
        };

        let (high_minutes, high_pos) = self
            .high_precision
            .get(high_index)
            .context("empty high_precision")?;

        let points = self.low_precision.get(high_minutes);
        let delta_minutes = (minutes - (*high_minutes)) as usize;

        let latlng = match (delta_minutes, points) {
            (_, None) | (0, _) => *high_pos,
            (_, Some(p)) => {
                let delta = p
                    .get(delta_minutes - 1)
                    .or_else(|| p.last())
                    .context("no last point")?;

                self.pos_from_delta(*high_pos, *delta)
            }
        };

        Ok(latlng)
    }
}

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

    #[test]
    fn distances() {
        let mut db_builder = Builder::new();

        let points = vec![
            ("2022-10-21T05:16:00.444Z", -23., -46.),
            ("2022-10-21T05:17:00.444Z", -23.003, -46.006),
            ("2022-10-21T05:23:00.444Z", -23.002, -45.998),
            ("2022-10-21T05:28:00.444Z", 42., 64.),
            ("2022-10-21T05:29:00.444Z", 42.001, 63.999),
        ];

        for (time_str, lat, lng) in &points {
            let time = DateTime::parse_from_rfc3339(time_str).unwrap();
            db_builder.add(time, (*lat, *lng)).unwrap();
        }

        let db = db_builder.build();

        for (time_str, lat, lng) in points {
            let time = DateTime::parse_from_rfc3339(time_str).unwrap();
            let (latitude, longitude) = db.pos(time).unwrap();
            let error = distance_meters((lat, lng), (latitude, longitude));
            assert!(error < 15.);
        }

        assert_eq!(db.high_precision.len(), 2);
        assert_eq!(db.low_precision_point_count(), 8);

        let time = DateTime::parse_from_rfc3339("2022-10-21T05:18:00.444Z").unwrap();
        let (lat, lng) = db.pos(time).unwrap();
        assert!((-23.003 - lat).abs() < 0.0001);
        assert!((-46.006 - lng).abs() < 0.0002);

        let time = DateTime::parse_from_rfc3339("2022-10-21T05:27:00.444Z").unwrap();
        let (lat, lng) = db.pos(time).unwrap();
        assert!((-23.002 - lat).abs() < 0.0001);
        assert!((-45.998 - lng).abs() < 0.0002);
    }
}