rusty-telemetry/src/analysis.rs

//! Telemetry analysis algorithms for sim racing data.
//!
//! Provides shift point analysis, track map reconstruction, and a dispatch
//! function that routes a [`UseCase`] to the appropriate analysis pipeline.

use serde::Serialize;
use std::collections::HashMap;

use crate::recording::UseCase;
use crate::telemetry_frame::{gear_display, TelemetryFrame};

// ---------------------------------------------------------------------------
// Shift Point Analysis
// ---------------------------------------------------------------------------

#[derive(Debug, Clone, Serialize)]
pub struct AccelerationPoint {
    pub speed_kmh: f32,
    pub rpm: f32,
    pub acceleration_g: f32,
}

#[derive(Debug, Clone, Serialize)]
pub struct GearAccelerationCurve {
    pub gear: i32,
    pub gear_display: String,
    pub run_label: String,
    pub data_points: Vec<AccelerationPoint>,
    pub min_rpm: f32,
    pub max_rpm: f32,
    pub min_speed_kmh: f32,
    pub max_speed_kmh: f32,
    pub sample_count: usize,
}

#[derive(Debug, Clone, Serialize)]
pub struct ShiftCrossoverPoint {
    pub from_gear: i32,
    pub to_gear: i32,
    pub from_gear_display: String,
    pub to_gear_display: String,
    pub crossover_speed_kmh: f32,
    pub crossover_rpm: f32,
}

#[derive(Debug, Clone, Serialize)]
pub struct ShiftPointAnalysis {
    pub gear_curves: Vec<GearAccelerationCurve>,
    pub crossover_points: Vec<ShiftCrossoverPoint>,
    pub recommended_shift_rpm: Option<f32>,
    pub runs_analyzed: usize,
    pub total_frames_analyzed: usize,
}

pub fn analyze_shift_points(
    runs: &[Vec<TelemetryFrame>],
    run_labels: &[String],
) -> ShiftPointAnalysis {
    let mut gear_data: HashMap<i32, Vec<AccelerationPoint>> = HashMap::new();
    let mut gear_labels: HashMap<i32, Vec<String>> = HashMap::new();
    let mut total_frames_analyzed: usize = 0;
    let mut runs_analyzed: usize = 0;

    for (run_idx, run) in runs.iter().enumerate() {
        let label = run_labels.get(run_idx).cloned().unwrap_or_default();

        let valid: Vec<&TelemetryFrame> = run
            .iter()
            .filter(|f| f.throttle >= 0.9 && f.gear >= 2 && f.speed_kmh > 1.0 && f.engine_rpm > 500.0)
            .collect();

        if valid.len() < 10 {
            continue;
        }

        let dominant_gear = {
            let mut freq: HashMap<i32, usize> = HashMap::new();
            for f in &valid {
                *freq.entry(f.gear).or_default() += 1;
            }
            *freq.iter().max_by_key(|(_, c)| *c).map(|(g, _)| g).unwrap_or(&2)
        };

        for f in valid.into_iter().filter(|f| f.gear == dominant_gear) {
            gear_data.entry(dominant_gear).or_default().push(AccelerationPoint {
                speed_kmh: f.speed_kmh,
                rpm: f.engine_rpm,
                acceleration_g: f.acc_g_frontal,
            });
        }

        gear_labels.entry(dominant_gear).or_default().push(label);
        runs_analyzed += 1;
    }

    let mut gear_curves: Vec<GearAccelerationCurve> = Vec::new();

    for (gear, mut points) in gear_data {
        total_frames_analyzed += points.len();

        points.sort_by(|a, b| a.speed_kmh.partial_cmp(&b.speed_kmh).unwrap_or(std::cmp::Ordering::Equal));

        let window = (points.len() / 5).max(3).min(15);
        let acc_values: Vec<f32> = points.iter().map(|p| p.acceleration_g).collect();
        let smoothed = smooth(&acc_values, window);

        for (i, s) in smoothed.into_iter().enumerate() {
            points[i].acceleration_g = s;
        }

        let min_rpm = points.iter().map(|p| p.rpm).fold(f32::INFINITY, f32::min);
        let max_rpm = points.iter().map(|p| p.rpm).fold(f32::NEG_INFINITY, f32::max);
        let min_speed = points.iter().map(|p| p.speed_kmh).fold(f32::INFINITY, f32::min);
        let max_speed = points.iter().map(|p| p.speed_kmh).fold(f32::NEG_INFINITY, f32::max);
        let sample_count = points.len();

        let labels = gear_labels.get(&gear).cloned().unwrap_or_default();
        let run_label = if labels.len() == 1 {
            labels[0].clone()
        } else {
            format!("{} ({} runs)", labels.join(", "), labels.len())
        };

        gear_curves.push(GearAccelerationCurve {
            gear,
            gear_display: gear_display(gear),
            run_label,
            data_points: points,
            min_rpm,
            max_rpm,
            min_speed_kmh: min_speed,
            max_speed_kmh: max_speed,
            sample_count,
        });
    }

    gear_curves.sort_by_key(|c| c.gear);

    let crossover_points = find_crossovers(&gear_curves);

    let recommended_shift_rpm = {
        let rpms: Vec<f32> = crossover_points.iter().map(|c| c.crossover_rpm).collect();
        if rpms.is_empty() {
            None
        } else {
            Some(rpms.iter().sum::<f32>() / rpms.len() as f32)
        }
    };

    ShiftPointAnalysis {
        gear_curves,
        crossover_points,
        recommended_shift_rpm,
        runs_analyzed,
        total_frames_analyzed,
    }
}

fn smooth(values: &[f32], window: usize) -> Vec<f32> {
    let half = window / 2;
    let n = values.len();
    let mut result = Vec::with_capacity(n);
    for i in 0..n {
        let start = if i >= half { i - half } else { 0 };
        let end = (i + half + 1).min(n);
        let sum: f32 = values[start..end].iter().sum();
        result.push(sum / (end - start) as f32);
    }
    result
}

fn find_crossovers(curves: &[GearAccelerationCurve]) -> Vec<ShiftCrossoverPoint> {
    let mut crossovers = Vec::new();

    for pair in curves.windows(2) {
        let low = &pair[0];
        let high = &pair[1];

        if low.gear == high.gear {
            continue;
        }

        let overlap_min = low.min_speed_kmh.max(high.min_speed_kmh);
        let overlap_max = low.max_speed_kmh.min(high.max_speed_kmh);

        if overlap_max - overlap_min < 1.0 {
            continue;
        }

        let grid_size = 100;
        let step = (overlap_max - overlap_min) / (grid_size as f32 - 1.0);

        let mut found_crossover: Option<(f32, f32)> = None;

        for i in 0..grid_size {
            let target_speed = overlap_min + i as f32 * step;
            let low_accel = match interpolate_curve(&low.data_points, target_speed) {
                Some(a) => a,
                None => continue,
            };
            let high_accel = match interpolate_curve(&high.data_points, target_speed) {
                Some(a) => a,
                None => continue,
            };

            if low_accel < high_accel {
                let rpm = interpolate_rpm(&low.data_points, target_speed).unwrap_or(0.0);
                found_crossover = Some((target_speed, rpm));
                break;
            }
        }

        if let Some((speed, rpm)) = found_crossover {
            crossovers.push(ShiftCrossoverPoint {
                from_gear: low.gear,
                to_gear: high.gear,
                from_gear_display: low.gear_display.clone(),
                to_gear_display: high.gear_display.clone(),
                crossover_speed_kmh: speed,
                crossover_rpm: rpm,
            });
        }
    }

    crossovers
}

fn interpolate_field<F>(curve: &[AccelerationPoint], target_speed: f32, field: F) -> Option<f32>
where
    F: Fn(&AccelerationPoint) -> f32,
{
    let (t, a, b) = lerp_search(curve, |p| p.speed_kmh, target_speed)?;
    Some(field(a) + t * (field(b) - field(a)))
}

fn interpolate_curve(curve: &[AccelerationPoint], target_speed: f32) -> Option<f32> {
    interpolate_field(curve, target_speed, |p| p.acceleration_g)
}

fn interpolate_rpm(curve: &[AccelerationPoint], target_speed: f32) -> Option<f32> {
    interpolate_field(curve, target_speed, |p| p.rpm)
}

fn lerp_search<T>(
    items: &[T],
    key: impl Fn(&T) -> f32,
    target: f32,
) -> Option<(f32, &T, &T)> {
    if items.is_empty() {
        return None;
    }
    if items.len() == 1 {
        return Some((0.0, &items[0], &items[0]));
    }

    let idx = items.partition_point(|p| key(p) < target);
    if idx == 0 {
        return Some((0.0, &items[0], &items[0]));
    }
    if idx >= items.len() {
        return Some((0.0, &items[items.len() - 1], &items[items.len() - 1]));
    }

    let a = &items[idx - 1];
    let b = &items[idx];
    let t = if (key(b) - key(a)).abs() < f32::EPSILON {
        0.0
    } else {
        (target - key(a)) / (key(b) - key(a))
    };

    Some((t, a, b))
}

// ---------------------------------------------------------------------------
// Track Map Analysis
// ---------------------------------------------------------------------------

#[derive(Debug, Clone, Serialize)]
pub struct TrackPoint {
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub spline: f32,
}

#[derive(Debug, Clone, Serialize)]
pub struct TrackBoundary {
    pub label: String,
    pub point_count: usize,
    pub points: Vec<TrackPoint>,
}

#[derive(Debug, Clone, Serialize)]
pub struct TrackMapAnalysis {
    pub boundaries: Vec<TrackBoundary>,
    pub center_line: Option<TrackBoundary>,
    pub total_points: usize,
}

pub fn analyze_track_map(
    runs: &[Vec<TelemetryFrame>],
    labels: &[String],
) -> TrackMapAnalysis {
    let mut boundaries: Vec<TrackBoundary> = Vec::new();

    for (run, label) in runs.iter().zip(labels.iter()) {
        let mut points: Vec<TrackPoint> = run
            .iter()
            .filter(|f| f.spline_position != 0.0)
            .map(|f| TrackPoint {
                x: f.world_x,
                y: f.world_y,
                z: f.world_z,
                spline: f.spline_position,
            })
            .collect();
        points.sort_by(|a, b| a.spline.partial_cmp(&b.spline).unwrap_or(std::cmp::Ordering::Equal));
        let point_count = points.len();
        boundaries.push(TrackBoundary {
            label: label.clone(),
            point_count,
            points,
        });
    }

    let center_line = compute_center_line(&boundaries);

    let total_points: usize = boundaries.iter().map(|b| b.point_count).sum::<usize>()
        + center_line.as_ref().map_or(0, |c| c.point_count);

    TrackMapAnalysis {
        boundaries,
        center_line,
        total_points,
    }
}

fn compute_center_line(boundaries: &[TrackBoundary]) -> Option<TrackBoundary> {
    if boundaries.len() != 2 {
        return None;
    }

    let left_idx = boundaries
        .iter()
        .position(|b| b.label == "left")?;
    let right_idx = boundaries
        .iter()
        .position(|b| b.label == "right")?;

    if left_idx == right_idx {
        return None;
    }

    let left = &boundaries[left_idx];
    let right = &boundaries[right_idx];

    if left.points.is_empty() || right.points.is_empty() {
        return None;
    }

    let num_samples = left.point_count.max(right.point_count).min(2000);
    let center_points: Vec<TrackPoint> = (0..num_samples)
        .filter_map(|i| {
            let spline = i as f32 / (num_samples - 1).max(1) as f32;
            let lp = interpolate_boundary(left, spline)?;
            let rp = interpolate_boundary(right, spline)?;
            Some(TrackPoint {
                x: (lp.x + rp.x) / 2.0,
                y: (lp.y + rp.y) / 2.0,
                z: (lp.z + rp.z) / 2.0,
                spline,
            })
        })
        .collect();

    if center_points.is_empty() {
        return None;
    }

    Some(TrackBoundary {
        label: "center_line".to_owned(),
        point_count: center_points.len(),
        points: center_points,
    })
}

fn interpolate_boundary(boundary: &TrackBoundary, target_spline: f32) -> Option<TrackPoint> {
    let points = &boundary.points;
    let (t, a, b) = lerp_search(points, |p| p.spline, target_spline)?;

    Some(TrackPoint {
        x: a.x + t * (b.x - a.x),
        y: a.y + t * (b.y - a.y),
        z: a.z + t * (b.z - a.z),
        spline: target_spline,
    })
}

// ---------------------------------------------------------------------------
// Dispatch
// ---------------------------------------------------------------------------

pub fn analyze(
    use_case: &UseCase,
    frames_by_run: &[Vec<TelemetryFrame>],
    run_labels: &[String],
) -> serde_json::Value {
    match use_case {
        UseCase::ShiftPoints => {
            if frames_by_run.is_empty() || frames_by_run.iter().all(|r| r.is_empty()) {
                serde_json::json!({"error": "no frames provided"})
            } else {
                serde_json::to_value(analyze_shift_points(frames_by_run, run_labels))
                    .unwrap_or_else(|_| serde_json::json!({"error": "serialization failed"}))
            }
        }
        UseCase::TrackMap => serde_json::to_value(analyze_track_map(frames_by_run, run_labels))
            .unwrap_or_else(|_| serde_json::json!({"error": "serialization failed"})),
        _ => serde_json::json!({
            "status": "not_implemented",
            "use_case": format!("{:?}", use_case),
        }),
    }
}