condor-pathfinding-grid 0.4.0

//! Flat/cone interval successors, turn roots, goal candidates, and dominance.
//!
//! Expansion side effects land in [`SuccessorContext`]: arena allocation, dominance
//! map updates, diagnostics, incumbent goal connection, and pending heap pushes.
//! Successor completeness for the interval candidate is still under evaluation
//! (see private [`super::row_interval`] stub).

use std::cell::Cell;
use std::collections::HashMap;

use crate::Grid;
use condor_core::Point2;

use super::{
    diagnostics::AnyaDiagnostics,
    geometry::{
        IntervalKind, best_interval_goal_candidate, interval_state_key, is_turn_candidate,
        project_cone_to_row, segment_legal, visible_flat_span_from_corner,
    },
    runs::{RowRun, RowRunIndex},
    state::{HeapEntry, IntervalState, StateArena, StateId, heap_entry_for},
};

#[derive(Debug, Clone, Copy, PartialEq)]
struct IntervalSpan {
    left: f64,
    right: f64,
}

/// Closed-set key for interval states: same root/row/kind/span collapses duplicates.
#[derive(Clone, Hash, PartialEq, Eq)]
pub(crate) struct DominanceKey {
    row: i32,
    root_x: i32,
    root_y: i32,
    kind: IntervalKind,
    left: u64,
    right: u64,
}

/// The terminal edge that produced the incumbent goal cost.
///
/// Keeping this is essential: re-deriving a convenient interval probe during
/// reconstruction can produce a different path from the one whose cost won.
#[derive(Debug, Clone, Copy)]
pub(crate) struct GoalConnection {
    pub terminal_state: StateId,
    pub via: Option<Point2>,
}

/// Mutable workspace shared while expanding one interval state.
///
/// Owns the open-list side effects of expansion: arena allocation, dominance map
/// updates, diagnostics, incumbent goal connection, and pending heap pushes.
pub struct SuccessorContext<'a> {
    pub grid: &'a Grid,
    pub runs: &'a RowRunIndex,
    pub goal: Point2,
    pub arena: &'a mut StateArena,
    pub best_by_interval: &'a mut HashMap<DominanceKey, (f64, StateId)>,
    pub diagnostics: &'a mut AnyaDiagnostics,
    pub best_goal_cost: &'a Cell<f64>,
    pub goal_connection: &'a Cell<Option<GoalConnection>>,
    pub pending_heap: &'a mut Vec<HeapEntry>,
}

/// Expands one interval: goal probes, adjacent-row projections, and turn roots.
///
/// Side effects land in `ctx` (diagnostics, heap, dominance map, goal incumbent).
pub fn expand_state(ctx: &mut SuccessorContext<'_>, state_id: StateId, state: IntervalState) {
    ctx.diagnostics.expanded += 1;
    try_goal_on_interval(ctx, state_id, &state);
    try_goal_via_interval_point(ctx, state_id, &state);
    try_goal_from_root(ctx, state_id, &state);

    let row = state.row;
    for delta in [-1, 1] {
        let target_row = row + delta;
        if target_row < 0 || target_row as usize > ctx.runs.height() {
            continue;
        }
        project_to_row(ctx, state_id, &state, target_row);
    }

    emit_turn_successors(ctx, state_id, &state);
}

fn try_goal_on_interval(ctx: &mut SuccessorContext<'_>, state_id: StateId, state: &IntervalState) {
    let goal = ctx.goal;
    let goal_row = goal.y.round() as i32;
    if goal_row != state.row {
        return;
    }
    if goal.x + 1e-12 < state.left || goal.x > state.right + 1e-12 {
        return;
    }
    if !segment_legal(ctx.grid, state.root, goal) {
        return;
    }
    let cost = state.root_g + state.root.distance_to(goal);
    record_goal(ctx, state_id, None, cost);
}

fn try_goal_via_interval_point(
    ctx: &mut SuccessorContext<'_>,
    state_id: StateId,
    state: &IntervalState,
) {
    let Some((probe, cost)) = best_interval_goal_candidate(
        ctx.grid,
        state.root,
        state.root_g,
        state.row as f64,
        state.left,
        state.right,
        ctx.goal,
    ) else {
        return;
    };
    if !segment_legal(ctx.grid, probe, ctx.goal) {
        return;
    }
    record_goal(ctx, state_id, Some(probe), cost);
}

fn try_goal_from_root(ctx: &mut SuccessorContext<'_>, state_id: StateId, state: &IntervalState) {
    if !segment_legal(ctx.grid, state.root, ctx.goal) {
        return;
    }
    let cost = state.root_g + state.root.distance_to(ctx.goal);
    record_goal(ctx, state_id, None, cost);
}

fn record_goal(
    ctx: &mut SuccessorContext<'_>,
    predecessor: StateId,
    via: Option<Point2>,
    cost: f64,
) {
    ctx.diagnostics.goal_candidates += 1;
    if cost + 1e-12 < ctx.best_goal_cost.get() {
        ctx.best_goal_cost.set(cost);
        ctx.goal_connection.set(Some(GoalConnection {
            terminal_state: predecessor,
            via,
        }));
        ctx.diagnostics.goal_improvements += 1;
    }
}

fn project_to_row(
    ctx: &mut SuccessorContext<'_>,
    predecessor: StateId,
    state: &IntervalState,
    target_row: i32,
) {
    let Some((cone_left, cone_right)) = project_cone_to_row(
        state.root,
        state.row as f64,
        state.left,
        state.right,
        target_row as f64,
    ) else {
        return;
    };
    ctx.diagnostics.run_projections += 1;

    for run in ctx.runs.runs_on_row(target_row) {
        let Some(span) = intersect_spans(cone_left, cone_right, run.left, run.right) else {
            continue;
        };
        push_interval(
            ctx,
            predecessor,
            IntervalState {
                row: target_row,
                left: span.left,
                right: span.right,
                root: state.root,
                root_g: state.root_g,
                predecessor: Some(predecessor),
                kind: IntervalKind::Cone,
                generation: 0,
            },
        );
        ctx.diagnostics.cone_successors += 1;
        emit_turn_corners_in_span(
            ctx,
            predecessor,
            state,
            target_row,
            span.left,
            span.right,
            TurnSpanMode::Projected,
        );
    }
}

fn emit_turn_successors(
    ctx: &mut SuccessorContext<'_>,
    predecessor: StateId,
    state: &IntervalState,
) {
    emit_turn_corners_in_span(
        ctx,
        predecessor,
        state,
        state.row,
        state.left,
        state.right,
        TurnSpanMode::Horizontal,
    );
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum TurnSpanMode {
    /// Clip to the projected cone interval on the target row.
    Projected,
    /// Walk along the row from the corner using segment legality.
    Horizontal,
}

fn emit_turn_corners_in_span(
    ctx: &mut SuccessorContext<'_>,
    predecessor: StateId,
    parent: &IntervalState,
    row: i32,
    left: f64,
    right: f64,
    span_mode: TurnSpanMode,
) {
    let span_left = left.round() as i32;
    let span_right = right.round() as i32;
    let mut corner_xs = Vec::new();
    let mut consider_corner = |vx: i32| {
        if vx < span_left || vx > span_right {
            let corner = Point2::new(vx as f64, row as f64);
            if !segment_legal(ctx.grid, parent.root, corner) {
                return;
            }
        }
        if !corner_xs.contains(&vx) {
            corner_xs.push(vx);
        }
    };
    for vx in span_left..=span_right {
        consider_corner(vx);
    }
    if span_mode == TurnSpanMode::Projected {
        for run in ctx.runs.runs_on_row(row) {
            for vx in run.left.round() as i32..=run.right.round() as i32 {
                if !is_turn_candidate(ctx.grid, vx, row) {
                    continue;
                }
                consider_corner(vx);
            }
        }
    }
    corner_xs.sort_unstable();
    corner_xs.dedup();
    for vx in corner_xs {
        if !is_turn_candidate(ctx.grid, vx, row) {
            continue;
        }
        let corner = Point2::new(vx as f64, row as f64);
        if !segment_legal(ctx.grid, parent.root, corner) {
            continue;
        }
        let new_g = parent.root_g + parent.root.distance_to(corner);
        let Some(run) = ctx.runs.run_containing(corner) else {
            continue;
        };
        let clip_left = match span_mode {
            TurnSpanMode::Projected => run.left,
            TurnSpanMode::Horizontal => parent.left.max(run.left).max(left),
        };
        let clip_right = match span_mode {
            TurnSpanMode::Projected => run.right,
            TurnSpanMode::Horizontal => parent.right.min(run.right).min(right),
        };
        let Some((flat_left, flat_right)) =
            visible_flat_span_from_corner(ctx.grid, corner, clip_left, clip_right)
        else {
            continue;
        };
        if flat_right + 1e-12 < flat_left {
            continue;
        }
        push_interval(
            ctx,
            predecessor,
            IntervalState {
                row,
                left: flat_left,
                right: flat_right,
                root: corner,
                root_g: new_g,
                predecessor: Some(predecessor),
                kind: IntervalKind::Flat,
                generation: 0,
            },
        );
        ctx.diagnostics.flat_successors += 1;
    }
}

fn intersect_spans(a0: f64, a1: f64, b0: f64, b1: f64) -> Option<IntervalSpan> {
    let left = if a0.is_finite() { a0.max(b0) } else { b0 };
    let right = if a1.is_finite() { a1.min(b1) } else { b1 };
    if right + 1e-12 < left {
        return None;
    }
    Some(IntervalSpan { left, right })
}

/// Inserts a candidate interval if it improves the dominance map, then heaps it.
///
/// Dominated or non-finite candidates update diagnostics only and are discarded.
pub fn push_interval(ctx: &mut SuccessorContext<'_>, _predecessor: StateId, state: IntervalState) {
    ctx.diagnostics.generated += 1;
    if !state.root_g.is_finite() || state.right + 1e-12 < state.left {
        return;
    }

    let key = DominanceKey {
        row: state.row,
        root_x: state.root.x.round() as i32,
        root_y: state.root.y.round() as i32,
        kind: state.kind,
        left: state.left.to_bits(),
        right: state.right.to_bits(),
    };

    if let Some((best_g, _)) = ctx.best_by_interval.get(&key)
        && *best_g <= state.root_g + 1e-12
    {
        ctx.diagnostics.dominated += 1;
        return;
    }

    let key_f = interval_state_key(
        ctx.grid,
        state.root,
        state.root_g,
        state.row as f64,
        state.left,
        state.right,
        ctx.goal,
    );
    if !key_f.is_finite() {
        return;
    }

    let state_id = ctx.arena.push(state);
    ctx.best_by_interval.insert(key, (state.root_g, state_id));

    match state.kind {
        IntervalKind::Flat => ctx.diagnostics.flat_states += 1,
        IntervalKind::Cone => ctx.diagnostics.cone_states += 1,
    }

    ctx.diagnostics.pushed += 1;
    ctx.diagnostics.state_bytes = ctx.arena.bytes();
    ctx.pending_heap
        .push(heap_entry_for(state_id, &state, key_f));
}

/// Root flat interval covering the open run that contains the start vertex.
pub fn initial_state(start: Point2, run: RowRun) -> IntervalState {
    IntervalState {
        row: start.y.round() as i32,
        left: run.left,
        right: run.right,
        root: start,
        root_g: 0.0,
        predecessor: None,
        kind: IntervalKind::Flat,
        generation: 0,
    }
}