condor-pathfinding-grid 0.4.0

//! Independent any-angle grid geometry for exact oracles and prepared graphs.
//!
//! Authority clipping LOS (`segment_is_legal`) and the independent sampling DDA
//! predicate (`sampling_segment_is_legal`) both live here. Does **not** call
//! public [`crate::any_angle::has_line_of_sight`] for legality decisions (that
//! helper wraps the sampling predicate). Retained-vertex sets are deliberately
//! conservative until corner-only optimality is fully proven.

use std::cmp::Ordering;

use crate::{
    Grid, Point,
    any_angle::{AnyAnglePath, AnyAnglePathBuildError},
};
use condor_core::Point2;

/// Scale-aware floating equality for derived geometry values.
#[must_use]
pub fn approximately_equal(a: f64, b: f64) -> bool {
    let scale = 1.0_f64.max(a.abs()).max(b.abs());
    (a - b).abs() <= 1e-12 * scale
}

/// Whether `value` is within `1e-9` of a non-negative integer grid coordinate.
#[must_use]
pub fn is_grid_vertex_coordinate(value: f64) -> bool {
    value.is_finite() && value >= 0.0 && (value - value.round()).abs() <= 1e-9
}

/// Parses a grid vertex from continuous coordinates.
#[must_use]
pub fn parse_grid_vertex(point: Point2) -> Option<(usize, usize)> {
    let canonical = canonicalize_grid_vertex(point)?;
    Some((canonical.x as usize, canonical.y as usize))
}

/// Rounds a near-integer coordinate to its canonical grid vertex.
#[must_use]
pub fn canonicalize_grid_vertex(point: Point2) -> Option<Point2> {
    if !is_grid_vertex_coordinate(point.x) || !is_grid_vertex_coordinate(point.y) {
        return None;
    }
    Some(Point2::new(point.x.round(), point.y.round()))
}

/// v0 endpoint validity: finite grid-aligned vertex inside inclusive bounds.
#[must_use]
pub fn is_endpoint_valid(grid: &Grid, point: Point2) -> bool {
    let Some((x, y)) = parse_grid_vertex(point) else {
        return false;
    };
    x <= grid.width() && y <= grid.height()
}

/// Local `2×2` occupancy around integer vertex `(vx, vy)`.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct QuadrantOccupancy {
    pub northwest: bool,
    pub northeast: bool,
    pub southwest: bool,
    pub southeast: bool,
}

impl QuadrantOccupancy {
    /// Number of blocked cells among the four incident quadrants (0..=4).
    #[must_use]
    pub const fn blocked_count(self) -> u8 {
        (self.northwest as u8)
            + (self.northeast as u8)
            + (self.southwest as u8)
            + (self.southeast as u8)
    }

    /// True for diagonal pinch masks that forbid treating the vertex as free.
    #[must_use]
    pub const fn has_forbidden_pinch_mask(self) -> bool {
        (self.northwest && self.southeast) || (self.northeast && self.southwest)
    }
}

/// Classification of a grid vertex from its local `2×2` mask.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[allow(dead_code)]
pub enum VertexClass {
    /// No blocked quadrant touches this vertex.
    Open,
    /// All four incident cells are blocked.
    FullyBlocked,
    /// Exactly one blocked quadrant: convex obstacle corner.
    ConvexObstacleCorner,
    /// Two adjacent blocked quadrants: concave obstacle corner.
    ConcaveObstacleCorner,
    /// Two diagonally opposite blocked quadrants.
    ForbiddenPinch,
    /// Three blocked quadrants.
    ConcaveFreeSpace,
    /// Mixed boundary without a retained turning role.
    Boundary,
}

/// One directed obstacle-union boundary segment between grid vertices.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct BoundaryEdge {
    pub start: Point2,
    pub end: Point2,
}

/// Whether cell `(cx, cy)` is blocked. Out-of-bounds counts as blocked.
#[must_use]
pub fn cell_is_blocked(grid: &Grid, cx: i32, cy: i32) -> bool {
    if cx < 0 || cy < 0 {
        return true;
    }
    let (cx, cy) = (cx as usize, cy as usize);
    if cx >= grid.width() || cy >= grid.height() {
        return true;
    }
    !grid.is_walkable(Point::new(cx, cy))
}

/// Reads quadrant occupancy around integer vertex `(vx, vy)`.
#[must_use]
pub fn quadrant_occupancy(grid: &Grid, vx: i32, vy: i32) -> QuadrantOccupancy {
    QuadrantOccupancy {
        northwest: quadrant_cell_is_blocked(grid, vx - 1, vy - 1),
        northeast: quadrant_cell_is_blocked(grid, vx, vy - 1),
        southwest: quadrant_cell_is_blocked(grid, vx - 1, vy),
        southeast: quadrant_cell_is_blocked(grid, vx, vy),
    }
}

/// Quadrant mask for corner retention; out-of-bounds is open exterior, not blocked.
fn quadrant_cell_is_blocked(grid: &Grid, cx: i32, cy: i32) -> bool {
    if cx < 0 || cy < 0 {
        return false;
    }
    let (cx, cy) = (cx as usize, cy as usize);
    if cx >= grid.width() || cy >= grid.height() {
        return false;
    }
    !grid.is_walkable(Point::new(cx, cy))
}

/// Classifies a grid vertex for corner retention and diagnostics.
#[must_use]
#[allow(dead_code)]
pub fn classify_vertex(grid: &Grid, vx: i32, vy: i32) -> VertexClass {
    let mask = quadrant_occupancy(grid, vx, vy);
    match mask.blocked_count() {
        0 => VertexClass::Open,
        4 => VertexClass::FullyBlocked,
        1 => VertexClass::ConvexObstacleCorner,
        2 if mask.has_forbidden_pinch_mask() => VertexClass::ForbiddenPinch,
        2 => VertexClass::ConcaveObstacleCorner,
        3 => VertexClass::ConcaveFreeSpace,
        _ => VertexClass::Boundary,
    }
}

/// Whether `vertex` should be retained in the corner-visibility oracle graph.
///
/// Retains every grid vertex whose local `2×2` mask mixes blocked and open
/// quadrants. This is intentionally conservative: ambiguous boundary vertices
/// stay available rather than being optimistically pruned.
#[must_use]
pub fn retain_as_corner(grid: &Grid, vx: i32, vy: i32) -> bool {
    if vx < 0 || vy < 0 || vx > grid.width() as i32 || vy > grid.height() as i32 {
        return false;
    }
    let blocked = quadrant_occupancy(grid, vx, vy).blocked_count();
    (1..=3).contains(&blocked)
}

/// Grid vertices retained by the corner-visibility oracle.
///
/// Turning corners and boundary-edge endpoints are always retained. Every
/// in-bounds grid vertex is also included until corner-only optimality is
/// proven by the owning oracle tests.
#[must_use]
pub fn retained_visibility_vertices(grid: &Grid) -> Vec<Point2> {
    let mut vertices = retained_turning_corners(grid);
    for edge in extract_boundary_edges(grid) {
        vertices.push(edge.start);
        vertices.push(edge.end);
    }
    for vy in 0..=grid.height() {
        for vx in 0..=grid.width() {
            vertices.push(Point2::new(vx as f64, vy as f64));
        }
    }
    dedup_vertices(&mut vertices);
    vertices
}

fn dedup_vertices(vertices: &mut Vec<Point2>) {
    vertices.sort_by(|left, right| {
        left.x
            .partial_cmp(&right.x)
            .unwrap_or(Ordering::Equal)
            .then_with(|| left.y.partial_cmp(&right.y).unwrap_or(Ordering::Equal))
    });
    vertices.dedup_by(|left, right| {
        approximately_equal(left.x, right.x) && approximately_equal(left.y, right.y)
    });
}

/// Whether the neighbor across a blocked-cell side is open or outside the grid.
fn neighbor_is_open_or_outside_for_boundary(grid: &Grid, cx: i32, cy: i32) -> bool {
    if cx < 0 || cy < 0 || cx >= grid.width() as i32 || cy >= grid.height() as i32 {
        return true;
    }
    grid.is_walkable(Point::new(cx as usize, cy as usize))
}

/// Extracts maximal obstacle-union boundary edges from blocked cells.
#[must_use]
pub fn extract_boundary_edges(grid: &Grid) -> Vec<BoundaryEdge> {
    let mut edges = Vec::new();
    for cy in 0..grid.height() as i32 {
        for cx in 0..grid.width() as i32 {
            if !cell_is_blocked(grid, cx, cy) {
                continue;
            }
            let x0 = cx as f64;
            let y0 = cy as f64;
            let x1 = (cx + 1) as f64;
            let y1 = (cy + 1) as f64;
            if neighbor_is_open_or_outside_for_boundary(grid, cx - 1, cy) {
                edges.push(BoundaryEdge {
                    start: Point2::new(x0, y0),
                    end: Point2::new(x0, y1),
                });
            }
            if neighbor_is_open_or_outside_for_boundary(grid, cx + 1, cy) {
                edges.push(BoundaryEdge {
                    start: Point2::new(x1, y0),
                    end: Point2::new(x1, y1),
                });
            }
            if neighbor_is_open_or_outside_for_boundary(grid, cx, cy - 1) {
                edges.push(BoundaryEdge {
                    start: Point2::new(x0, y0),
                    end: Point2::new(x1, y0),
                });
            }
            if neighbor_is_open_or_outside_for_boundary(grid, cx, cy + 1) {
                edges.push(BoundaryEdge {
                    start: Point2::new(x0, y1),
                    end: Point2::new(x1, y1),
                });
            }
        }
    }
    edges
}

/// Collects retained turning corners for the visibility graph.
#[must_use]
pub fn retained_turning_corners(grid: &Grid) -> Vec<Point2> {
    let mut corners = Vec::new();
    for vy in 0..=grid.height() as i32 {
        for vx in 0..=grid.width() as i32 {
            if retain_as_corner(grid, vx, vy) {
                corners.push(Point2::new(vx as f64, vy as f64));
            }
        }
    }
    corners
}

/// Authority no-corner-cut LOS: segment is legal iff it avoids blocked interiors and pinch diagonals.
///
/// Endpoints must be grid-aligned free vertices. This is the oracle / prepared-graph
/// legality contract; public [`crate::has_line_of_sight`] wraps the independent sampling
/// predicate instead.
#[must_use]
pub fn segment_is_legal(grid: &Grid, start: Point2, end: Point2) -> bool {
    let Some(start) = canonicalize_grid_vertex(start) else {
        return false;
    };
    let Some(end) = canonicalize_grid_vertex(end) else {
        return false;
    };
    if !is_endpoint_valid(grid, start) || !is_endpoint_valid(grid, end) {
        return false;
    }
    if approximately_equal(start.x, end.x) && approximately_equal(start.y, end.y) {
        return true;
    }

    if segment_crosses_blocked_interior(grid, start, end) {
        return false;
    }

    !segment_has_interior_pinch_violation(grid, start, end)
}

/// Independent DDA-based segment legality for differential reference tests.
///
/// Uses grid DDA traversal for blocked interiors and a separately implemented
/// pinch predicate. It does not call the authority bbox/rectangle-clipping path
/// or the authority pinch helpers.
#[must_use]
pub fn sampling_segment_is_legal(grid: &Grid, start: Point2, end: Point2) -> bool {
    let Some(start) = canonicalize_grid_vertex(start) else {
        return false;
    };
    let Some(end) = canonicalize_grid_vertex(end) else {
        return false;
    };
    if !is_endpoint_valid(grid, start) || !is_endpoint_valid(grid, end) {
        return false;
    }
    if approximately_equal(start.x, end.x) && approximately_equal(start.y, end.y) {
        return true;
    }
    if reference_segment_crosses_blocked_interior(grid, start, end) {
        return false;
    }
    !reference_segment_has_interior_pinch_violation(grid, start, end)
}

/// Validates a polyline with the sampling reference predicate.
#[must_use]
pub fn validate_sampling_path(grid: &Grid, points: &[Point2]) -> bool {
    if points.is_empty() {
        return false;
    }
    if !is_endpoint_valid(grid, points[0]) || !is_endpoint_valid(grid, *points.last().unwrap()) {
        return false;
    }
    points
        .windows(2)
        .all(|pair| sampling_segment_is_legal(grid, pair[0], pair[1]))
}

/// Validates every consecutive segment and endpoint identity.
#[must_use]
pub fn validate_path(grid: &Grid, points: &[Point2]) -> bool {
    if points.is_empty() {
        return false;
    }
    if !is_endpoint_valid(grid, points[0]) || !is_endpoint_valid(grid, *points.last().unwrap()) {
        return false;
    }
    points
        .windows(2)
        .all(|pair| segment_is_legal(grid, pair[0], pair[1]))
}

/// Recomputes Euclidean polyline length.
#[must_use]
pub fn recompute_path_cost(points: &[Point2]) -> f64 {
    points
        .windows(2)
        .map(|pair| pair[0].distance_to(pair[1]))
        .sum()
}

/// Builds an [`AnyAnglePath`] after independent validation.
///
/// # Errors
///
/// Returns [`AnyAnglePathBuildError::Empty`] when `points` is empty.
pub fn validated_any_angle_path(
    grid: &Grid,
    points: Vec<Point2>,
) -> Result<AnyAnglePath, AnyAnglePathBuildError> {
    if points.is_empty() {
        return Err(AnyAnglePathBuildError::Empty);
    }
    if !validate_path(grid, &points) {
        return Err(AnyAnglePathBuildError::Empty);
    }
    AnyAnglePath::from_points(points)
}

fn segment_crosses_blocked_interior(grid: &Grid, start: Point2, end: Point2) -> bool {
    let min_cx = start.x.min(end.x).floor() as i32;
    let max_cx = start.x.max(end.x).ceil() as i32;
    let min_cy = start.y.min(end.y).floor() as i32;
    let max_cy = start.y.max(end.y).ceil() as i32;
    for cy in min_cy..max_cy {
        for cx in min_cx..max_cx {
            if !cell_is_blocked(grid, cx, cy) {
                continue;
            }
            if segment_intersects_open_cell_interior(start, end, cx, cy) {
                return true;
            }
        }
    }
    false
}

fn segment_intersects_open_cell_interior(start: Point2, end: Point2, cx: i32, cy: i32) -> bool {
    let min_x = cx as f64;
    let min_y = cy as f64;
    let max_x = min_x + 1.0;
    let max_y = min_y + 1.0;
    segment_intersects_open_rectangle(start, end, min_x, min_y, max_x, max_y)
}

fn segment_intersects_open_rectangle(
    start: Point2,
    end: Point2,
    min_x: f64,
    min_y: f64,
    max_x: f64,
    max_y: f64,
) -> bool {
    if point_in_open_rectangle(start, min_x, min_y, max_x, max_y)
        || point_in_open_rectangle(end, min_x, min_y, max_x, max_y)
    {
        return true;
    }
    clip_segment_to_open_rectangle(start, end, min_x, min_y, max_x, max_y).is_some()
}

fn clip_segment_to_open_rectangle(
    start: Point2,
    end: Point2,
    min_x: f64,
    min_y: f64,
    max_x: f64,
    max_y: f64,
) -> Option<(f64, f64)> {
    const EPS: f64 = 1e-12;
    let mut t0 = 0.0_f64;
    let mut t1 = 1.0_f64;
    let dx = end.x - start.x;
    let dy = end.y - start.y;

    for (p, dp, min, max) in [(start.x, dx, min_x, max_x), (start.y, dy, min_y, max_y)] {
        if dp.abs() <= 1e-15 {
            if !(p > min + EPS && p < max - EPS) {
                return None;
            }
        } else if dp > 0.0 {
            t0 = t0.max((min - p) / dp + EPS);
            t1 = t1.min((max - p) / dp - EPS);
        } else {
            t0 = t0.max((max - p) / dp + EPS);
            t1 = t1.min((min - p) / dp - EPS);
        }
        if t0 > t1 {
            return None;
        }
    }

    if t1 - t0 <= EPS {
        return None;
    }
    Some((t0.max(0.0), t1.min(1.0)))
}

fn point_in_open_rectangle(point: Point2, min_x: f64, min_y: f64, max_x: f64, max_y: f64) -> bool {
    point.x > min_x && point.x < max_x && point.y > min_y && point.y < max_y
}

fn segment_has_interior_pinch_violation(grid: &Grid, start: Point2, end: Point2) -> bool {
    let (x0, y0) = (start.x.round() as i32, start.y.round() as i32);
    let (x1, y1) = (end.x.round() as i32, end.y.round() as i32);
    let dx = x1 - x0;
    let dy = y1 - y0;

    let gcd = gcd_i32(dx.abs(), dy.abs()).max(1);
    let step_x = dx / gcd;
    let step_y = dy / gcd;

    let mut vx = x0;
    let mut vy = y0;
    for step in 0..=gcd {
        if step != 0 && step != gcd && pinch_violation_at_vertex(grid, vx, vy, dx, dy) {
            return true;
        }
        if step < gcd {
            vx += step_x;
            vy += step_y;
        }
    }
    false
}

fn pinch_violation_at_vertex(grid: &Grid, vx: i32, vy: i32, dx: i32, dy: i32) -> bool {
    let mask = quadrant_occupancy(grid, vx, vy);
    if dx != 0 && dy != 0 {
        return mask.has_forbidden_pinch_mask();
    }
    if dy == 0 && dx != 0 {
        if axis_aligned_horizontal_boundary_following(grid, vx, vy) {
            return false;
        }
        return mask.southwest && mask.southeast;
    }
    if dx == 0 && dy != 0 {
        if axis_aligned_vertical_boundary_following(grid, vx, vy) {
            return false;
        }
        return mask.southwest && mask.northwest;
    }
    false
}

/// Whether a horizontal segment at `y = vy` is following blocked-cell boundaries.
fn axis_aligned_horizontal_boundary_following(grid: &Grid, vx: i32, vy: i32) -> bool {
    let south_boundary =
        quadrant_cell_is_blocked(grid, vx - 1, vy) && quadrant_cell_is_blocked(grid, vx, vy);
    let north_boundary = quadrant_cell_is_blocked(grid, vx - 1, vy - 1)
        && quadrant_cell_is_blocked(grid, vx, vy - 1);
    south_boundary || north_boundary
}

/// Whether a vertical segment at `x = vx` is following blocked-cell boundaries.
fn axis_aligned_vertical_boundary_following(grid: &Grid, vx: i32, vy: i32) -> bool {
    let west_boundary =
        quadrant_cell_is_blocked(grid, vx, vy - 1) && quadrant_cell_is_blocked(grid, vx, vy);
    let east_boundary = quadrant_cell_is_blocked(grid, vx - 1, vy - 1)
        && quadrant_cell_is_blocked(grid, vx - 1, vy);
    west_boundary || east_boundary
}

fn reference_quadrant_cell_is_blocked(grid: &Grid, cx: i32, cy: i32) -> bool {
    if cx < 0 || cy < 0 {
        return false;
    }
    let (cx, cy) = (cx as usize, cy as usize);
    if cx >= grid.width() || cy >= grid.height() {
        return false;
    }
    !grid.is_walkable(Point::new(cx, cy))
}

fn reference_quadrant_occupancy(grid: &Grid, vx: i32, vy: i32) -> QuadrantOccupancy {
    QuadrantOccupancy {
        northwest: reference_quadrant_cell_is_blocked(grid, vx - 1, vy - 1),
        northeast: reference_quadrant_cell_is_blocked(grid, vx, vy - 1),
        southwest: reference_quadrant_cell_is_blocked(grid, vx - 1, vy),
        southeast: reference_quadrant_cell_is_blocked(grid, vx, vy),
    }
}

fn reference_axis_aligned_horizontal_boundary_following(grid: &Grid, vx: i32, vy: i32) -> bool {
    let south_boundary = reference_quadrant_cell_is_blocked(grid, vx - 1, vy)
        && reference_quadrant_cell_is_blocked(grid, vx, vy);
    let north_boundary = reference_quadrant_cell_is_blocked(grid, vx - 1, vy - 1)
        && reference_quadrant_cell_is_blocked(grid, vx, vy - 1);
    south_boundary || north_boundary
}

fn reference_axis_aligned_vertical_boundary_following(grid: &Grid, vx: i32, vy: i32) -> bool {
    let west_boundary = reference_quadrant_cell_is_blocked(grid, vx, vy - 1)
        && reference_quadrant_cell_is_blocked(grid, vx, vy);
    let east_boundary = reference_quadrant_cell_is_blocked(grid, vx - 1, vy - 1)
        && reference_quadrant_cell_is_blocked(grid, vx - 1, vy);
    west_boundary || east_boundary
}

fn reference_pinch_violation_at_vertex(grid: &Grid, vx: i32, vy: i32, dx: i32, dy: i32) -> bool {
    let mask = reference_quadrant_occupancy(grid, vx, vy);
    if dx != 0 && dy != 0 {
        return mask.has_forbidden_pinch_mask();
    }
    if dy == 0 && dx != 0 {
        if reference_axis_aligned_horizontal_boundary_following(grid, vx, vy) {
            return false;
        }
        return mask.southwest && mask.southeast;
    }
    if dx == 0 && dy != 0 {
        if reference_axis_aligned_vertical_boundary_following(grid, vx, vy) {
            return false;
        }
        return mask.southwest && mask.northwest;
    }
    false
}

fn reference_segment_has_interior_pinch_violation(grid: &Grid, start: Point2, end: Point2) -> bool {
    let (x0, y0) = (start.x.round() as i32, start.y.round() as i32);
    let (x1, y1) = (end.x.round() as i32, end.y.round() as i32);
    let dx = x1 - x0;
    let dy = y1 - y0;

    let gcd = gcd_i32(dx.abs(), dy.abs()).max(1);
    let step_x = dx / gcd;
    let step_y = dy / gcd;

    let mut vx = x0;
    let mut vy = y0;
    for step in 0..=gcd {
        if step != 0 && step != gcd && reference_pinch_violation_at_vertex(grid, vx, vy, dx, dy) {
            return true;
        }
        if step < gcd {
            vx += step_x;
            vy += step_y;
        }
    }
    false
}

fn reference_segment_crosses_blocked_interior(grid: &Grid, start: Point2, end: Point2) -> bool {
    for (cx, cy) in dda_traverse_cells(start, end) {
        if !cell_is_blocked(grid, cx, cy) {
            continue;
        }
        if reference_segment_hits_cell_open_interior(start, end, cx, cy) {
            return true;
        }
    }
    false
}

fn reference_segment_hits_cell_open_interior(start: Point2, end: Point2, cx: i32, cy: i32) -> bool {
    let min_x = cx as f64;
    let min_y = cy as f64;
    let max_x = min_x + 1.0;
    let max_y = min_y + 1.0;
    let dx = end.x - start.x;
    let dy = end.y - start.y;
    let mut t_enter = 0.0_f64;
    let mut t_exit = 1.0_f64;

    for (p, dp, min, max) in [(start.x, dx, min_x, max_x), (start.y, dy, min_y, max_y)] {
        if dp.abs() <= 1e-15 {
            if !(p > min && p < max) {
                return false;
            }
        } else {
            let mut enter = (min - p) / dp;
            let mut exit = (max - p) / dp;
            if enter > exit {
                std::mem::swap(&mut enter, &mut exit);
            }
            t_enter = t_enter.max(enter);
            t_exit = t_exit.min(exit);
            if t_enter > t_exit {
                return false;
            }
        }
    }

    const EPS: f64 = 1e-12;
    if t_exit - t_enter <= EPS {
        return false;
    }

    let open_enter = t_enter.max(0.0) + EPS;
    let open_exit = t_exit.min(1.0) - EPS;
    if open_exit <= open_enter {
        return false;
    }

    let mid = (open_enter + open_exit) * 0.5;
    let x = start.x + mid * dx;
    let y = start.y + mid * dy;
    x > min_x && x < max_x && y > min_y && y < max_y
}

/// Grid DDA traversal: every cell the open segment may intersect.
fn dda_traverse_cells(start: Point2, end: Point2) -> Vec<(i32, i32)> {
    let mut cells = Vec::new();
    let mut cx = start.x.floor() as i32;
    let mut cy = start.y.floor() as i32;
    let end_cx = end.x.floor() as i32;
    let end_cy = end.y.floor() as i32;

    let dx = end.x - start.x;
    let dy = end.y - start.y;
    let step_x = if dx >= 0.0 { 1 } else { -1 };
    let step_y = if dy >= 0.0 { 1 } else { -1 };

    let t_delta_x = if dx.abs() <= 1e-15 {
        f64::INFINITY
    } else {
        (step_x as f64) / dx
    };
    let t_delta_y = if dy.abs() <= 1e-15 {
        f64::INFINITY
    } else {
        (step_y as f64) / dy
    };

    let mut t_max_x = if dx.abs() <= 1e-15 {
        f64::INFINITY
    } else if dx > 0.0 {
        ((cx + 1) as f64 - start.x) / dx
    } else {
        (start.x - cx as f64) / -dx
    };
    let mut t_max_y = if dy.abs() <= 1e-15 {
        f64::INFINITY
    } else if dy > 0.0 {
        ((cy + 1) as f64 - start.y) / dy
    } else {
        (start.y - cy as f64) / -dy
    };

    cells.push((cx, cy));
    let max_steps = (cx - end_cx).unsigned_abs() + (cy - end_cy).unsigned_abs() + 1;
    let mut steps = 0_u32;
    while cx != end_cx || cy != end_cy {
        steps += 1;
        debug_assert!(
            steps <= max_steps,
            "grid DDA failed to terminate between ({cx},{cy}) and ({end_cx},{end_cy})"
        );
        if steps > max_steps {
            break;
        }

        if cx == end_cx {
            t_max_y += t_delta_y.abs();
            cy += step_y;
        } else if cy == end_cy || t_max_x < t_max_y {
            t_max_x += t_delta_x.abs();
            cx += step_x;
        } else if t_max_y < t_max_x {
            t_max_y += t_delta_y.abs();
            cy += step_y;
        } else {
            t_max_x += t_delta_x.abs();
            t_max_y += t_delta_y.abs();
            cx += step_x;
            cy += step_y;
        }
        cells.push((cx, cy));
    }
    cells
}

fn gcd_i32(mut a: i32, mut b: i32) -> i32 {
    while b != 0 {
        let remainder = a % b;
        a = b;
        b = remainder;
    }
    a.abs()
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::grid::Cell;

    fn block(grid: &mut Grid, cells: &[(usize, usize)]) {
        for &(x, y) in cells {
            grid.set_cell(Point::new(x, y), Cell::Blocked)
                .expect("cell in bounds");
        }
    }

    #[test]
    fn open_diagonal_is_legal() {
        let grid = Grid::new(5, 5).expect("grid");
        assert!(segment_is_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(4.0, 4.0)
        ));
    }

    #[test]
    fn blocked_interior_is_illegal() {
        let mut grid = Grid::new(5, 5).expect("grid");
        block(&mut grid, &[(2, 2)]);
        assert!(!segment_is_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(4.0, 4.0)
        ));
    }

    #[test]
    fn opposite_blocked_pinch_is_illegal() {
        let mut grid = Grid::new(4, 4).expect("grid");
        block(&mut grid, &[(1, 1), (2, 2)]);
        assert!(!segment_is_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(3.0, 3.0)
        ));
    }

    #[test]
    fn boundary_edge_touch_is_legal() {
        let mut grid = Grid::new(5, 5).expect("grid");
        block(&mut grid, &[(2, 2)]);
        assert!(segment_is_legal(
            &grid,
            Point2::new(0.0, 2.0),
            Point2::new(4.0, 2.0)
        ));
    }

    #[test]
    fn kernel_and_sampling_reject_blocked_interior() {
        let mut grid = Grid::new(5, 5).expect("grid");
        block(&mut grid, &[(2, 2)]);
        let start = Point2::new(0.0, 0.0);
        let end = Point2::new(4.0, 4.0);
        assert!(!segment_is_legal(&grid, start, end));
        assert!(!sampling_segment_is_legal(&grid, start, end));
    }

    #[test]
    fn pinch_checks_skip_segment_endpoints() {
        let mut grid = Grid::new(3, 3).expect("grid");
        block(&mut grid, &[(0, 0), (1, 0)]);
        assert!(segment_is_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 0.0)
        ));
    }

    #[test]
    fn boundary_following_is_independent_of_waypoint_insertion() {
        let mut grid = Grid::new(7, 5).expect("grid");
        block(&mut grid, &[(3, 1), (3, 2), (3, 3), (4, 2), (5, 2)]);
        let start = Point2::new(0.0, 2.0);
        let goal = Point2::new(6.0, 2.0);
        assert!(segment_is_legal(&grid, start, goal));
        assert!(segment_is_legal(&grid, start, Point2::new(4.0, 2.0)));
        assert!(segment_is_legal(
            &grid,
            Point2::new(4.0, 2.0),
            Point2::new(5.0, 2.0)
        ));
        assert!(segment_is_legal(&grid, Point2::new(5.0, 2.0), goal));
    }

    #[test]
    fn dda_traversal_terminates_on_all_vertex_pairs() {
        for height in 2..=3 {
            for width in 2..=3 {
                for sy in 0..=height {
                    for sx in 0..=width {
                        for gy in 0..=height {
                            for gx in 0..=width {
                                let start = Point2::new(sx as f64, sy as f64);
                                let end = Point2::new(gx as f64, gy as f64);
                                let cells = dda_traverse_cells(start, end);
                                assert!(
                                    !cells.is_empty(),
                                    "DDA returned no cells for ({sx},{sy})->({gx},{gy})"
                                );
                                assert_eq!(
                                    *cells.last().expect("terminal cell"),
                                    (end.x.floor() as i32, end.y.floor() as i32),
                                    "DDA missed terminal cell for ({sx},{sy})->({gx},{gy})"
                                );
                            }
                        }
                    }
                }
            }
        }
    }

    #[test]
    fn dda_reference_detects_early_blocked_crossing() {
        let mut grid = Grid::new(130, 1).expect("grid");
        block(&mut grid, &[(1, 0)]);
        let start = Point2::new(0.0, 0.0);
        let end = Point2::new(130.0, 1.0);
        assert!(!sampling_segment_is_legal(&grid, start, end));
        assert!(!segment_is_legal(&grid, start, end));
    }

    #[test]
    fn near_integer_endpoints_canonicalize_for_search() {
        let grid = Grid::new(4, 4).expect("grid");
        let start = canonicalize_grid_vertex(Point2::new(2.0000000001, 2.0)).expect("start");
        let goal = canonicalize_grid_vertex(Point2::new(3.0, 3.9999999999)).expect("goal");
        assert!(segment_is_legal(&grid, start, goal));
    }
}
#[cfg(test)]
mod approx_tests {
    use super::*;
    use condor_core::Point2;

    #[test]
    fn eight_and_nine_are_distinct() {
        assert!(!approximately_equal(8.0, 9.0));
        assert!(!approximately_equal(
            Point2::new(0.0, 8.0).y,
            Point2::new(0.0, 9.0).y
        ));
    }
}