//! Autonomous vehicle perception benchmarks
//!
//! Performance benchmarks for:
//! - LiDAR point cloud processing
//! - Multi-sensor fusion
//! - 3D object detection and tracking
//! - Real-time perception pipelines
//! - Safety-critical validation

use criterion::{BenchmarkId, Criterion, Throughput, criterion_group, criterion_main};
use rtx_tensor::{DType, Device, Tensor};
use rtx_vision_advanced::*;

/// Autonomous vehicle benchmark helper
struct AutonomousBenchHelper;

impl AutonomousBenchHelper {
    fn create_point_cloud(num_points: usize) -> Tensor {
        // Create realistic LiDAR point cloud with x, y, z, intensity
        let mut points = Tensor::randn(&[num_points, 4], &Device::default())
            .expect("Failed to create point cloud");

        // Scale to realistic LiDAR ranges (±50m x, ±50m y, ±3m z)
        let scale = Tensor::new(&[[50.0, 50.0, 3.0, 255.0]], DType::F32, &Device::default())
            .expect("Failed to create scale tensor");
        points = points
            .broadcast_mul(&scale)
            .expect("Failed to scale points");

        // Add realistic intensity values (0-255)
        let intensity_col = points.narrow(1, 3, 1).expect("Failed to get intensity");
        let scaled_intensity = intensity_col.abs().expect("Failed to abs intensity");

        points
            .slice_assign(&[None, Some((3, 4))], &scaled_intensity)
            .expect("Failed to assign intensity");

        points
    }

    fn create_camera_image(height: usize, width: usize) -> Tensor {
        Tensor::randn(&[1, 3, height, width], &Device::default())
            .expect("Failed to create camera image")
    }

    fn create_radar_data(num_targets: usize) -> Tensor {
        // Radar data: range, azimuth, elevation, velocity, RCS
        Tensor::randn(&[num_targets, 5], &Device::default()).expect("Failed to create radar data")
    }
}

/// Benchmark LiDAR point cloud processing
fn bench_lidar_processing(c: &mut Criterion) {
    let mut group = c.benchmark_group("lidar_processing");

    // Different point cloud sizes (typical for various LiDAR sensors)
    let point_cloud_sizes = vec![
        ("velodyne_16", 20000),  // Velodyne VLP-16
        ("velodyne_32", 40000),  // Velodyne VLP-32
        ("velodyne_64", 80000),  // Velodyne HDL-64E
        ("ouster_128", 160000),  // Ouster OS1-128
        ("solid_state", 300000), // High-resolution solid-state LiDAR
    ];

    for (lidar_name, num_points) in point_cloud_sizes {
        let point_cloud = AutonomousBenchHelper::create_point_cloud(num_points);
        let config = autonomous::LidarConfig::default();
        let mut processor =
            autonomous::LidarProcessor::new(config).expect("Failed to create LiDAR processor");

        group.throughput(Throughput::Elements(num_points as u64));
        group.bench_with_input(
            BenchmarkId::new("lidar_processing", lidar_name),
            &point_cloud,
            |b, pc| {
                b.iter(|| {
                    processor
                        .process_point_cloud(pc)
                        .expect("LiDAR processing failed")
                });
            },
        );
    }

    group.finish();
}

/// Benchmark point cloud filtering operations
fn bench_point_cloud_filtering(c: &mut Criterion) {
    let mut group = c.benchmark_group("point_cloud_filtering");

    let point_cloud = AutonomousBenchHelper::create_point_cloud(100000);

    // Different filtering operations
    let filtering_ops = vec![
        ("ground_removal", "statistical_outlier_removal"),
        ("statistical_outlier", "ground_plane_removal"),
        ("roi_filtering", "region_of_interest"),
        ("intensity_filter", "intensity_threshold"),
        ("range_filter", "distance_threshold"),
    ];

    for (filter_name, filter_type) in filtering_ops {
        group.bench_with_input(
            BenchmarkId::new("filtering", filter_name),
            &point_cloud,
            |b, pc| {
                b.iter(|| {
                    match filter_type {
                        "statistical_outlier_removal" => {
                            // Remove statistical outliers based on neighbor distance
                            // In practice, this would use k-d tree for nearest neighbors
                            let filtered_indices = pc
                                .narrow(1, 2, 1)
                                .expect("Z coordinate")
                                .gt_scalar(-2.0)
                                .expect("Height filter")
                                .logical_and(
                                    &pc.narrow(1, 2, 1)
                                        .expect("Z coordinate")
                                        .lt_scalar(3.0)
                                        .expect("Height filter"),
                                )
                                .expect("Logical and failed");
                            filtered_indices
                        }
                        "ground_plane_removal" => {
                            // RANSAC-based ground plane removal
                            let z_coords = pc.narrow(1, 2, 1).expect("Z coordinates");
                            let non_ground_mask =
                                z_coords.gt_scalar(-1.5).expect("Ground threshold");
                            non_ground_mask
                        }
                        "region_of_interest" => {
                            // Filter points within ROI (e.g., ±25m x, ±25m y)
                            let x_coords = pc.narrow(1, 0, 1).expect("X coordinates");
                            let y_coords = pc.narrow(1, 1, 1).expect("Y coordinates");

                            let x_mask = x_coords
                                .abs()
                                .expect("X abs")
                                .lt_scalar(25.0)
                                .expect("X ROI");
                            let y_mask = y_coords
                                .abs()
                                .expect("Y abs")
                                .lt_scalar(25.0)
                                .expect("Y ROI");
                            x_mask.logical_and(&y_mask).expect("ROI mask")
                        }
                        "intensity_threshold" => {
                            // Filter by intensity values
                            let intensity = pc.narrow(1, 3, 1).expect("Intensity");
                            intensity.gt_scalar(50.0).expect("Intensity threshold")
                        }
                        "distance_threshold" => {
                            // Filter by distance from sensor
                            let x = pc.narrow(1, 0, 1).expect("X");
                            let y = pc.narrow(1, 1, 1).expect("Y");
                            let z = pc.narrow(1, 2, 1).expect("Z");

                            let dist_sq = x
                                .pow_scalar(2.0)
                                .expect("X squared")
                                .add(&y.pow_scalar(2.0).expect("Y squared"))
                                .expect("X+Y squared")
                                .add(&z.pow_scalar(2.0).expect("Z squared"))
                                .expect("Distance squared");

                            dist_sq.lt_scalar(50.0 * 50.0).expect("Distance filter") // 50m max range
                        }
                        _ => pc
                            .narrow(1, 0, 1)
                            .expect("Default filter")
                            .gt_scalar(-1000.0)
                            .expect("Default mask"),
                    }
                });
            },
        );
    }

    group.finish();
}

/// Benchmark 3D object detection in point clouds
fn bench_3d_object_detection(c: &mut Criterion) {
    let mut group = c.benchmark_group("3d_object_detection");

    let point_cloud_configs = vec![
        ("urban_scene", 100000),
        ("highway_scene", 150000),
        ("parking_lot", 80000),
        ("intersection", 120000),
    ];

    for (scene_name, num_points) in point_cloud_configs {
        let point_cloud = AutonomousBenchHelper::create_point_cloud(num_points);
        let mut detector =
            detection::three_d::PointRCNN::new().expect("Failed to create 3D detector");

        group.throughput(Throughput::Elements(num_points as u64));
        group.bench_with_input(
            BenchmarkId::new("3d_detection", scene_name),
            &point_cloud,
            |b, pc| {
                b.iter(|| detector.detect_3d(pc).expect("3D detection failed"));
            },
        );
    }

    group.finish();
}

/// Benchmark sensor fusion
fn bench_sensor_fusion(c: &mut Criterion) {
    let mut group = c.benchmark_group("sensor_fusion");

    // Different sensor fusion scenarios
    let fusion_scenarios = vec![
        ("camera_lidar", true, true, false, false, false),
        ("camera_lidar_radar", true, true, true, false, false),
        ("full_sensor_suite", true, true, true, true, true),
        ("lidar_only", false, true, false, false, false),
        ("camera_radar", true, false, true, false, false),
    ];

    for (scenario_name, use_camera, use_lidar, use_radar, use_imu, use_gnss) in fusion_scenarios {
        let camera_image = if use_camera {
            Some(AutonomousBenchHelper::create_camera_image(720, 1280))
        } else {
            None
        };

        let lidar_points = if use_lidar {
            Some(AutonomousBenchHelper::create_point_cloud(50000))
        } else {
            None
        };

        let radar_data = if use_radar {
            Some(AutonomousBenchHelper::create_radar_data(20))
        } else {
            None
        };

        let sensor_data = autonomous::MultiSensorData {
            camera_image,
            lidar_points,
            radar_data,
            imu_data: if use_imu {
                Some([0.1, 0.2, 9.8, 0.01, 0.02, 0.03])
            } else {
                None
            },
            gnss_data: if use_gnss {
                Some([37.7749, -122.4194, 10.0, 1.0])
            } else {
                None
            },
            timestamp: 1234567890.0,
        };

        let fusion_config = autonomous::FusionConfig::default();
        let mut fusion_processor = autonomous::SensorFusionProcessor::new(fusion_config)
            .expect("Failed to create fusion processor");

        group.bench_with_input(
            BenchmarkId::new("sensor_fusion", scenario_name),
            &sensor_data,
            |b, data| {
                b.iter(|| {
                    fusion_processor
                        .fuse_sensors(data)
                        .expect("Sensor fusion failed")
                });
            },
        );
    }

    group.finish();
}

/// Benchmark multi-object tracking in 3D
fn bench_3d_multi_object_tracking(c: &mut Criterion) {
    let mut group = c.benchmark_group("3d_multi_object_tracking");

    let mut tracker = autonomous::MultiObjectTracker::new().expect("Failed to create tracker");

    // Different tracking scenarios
    let tracking_scenarios = vec![
        ("sparse_traffic", 3),
        ("normal_traffic", 8),
        ("dense_traffic", 15),
        ("traffic_jam", 25),
        ("intersection", 20),
    ];

    for (scenario_name, num_objects) in tracking_scenarios {
        // Create synthetic 3D detections
        let mut detections_3d = Vec::new();
        for i in 0..num_objects {
            detections_3d.push(detection::three_d::BoundingBox3D {
                center: [
                    (i as f32 - num_objects as f32 / 2.0) * 5.0, // Spread along X
                    10.0 + (i % 3) as f32 * 3.0,                 // Varying Y distance
                    0.8,                                         // Ground level
                ],
                dimensions: [4.5, 2.0, 1.8], // Typical car dimensions
                rotation: 0.0,
                confidence: 0.8 + (i % 3) as f32 * 0.05,
                class_id: i % 3, // Mix of different object types
            });
        }

        group.throughput(Throughput::Elements(num_objects as u64));
        group.bench_with_input(
            BenchmarkId::new("3d_tracking", scenario_name),
            &detections_3d,
            |b, detections| {
                b.iter(|| tracker.update(detections, 0.0).expect("3D tracking failed"));
            },
        );
    }

    group.finish();
}

/// Benchmark real-time perception pipeline
fn bench_realtime_perception_pipeline(c: &mut Criterion) {
    let mut group = c.benchmark_group("realtime_perception_pipeline");

    // Different real-time scenarios with frame rates
    let realtime_scenarios = vec![
        ("autonomous_highway", 30), // 30 Hz highway driving
        ("urban_navigation", 20),   // 20 Hz urban driving
        ("parking_assistance", 10), // 10 Hz parking
        ("emergency_braking", 100), // 100 Hz emergency systems
    ];

    for (scenario_name, target_fps) in realtime_scenarios {
        let camera_image = AutonomousBenchHelper::create_camera_image(720, 1280);
        let point_cloud = AutonomousBenchHelper::create_point_cloud(60000);
        let radar_data = AutonomousBenchHelper::create_radar_data(15);

        let sensor_data = autonomous::MultiSensorData {
            camera_image: Some(camera_image),
            lidar_points: Some(point_cloud),
            radar_data: Some(radar_data),
            imu_data: Some([0.1, 0.2, 9.8, 0.01, 0.02, 0.03]),
            gnss_data: Some([37.7749, -122.4194, 10.0, 1.0]),
            timestamp: 1234567890.0,
        };

        // Create perception pipeline components
        let mut detector_2d = detection::YOLOv8::new(detection::YOLOConfig {
            model_size: detection::YOLOSize::Small, // Optimized for speed
            ..Default::default()
        })
        .expect("Failed to create 2D detector");

        let mut detector_3d =
            detection::three_d::PointRCNN::new().expect("Failed to create 3D detector");

        let fusion_config = autonomous::FusionConfig::default();
        let mut fusion_processor = autonomous::SensorFusionProcessor::new(fusion_config)
            .expect("Failed to create fusion processor");

        let mut tracker = autonomous::MultiObjectTracker::new().expect("Failed to create tracker");

        group.throughput(Throughput::Elements(target_fps as u64));
        group.bench_with_input(
            BenchmarkId::new("realtime_pipeline", scenario_name),
            &sensor_data,
            |b, data| {
                b.iter(|| {
                    // Complete perception pipeline

                    // 1. 2D object detection on camera
                    let detections_2d = if let Some(ref camera) = data.camera_image {
                        detector_2d.detect(camera).expect("2D detection failed")
                    } else {
                        Vec::new()
                    };

                    // 2. 3D object detection on LiDAR
                    let detections_3d = if let Some(ref lidar) = data.lidar_points {
                        detector_3d.detect_3d(lidar).expect("3D detection failed")
                    } else {
                        Vec::new()
                    };

                    // 3. Sensor fusion
                    let fusion_result = fusion_processor
                        .fuse_sensors(data)
                        .expect("Sensor fusion failed");

                    // 4. Multi-object tracking
                    let tracked_objects = tracker
                        .update(&detections_3d, data.timestamp)
                        .expect("Tracking failed");

                    // 5. Safety assessment
                    let safety_assessor = autonomous::SafetyAssessment::new();
                    let risk_assessment = safety_assessor
                        .assess_collision_risk(&tracked_objects)
                        .expect("Safety assessment failed");

                    // Return some result to prevent optimization
                    (
                        detections_2d.len(),
                        detections_3d.len(),
                        risk_assessment.overall_risk,
                    )
                });
            },
        );
    }

    group.finish();
}

/// Benchmark path planning and prediction
fn bench_path_planning(c: &mut Criterion) {
    let mut group = c.benchmark_group("path_planning");

    // Create tracked objects for path planning scenarios
    let planning_scenarios = vec![
        ("highway_merge", 5),
        ("intersection_turn", 8),
        ("lane_change", 3),
        ("parking_maneuver", 6),
    ];

    for (scenario_name, num_objects) in planning_scenarios {
        let mut tracked_objects = Vec::new();

        for i in 0..num_objects {
            tracked_objects.push(autonomous::TrackedObject {
                track_id: i,
                bbox_3d: detection::three_d::BoundingBox3D {
                    center: [i as f32 * 8.0, 15.0, 0.8],
                    dimensions: [4.5, 2.0, 1.8],
                    rotation: 0.0,
                    confidence: 0.9,
                    class_id: 0,
                },
                velocity: [-5.0 + i as f32 * 2.0, 0.0, 0.0],
                track_confidence: 0.9,
                object_class: autonomous::ObjectClass::Vehicle,
                age: 10,
                hits: 10,
                time_since_update: 0.1,
                predicted_trajectory: Vec::new(),
            });
        }

        group.bench_with_input(
            BenchmarkId::new("path_planning", scenario_name),
            &tracked_objects,
            |b, objects| {
                b.iter(|| {
                    // Simulate path planning algorithm
                    for obj in objects {
                        // 1. Trajectory prediction
                        let mut predicted_trajectory = Vec::new();
                        for t in 1..6 {
                            // 5 future time steps
                            let dt = t as f32 * 0.2; // 0.2 second intervals
                            predicted_trajectory.push(autonomous::TrajectoryPoint {
                                position: [
                                    obj.bbox_3d.center[0] + obj.velocity[0] * dt,
                                    obj.bbox_3d.center[1] + obj.velocity[1] * dt,
                                    obj.bbox_3d.center[2] + obj.velocity[2] * dt,
                                ],
                                time_offset: dt,
                                covariance: [[0.1, 0.0, 0.0], [0.0, 0.1, 0.0], [0.0, 0.0, 0.1]],
                            });
                        }

                        // 2. Collision risk assessment
                        let risk_score = predicted_trajectory
                            .iter()
                            .map(|point| {
                                let distance =
                                    (point.position[0].powi(2) + point.position[1].powi(2)).sqrt();
                                if distance < 5.0 { 1.0 } else { 0.1 / distance }
                            })
                            .fold(0.0f32, f32::max);

                        // 3. Path optimization (simplified)
                        let _ = risk_score < 0.5; // Safe path flag
                    }
                });
            },
        );
    }

    group.finish();
}

/// Benchmark safety-critical validation
fn bench_safety_validation(c: &mut Criterion) {
    let mut group = c.benchmark_group("safety_validation");

    // Safety-critical scenarios
    let safety_scenarios = vec![
        ("emergency_braking", 1),
        ("collision_imminent", 2),
        ("pedestrian_crossing", 3),
        ("obstacle_avoidance", 5),
        ("multi_vehicle_interaction", 8),
    ];

    for (scenario_name, num_critical_objects) in safety_scenarios {
        let mut tracked_objects = Vec::new();

        // Create critical objects (close, fast-moving)
        for i in 0..num_critical_objects {
            tracked_objects.push(autonomous::TrackedObject {
                track_id: i,
                bbox_3d: detection::three_d::BoundingBox3D {
                    center: [5.0 + i as f32 * 2.0, 2.0, 0.8], // Close objects
                    dimensions: if i == 0 {
                        [0.6, 0.6, 1.7]
                    } else {
                        [4.5, 2.0, 1.8]
                    }, // Mix pedestrian/vehicle
                    rotation: 0.0,
                    confidence: 0.95,
                    class_id: if i == 0 { 1 } else { 0 }, // Pedestrian vs vehicle
                },
                velocity: [-10.0, -2.0, 0.0], // Fast approaching
                track_confidence: 0.95,
                object_class: if i == 0 {
                    autonomous::ObjectClass::Pedestrian
                } else {
                    autonomous::ObjectClass::Vehicle
                },
                age: 5,
                hits: 5,
                time_since_update: 0.05,
                predicted_trajectory: Vec::new(),
            });
        }

        let safety_assessor = autonomous::SafetyAssessment::new();

        group.throughput(Throughput::Elements(num_critical_objects as u64));
        group.bench_with_input(
            BenchmarkId::new("safety_validation", scenario_name),
            &tracked_objects,
            |b, objects| {
                b.iter(|| {
                    // Safety-critical assessment
                    let risk_assessment = safety_assessor
                        .assess_collision_risk(objects)
                        .expect("Safety assessment failed");

                    // Critical decision making
                    let requires_emergency_action = risk_assessment.overall_risk > 0.8;
                    let critical_objects = &risk_assessment.critical_objects;

                    // Time to collision calculation for each object
                    for obj in objects {
                        let relative_velocity =
                            (obj.velocity[0].powi(2) + obj.velocity[1].powi(2)).sqrt();
                        let distance =
                            (obj.bbox_3d.center[0].powi(2) + obj.bbox_3d.center[1].powi(2)).sqrt();
                        let ttc = if relative_velocity > 0.1 {
                            distance / relative_velocity
                        } else {
                            f32::INFINITY
                        };

                        let _ = ttc < 2.0; // Critical TTC threshold
                    }

                    (requires_emergency_action, critical_objects.len())
                });
            },
        );
    }

    group.finish();
}

criterion_group!(
    benches,
    bench_lidar_processing,
    bench_point_cloud_filtering,
    bench_3d_object_detection,
    bench_sensor_fusion,
    bench_3d_multi_object_tracking,
    bench_realtime_perception_pipeline,
    bench_path_planning,
    bench_safety_validation
);

criterion_main!(benches);