blockworx/router/

mod.rs

use std::collections::{BTreeMap, BTreeSet};

use blockworx_geom::Pos2;
use pathfinding::directed::dijkstra::dijkstra;
use petgraph::{
    graph::{NodeIndex, UnGraph},
    visit::EdgeRef,
};

use crate::router::{
    block::{Block, ROUTE_GUTTER},
    channel::{Channel, ChannelOrientation, h_channel, v_channel},
    coord::{CoordX, CoordY, INFINITY_X, INFINITY_Y, NEG_INFINITY_X, NEG_INFINITY_Y},
    cost::{COST_ZERO, Cost},
    event::{Event, EventSense},
    point::{Point, point},
    segment::{HSegment, Segment, VSegment, hseg, vseg},
    turtle::{Mark, Turtle},
};
pub mod block;
pub mod channel;
pub mod coord;
pub mod cost;
pub mod event;
pub mod point;
pub mod segment;
pub mod turtle;

#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
pub enum Direction {
    North,
    South,
    East,
    West,
}

impl Direction {
    fn opposite(self) -> Self {
        match self {
            Direction::North => Direction::South,
            Direction::South => Direction::North,
            Direction::East => Direction::West,
            Direction::West => Direction::East,
        }
    }
}

/// The cardinal direction of travel from `from` to `to`, or `None` if the points
/// coincide. Matches the convention used by [`RouterNG::successors`] (greater `y`
/// is `South`).
pub fn direction_between(from: Point, to: Point) -> Option<Direction> {
    if to.x > from.x {
        Some(Direction::East)
    } else if to.x < from.x {
        Some(Direction::West)
    } else if to.y > from.y {
        Some(Direction::South)
    } else if to.y < from.y {
        Some(Direction::North)
    } else {
        None
    }
}

const TURN_COST: Cost = Cost::new(25.0);
const MOVE_COST: Cost = Cost::new(1.0);
pub const WIRE_COST: Cost = Cost::new(10.0);

fn cross_cost(
    from: Option<Direction>,
    to: Direction,
    cost_to_cross_east_west: Cost,
    cost_to_cross_north_south: Cost,
) -> Cost {
    if let Some(from_dir) = from {
        match (from_dir, to) {
            (Direction::North, Direction::South) | (Direction::South, Direction::North) => {
                cost_to_cross_east_west
            }
            (Direction::East, Direction::West) | (Direction::West, Direction::East) => {
                cost_to_cross_north_south
            }
            _ => COST_ZERO,
        }
    } else {
        COST_ZERO
    }
}

fn turn_cost(from: Option<Direction>, to: Direction) -> Cost {
    if let Some(from_dir) = from {
        if from_dir == to {
            COST_ZERO
        } else if to == from_dir.opposite() {
            TURN_COST * 100.0
        } else {
            TURN_COST
        }
    } else {
        COST_ZERO
    }
}

#[derive(Default, Copy, Clone, Debug, PartialEq, Eq, Hash)]
struct SearchState {
    node: NodeIndex,
    dir: Option<Direction>,
}

#[derive(Debug, Clone, Default)]
pub struct RouterNGBuilder {
    /// The blocking rectangles
    blocks: Vec<Block>,
    /// The routing channels
    channels: Vec<Channel>,
    /// Points at which [`Self::build_closed`] seeds a full (H+V) channel — route
    /// endpoints and waypoints, whose channels the old per-route path added at
    /// route time via `seed_channels`. Ignored by the test-only `build`.
    seed_points: Vec<Point>,
}

impl RouterNGBuilder {
    pub fn add_h_channel(&mut self, seed: impl Into<Point>, cost: impl Into<Cost>) {
        self.channels.push(h_channel(seed, cost));
    }
    /// Register a point (a route endpoint or waypoint) at which `build_closed`
    /// seeds a full channel, so the closed graph already contains the nodes every
    /// route needs — no geometry is added once the graph is closed.
    pub fn add_seed_point(&mut self, p: impl Into<Point>) {
        self.seed_points.push(p.into());
    }
    pub fn add_v_channel(&mut self, seed: impl Into<Point>, cost: impl Into<Cost>) {
        self.channels.push(v_channel(seed, cost));
    }
    fn add_routing_moat(
        &mut self,
        top_left: Point,
        bottom_right: Point,
        distance: i32,
        cost: Cost,
    ) {
        let min_x = top_left.x.min(bottom_right.x);
        let max_x = top_left.x.max(bottom_right.x);
        let min_y = top_left.y.min(bottom_right.y);
        let max_y = top_left.y.max(bottom_right.y);
        self.add_v_channel(point(min_x - distance - 2, min_y), cost);
        self.add_v_channel(point(min_x - distance - 2, max_y), cost);
        self.add_v_channel(point(max_x + distance + 2, min_y), cost);
        self.add_v_channel(point(max_x + distance + 2, max_y), cost);
        self.add_h_channel(point(min_x, min_y - distance - 2), cost);
        self.add_h_channel(point(max_x, min_y - distance - 2), cost);
        self.add_h_channel(point(min_x, max_y + distance + 2), cost);
        self.add_h_channel(point(max_x, max_y + distance + 2), cost);
    }
    pub fn add_block(&mut self, top_left: impl Into<Point>, bottom_right: impl Into<Point>) {
        let top_left: Point = top_left.into();
        let bottom_right: Point = bottom_right.into();
        let min_x = top_left.x.min(bottom_right.x);
        let max_x = top_left.x.max(bottom_right.x);
        let min_y = top_left.y.min(bottom_right.y);
        let max_y = top_left.y.max(bottom_right.y);
        let block = Block {
            top_left: point(min_x, min_y),
            bottom_right: point(max_x, max_y),
        };
        self.blocks.push(block);
        // Add the routing channels around the blocked rectangle.
        for moat_lane in 0..5 {
            let cost = if moat_lane == 0 {
                Cost::new(0.2)
            } else {
                Cost::new(0.1)
            };
            self.add_routing_moat(top_left, bottom_right, moat_lane, cost);
        }
    }
    /// Construct a dirty router with all channels seeded but not yet built into a
    /// graph. Shared by [`Self::build_closed`] and the test-only `build`.
    fn seed_channels_into_router(&self) -> RouterNG {
        let block_index = BlockAxisIndex::build(&self.blocks);
        let mut router = RouterNG {
            blocks: self.blocks.clone(),
            block_index,
            h_segments: BTreeMap::new(),
            v_segments: BTreeMap::new(),
            nodes: BTreeSet::new(),
            graph: UnGraph::default(),
            node_to_index: BTreeMap::new(),
            dirty: true,
        };
        for channel in &self.channels {
            match channel.orientation {
                ChannelOrientation::Horizontal => {
                    router.seed_horiz_channel(channel.seed, channel.cost);
                }
                ChannelOrientation::Vertical => {
                    router.seed_vert_channel(channel.seed, channel.cost);
                }
            }
        }
        router
    }
    /// Build the mutable [`RouterNG`] directly. Only the router's own unit tests
    /// construct one this way now; production code uses [`Self::build_closed`].
    #[cfg(test)]
    pub fn build(self) -> RouterNG {
        let mut router = self.seed_channels_into_router();
        router.update();
        router
    }
    /// Build a [`ClosedRouter`]: seed all channels AND the endpoint/waypoint
    /// channels registered via [`Self::add_seed_point`], build the graph once,
    /// then freeze the geometry. Routing on the result only mutates edge weights.
    pub fn build_closed(self) -> ClosedRouter {
        let mut router = self.seed_channels_into_router();
        for &p in &self.seed_points {
            router.seed_channels(p, COST_ZERO);
        }
        router.update();
        ClosedRouter { inner: router }
    }
}

/// A [`RouterNG`] whose geometry has been built and frozen: it exposes routing
/// and in-place edge-weight mutation, but no method to add segments/channels or
/// rebuild the graph. Occupancy of a completed route is applied by adding
/// [`WIRE_COST`] to the existing graph edges the wire covers, so no rebuild is
/// needed (see Finding 1 in TUNING.md).
#[derive(Debug, Clone)]
pub struct ClosedRouter {
    inner: RouterNG,
}

impl ClosedRouter {
    /// Route one leg from `start` to `end`, threading `incoming` (the direction
    /// already travelled) so the leg can't double back. Read-only w.r.t.
    /// geometry — the graph is already closed, so the internal `update()` is a
    /// no-op. Returns the path points and the outgoing direction.
    pub fn route_leg(
        &mut self,
        start: Point,
        end: Point,
        incoming: Option<Direction>,
    ) -> (Vec<Point>, Option<Direction>) {
        self.inner.path_find_with_fallback(start, end, incoming)
    }

    /// Add `cost` to every graph edge along the (axis-aligned) `path`, leg by leg.
    /// The in-place equivalent of the old `add_subpath_cost`.
    pub fn bump_leg(&mut self, path: &[Point], cost: Cost) {
        for w in path.windows(2) {
            self.add_wire_cost(w[0], w[1], cost);
        }
    }

    /// Occupy the axis-aligned wire `a → b` by adding `cost` to each of the
    /// node-to-node graph edges it covers. Walks adjacency in the travel
    /// direction (edges are split at every node, so each hop is one graph edge).
    /// Never changes topology or sets `dirty`, so the graph stays closed and
    /// `successors` reads the new weights live on the next route.
    pub fn add_wire_cost(&mut self, a: Point, b: Point, cost: Cost) {
        let Some(dir) = direction_between(a, b) else {
            return;
        };
        let Some(&target) = self.inner.node_to_index.get(&b) else {
            return;
        };
        let Some(&start) = self.inner.node_to_index.get(&a) else {
            return;
        };
        let mut cur = start;
        let mut cur_pt = a;
        // Bounded by the number of nodes; the guard only trips on a malformed
        // (disconnected) graph, in which case we stop rather than loop forever.
        for _ in 0..self.inner.node_to_index.len() {
            if cur == target {
                break;
            }
            let step = self.inner.graph.edges(cur).find_map(|e| {
                let np = self.inner.point(e.target());
                (direction_between(cur_pt, np) == Some(dir)).then_some((e.id(), e.target(), np))
            });
            let Some((edge_id, next, next_pt)) = step else {
                break;
            };
            if let Some(w) = self.inner.graph.edge_weight_mut(edge_id) {
                *w += cost;
            }
            cur = next;
            cur_pt = next_pt;
        }
    }

    /// Whether the axis-aligned wire `a → b` crosses any blocked rectangle.
    pub fn is_wire_blocked(&self, a: Point, b: Point) -> bool {
        self.inner
            .blocks
            .iter()
            .any(|block| block.intersects_edge(a, b))
    }

    /// Whether the straight wire `a → b` hugs any block (runs within the routing
    /// gutter alongside an edge). Complements [`Self::is_wire_blocked`]: together
    /// they decide whether a straight leg may be taken verbatim or must route around.
    pub fn wire_hugs_block(&self, a: Point, b: Point) -> bool {
        self.inner
            .blocks
            .iter()
            .any(|blk| blk.hugs_wire(a, b, ROUTE_GUTTER))
    }

    pub fn is_accessible(&self, test: impl Into<Point>) -> bool {
        self.inner.is_accessible(test)
    }

    pub fn debug_marks(&self) -> Vec<Mark> {
        self.inner.debug_marks()
    }
}

/// A per-axis index of the (immutable) block set, so channel seeding iterates only
/// the blocks that intersect a query coordinate instead of scanning them all. Built
/// once when the closed geometry is assembled. `by_y[y]` lists the blocks whose
/// vertical extent (expanded by one, matching [`RouterNG::seed_horiz_channel`])
/// contains `y`; `by_x[x]` the blocks whose horizontal extent (expanded by one)
/// contains `x`. Same coordinate-keyed `BTreeMap<Coord, Vec<_>>` shape as the
/// segment maps.
#[derive(Debug, Clone, Default)]
struct BlockAxisIndex {
    by_y: BTreeMap<CoordY, Vec<usize>>,
    by_x: BTreeMap<CoordX, Vec<usize>>,
}

impl BlockAxisIndex {
    fn build(blocks: &[Block]) -> Self {
        let mut by_y: BTreeMap<CoordY, Vec<usize>> = BTreeMap::new();
        let mut by_x: BTreeMap<CoordX, Vec<usize>> = BTreeMap::new();
        for (i, block) in blocks.iter().enumerate() {
            let ey = block.expand_y(1);
            for y in ey.top_left.y.raw()..=ey.bottom_right.y.raw() {
                by_y.entry(CoordY::from(y)).or_default().push(i);
            }
            let ex = block.expand_x(1);
            for x in ex.top_left.x.raw()..=ex.bottom_right.x.raw() {
                by_x.entry(CoordX::from(x)).or_default().push(i);
            }
        }
        Self { by_y, by_x }
    }

    /// The blocks whose one-expanded vertical extent contains `y`.
    fn spanning_y<'a>(&'a self, y: CoordY, blocks: &'a [Block]) -> impl Iterator<Item = &'a Block> {
        self.by_y.get(&y).into_iter().flatten().map(|&i| &blocks[i])
    }

    /// The blocks whose one-expanded horizontal extent contains `x`.
    fn spanning_x<'a>(&'a self, x: CoordX, blocks: &'a [Block]) -> impl Iterator<Item = &'a Block> {
        self.by_x.get(&x).into_iter().flatten().map(|&i| &blocks[i])
    }
}

#[derive(Debug, Clone)]
pub struct RouterNG {
    /// The blocking rectangles - not mutable
    blocks: Vec<Block>,
    /// Per-axis index of `blocks` for channel seeding (built from `blocks`).
    block_index: BlockAxisIndex,
    /// Horizontal segments, keyed by their vertical coordinate
    h_segments: BTreeMap<CoordY, Vec<HSegment>>,
    /// Vertical segments, keyed by their horizontal coordinate
    v_segments: BTreeMap<CoordX, Vec<VSegment>>,
    /// Nodes: the intersection points of the segments
    nodes: BTreeSet<Point>,
    /// The graph to be used for pathfinding, built from the segments and nodes
    graph: UnGraph<Point, Cost>,
    /// A map from node to index in the graph, for quick lookup
    node_to_index: BTreeMap<Point, petgraph::graph::NodeIndex>,
    /// Dirty flag that indicates `h_segments` or `v_segments` have been modified and the graph needs to be rebuilt.
    dirty: bool,
}

impl RouterNG {
    pub fn debug_marks(&self) -> Vec<Mark> {
        assert!(
            !self.dirty,
            "Cannot generate debug marks when the graph is dirty"
        );
        let mut turtle = Turtle::default();
        for node in self.graph.node_indices() {
            let pos = self.point(node);
            turtle.move_to(pos.into());
            turtle.circle(blockworx_geom::WorldPx::new(3.0));
            // To make the edges more visible, we add a gap at the beginning and
            // end of the edge line, so that it looks like this * ---- * rather than this *------------------*
            for edge in self.graph.edges(node) {
                let target = edge.target();
                let edge_weight = edge.weight();
                let target_pos = self.point(target);

                // Calculate direction and distance
                let start_pos: Pos2 = pos.into();
                let end_pos: Pos2 = target_pos.into();
                let dx = end_pos.x - start_pos.x;
                let dy = end_pos.y - start_pos.y;
                let distance = (dx * dx + dy * dy).sqrt();

                // Skip very short edges
                if distance < 8.0 {
                    continue;
                }

                // Create 4-pixel gap at each end
                let gap = 4.0;
                let gap_ratio = gap / distance;

                // Start point with gap from the node
                let line_start =
                    Pos2::new(start_pos.x + dx * gap_ratio, start_pos.y + dy * gap_ratio);

                // End point with gap before the target
                let line_end = Pos2::new(end_pos.x - dx * gap_ratio, end_pos.y - dy * gap_ratio);

                turtle.move_to(line_start);
                turtle.line_to(line_end);
                let mid_point = line_start + (line_end - line_start) / 2.0;
                let weight: f64 = (*edge_weight).into();
                turtle.label(mid_point, weight as f32);
            }
        }
        turtle.compile()
    }
    pub fn is_accessible(&self, test: impl Into<Point>) -> bool {
        let test: Point = test.into();
        !self.blocks.iter().any(|block| block.contains(test))
    }
    fn seed_horiz_channel(&mut self, center: impl Into<Point>, cost: impl Into<Cost>) {
        let center: Point = center.into();
        let cost: Cost = cost.into();
        let mut left_endpoint = NEG_INFINITY_X;
        let mut right_endpoint = INFINITY_X;
        // Only blocks spanning `center.y` can clip a horizontal channel there; the
        // index yields exactly those (matching the old `expand_y(1).spans_y` gate).
        for block in self.block_index.spanning_y(center.y, &self.blocks) {
            let block = block.expand_x(1).expand_y(1);
            if block.spans_x(center.x) {
                // Seed point is inside a block, so no channel can pass through here.
                return;
            }
            if block.is_left_of(center.x) {
                left_endpoint = left_endpoint.max(block.bottom_right.x);
            }
            if block.is_right_of(center.x) {
                right_endpoint = right_endpoint.min(block.top_left.x);
            }
        }
        if left_endpoint < right_endpoint {
            self.add_horiz_segment(center.y, left_endpoint, right_endpoint, cost);
        }
    }
    fn seed_vert_channel(&mut self, center: impl Into<Point>, cost: impl Into<Cost>) {
        let center: Point = center.into();
        let cost: Cost = cost.into();
        let mut top_endpoint = NEG_INFINITY_Y;
        let mut bottom_endpoint = INFINITY_Y;
        // Only blocks spanning `center.x` (expanded, leaving a one-unit gutter) can
        // clip a vertical channel there; the index yields exactly those.
        for block in self.block_index.spanning_x(center.x, &self.blocks) {
            if block.spans_y(center.y) {
                // Seed point is inside a block, so no channel can pass through here.
                return;
            }
            if block.is_above(center.y) {
                top_endpoint = top_endpoint.max(block.bottom_right.y);
            }
            if block.is_below(center.y) {
                bottom_endpoint = bottom_endpoint.min(block.top_left.y);
            }
        }
        if top_endpoint < bottom_endpoint {
            self.add_vert_segment(center.x, top_endpoint, bottom_endpoint, cost);
        }
    }
    pub fn seed_channels(&mut self, center: impl Into<Point>, cost: impl Into<Cost>) {
        let center: Point = center.into();
        let cost: Cost = cost.into();
        self.seed_horiz_channel(center, cost);
        self.seed_vert_channel(center, cost);
    }
    pub fn add_horiz_segment(
        &mut self,
        vert: impl Into<CoordY>,
        left: impl Into<CoordX>,
        right: impl Into<CoordX>,
        cost: impl Into<Cost>,
    ) {
        let vert: CoordY = vert.into();
        let left: CoordX = left.into();
        let right: CoordX = right.into();
        let cost: Cost = cost.into();
        if right > left {
            self.h_segments
                .entry(vert)
                .or_default()
                .push(hseg(left, right, cost));
            self.dirty = true;
        }
    }
    pub fn add_vert_segment(
        &mut self,
        horiz: impl Into<CoordX>,
        top: impl Into<CoordY>,
        bottom: impl Into<CoordY>,
        cost: impl Into<Cost>,
    ) {
        let horiz: CoordX = horiz.into();
        let top: CoordY = top.into();
        let bottom: CoordY = bottom.into();
        let cost: Cost = cost.into();
        if bottom > top {
            self.v_segments
                .entry(horiz)
                .or_default()
                .push(vseg(top, bottom, cost));
            self.dirty = true;
        }
    }
    pub fn update(&mut self) {
        if !self.dirty {
            return;
        }
        // Only real (dirty) rebuilds are timed; the no-op early return above is
        // the common case and would drown the trace.
        let _span = tracing::info_span!("router_rebuild").entered();
        let h_segments = std::mem::take(&mut self.h_segments);
        for (vert, segments) in h_segments {
            normalize_collinear_segments(segments, |left, right, cost| {
                self.add_horiz_segment(vert, left, right, cost);
            });
        }
        let v_segments = std::mem::take(&mut self.v_segments);
        for (horiz, segments) in v_segments {
            normalize_collinear_segments(segments, |top, bottom, cost| {
                self.add_vert_segment(horiz, top, bottom, cost);
            });
        }
        self.nodes = collect_intersections(self.iter_hsegs(), self.iter_vsegs());
        // Re-segment, but now add segments for each node.
        let mut h_segments = std::mem::take(&mut self.h_segments);
        self.nodes.iter().for_each(|&node| {
            h_segments
                .entry(node.y)
                .or_default()
                .push(hseg(node.x, node.x, COST_ZERO));
        });
        for (vert, segments) in h_segments {
            normalize_collinear_segments(segments, |left, right, cost| {
                self.add_horiz_segment(vert, left, right, cost);
            });
        }
        let mut v_segments = std::mem::take(&mut self.v_segments);
        self.nodes.iter().for_each(|&node| {
            v_segments
                .entry(node.x)
                .or_default()
                .push(vseg(node.y, node.y, COST_ZERO));
        });
        for (horiz, segments) in v_segments {
            normalize_collinear_segments(segments, |top, bottom, cost| {
                self.add_vert_segment(horiz, top, bottom, cost);
            });
        }
        // The final node set is exactly the endpoints of the now-split segments.
        // Pass 2 injected a zero-length marker at every crossing (found by the first
        // `collect_intersections`) and re-normalized, which splits each segment at
        // those crossings — so every crossing is now a segment endpoint. A second
        // intersection sweep would only recompute those same points, so skip it.
        let nodes = self
            .iter_hsegs()
            .flat_map(|(y, h_seg)| [point(h_seg.start, y), point(h_seg.end, y)])
            .chain(
                self.iter_vsegs()
                    .flat_map(|(x, v_seg)| [point(x, v_seg.start), point(x, v_seg.end)]),
            )
            .collect();
        self.nodes = nodes;
        self.rebuild_graph();
        self.dirty = false;
    }
    fn iter_hsegs(&self) -> impl Iterator<Item = (CoordY, HSegment)> + '_ {
        self.h_segments
            .iter()
            .flat_map(|(&y, h_segs)| h_segs.iter().map(move |h_seg| (y, *h_seg)))
    }
    fn iter_vsegs(&self) -> impl Iterator<Item = (CoordX, VSegment)> + '_ {
        self.v_segments
            .iter()
            .flat_map(|(&x, v_segs)| v_segs.iter().map(move |v_seg| (x, *v_seg)))
    }
    fn rebuild_graph(&mut self) {
        let mut node_to_index: BTreeMap<Point, petgraph::graph::NodeIndex> = BTreeMap::new();
        let mut graph = UnGraph::default();
        for &node in &self.nodes {
            let index = graph.add_node(node);
            node_to_index.insert(node, index);
        }
        for hseg in self.iter_hsegs() {
            let y = hseg.0;
            let h_seg = hseg.1;
            let start_node = point(h_seg.start, y);
            let end_node = point(h_seg.end, y);
            graph.add_edge(
                node_to_index[&start_node],
                node_to_index[&end_node],
                h_seg.cost,
            );
        }
        for vseg in self.iter_vsegs() {
            let x = vseg.0;
            let v_seg = vseg.1;
            let start_node = point(x, v_seg.start);
            let end_node = point(x, v_seg.end);
            graph.add_edge(
                node_to_index[&start_node],
                node_to_index[&end_node],
                v_seg.cost,
            );
        }
        self.graph = graph;
        self.node_to_index = node_to_index;
    }
    /// The grid point a node index stands for. Every index the graph hands out
    /// resolves; a stray one yields the origin rather than taking the editor down.
    fn point(&self, node: NodeIndex) -> Point {
        self.graph.node_weight(node).copied().unwrap_or(Point::ZERO)
    }

    fn successors(&self, state: SearchState) -> Vec<(SearchState, Cost)> {
        let prev_dir = state.dir;
        let prev_point = self.point(state.node);
        let mut north_cost: Option<Cost> = None;
        let mut south_cost: Option<Cost> = None;
        let mut east_cost: Option<Cost> = None;
        let mut west_cost: Option<Cost> = None;
        // Get the costs to move in the 4 cardinal directions from the current node.
        for edge in self.graph.edges(state.node) {
            let neighbor = edge.target();
            let cost = *edge.weight();
            let neighbor_point = self.point(neighbor);
            if neighbor_point.x > prev_point.x {
                east_cost = Some(cost);
            } else if neighbor_point.x < prev_point.x {
                west_cost = Some(cost);
            } else if neighbor_point.y > prev_point.y {
                south_cost = Some(cost);
            } else {
                north_cost = Some(cost);
            }
        }
        // Calculate the east and west cost as a single cost,
        // since if we are north/south bound, we should consider
        // this a crossing.
        let east_west_crossing_cost = match (east_cost, west_cost) {
            (Some(east), Some(west)) => east.max(west),
            _ => COST_ZERO,
        };
        let north_south_crossing_cost = match (north_cost, south_cost) {
            (Some(north), Some(south)) => north.max(south),
            _ => COST_ZERO,
        };
        // Rescan the edges to generate the successors with the correct costs.
        self.graph
            .edges(state.node)
            .map(|edge| {
                let neighbor = edge.target();
                let cost = *edge.weight();
                let neighbor_point = self.point(neighbor);
                let dir = if neighbor_point.x > prev_point.x {
                    Direction::East
                } else if neighbor_point.x < prev_point.x {
                    Direction::West
                } else if neighbor_point.y > prev_point.y {
                    Direction::South
                } else {
                    Direction::North
                };
                let step_length = neighbor_point.manhattan_distance(prev_point) as f64;
                let step_cost = turn_cost(prev_dir, dir)
                    + MOVE_COST * step_length
                    + cost * step_length
                    + cross_cost(
                        prev_dir,
                        dir,
                        east_west_crossing_cost,
                        north_south_crossing_cost,
                    );
                (
                    SearchState {
                        node: neighbor,
                        dir: Some(dir),
                    },
                    step_cost,
                )
            })
            .collect()
    }
    /// Route from `start` to `end`, seeding the search with `incoming` as the
    /// direction already being travelled (so reversing it out of `start` pays the
    /// `turn_cost` reversal penalty). Returns the path together with the direction
    /// of its final segment — the direction of travel arriving at `end` — so a
    /// caller routing several segments in series can forbid the next one doubling
    /// back.
    fn path_find(
        &mut self,
        start: impl Into<Point>,
        end: impl Into<Point>,
        incoming: Option<Direction>,
    ) -> Option<(Vec<Point>, Option<Direction>)> {
        self.update();
        let start: Point = start.into();
        let end: Point = end.into();
        let &start_node = self.node_to_index.get(&start)?;
        let end_node = self.node_to_index.get(&end)?;
        let start = SearchState {
            node: start_node,
            dir: incoming,
        };
        let result = dijkstra(
            &start,
            |state| self.successors(*state),
            |state| state.node == *end_node,
        );
        result.map(|(path, _cost)| {
            let outgoing = path.last().and_then(|state| state.dir);
            let points = path
                .into_iter()
                .map(|state| self.point(state.node))
                .collect();
            (points, outgoing)
        })
    }
    pub fn path_find_with_fallback(
        &mut self,
        start: impl Into<Point>,
        end: impl Into<Point>,
        incoming: Option<Direction>,
    ) -> (Vec<Point>, Option<Direction>) {
        let start: Point = start.into();
        let end: Point = end.into();
        if let Some(result) = self.path_find(start, end, incoming) {
            return result;
        }
        // Couldn't find a path. so just connect the two points with a horizontal and vertical segment.
        let path = vec![start, point(end.x, start.y), end];
        let outgoing = path
            .windows(2)
            .rev()
            .find_map(|w| direction_between(w[0], w[1]))
            .or(incoming);
        (path, outgoing)
    }
}

// Run a line-sweep style algorithm to collect the intersections.
// The algorithm works by creating a list of events sorted in x.  Each event
// is either the start or end of a horizontal segment (using the Enter/Exit events)
// or a vertical segment (using the Scan event).  The events are sorted by their X-coordinate,
// and then processed in order.  We maintain a list of active horizontal segments at any given
// time, and then when we encounter a scan event, we list out all intersections of that vertical
// segment with the active horizontal segments.
/// Algorithmic counters for the sweep, compiled only under `cfg(test)` so
/// production is untouched. Used by the profiling benchmarks to quantify the
/// inner-loop work vs. the intersections it actually produces.
#[cfg(test)]
pub(crate) mod ci_stats {
    use std::sync::atomic::{AtomicU64, Ordering};
    pub static CALLS: AtomicU64 = AtomicU64::new(0);
    pub static EVENTS: AtomicU64 = AtomicU64::new(0);
    pub static SCANS: AtomicU64 = AtomicU64::new(0);
    pub static INNER_ITERS: AtomicU64 = AtomicU64::new(0);
    pub static INTERSECTIONS: AtomicU64 = AtomicU64::new(0);
    pub fn reset() {
        for c in [&CALLS, &EVENTS, &SCANS, &INNER_ITERS, &INTERSECTIONS] {
            c.store(0, Ordering::Relaxed);
        }
    }
    pub fn dump(label: &str) {
        let g = |c: &AtomicU64| c.load(Ordering::Relaxed);
        eprintln!(
            "[{label}] collect_intersections calls={} events={} scans(V)={} \
             inner_iters={} intersections={} (waste = inner_iters/intersections = {:.1}x)",
            g(&CALLS),
            g(&EVENTS),
            g(&SCANS),
            g(&INNER_ITERS),
            g(&INTERSECTIONS),
            g(&INNER_ITERS) as f64 / (g(&INTERSECTIONS).max(1)) as f64,
        );
    }
}

fn collect_intersections(
    h_segments: impl IntoIterator<Item = (CoordY, HSegment)>,
    v_segments: impl IntoIterator<Item = (CoordX, VSegment)>,
) -> BTreeSet<Point> {
    let mut events: Vec<Event<CoordX, (CoordY, CoordY)>> = h_segments
        .into_iter()
        .flat_map(|(y, h_seg)| {
            [
                Event::enter(h_seg.start, (y, y)),
                Event::exit(h_seg.end, (y, y)),
            ]
        })
        .chain(
            v_segments
                .into_iter()
                .map(|(x, v_seg)| Event::scan(x, (v_seg.start, v_seg.end))),
        )
        .collect::<Vec<_>>();
    events.sort();
    #[cfg(test)]
    {
        use std::sync::atomic::Ordering::Relaxed;
        ci_stats::CALLS.fetch_add(1, Relaxed);
        ci_stats::EVENTS.fetch_add(events.len() as u64, Relaxed);
    }
    let mut intersections = BTreeSet::new();
    // Use a map to count active segments at each y-coordinate
    // This handles segments that touch at boundaries (e.g., one ends at x=10, another starts at x=10)
    let mut active_h_segments: BTreeMap<CoordY, usize> = BTreeMap::new();
    for event in events {
        match event.sense() {
            EventSense::Enter => {
                let y = event.cost().0;
                *active_h_segments.entry(y).or_insert(0) += 1;
            }
            EventSense::Exit => {
                let y = event.cost().0;
                if let Some(count) = active_h_segments.get_mut(&y) {
                    *count = count.saturating_sub(1);
                    if *count == 0 {
                        active_h_segments.remove(&y);
                    }
                }
            }
            EventSense::Scan => {
                let (start, end) = event.cost();
                // Only the active horizontal segments whose `y` falls in this
                // vertical segment's span intersect it. `active_h_segments` is
                // keyed by `y` and only holds entries with a non-zero count, so a
                // range query yields exactly those — no scanning past the span.
                let in_span = active_h_segments.range(start..=end);
                #[cfg(test)]
                {
                    use std::sync::atomic::Ordering::Relaxed;
                    ci_stats::SCANS.fetch_add(1, Relaxed);
                    ci_stats::INNER_ITERS.fetch_add(in_span.clone().count() as u64, Relaxed);
                }
                for (&y, _) in in_span {
                    intersections.insert(point(event.t(), y));
                    #[cfg(test)]
                    ci_stats::INTERSECTIONS.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
                }
            }
        }
    }
    intersections
}

fn normalize_collinear_segments<T: Ord + Copy>(
    segments: impl IntoIterator<Item = Segment<T>>,
    mut maker: impl FnMut(T, T, Cost),
) {
    let mut events = segments
        .into_iter()
        .flat_map(|seg| {
            [
                Event::enter(seg.start, seg.cost),
                Event::exit(seg.end, seg.cost),
            ]
        })
        .collect::<Vec<_>>();
    // Sort the events by their coordinate, with Enter events before Exit events in case of ties.
    events.sort();
    scan_disjoint_segments(events, |start, end, cost| {
        maker(start, end, cost);
    });
}

fn scan_disjoint_segments<T: Ord + Copy>(
    events: impl IntoIterator<Item = Event<T, Cost>>,
    mut maker: impl FnMut(T, T, Cost),
) {
    let mut events_iter = events.into_iter();

    // Handle the first event to initialize state
    let Some(first_event) = events_iter.next() else {
        return;
    };

    let mut last_t = first_event.t();
    let mut line_count = first_event.count();
    let mut current_cost = if first_event.is_enter() {
        first_event.cost()
    } else {
        COST_ZERO - first_event.cost()
    };

    // Process remaining events
    for event in events_iter {
        let t = event.t();
        // Invariant: line_count > 0 means last_t was assigned in a previous iteration
        if line_count != 0 {
            maker(last_t, t, current_cost);
        }
        last_t = t;
        line_count += event.count();
        current_cost = if event.is_enter() {
            current_cost + event.cost()
        } else {
            current_cost - event.cost()
        };
    }
}

fn interval_overlap<T: Ord + Copy>(a_start: T, a_end: T, b_start: T, b_end: T) -> bool {
    a_start < b_end && b_start < a_end
}

#[cfg(test)]
mod tests {

    use super::*;

    #[test]
    fn block_axis_index_selects_the_same_blocks_as_a_scan() {
        use crate::router::block::Block;
        use crate::router::point::point;
        use std::collections::BTreeSet;
        // Distinct x's and distinct y's so each block is identifiable per axis.
        let blocks = vec![
            Block {
                top_left: point(0, 0),
                bottom_right: point(8, 8),
            },
            Block {
                top_left: point(20, 4),
                bottom_right: point(30, 12),
            },
            Block {
                top_left: point(-10, -3),
                bottom_right: point(-2, 40),
            },
        ];
        let index = BlockAxisIndex::build(&blocks);
        for c in -14..=44 {
            let y = CoordY::from(c);
            let from_index: BTreeSet<i32> = index
                .spanning_y(y, &blocks)
                .map(|b| b.top_left.x.raw())
                .collect();
            let brute: BTreeSet<i32> = blocks
                .iter()
                .filter(|b| b.expand_y(1).spans_y(y))
                .map(|b| b.top_left.x.raw())
                .collect();
            assert_eq!(from_index, brute, "spanning_y mismatch at y={c}");

            let x = CoordX::from(c);
            let from_index_x: BTreeSet<i32> = index
                .spanning_x(x, &blocks)
                .map(|b| b.top_left.y.raw())
                .collect();
            let brute_x: BTreeSet<i32> = blocks
                .iter()
                .filter(|b| b.expand_x(1).spans_x(x))
                .map(|b| b.top_left.y.raw())
                .collect();
            assert_eq!(from_index_x, brute_x, "spanning_x mismatch at x={c}");
        }
    }

    macro_rules! hseg {
        (y=$y:expr, [$(($start:expr => $end:expr, $cost:expr)),* $(,)?]) => {
            BTreeMap::from([(
                CoordY::from($y),
                vec![
                    $(HSegment {
                        start: CoordX::from($start),
                        end: CoordX::from($end),
                        cost: $cost.into(),
                    }),*
                ]
            )])
        };
    }

    macro_rules! vseg {
        (x=$x:expr, [$(($start:expr => $end:expr, $cost:expr)),* $(,)?]) => {
            BTreeMap::from([(
                CoordX::from($x),
                vec![
                    $(VSegment {
                        start: CoordY::from($start),
                        end: CoordY::from($end),
                        cost: $cost.into(),
                    }),*
                ]
            )])
        };
    }

    // Brute force algorithm
    fn collect_intersections_brute_force(
        h_segments: impl IntoIterator<Item = (CoordY, HSegment)>,
        v_segments: impl IntoIterator<Item = (CoordX, VSegment)>,
    ) -> Vec<Point> {
        let mut points = vec![];
        let v_segments = v_segments.into_iter().collect::<Vec<_>>();
        for (y, hseg) in h_segments {
            for (x, vseg) in &v_segments {
                if hseg.start <= *x && hseg.end >= *x && vseg.start <= y && vseg.end >= y {
                    points.push(point(*x, y));
                }
            }
        }
        points
    }

    #[test]
    fn test_vseed() {
        let mut router = RouterNGBuilder::default().build();
        router.seed_vert_channel(point(0, 0), 1.0);
        router.update();
        assert_eq!(
            router.v_segments,
            BTreeMap::from([(CoordX::from(0), vec![vseg(NEG_INFINITY_Y, INFINITY_Y, 1.0)])])
        );
    }

    #[test]
    fn test_normalize() {
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.add_horiz_segment(0, 5, 15, 2.0);
        router.update();
        assert_eq!(
            router.h_segments,
            hseg!(y=0, [(0=>5, 1.0), (5=>10, 3.0), (10=>15, 2.0)])
        );
    }

    #[test]
    fn test_normalize_complete_overlap() {
        // One segment completely contains another
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 20, 1.0);
        router.add_horiz_segment(0, 5, 15, 2.0);
        router.update();
        assert_eq!(
            router.h_segments,
            hseg!(y=0, [(0=>5, 1.0), (5=>15, 3.0), (15=>20, 1.0)])
        );
    }

    #[test]
    fn test_normalize_no_overlap() {
        // Segments don't overlap at all
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.add_horiz_segment(0, 20, 30, 2.0);
        router.update();
        assert_eq!(router.h_segments, hseg!(y=0, [(0=>10, 1.0), (20=>30, 2.0)]));
    }

    #[test]
    fn test_normalize_adjacent_segments() {
        // Segments touch at endpoints but don't overlap
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.add_horiz_segment(0, 10, 20, 2.0);
        router.update();
        assert_eq!(router.h_segments, hseg!(y=0, [(0=>10, 1.0), (10=>20, 2.0)]));
    }

    #[test]
    fn test_normalize_triple_overlap() {
        // Three segments with various overlaps
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 15, 1.0);
        router.add_horiz_segment(0, 5, 20, 2.0);
        router.add_horiz_segment(0, 10, 25, 3.0);
        router.update();
        assert_eq!(
            router.h_segments,
            hseg!(y=0, [(0=>5, 1.0), (5=>10, 3.0), (10=>15, 6.0), (15=>20, 5.0), (20=>25, 3.0)])
        );
    }

    #[test]
    fn test_normalize_multiple_rows() {
        // Segments on different rows should be handled independently
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.add_horiz_segment(0, 5, 15, 2.0);
        router.add_horiz_segment(5, 0, 10, 3.0);
        router.add_horiz_segment(5, 5, 15, 4.0);
        router.update();

        let mut expected = BTreeMap::new();
        expected.extend(hseg!(y=0, [(0=>5, 1.0), (5=>10, 3.0), (10=>15, 2.0)]));
        expected.extend(hseg!(y=5, [(0=>5, 3.0), (5=>10, 7.0), (10=>15, 4.0)]));
        assert_eq!(router.h_segments, expected);
    }

    #[test]
    fn test_normalize_negative_coords() {
        // Segments in negative coordinate space
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(-5, -20, -10, 1.0);
        router.add_horiz_segment(-5, -15, -5, 2.0);
        router.update();
        assert_eq!(
            router.h_segments,
            hseg!(y=-5, [(-20 => -15, 1.0), (-15 => -10, 3.0), (-10 => -5, 2.0)])
        );
    }

    #[test]
    fn test_normalize_vertical_segments() {
        // Test vertical segment normalization
        let mut router = RouterNGBuilder::default().build();
        router.add_vert_segment(0, 0, 10, 1.0);
        router.add_vert_segment(0, 5, 15, 2.0);
        router.update();
        assert_eq!(
            router.v_segments,
            vseg!(x=0, [(0=>5, 1.0), (5=>10, 3.0), (10=>15, 2.0)])
        );
    }

    #[test]
    fn test_normalize_identical_segments() {
        // Same segment added twice
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.add_horiz_segment(0, 0, 10, 1.0);
        router.update();
        assert_eq!(router.h_segments, hseg!(y=0, [(0=>10, 2.0)]));
    }

    #[test]
    fn test_normalize_reverse_order() {
        // Segments added in decreasing coordinate order
        let mut router = RouterNGBuilder::default().build();
        router.add_horiz_segment(0, 20, 30, 1.0);
        router.add_horiz_segment(0, 10, 25, 2.0);
        router.add_horiz_segment(0, 0, 15, 3.0);
        router.update();
        assert_eq!(
            router.h_segments,
            hseg!(y=0, [(0=>10, 3.0), (10=>15, 5.0), (15=>20, 2.0), (20=>25, 3.0), (25=>30, 1.0)])
        );
    }

    // Tests for collect_intersections function

    #[test]
    fn test_collect_intersections_no_intersections() {
        // Horizontal segments with no vertical intersections
        let h_segs = vec![
            (CoordY::from(0), hseg(0, 10, 1.0)),
            (CoordY::from(5), hseg(15, 25, 1.0)),
        ];
        // Vertical segment outside the y-range of all horizontal segments
        let v_segs = vec![(CoordX::from(20), vseg(10, 15, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(intersections, BTreeSet::new());
    }

    #[test]
    fn test_collect_intersections_at_boundaries() {
        // Intersections at segment boundaries (start/end)
        let h_segs = vec![(CoordY::from(5), hseg(0, 10, 1.0))];
        // Vertical segment at the start of horizontal segment
        let v_segs_start = vec![(CoordX::from(0), vseg(0, 10, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs_start);
        assert_eq!(intersections, BTreeSet::from([point(0, 5)]));

        // Vertical segment at the end of horizontal segment
        let v_segs_end = vec![(CoordX::from(10), vseg(0, 10, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs_end);
        assert_eq!(intersections, BTreeSet::from([point(10, 5)]));

        // Vertical segment spanning from horizontal's y-coordinate exactly
        let v_segs_y_start = vec![(CoordX::from(5), vseg(5, 15, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs_y_start);
        assert_eq!(intersections, BTreeSet::from([point(5, 5)]));

        // Vertical segment ending at horizontal's y-coordinate exactly
        let v_segs_y_end = vec![(CoordX::from(5), vseg(0, 5, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs_y_end);
        assert_eq!(intersections, BTreeSet::from([point(5, 5)]));
    }

    #[test]
    fn test_collect_intersections_corners() {
        // Corner case: vertical start == horizontal start
        let h_segs = vec![(CoordY::from(5), hseg(10, 20, 1.0))];
        let v_segs = vec![(CoordX::from(10), vseg(5, 15, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(intersections, BTreeSet::from([point(10, 5)]));
    }

    #[test]
    fn test_collect_intersections_corner_all_endpoints() {
        // All four corners: (h.start, v.start), (h.start, v.end), (h.end, v.start), (h.end, v.end)
        let h_seg_y = CoordY::from(10);
        let h_segs = vec![(h_seg_y, hseg(5, 15, 1.0))];

        // Test (h.start, v.start) corner
        let v_segs = vec![(CoordX::from(5), vseg(10, 20, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs);
        assert_eq!(intersections, BTreeSet::from([point(5, 10)]));

        // Test (h.start, v.end) corner
        let v_segs = vec![(CoordX::from(5), vseg(0, 10, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs);
        assert_eq!(intersections, BTreeSet::from([point(5, 10)]));

        // Test (h.end, v.start) corner
        let v_segs = vec![(CoordX::from(15), vseg(10, 20, 1.0))];
        let intersections = collect_intersections(h_segs.clone(), v_segs);
        assert_eq!(intersections, BTreeSet::from([point(15, 10)]));

        // Test (h.end, v.end) corner
        let v_segs = vec![(CoordX::from(15), vseg(0, 10, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(intersections, BTreeSet::from([point(15, 10)]));
    }

    #[test]
    fn test_collect_intersections_multiple() {
        // Multiple intersections from a single vertical segment crossing multiple horizontal segments
        let h_segs = vec![
            (CoordY::from(5), hseg(0, 20, 1.0)),
            (CoordY::from(10), hseg(0, 20, 1.0)),
            (CoordY::from(15), hseg(0, 20, 1.0)),
        ];
        let v_segs = vec![(CoordX::from(10), vseg(0, 20, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(
            intersections,
            BTreeSet::from([point(10, 5), point(10, 10), point(10, 15)])
        );
    }

    #[test]
    fn test_collect_intersections_multiple_verticals() {
        // Single horizontal segment crossing multiple vertical segments
        let h_segs = vec![(CoordY::from(10), hseg(0, 30, 1.0))];
        let v_segs = vec![
            (CoordX::from(5), vseg(5, 15, 1.0)),
            (CoordX::from(15), vseg(5, 15, 1.0)),
            (CoordX::from(25), vseg(5, 15, 1.0)),
        ];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(
            intersections,
            BTreeSet::from([point(5, 10), point(15, 10), point(25, 10)])
        );
    }

    #[test]
    fn test_collect_intersections_vertical_outside_horizontal_y_range() {
        // Vertical segment exists in x-range of horizontal but y is outside
        let h_segs = vec![(CoordY::from(10), hseg(0, 20, 1.0))];
        let v_segs = vec![(CoordX::from(10), vseg(15, 25, 1.0))];
        let intersections = collect_intersections(h_segs, v_segs);
        assert_eq!(intersections, BTreeSet::new());
    }

    #[test]
    fn test_collect_intersections_empty_inputs() {
        // Empty horizontal segments
        let intersections =
            collect_intersections(vec![], vec![(CoordX::from(5), vseg(0, 10, 1.0))]);
        assert_eq!(intersections, BTreeSet::new());

        // Empty vertical segments
        let intersections =
            collect_intersections(vec![(CoordY::from(5), hseg(0, 10, 1.0))], vec![]);
        assert_eq!(intersections, BTreeSet::new());

        // Both empty
        let intersections: BTreeSet<Point> = collect_intersections(
            Vec::<(CoordY, HSegment)>::new(),
            Vec::<(CoordX, VSegment)>::new(),
        );
        assert_eq!(intersections, BTreeSet::new());
    }

    #[test]
    fn test_random_segments_line_sweep_matches_brute_force() {
        use rand::rngs::StdRng;
        use rand::{Rng, SeedableRng};

        const NUM_H_SEGMENTS: usize = 1000;
        const NUM_V_SEGMENTS: usize = 1000;
        const FIELD_SIZE: i32 = 200;

        // Use a fixed seed for reproducibility
        let mut rng = StdRng::seed_from_u64(42);

        let mut router = RouterNGBuilder::default().build();

        // Generate random horizontal segments
        for _ in 0..NUM_H_SEGMENTS {
            let y = rng.random_range(0..FIELD_SIZE);
            let x1 = rng.random_range(0..FIELD_SIZE);
            let x2 = rng.random_range(0..FIELD_SIZE);
            let (start, end) = if x1 <= x2 { (x1, x2) } else { (x2, x1) };
            // Ensure non-zero length segments
            if start < end {
                router.add_horiz_segment(y, start, end, rng.random_range(0.1..10.0));
            }
        }

        // Generate random vertical segments
        for _ in 0..NUM_V_SEGMENTS {
            let x = rng.random_range(0..FIELD_SIZE);
            let y1 = rng.random_range(0..FIELD_SIZE);
            let y2 = rng.random_range(0..FIELD_SIZE);
            let (start, end) = if y1 <= y2 { (y1, y2) } else { (y2, y1) };
            // Ensure non-zero length segments
            if start < end {
                router.add_vert_segment(x, start, end, rng.random_range(0.1..10.0));
            }
        }

        // Normalize segments (merge overlapping segments)
        let normalize_start = std::time::Instant::now();
        router.update();
        let normalize_time = normalize_start.elapsed();

        println!("Normalization took: {normalize_time:?}");
        println!(
            "Normalized to {} horizontal segments and {} vertical segments",
            router
                .h_segments
                .values()
                .map(std::vec::Vec::len)
                .sum::<usize>(),
            router
                .v_segments
                .values()
                .map(std::vec::Vec::len)
                .sum::<usize>()
        );

        // Collect intersections using line-sweep algorithm
        let line_sweep_start = std::time::Instant::now();
        let line_sweep_intersections =
            collect_intersections(router.iter_hsegs(), router.iter_vsegs());
        let line_sweep_time = line_sweep_start.elapsed();

        println!("Line-sweep algorithm took: {line_sweep_time:?}");
        println!("Found {} intersections", line_sweep_intersections.len());

        // Collect intersections using brute force algorithm
        let brute_force_start = std::time::Instant::now();
        let brute_force_intersections: BTreeSet<Point> =
            collect_intersections_brute_force(router.iter_hsegs(), router.iter_vsegs())
                .into_iter()
                .collect();
        let brute_force_time = brute_force_start.elapsed();

        println!("Brute-force algorithm took: {brute_force_time:?}");

        let speedup = brute_force_time.as_secs_f64() / line_sweep_time.as_secs_f64();
        println!("Line-sweep is {speedup:.2}x faster than brute-force");

        // Compare results
        assert_eq!(
            line_sweep_intersections.len(),
            brute_force_intersections.len(),
            "Number of intersections differs: line-sweep found {}, brute-force found {}",
            line_sweep_intersections.len(),
            brute_force_intersections.len()
        );

        assert_eq!(
            line_sweep_intersections, brute_force_intersections,
            "Intersection sets differ between line-sweep and brute-force algorithms"
        );
    }

    #[test]
    fn incoming_direction_prevents_double_back_out_of_waypoint() {
        // Four free channels (no blocks) form a 2x2 grid with corners at
        // (0,0), (10,0), (0,-10), (10,-10). The target sits north-east of the
        // start, reachable equally cheaply by "east then north" or "north then
        // east" — the tie is broken only by the incoming direction.
        let mut router = RouterNGBuilder::default().build();
        router.seed_channels(point(0, 0), 0.0);
        router.seed_channels(point(10, -10), 0.0);

        // Arriving at the start heading south, the route must turn east first:
        // leaving north would reverse back over the incoming wire (the kink) and
        // is forbidden by the reversal penalty.
        let (path, outgoing) =
            router.path_find_with_fallback(point(0, 0), point(10, -10), Some(Direction::South));

        assert!(path.len() >= 2, "expected a real path, got {path:?}");
        assert!(
            path[1].x > path[0].x,
            "router doubled back instead of turning east first: {path:?}"
        );
        // Its final segment climbs north into the target — the direction threaded
        // to the next segment.
        assert_eq!(outgoing, Some(Direction::North));
    }
}