//! Advanced pathfinding algorithms.
//!
//! Ported directly from RustworkxCore and classical algorithm sources,
//! adapted to work with NetworKit's Graph structure (zero conversion overhead).
//!
//! Algorithms:
//! - A*: Heuristic shortest path (ported from RustworkxCore)
//! - Bellman-Ford: Shortest path with negative weights (ported from classical)
//! - Floyd-Warshall: All-pairs shortest path (ported from classical)
//! - All-pairs Dijkstra: All-pairs shortest paths using Dijkstra per source

use super::super::graph::{Graph, NodeId};
use std::collections::{BinaryHeap, HashMap, HashSet};
use std::cmp::Ordering;

/// Error type for Bellman-Ford: indicates a negative cycle was detected.
#[derive(Debug, Clone, PartialEq)]
pub struct NegativeCycleError;

// ─────────────────────────────────────────────────────────────────────────────
// Helper types

/// State for priority queue in path algorithms.
#[derive(Copy, Clone)]
struct State {
    cost: f64,
    node: NodeId,
}

impl Ord for State {
    fn cmp(&self, other: &Self) -> Ordering {
        // Min-heap: smaller cost = higher priority
        other.cost.partial_cmp(&self.cost).unwrap_or(Ordering::Equal)
    }
}

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

impl PartialEq for State {
    fn eq(&self, other: &Self) -> bool {
        self.cost == other.cost && self.node == other.node
    }
}

impl Eq for State {}

// ─────────────────────────────────────────────────────────────────────────────
// A*

/// A* shortest path algorithm.
///
/// Finds the shortest path from `start` to `goal` using a heuristic function to guide
/// exploration toward the goal. Faster than Dijkstra when a good heuristic is available.
///
/// Ported from RustworkxCore:
/// <https://github.com/Qiskit/rustworkx/blob/main/rustworkx-core/src/shortest_path/astar.rs>
///
/// # Arguments
/// * `graph` - The graph to search
/// * `start` - Starting node ID
/// * `goal` - Goal node ID
/// * `heuristic` - Function estimating cost from any node to the goal (must be admissible)
///
/// # Returns
/// `Some((cost, path))` if a path exists, `None` if unreachable
///
/// # Example
/// ```ignore
/// let path = astar(&graph, start, goal, |n| 0.0); // 0-heuristic = Dijkstra
/// ```
pub fn astar<F>(
    graph: &Graph,
    start: NodeId,
    goal: NodeId,
    heuristic: F,
) -> Option<(f64, Vec<NodeId>)>
where
    F: Fn(NodeId) -> f64,
{
    if !graph.has_node(start) || !graph.has_node(goal) {
        return None;
    }

    if start == goal {
        return Some((0.0, vec![start]));
    }

    // g_score: cost from start to node
    let mut g_score: HashMap<NodeId, f64> = HashMap::new();
    g_score.insert(start, 0.0);

    // came_from: parent in path
    let mut came_from: HashMap<NodeId, NodeId> = HashMap::new();

    // f_score: g_score + heuristic (used for priority)
    let mut open_set = BinaryHeap::new();
    open_set.push(State { cost: heuristic(start), node: start });

    let mut closed_set: HashSet<NodeId> = HashSet::new();

    while let Some(State { node: current, .. }) = open_set.pop() {
        if current == goal {
            // Reconstruct path
            let mut path = vec![goal];
            let mut curr = goal;
            while let Some(&parent) = came_from.get(&curr) {
                path.push(parent);
                curr = parent;
            }
            path.reverse();
            return Some((*g_score.get(&goal).unwrap_or(&0.0), path));
        }

        if closed_set.contains(&current) {
            continue;
        }
        closed_set.insert(current);

        for neighbor in graph.out_neighbors(current) {
            if closed_set.contains(&neighbor.target) {
                continue;
            }

            let edge_cost = neighbor.weight.unwrap_or(1.0);
            let tentative_g = g_score.get(&current).copied().unwrap_or(f64::INFINITY) + edge_cost;

            if tentative_g < g_score.get(&neighbor.target).copied().unwrap_or(f64::INFINITY) {
                came_from.insert(neighbor.target, current);
                g_score.insert(neighbor.target, tentative_g);
                let f = tentative_g + heuristic(neighbor.target);
                open_set.push(State { cost: f, node: neighbor.target });
            }
        }
    }

    None // No path found
}

// ─────────────────────────────────────────────────────────────────────────────
// Bellman-Ford

/// Bellman-Ford shortest path algorithm.
///
/// Computes shortest paths from `source` to all reachable nodes, handling negative
/// edge weights. Also detects negative cycles.
///
/// Time complexity: O(V * E)
///
/// Ported from classical algorithm, adapted to NetworKit graph structure.
///
/// # Returns
/// `Ok(HashMap<NodeId, f64>)` with shortest distances if no negative cycle exists,
/// `Err(NegativeCycleError)` if a negative cycle is detected.
///
/// # Example
/// ```ignore
/// match bellman_ford(&graph, start) {
///     Ok(distances) => println!("Distances: {:?}", distances),
///     Err(_) => println!("Negative cycle detected!"),
/// }
/// ```
pub fn bellman_ford(graph: &Graph, source: NodeId) -> Result<HashMap<NodeId, f64>, NegativeCycleError> {
    let mut dist: HashMap<NodeId, f64> = HashMap::new();
    let mut prev: HashMap<NodeId, NodeId> = HashMap::new();

    // Initialize: all nodes at infinity, source at 0
    for node in graph.nodes() {
        dist.insert(node, f64::INFINITY);
    }
    dist.insert(source, 0.0);

    let node_count = graph.node_count();

    // Relax edges (V-1) times
    for _ in 0..node_count.saturating_sub(1) {
        let mut changed = false;
        for (u, v, weight, _) in graph.edges() {
            let w = weight.unwrap_or(1.0);
            let dist_u = dist.get(&u).copied().unwrap_or(f64::INFINITY);
            let dist_v = dist.get(&v).copied().unwrap_or(f64::INFINITY);

            if dist_u != f64::INFINITY && dist_u + w < dist_v {
                dist.insert(v, dist_u + w);
                prev.insert(v, u);
                changed = true;
            }
        }
        if !changed {
            break; // Converged early
        }
    }

    // Check for negative cycles (one more relaxation pass)
    for (u, v, weight, _) in graph.edges() {
        let w = weight.unwrap_or(1.0);
        let dist_u = dist.get(&u).copied().unwrap_or(f64::INFINITY);
        let dist_v = dist.get(&v).copied().unwrap_or(f64::INFINITY);

        if dist_u != f64::INFINITY && dist_u + w < dist_v {
            return Err(NegativeCycleError); // Negative cycle detected
        }
    }

    Ok(dist)
}

/// Bellman-Ford with path reconstruction.
///
/// Returns shortest path from source to target, or None if unreachable.
pub fn bellman_ford_path(
    graph: &Graph,
    source: NodeId,
    target: NodeId,
) -> Option<(f64, Vec<NodeId>)> {
    let mut dist: HashMap<NodeId, f64> = HashMap::new();
    let mut prev: HashMap<NodeId, NodeId> = HashMap::new();

    for node in graph.nodes() {
        dist.insert(node, f64::INFINITY);
    }
    dist.insert(source, 0.0);

    let node_count = graph.node_count();

    for _ in 0..node_count.saturating_sub(1) {
        let mut changed = false;
        for (u, v, weight, _) in graph.edges() {
            let w = weight.unwrap_or(1.0);
            let dist_u = dist.get(&u).copied().unwrap_or(f64::INFINITY);
            let dist_v = dist.get(&v).copied().unwrap_or(f64::INFINITY);

            if dist_u != f64::INFINITY && dist_u + w < dist_v {
                dist.insert(v, dist_u + w);
                prev.insert(v, u);
                changed = true;
            }
        }
        if !changed {
            break;
        }
    }

    let cost = *dist.get(&target)?;
    if cost == f64::INFINITY {
        return None;
    }

    // Reconstruct path
    let mut path = vec![target];
    let mut current = target;
    while let Some(&p) = prev.get(&current) {
        path.push(p);
        current = p;
        if current == source {
            break;
        }
    }
    path.reverse();

    Some((cost, path))
}

// ─────────────────────────────────────────────────────────────────────────────
// Floyd-Warshall

/// Floyd-Warshall all-pairs shortest path algorithm.
///
/// Computes shortest paths between all pairs of nodes.
/// Handles negative edge weights but not negative cycles.
///
/// Time complexity: O(V³)
/// Space complexity: O(V²)
///
/// Ported from classical algorithm, adapted to NetworKit graph structure.
///
/// # Returns
/// A map from (source, target) pair to shortest distance.
/// Returns `f64::INFINITY` for unreachable pairs.
///
/// # Example
/// ```ignore
/// let distances = floyd_warshall(&graph);
/// let dist_1_to_5 = distances.get(&(1, 5)).copied().unwrap_or(f64::INFINITY);
/// ```
pub fn floyd_warshall(graph: &Graph) -> HashMap<(NodeId, NodeId), f64> {
    let nodes: Vec<NodeId> = graph.nodes().collect();
    let n = nodes.len();

    // Index mapping for matrix access
    let idx: HashMap<NodeId, usize> = nodes.iter().enumerate().map(|(i, &id)| (id, i)).collect();

    // Initialize distance matrix
    let mut dist = vec![vec![f64::INFINITY; n]; n];

    // Distance from node to itself is 0
    for (i, row) in dist.iter_mut().enumerate() {
        row[i] = 0.0;
    }

    // Set edge weights
    for (u, v, weight, _) in graph.edges() {
        if let (Some(&ui), Some(&vi)) = (idx.get(&u), idx.get(&v)) {
            let w = weight.unwrap_or(1.0);
            // Take minimum if multiple edges exist between same nodes
            if w < dist[ui][vi] {
                dist[ui][vi] = w;
            }
            // For undirected graphs, set both directions
            if !graph.has_node(v) || !graph.has_node(u) {
                continue;
            }
            // Check if graph is undirected by seeing if reverse edge exists
            // (NetworKit undirected stores both ways, directed stores only one)
        }
    }

    // Main Floyd-Warshall relaxation
    for k in 0..n {
        for i in 0..n {
            if dist[i][k] == f64::INFINITY {
                continue; // Skip if no path through k
            }
            for j in 0..n {
                let through_k = dist[i][k] + dist[k][j];
                if through_k < dist[i][j] {
                    dist[i][j] = through_k;
                }
            }
        }
    }

    // Convert back to HashMap
    let mut result = HashMap::new();
    for i in 0..n {
        for j in 0..n {
            result.insert((nodes[i], nodes[j]), dist[i][j]);
        }
    }

    result
}

// ─────────────────────────────────────────────────────────────────────────────
// All-Pairs Dijkstra

/// All-pairs shortest path using Dijkstra from each source.
///
/// More efficient than Floyd-Warshall for sparse graphs with non-negative weights.
///
/// Time complexity: O(V * (E + V) log V) for sparse graphs
///
/// # Returns
/// A nested map: source → target → shortest distance
///
/// # Example
/// ```ignore
/// let all_paths = all_pairs_dijkstra(&graph);
/// let dist_1_to_5 = all_paths.get(&1).and_then(|d| d.get(&5)).copied().unwrap_or(f64::INFINITY);
/// ```
pub fn all_pairs_dijkstra(graph: &Graph) -> HashMap<NodeId, HashMap<NodeId, f64>> {
    graph.nodes()
        .map(|source| {
            let distances = dijkstra_from_source(graph, source);
            (source, distances)
        })
        .collect()
}

/// Dijkstra from a single source, returning distances to all reachable nodes.
fn dijkstra_from_source(graph: &Graph, source: NodeId) -> HashMap<NodeId, f64> {
    let mut dist: HashMap<NodeId, f64> = HashMap::new();
    let mut heap = BinaryHeap::new();

    dist.insert(source, 0.0);
    heap.push(State { cost: 0.0, node: source });

    while let Some(State { cost, node }) = heap.pop() {
        if cost > dist.get(&node).copied().unwrap_or(f64::INFINITY) {
            continue; // Already found a shorter path
        }

        for neighbor in graph.out_neighbors(node) {
            let next_cost = cost + neighbor.weight.unwrap_or(1.0);
            let curr_dist = dist.get(&neighbor.target).copied().unwrap_or(f64::INFINITY);

            if next_cost < curr_dist {
                dist.insert(neighbor.target, next_cost);
                heap.push(State { cost: next_cost, node: neighbor.target });
            }
        }
    }

    dist
}

// ─────────────────────────────────────────────────────────────────────────────
// Negative Cycle Detection

/// Detect if a graph contains any negative weight cycle.
///
/// Uses Bellman-Ford's cycle detection mechanism.
///
/// # Returns
/// `true` if a negative cycle exists, `false` otherwise
pub fn has_negative_cycle(graph: &Graph) -> bool {
    // Run Bellman-Ford from an arbitrary source (add virtual source connected to all nodes)
    for source in graph.nodes() {
        if bellman_ford(graph, source).is_err() {
            return true;
        }
    }
    false
}

// ─────────────────────────────────────────────────────────────────────────────
// Tests

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::super::graph::GraphConfig;

    fn make_directed_weighted() -> (Graph, NodeId, NodeId, NodeId, NodeId) {
        let mut g = Graph::new(GraphConfig::directed().with_edge_index());
        let a = g.add_node();
        let b = g.add_node();
        let c = g.add_node();
        let d = g.add_node();
        // a -1-> b -2-> d
        // a -4-> c -1-> d
        g.add_edge(a, b, Some(1.0));
        g.add_edge(b, d, Some(2.0));
        g.add_edge(a, c, Some(4.0));
        g.add_edge(c, d, Some(1.0));
        (g, a, b, c, d)
    }

    #[test]
    fn test_astar_direct_path() {
        let (g, a, _b, _c, d) = make_directed_weighted();
        // A* with zero heuristic = Dijkstra
        let result = astar(&g, a, d, |_| 0.0);
        assert!(result.is_some());
        let (cost, path) = result.unwrap();
        assert_eq!(cost, 3.0); // a→b→d costs 1+2=3
        assert_eq!(path.first(), Some(&a));
        assert_eq!(path.last(), Some(&d));
    }

    #[test]
    fn test_astar_same_start_goal() {
        let (g, a, _, _, _) = make_directed_weighted();
        let result = astar(&g, a, a, |_| 0.0);
        assert!(result.is_some());
        assert_eq!(result.unwrap().1, vec![a]);
    }

    #[test]
    fn test_astar_no_path() {
        let mut g = Graph::new(GraphConfig::directed());
        let a = g.add_node();
        let b = g.add_node();
        // No edge - no path
        let result = astar(&g, a, b, |_| 0.0);
        assert!(result.is_none());
    }

    #[test]
    fn test_bellman_ford_basic() {
        let (g, a, _b, _c, d) = make_directed_weighted();
        let result = bellman_ford(&g, a);
        assert!(result.is_ok());
        let dist = result.unwrap();
        assert_eq!(dist[&a], 0.0);
        assert_eq!(dist[&d], 3.0); // a→b→d
    }

    #[test]
    fn test_bellman_ford_negative_edge() {
        let mut g = Graph::new(GraphConfig::directed().with_edge_index());
        let a = g.add_node();
        let b = g.add_node();
        let c = g.add_node();
        g.add_edge(a, b, Some(4.0));
        g.add_edge(b, c, Some(-2.0)); // Negative edge, but no negative cycle
        g.add_edge(a, c, Some(5.0));

        let result = bellman_ford(&g, a);
        assert!(result.is_ok());
        let dist = result.unwrap();
        assert_eq!(dist[&c], 2.0); // a→b→c = 4 + (-2) = 2
    }

    #[test]
    fn test_bellman_ford_negative_cycle() {
        let mut g = Graph::new(GraphConfig::directed().with_edge_index());
        let a = g.add_node();
        let b = g.add_node();
        g.add_edge(a, b, Some(1.0));
        g.add_edge(b, a, Some(-5.0)); // Negative cycle!
        assert!(bellman_ford(&g, a).is_err());
    }

    #[test]
    fn test_floyd_warshall() {
        let (g, a, b, _c, d) = make_directed_weighted();
        let distances = floyd_warshall(&g);
        assert_eq!(distances[&(a, d)], 3.0); // a→b→d
        assert_eq!(distances[&(a, a)], 0.0);
        assert_eq!(distances[&(a, b)], 1.0);
        assert_eq!(distances[&(b, a)], f64::INFINITY); // Directed graph, no reverse
    }

    #[test]
    fn test_all_pairs_dijkstra() {
        let (g, a, b, _c, d) = make_directed_weighted();
        let all = all_pairs_dijkstra(&g);
        assert_eq!(all[&a][&d], 3.0);
        assert_eq!(all[&a][&b], 1.0);
        assert_eq!(all[&b][&d], 2.0);
    }
}
