//! Geospatial utilities for drone swarm and location-based identity resolution.
//!
//! Provides geohash encoding for blocking keys and proximity-based comparators.
//! No external crate dependencies - pure Rust implementation.

/// Base-32 alphabet used by the standard geohash encoding.
const BASE32: &[u8; 32] = b"0123456789bcdefghjkmnpqrstuvwxyz";

/// Encode a (latitude, longitude) pair into a geohash string of the given precision.
///
/// Precision determines the cell size:
/// - 4 chars: ~39km x 20km
/// - 5 chars: ~5km x 5km
/// - 6 chars: ~1.2km x 0.6km
/// - 7 chars: ~153m x 153m
/// - 8 chars: ~38m x 19m
///
/// For drone swarm blocking, precision 6 (~1km) is typical for blocking keys,
/// and precision 8 (~38m) is used for fine-grained comparator scoring.
pub fn encode_geohash(lat: f64, lon: f64, precision: usize) -> String {
    let mut lat_range = (-90.0_f64, 90.0_f64);
    let mut lon_range = (-180.0_f64, 180.0_f64);
    let mut hash = String::with_capacity(precision);
    let mut bits: u8 = 0;
    let mut bit_count: u8 = 0;
    let mut is_lon = true;

    while hash.len() < precision {
        if is_lon {
            let mid = (lon_range.0 + lon_range.1) / 2.0;
            if lon >= mid {
                bits = (bits << 1) | 1;
                lon_range.0 = mid;
            } else {
                bits <<= 1;
                lon_range.1 = mid;
            }
        } else {
            let mid = (lat_range.0 + lat_range.1) / 2.0;
            if lat >= mid {
                bits = (bits << 1) | 1;
                lat_range.0 = mid;
            } else {
                bits <<= 1;
                lat_range.1 = mid;
            }
        }
        is_lon = !is_lon;
        bit_count += 1;

        if bit_count == 5 {
            hash.push(BASE32[bits as usize] as char);
            bits = 0;
            bit_count = 0;
        }
    }

    hash
}

/// Compute the Haversine distance in meters between two (lat, lon) points.
pub fn haversine_distance_m(lat1: f64, lon1: f64, lat2: f64, lon2: f64) -> f64 {
    const R: f64 = 6_371_000.0; // Earth radius in meters
    let dlat = (lat2 - lat1).to_radians();
    let dlon = (lon2 - lon1).to_radians();
    let a = (dlat / 2.0).sin().powi(2)
        + lat1.to_radians().cos() * lat2.to_radians().cos() * (dlon / 2.0).sin().powi(2);
    R * 2.0 * a.sqrt().asin()
}

/// Cosine similarity on comma-separated float embedding vectors (e.g. OSNet 512-d).
/// Returns a value in [0.0, 1.0]. Vectors must be the same length.
pub fn embedding_cosine_similarity(a: &str, b: &str) -> f64 {
    let va: Vec<f64> = a.split(',').filter_map(|s| s.trim().parse().ok()).collect();
    let vb: Vec<f64> = b.split(',').filter_map(|s| s.trim().parse().ok()).collect();
    if va.is_empty() || vb.is_empty() || va.len() != vb.len() {
        return 0.0;
    }
    let dot: f64 = va.iter().zip(vb.iter()).map(|(x, y)| x * y).sum();
    let mag_a: f64 = va.iter().map(|x| x * x).sum::<f64>().sqrt();
    let mag_b: f64 = vb.iter().map(|x| x * x).sum::<f64>().sqrt();
    if mag_a == 0.0 || mag_b == 0.0 {
        return 0.0;
    }
    (dot / (mag_a * mag_b)).clamp(0.0, 1.0)
}

/// Speed-constraint plausibility score for moving-target re-identification.
/// Input format: "lat,lon,epoch_secs" for each value.
/// Returns 0.0 if implied speed exceeds 55 m/s (200 km/h).
/// Returns 1.0 - (speed / max_speed) for plausible transitions.
pub fn speed_constraint_score(a: &str, b: &str) -> f64 {
    const MAX_SPEED_MS: f64 = 55.0;

    let parse_triple = |s: &str| -> Option<(f64, f64, f64)> {
        let parts: Vec<&str> = s.split(',').collect();
        if parts.len() >= 3 {
            Some((
                parts[0].trim().parse().ok()?,
                parts[1].trim().parse().ok()?,
                parts[2].trim().parse().ok()?,
            ))
        } else {
            None
        }
    };

    let (lat1, lon1, t1) = match parse_triple(a) { Some(v) => v, None => return 0.0 };
    let (lat2, lon2, t2) = match parse_triple(b) { Some(v) => v, None => return 0.0 };

    let dist = haversine_distance_m(lat1, lon1, lat2, lon2);
    let dt = (t2 - t1).abs();

    if dt < 0.001 {
        return if dist < 50.0 { 1.0 } else { 0.0 };
    }

    let speed = dist / dt;
    if speed > MAX_SPEED_MS {
        0.0
    } else {
        (1.0 - speed / MAX_SPEED_MS).clamp(0.0, 1.0)
    }
}

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

    #[test]
    fn test_geohash_london() {
        // London: 51.5074, -0.1278
        let gh = encode_geohash(51.5074, -0.1278, 6);
        assert_eq!(gh.len(), 6);
        // Known geohash prefix for London area
        assert!(gh.starts_with("gcpvj"), "London geohash should start with gcpvj, got {}", gh);
    }

    #[test]
    fn test_geohash_precision_8() {
        let gh = encode_geohash(51.5074, -0.1278, 8);
        assert_eq!(gh.len(), 8);
    }

    #[test]
    fn test_nearby_points_share_prefix() {
        // Two points ~5m apart should share at least 7 geohash chars
        let gh_a = encode_geohash(51.50740, -0.12780, 8);
        let gh_b = encode_geohash(51.50744, -0.12782, 8);
        let shared = gh_a.chars().zip(gh_b.chars()).take_while(|(a, b)| a == b).count();
        assert!(shared >= 7, "Nearby points should share >= 7 chars, got {} (a={}, b={})", shared, gh_a, gh_b);
    }

    #[test]
    fn test_distant_points_differ_early() {
        // London vs New York
        let gh_london = encode_geohash(51.5074, -0.1278, 6);
        let gh_nyc = encode_geohash(40.7128, -74.0060, 6);
        let shared = gh_london.chars().zip(gh_nyc.chars()).take_while(|(a, b)| a == b).count();
        assert!(shared <= 1, "Distant cities should share few chars, got {}", shared);
    }

    #[test]
    fn test_haversine_same_point() {
        let d = haversine_distance_m(51.5074, -0.1278, 51.5074, -0.1278);
        assert!(d < 0.01, "Same point should be 0m, got {}", d);
    }

    #[test]
    fn test_haversine_known_distance() {
        // London to Paris: ~340km
        let d = haversine_distance_m(51.5074, -0.1278, 48.8566, 2.3522);
        assert!((d - 340_000.0).abs() < 5_000.0, "London-Paris should be ~340km, got {}m", d);
    }

    #[test]
    fn test_haversine_nearby_drones() {
        // Two points ~5m apart (typical drone GPS noise)
        let d = haversine_distance_m(51.50740, -0.12780, 51.50744, -0.12782);
        assert!(d < 10.0, "Nearby points should be < 10m, got {}m", d);
    }
}
