pelorus 0.1.0

use crate::matching::{MapMatchingConfig, MapMatchingError};
use crate::types::{GpsMeasurement, Timestamp};
use geo::algorithm::euclidean_distance::EuclideanDistance;
use geo::Point;

/// Handles GPS trace preprocessing operations
pub struct TracePreprocessor {
    _config: MapMatchingConfig,
}

impl TracePreprocessor {
    /// Create a new trace preprocessor
    pub fn new(config: &MapMatchingConfig) -> Self {
        Self {
            _config: config.clone(),
        }
    }

    /// Preprocess a GPS trace by filtering and cleaning the data
    pub fn preprocess(
        &self,
        gps_trace: &[GpsMeasurement],
    ) -> Result<Vec<GpsMeasurement>, MapMatchingError> {
        if gps_trace.is_empty() {
            return Ok(Vec::new());
        }

        // Step 1: Sort by timestamp
        let mut sorted_trace = gps_trace.to_vec();
        sorted_trace.sort_by(|a, b| a.timestamp.partial_cmp(&b.timestamp).unwrap());

        // Step 2: Remove duplicate timestamps
        let deduplicated = self.remove_duplicate_timestamps(&sorted_trace)?;

        // Step 3: Remove points too close to previous point (within 2*sigma)
        let distance_filtered = self.remove_close_points(&deduplicated)?;

        // Step 4: Detect and filter out obvious outliers
        let outlier_filtered = self.filter_outliers(&distance_filtered)?;

        // Step 5: Validate minimum trace requirements
        self.validate_trace(&outlier_filtered)?;

        Ok(outlier_filtered)
    }

    /// Remove GPS measurements with duplicate timestamps
    fn remove_duplicate_timestamps(
        &self,
        trace: &[GpsMeasurement],
    ) -> Result<Vec<GpsMeasurement>, MapMatchingError> {
        let mut deduplicated = Vec::new();
        let mut last_timestamp: Option<Timestamp> = None;

        for measurement in trace {
            if let Some(last_ts) = last_timestamp {
                if (measurement.timestamp - last_ts).abs() < 1e-6 {
                    // Skip measurements with essentially the same timestamp
                    continue;
                }
            }

            deduplicated.push(measurement.clone());
            last_timestamp = Some(measurement.timestamp);
        }

        Ok(deduplicated)
    }

    /// Remove points that are too close to the previous point (within 2*sigma)
    fn remove_close_points(
        &self,
        trace: &[GpsMeasurement],
    ) -> Result<Vec<GpsMeasurement>, MapMatchingError> {
        if trace.len() < 2 {
            return Ok(trace.to_vec());
        }

        let mut filtered = Vec::new();
        filtered.push(trace[0].clone());

        let two_sigma_threshold = 2.0 * self._config.gps_sigma;

        for i in 1..trace.len() {
            let current = &trace[i];
            let previous = &filtered[filtered.len() - 1]; // Use last kept point

            let distance = previous.point.euclidean_distance(&current.point);
            let distance_meters = distance * 111_000.0; // Rough conversion to meters

            // Keep point if it's far enough from the previous kept point
            if distance_meters >= two_sigma_threshold {
                filtered.push(current.clone());
            }
            // Otherwise, skip this point (it's too close)
        }

        Ok(filtered)
    }

    /// Filter out obvious GPS outliers based on speed and distance
    fn filter_outliers(
        &self,
        trace: &[GpsMeasurement],
    ) -> Result<Vec<GpsMeasurement>, MapMatchingError> {
        if trace.len() < 3 {
            return Ok(trace.to_vec());
        }

        let mut filtered = Vec::new();
        filtered.push(trace[0].clone());

        for i in 1..trace.len() - 1 {
            let previous = &trace[i - 1];
            let current = &trace[i];
            let next = &trace[i + 1];

            if self.is_outlier(previous, current, next)? {
                // Skip outlier point
                continue;
            }

            if self.is_valid_measurement(previous, current)? {
                filtered.push(current.clone());
            }
        }

        // Always include the last point if it's valid
        if let Some(last) = trace.last() {
            if let Some(second_last) = trace.get(trace.len() - 2) {
                if self.is_valid_measurement(second_last, last)? {
                    filtered.push(last.clone());
                }
            }
        }

        Ok(filtered)
    }

    /// Detect if a GPS point is an outlier based on surrounding points
    fn is_outlier(
        &self,
        previous: &GpsMeasurement,
        current: &GpsMeasurement,
        next: &GpsMeasurement,
    ) -> Result<bool, MapMatchingError> {
        // Calculate expected position based on linear interpolation
        let time_total = next.timestamp - previous.timestamp;
        let time_to_current = current.timestamp - previous.timestamp;

        if time_total <= 0.0 {
            return Ok(false); // Can't interpolate
        }

        let interpolation_ratio = time_to_current / time_total;
        let expected_x = previous.point.x()
            + (next.point.x() - previous.point.x()) * interpolation_ratio;
        let expected_y = previous.point.y()
            + (next.point.y() - previous.point.y()) * interpolation_ratio;
        let expected_point = Point::new(expected_x, expected_y);

        // Calculate deviation from expected position
        let deviation = current.point.euclidean_distance(&expected_point);
        let deviation_meters = deviation * 111_000.0; // Rough conversion to meters

        // Mark as outlier if deviation is > 3*sigma from expected position
        let outlier_threshold = 3.0 * self._config.gps_sigma;

        Ok(deviation_meters > outlier_threshold)
    }

    /// Check if a GPS measurement is valid based on the previous measurement
    fn is_valid_measurement(
        &self,
        previous: &GpsMeasurement,
        current: &GpsMeasurement,
    ) -> Result<bool, MapMatchingError> {
        let time_diff = current.timestamp - previous.timestamp;

        // Reject negative time differences
        if time_diff <= 0.0 {
            return Ok(false);
        }

        // Calculate distance between points
        let distance = previous.point.euclidean_distance(&current.point);

        // TODO: Convert from degrees to meters for proper speed calculation
        // For now, use a rough approximation (will need proper geospatial conversion)
        let distance_meters = distance * 111_000.0; // Rough conversion: 1 degree ≈ 111km

        // Calculate speed in m/s
        let speed_ms = distance_meters / time_diff;

        // Reject unrealistic speeds (> 150 km/h = 41.67 m/s for road vehicles)
        if speed_ms > 41.67 {
            return Ok(false);
        }

        // Reject measurements that are too close in time but far in distance
        if time_diff < 5.0 && distance_meters > 100.0 {
            return Ok(false);
        }

        Ok(true)
    }

    /// Validate that the trace meets minimum requirements for map matching
    fn validate_trace(&self, trace: &[GpsMeasurement]) -> Result<(), MapMatchingError> {
        if trace.is_empty() {
            return Err(MapMatchingError::InvalidInput(
                "Empty GPS trace".to_string(),
            ));
        }

        if trace.len() < 2 {
            return Err(MapMatchingError::InvalidInput(
                "GPS trace must contain at least 2 measurements".to_string(),
            ));
        }

        // Check that timestamps are monotonically increasing
        for i in 1..trace.len() {
            if trace[i].timestamp <= trace[i - 1].timestamp {
                return Err(MapMatchingError::InvalidInput(
                    "GPS trace timestamps must be monotonically increasing".to_string(),
                ));
            }
        }

        // Check for reasonable time span
        let total_duration = trace.last().unwrap().timestamp - trace[0].timestamp;
        if total_duration > 86400.0 {
            // More than 24 hours
            return Err(MapMatchingError::InvalidInput(
                "GPS trace spans more than 24 hours - consider splitting".to_string(),
            ));
        }

        Ok(())
    }

    /// Calculate statistics about the GPS trace
    pub fn calculate_trace_statistics(
        &self,
        trace: &[GpsMeasurement],
    ) -> TraceStatistics {
        if trace.is_empty() {
            return TraceStatistics::default();
        }

        let total_duration = if trace.len() > 1 {
            trace.last().unwrap().timestamp - trace[0].timestamp
        } else {
            0.0
        };

        let mut total_distance = 0.0;
        let mut time_intervals = Vec::new();

        for i in 1..trace.len() {
            let distance = trace[i - 1].point.euclidean_distance(&trace[i].point);
            total_distance += distance * 111_000.0; // Rough conversion to meters

            let time_interval = trace[i].timestamp - trace[i - 1].timestamp;
            time_intervals.push(time_interval);
        }

        let average_time_interval = if !time_intervals.is_empty() {
            time_intervals.iter().sum::<f64>() / time_intervals.len() as f64
        } else {
            0.0
        };

        let average_speed = if total_duration > 0.0 {
            total_distance / total_duration // m/s
        } else {
            0.0
        };

        TraceStatistics {
            point_count: trace.len(),
            total_duration,
            total_distance,
            average_time_interval,
            average_speed,
        }
    }
}

/// Statistics about a GPS trace
#[derive(Debug, Clone, PartialEq)]
pub struct TraceStatistics {
    pub point_count: usize,
    pub total_duration: f64,        // seconds
    pub total_distance: f64,        // meters
    pub average_time_interval: f64, // seconds
    pub average_speed: f64,         // m/s
}

impl Default for TraceStatistics {
    fn default() -> Self {
        Self {
            point_count: 0,
            total_duration: 0.0,
            total_distance: 0.0,
            average_time_interval: 0.0,
            average_speed: 0.0,
        }
    }
}