scout_rs 0.95.59

//! Shared connectivity → observation metadata (tags, embeddings, segmentations, tracker): timestamp match + field copy.

use crate::db_client::ConnectivityWithCoordinates;
use crate::models::data::Tag;
use crate::models::EmbeddingVertexMultimodal001;
use crate::models::SegmentationSam3;
use crate::time_util::parse_rfc3339_to_unix_secs;

/// Default window for matching tag/frame time to connectivity `timestamp_start` (seconds).
pub const CONNECTIVITY_TIMESTAMP_MATCH_TOLERANCE_SECS: f64 = 10.0;

/// Parsed `timestamp_start` per connectivity row (reuse across many lookups).
pub fn connectivity_observation_unix_secs(connectivity: &[ConnectivityWithCoordinates]) -> Vec<Option<f64>> {
    connectivity
        .iter()
        .map(|c| {
            c.timestamp_start
                .as_ref()
                .and_then(|s| parse_rfc3339_to_unix_secs(s.as_str()))
        })
        .collect()
}

/// Index of the connectivity row whose time is nearest to `observation_unix_secs` within `tolerance_secs`.
pub fn best_connectivity_match_index(
    conn_unix: &[Option<f64>],
    observation_unix_secs: f64,
    tolerance_secs: f64,
) -> Option<usize> {
    let mut best_idx = None;
    let mut best_diff = tolerance_secs + 1.0;
    for (i, &conn_ts) in conn_unix.iter().enumerate() {
        if let Some(ct) = conn_ts {
            let diff = (observation_unix_secs - ct).abs();
            if diff <= tolerance_secs && diff < best_diff {
                best_diff = diff;
                best_idx = Some(i);
            }
        }
    }
    best_idx
}

fn apply_connectivity_observation_fields(
    origin_heading: &mut Option<f64>,
    sensor_yaw: &mut Option<f64>,
    origin_height: &mut Option<f64>,
    origin_location: &mut Option<String>,
    c: &ConnectivityWithCoordinates,
) {
    if c.heading.is_some() {
        *origin_heading = c.heading;
        *sensor_yaw = c.heading;
    }
    if c.altitude.is_some() {
        *origin_height = c.altitude;
    }
    if let (Some(lat), Some(lon)) = (c.latitude, c.longitude) {
        *origin_location = Some(Tag::format_location(lat, lon));
    }
}

/// Copy heading (→ origin_heading + sensor_yaw), altitude, and location from one connectivity row.
pub fn apply_connectivity_to_tag(tag: &mut Tag, c: &ConnectivityWithCoordinates) {
    apply_connectivity_observation_fields(
        &mut tag.origin_heading,
        &mut tag.sensor_yaw,
        &mut tag.origin_height,
        &mut tag.origin_location,
        c,
    );
}

/// Same columns as [`apply_connectivity_to_tag`] for multimodal embedding rows.
pub fn apply_connectivity_to_embedding_vertex(
    row: &mut EmbeddingVertexMultimodal001,
    c: &ConnectivityWithCoordinates,
) {
    apply_connectivity_observation_fields(
        &mut row.origin_heading,
        &mut row.sensor_yaw,
        &mut row.origin_height,
        &mut row.origin_location,
        c,
    );
}

/// Same columns as [`apply_connectivity_to_tag`] for SAM3 segmentation rows.
pub fn apply_connectivity_to_segmentation_sam3(
    row: &mut SegmentationSam3,
    c: &ConnectivityWithCoordinates,
) {
    apply_connectivity_observation_fields(
        &mut row.origin_heading,
        &mut row.sensor_yaw,
        &mut row.origin_height,
        &mut row.origin_location,
        c,
    );
}

/// For each tag with a parseable `timestamp_observation`, match connectivity and apply fields.
/// Returns how many tags received a match.
pub fn enrich_tags_with_connectivity(
    tags: &mut [Tag],
    connectivity: &[ConnectivityWithCoordinates],
    timestamp_match_tolerance_secs: f64,
) -> usize {
    if connectivity.is_empty() {
        return 0;
    }
    let conn_unix = connectivity_observation_unix_secs(connectivity);
    let mut matched = 0;
    for tag in tags.iter_mut() {
        let tag_ts = tag
            .timestamp_observation
            .as_ref()
            .and_then(|s| parse_rfc3339_to_unix_secs(s.as_str()));
        let Some(tag_ts) = tag_ts else {
            continue;
        };
        let Some(idx) = best_connectivity_match_index(&conn_unix, tag_ts, timestamp_match_tolerance_secs) else {
            continue;
        };
        matched += 1;
        apply_connectivity_to_tag(tag, &connectivity[idx]);
    }
    matched
}

/// For each segmentation row with a parseable `timestamp_observation`, match connectivity and apply fields.
/// Returns how many rows received a match.
pub fn enrich_segmentations_sam3_with_connectivity(
    rows: &mut [SegmentationSam3],
    connectivity: &[ConnectivityWithCoordinates],
    timestamp_match_tolerance_secs: f64,
) -> usize {
    if connectivity.is_empty() {
        return 0;
    }
    let conn_unix = connectivity_observation_unix_secs(connectivity);
    let mut matched = 0;
    for row in rows.iter_mut() {
        let ts = row
            .timestamp_observation
            .as_ref()
            .and_then(|s| parse_rfc3339_to_unix_secs(s.as_str()));
        let Some(ts) = ts else {
            continue;
        };
        let Some(idx) = best_connectivity_match_index(&conn_unix, ts, timestamp_match_tolerance_secs) else {
            continue;
        };
        matched += 1;
        apply_connectivity_to_segmentation_sam3(row, &connectivity[idx]);
    }
    matched
}