use geo::{Coord, Point};

use crate::crossings::BoundaryHit;

pub fn cross(a: Coord<f64>, b: Coord<f64>) -> f64 {
    a.x * b.y - a.y * b.x
}

pub fn interpolate(start: Point<f64>, end: Point<f64>, t: f64) -> Point<f64> {
    Point::new(
        start.x() + t * (end.x() - start.x()),
        start.y() + t * (end.y() - start.y()),
    )
}

pub fn segment_intersection(
    p: Coord<f64>,
    p2: Coord<f64>,
    q: Coord<f64>,
    q2: Coord<f64>,
) -> Option<(f64, Point<f64>)> {
    let r = Coord {
        x: p2.x - p.x,
        y: p2.y - p.y,
    };

    let s = Coord {
        x: q2.x - q.x,
        y: q2.y - q.y,
    };

    let denominator = cross(r, s);

    if denominator.abs() < 1e-14 {
        return None;
    }

    let qp = Coord {
        x: q.x - p.x,
        y: q.y - p.y,
    };

    let t = cross(qp, s) / denominator;
    let u = cross(qp, r) / denominator;

    const EPSILON: f64 = 1e-10;

    if !(-EPSILON..=1.0 + EPSILON).contains(&t) || !(-EPSILON..=1.0 + EPSILON).contains(&u) {
        return None;
    }

    let t = t.clamp(0.0, 1.0);

    let point = Point::new(p.x + t * r.x, p.y + t * r.y);

    Some((t, point))
}

pub fn deduplicate_hits(mut hits: Vec<BoundaryHit>) -> Vec<BoundaryHit> {
    hits.sort_by(|a, b| {
        a.position
            .partial_cmp(&b.position)
            .unwrap_or(std::cmp::Ordering::Equal)
    });

    let mut result = Vec::new();

    for hit in hits {
        let duplicate = result
            .iter()
            .any(|existing: &BoundaryHit| (existing.position - hit.position).abs() < 1e-9);

        if !duplicate {
            result.push(hit);
        }
    }

    result
}

/// Recalculate the point directly from the original track segment.
///
/// This guarantees that the returned point lies on the segment rather than
/// relying on the coordinates calculated during the intersection operation.
pub fn snap_to_segment(start: Point<f64>, end: Point<f64>, position: f64) -> Point<f64> {
    interpolate(start, end, position.clamp(0.0, 1.0))
}