pelorus 0.1.0

use geo::{Coord, LineString, Point};
use pelorus::road_network::RoadNetwork;
use pelorus::types::{GpsMeasurement, RoadSegment};
use std::f64::consts::PI;

/// Configuration for test scenarios
#[derive(Debug, Clone)]
pub struct TestConfig {
    pub grid_size: usize, // Grid dimensions (grid_size x grid_size)
    pub block_size_meters: f64, // Size of each block in meters
    pub sampling_interval: f64, // GPS sampling interval in seconds
    pub speed_ms: f64,    // Vehicle speed in m/s
    pub noise_std_dev: f64, // GPS noise standard deviation in meters
}

impl TestConfig {
    /// Good conditions: 5s sampling, 4m noise
    pub fn good_conditions() -> Self {
        Self {
            grid_size: 10,
            block_size_meters: 100.0,
            sampling_interval: 5.0,
            speed_ms: 10.0,
            noise_std_dev: 4.0,
        }
    }

    /// Difficult conditions: 30s sampling, 20m noise
    pub fn difficult_conditions() -> Self {
        Self {
            grid_size: 10,
            block_size_meters: 100.0,
            sampling_interval: 30.0,
            speed_ms: 10.0,
            noise_std_dev: 20.0,
        }
    }
}

/// Grid road network generator for Manhattan-style street layout
pub struct GridGenerator {
    config: TestConfig,
}

impl GridGenerator {
    pub fn new(config: TestConfig) -> Self {
        Self { config }
    }

    /// Create a Manhattan-style grid road network
    /// Returns a road network with all intersections connected
    pub fn create_grid_network(&self) -> RoadNetwork {
        let mut network = RoadNetwork::new();
        let mut segment_id = 1;

        let grid_size = self.config.grid_size;
        let block_size = self.config.block_size_meters;

        // Convert block size to approximate degrees (very rough approximation: 111km per degree)
        let block_size_degrees = block_size / 111_000.0;

        // Create horizontal streets (east-west)
        for row in 0..=grid_size {
            for col in 0..grid_size {
                let start_x = col as f64 * block_size_degrees;
                let end_x = (col + 1) as f64 * block_size_degrees;
                let y = row as f64 * block_size_degrees;

                let coords = vec![Coord::from((start_x, y)), Coord::from((end_x, y))];

                let segment = RoadSegment {
                    id: segment_id,
                    geometry: LineString::new(coords),
                };

                network.add_segment(segment);
                segment_id += 1;
            }
        }

        // Create vertical streets (north-south)
        for col in 0..=grid_size {
            for row in 0..grid_size {
                let x = col as f64 * block_size_degrees;
                let start_y = row as f64 * block_size_degrees;
                let end_y = (row + 1) as f64 * block_size_degrees;

                let coords = vec![Coord::from((x, start_y)), Coord::from((x, end_y))];

                let segment = RoadSegment {
                    id: segment_id,
                    geometry: LineString::new(coords),
                };

                network.add_segment(segment);
                segment_id += 1;
            }
        }

        network
    }

    /// Generate GPS trace from (0,0) to (grid_size*block_size, grid_size*block_size)
    /// Route follows a simple path: east to the destination column, then north to destination
    pub fn generate_gps_trace(&self) -> (Vec<GpsMeasurement>, Vec<Point<f64>>) {
        let block_size_degrees = self.config.block_size_meters / 111_000.0;
        let total_distance_meters =
            self.config.grid_size as f64 * self.config.block_size_meters * 2.0; // L-shaped route
        let total_time = total_distance_meters / self.config.speed_ms;
        let num_points = (total_time / self.config.sampling_interval) as usize + 1;

        let mut gps_trace = Vec::new();
        let mut true_path = Vec::new();

        // Create L-shaped route: first go east, then north
        let dest_x = self.config.grid_size as f64 * block_size_degrees;
        let dest_y = self.config.grid_size as f64 * block_size_degrees;

        for i in 0..num_points {
            let progress = i as f64 / (num_points - 1) as f64;
            let current_time = progress * total_time;

            // Calculate true position along L-shaped path
            let true_pos = if progress <= 0.5 {
                // First half: move east
                let east_progress = progress * 2.0;
                Point::new(east_progress * dest_x, 0.0)
            } else {
                // Second half: move north
                let north_progress = (progress - 0.5) * 2.0;
                Point::new(dest_x, north_progress * dest_y)
            };

            true_path.push(true_pos);

            // Add Gaussian noise to get GPS measurement
            let noisy_pos = self.add_gps_noise(true_pos);

            gps_trace.push(GpsMeasurement {
                point: noisy_pos,
                timestamp: current_time,
            });
        }

        (gps_trace, true_path)
    }

    /// Add Gaussian noise to a point to simulate GPS measurement error
    fn add_gps_noise(&self, true_pos: Point<f64>) -> Point<f64> {
        // Generate two independent Gaussian random numbers using Box-Muller transform
        let u1: f64 = fastrand::f64();
        let u2: f64 = fastrand::f64();

        let z0 = (-2.0 * u1.ln()).sqrt() * (2.0 * PI * u2).cos();
        let z1 = (-2.0 * u1.ln()).sqrt() * (2.0 * PI * u2).sin();

        // Convert noise from meters to degrees
        let noise_degrees = self.config.noise_std_dev / 111_000.0;

        Point::new(
            true_pos.x() + z0 * noise_degrees,
            true_pos.y() + z1 * noise_degrees,
        )
    }
}