KiCadRoutingTools

//! Shared types and constants for the grid router.

use std::cmp::Ordering;
use rustc_hash::FxHashSet;

/// 8 directions for octilinear routing
pub const DIRECTIONS: [(i32, i32); 8] = [
    (1, 0),   // East
    (1, -1),  // NE
    (0, -1),  // North
    (-1, -1), // NW
    (-1, 0),  // West
    (-1, 1),  // SW
    (0, 1),   // South
    (1, 1),   // SE
];

pub const ORTHO_COST: i32 = 1000;
pub const DIAG_COST: i32 = 1414; // sqrt(2) * 1000
pub const DEFAULT_TURN_COST: i32 = 1000;  // Default penalty for changing direction (encourages straighter paths)

/// Pack (gx, gy) into a single u64 for fast hashing
#[inline]
pub fn pack_xy(gx: i32, gy: i32) -> u64 {
    let x = (gx as u64) & 0xFFFFFFFF;
    let y = (gy as u64) & 0xFFFFFFFF;
    (x << 32) | y
}

/// Inverse of pack_xy
#[inline]
pub fn unpack_xy(key: u64) -> (i32, i32) {
    ((key >> 32) as u32 as i32, key as u32 as i32)
}

/// Grid state: (x, y, layer) packed into a single u64 for fast hashing
#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)]
pub struct GridState {
    pub gx: i32,
    pub gy: i32,
    pub layer: u8,
}

impl GridState {
    #[inline]
    pub fn new(gx: i32, gy: i32, layer: u8) -> Self {
        Self { gx, gy, layer }
    }

    #[inline]
    pub fn as_key(&self) -> u64 {
        // Pack into u64 for fast hashing: 20 bits x, 20 bits y, 8 bits layer
        let x = (self.gx as u64) & 0xFFFFF;
        let y = (self.gy as u64) & 0xFFFFF;
        let l = self.layer as u64;
        (x << 28) | (y << 8) | l
    }
}

/// A* open set entry with reverse ordering for min-heap
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct OpenEntry {
    pub f_score: i32,
    pub g_score: i32,
    pub state: GridState,
    pub counter: u32, // Tie-breaker for deterministic ordering
}

impl Ord for OpenEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        // Reverse ordering for min-heap (lowest f_score first)
        other.f_score.cmp(&self.f_score)
            .then_with(|| other.counter.cmp(&self.counter))
    }
}

impl PartialOrd for OpenEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Pose state: (x, y, theta_idx, layer) for orientation-aware routing
#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)]
pub struct PoseState {
    pub gx: i32,
    pub gy: i32,
    pub theta_idx: u8,  // 0-7, corresponding to DIRECTIONS indices
    pub layer: u8,
}

impl PoseState {
    #[inline]
    pub fn new(gx: i32, gy: i32, theta_idx: u8, layer: u8) -> Self {
        Self { gx, gy, theta_idx, layer }
    }

    #[inline]
    pub fn as_key(&self) -> u64 {
        // Pack into u64: 19 bits x, 19 bits y, 3 bits theta, 8 bits layer
        // This allows x,y in range [-262144, 262143]
        let x = (self.gx as u64) & 0x7FFFF;
        let y = (self.gy as u64) & 0x7FFFF;
        let t = (self.theta_idx as u64) & 0x7;
        let l = self.layer as u64;
        (x << 30) | (y << 11) | (t << 8) | l
    }

    /// Get the direction vector for this pose's heading
    #[inline]
    pub fn direction(&self) -> (i32, i32) {
        DIRECTIONS[self.theta_idx as usize]
    }

    /// Get theta in radians
    #[inline]
    pub fn theta_radians(&self) -> f64 {
        (self.theta_idx as f64) * std::f64::consts::FRAC_PI_4
    }
}

/// A* open set entry for pose-based search
#[derive(Clone, Copy, Debug)]
pub struct PoseOpenEntry {
    pub f_score: i32,
    pub g_score: i32,
    pub state: PoseState,
    pub counter: u32,
}

impl Eq for PoseOpenEntry {}

impl PartialEq for PoseOpenEntry {
    fn eq(&self, other: &Self) -> bool {
        self.f_score == other.f_score && self.counter == other.counter
    }
}

impl Ord for PoseOpenEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        // Reverse ordering for min-heap (lowest f_score first)
        other.f_score.cmp(&self.f_score)
            .then_with(|| other.counter.cmp(&self.counter))
    }
}

impl PartialOrd for PoseOpenEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Tracks blocked cells encountered during A* search.
/// Used by both GridRouter and PoseRouter to report frontier on failure.
pub struct BlockedCellTracker {
    blocked: FxHashSet<u64>,
}

impl BlockedCellTracker {
    pub fn new() -> Self {
        Self {
            blocked: FxHashSet::default(),
        }
    }

    /// Track a blocked cell (using GridState key format: 20 bits x, 20 bits y, 8 bits layer)
    #[inline]
    pub fn track(&mut self, gx: i32, gy: i32, layer: u8) {
        let key = GridState::new(gx, gy, layer).as_key();
        self.blocked.insert(key);
    }

    /// Get blocked cells as (gx, gy, layer) tuples.
    /// Returns at most MAX_BLOCKED_CELLS (10000), evenly sampled for determinism.
    pub fn get_blocked(&self) -> Vec<(i32, i32, u8)> {
        const MAX_BLOCKED_CELLS: usize = 10000;

        let total = self.blocked.len();
        if total <= MAX_BLOCKED_CELLS {
            // Return all cells, sorted for determinism
            let mut keys: Vec<u64> = self.blocked.iter().copied().collect();
            keys.sort_unstable();
            keys.iter()
                .map(|&key| Self::unpack_key(key))
                .collect()
        } else {
            // Evenly sample MAX_BLOCKED_CELLS from sorted keys
            let mut keys: Vec<u64> = self.blocked.iter().copied().collect();
            keys.sort_unstable();

            // Sample every Nth element to get exactly MAX_BLOCKED_CELLS
            let step = total as f64 / MAX_BLOCKED_CELLS as f64;
            (0..MAX_BLOCKED_CELLS)
                .map(|i| {
                    let idx = (i as f64 * step) as usize;
                    Self::unpack_key(keys[idx])
                })
                .collect()
        }
    }

    /// Unpack a key back to (gx, gy, layer)
    #[inline]
    fn unpack_key(key: u64) -> (i32, i32, u8) {
        let layer = (key & 0xFF) as u8;
        let y = ((key >> 8) & 0xFFFFF) as i32;
        let x = ((key >> 28) & 0xFFFFF) as i32;
        // Sign extension for negative coordinates
        let x = if x & 0x80000 != 0 { x | !0xFFFFF_i32 } else { x };
        let y = if y & 0x80000 != 0 { y | !0xFFFFF_i32 } else { y };
        (x, y, layer)
    }

}

impl Default for BlockedCellTracker {
    fn default() -> Self {
        Self::new()
    }
}

/// Statistics from an A* search for debugging and performance analysis.
/// (C2: the never-written max_f_score stat was removed.)
#[derive(Clone, Debug, Default)]
pub struct RouteStats {
    /// Cells popped from open set and processed (not skipped as duplicates)
    pub cells_expanded: u32,
    /// Total pushes to open set (includes duplicates from path improvements)
    pub cells_pushed: u32,
    /// Times we found a better g-cost for an existing node (path improvement)
    pub cells_revisited: u32,
    /// Times we skipped a cell because it was already in closed set
    pub duplicate_skips: u32,
    /// Final path length in grid steps (0 if no path found)
    pub path_length: u32,
    /// Final g-cost at goal (0 if no path found)
    pub path_cost: i32,
    /// Initial heuristic estimate from best source to targets
    pub initial_h: i32,
    /// Closest approach: min cost-to-go (f - g) over expanded nodes (#529)
    pub best_h: i32,
    /// Dynamic-iteration tranches granted beyond the base budget (#529)
    pub iteration_tranches: u32,
    /// Final g-cost (actual cost to reach goal)
    pub final_g: i32,
    /// Size of open set at termination
    pub open_set_size: u32,
    /// Size of closed set (unique cells visited)
    pub closed_set_size: u32,
    /// Number of via transitions in path
    pub via_count: u32,
}

/// C1 (issue #387): one search core, several sinks. The grid and pose A*
/// cores are generic over this trait; each variant's side-band behavior
/// (stats counting, blocked-frontier tracking, or nothing) is a sink, and
/// monomorphization compiles the no-op hooks out entirely. This replaces the
/// four hand-forked copies of the search loop (route_multi vs
/// route_with_frontier, route_pose vs route_pose_with_frontier, plus the
/// drifted visual_router fork -- B1 in #386).
pub trait SearchSink {
    /// A node was pushed to the open set (including the initial sources).
    #[inline]
    fn on_push(&mut self) {}
    /// A popped node was expanded (not a duplicate skip).
    #[inline]
    fn on_expand(&mut self) {}
    /// A popped node was skipped because it was already closed.
    #[inline]
    fn on_duplicate_skip(&mut self) {}
    /// A relax improved an already-visited node's g.
    #[inline]
    fn on_revisit(&mut self) {}
    /// Expansion into (gx, gy, layer) was vetoed by an obstacle.
    #[inline]
    fn on_blocked(&mut self, _gx: i32, _gy: i32, _layer: u8) {}
}

/// Sink that ignores everything (route_pose, and any caller that wants the
/// bare search result).
pub struct NullSink;
impl SearchSink for NullSink {}

/// Sink that records blocked cells for the failure frontier
/// (route_with_frontier / route_pose_with_frontier rip-up analysis).
pub struct FrontierSink {
    pub tracker: BlockedCellTracker,
}

impl FrontierSink {
    pub fn new() -> Self {
        Self { tracker: BlockedCellTracker::new() }
    }
}

impl SearchSink for FrontierSink {
    #[inline]
    fn on_blocked(&mut self, gx: i32, gy: i32, layer: u8) {
        self.tracker.track(gx, gy, layer);
    }
}

/// Sink that accumulates RouteStats (route_multi and the visualizer).
#[derive(Default)]
pub struct StatsSink {
    pub stats: RouteStats,
}

impl SearchSink for StatsSink {
    #[inline]
    fn on_push(&mut self) {
        self.stats.cells_pushed += 1;
    }
    #[inline]
    fn on_expand(&mut self) {
        self.stats.cells_expanded += 1;
    }
    #[inline]
    fn on_duplicate_skip(&mut self) {
        self.stats.duplicate_skips += 1;
    }
    #[inline]
    fn on_revisit(&mut self) {
        self.stats.cells_revisited += 1;
    }
}