use best::BestMapNonEmpty;
use config::*;
use direction::*;
use distance_map::*;
use error::*;
use grid::*;
use grid_2d::*;
use metadata::*;
use num_traits::{One, Zero};
use path::{self, PathNode};
use std::cmp::Ordering;
use std::collections::BinaryHeap;
use std::ops::{Add, Sub};

#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
#[derive(Debug, Clone, Copy)]
pub(crate) struct SearchNode<Cost> {
    pub(crate) seen: u64,
    pub(crate) visited: u64,
    pub(crate) coord: Coord,
    pub(crate) from_parent: Option<Direction>,
    pub(crate) cost: Cost,
}

impl<Cost: Zero> SearchNode<Cost> {
    fn new(coord: Coord) -> Self {
        Self {
            seen: 0,
            visited: 0,
            coord,
            from_parent: None,
            cost: Zero::zero(),
        }
    }
}

impl<Cost> PathNode for SearchNode<Cost> {
    fn from_parent(&self) -> Option<Direction> {
        self.from_parent
    }
    fn coord(&self) -> Coord {
        self.coord
    }
}

#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
#[derive(Debug, Clone)]
pub(crate) struct PriorityEntry<Cost: PartialOrd<Cost>> {
    pub(crate) node_index: usize,
    pub(crate) cost: Cost,
}

impl<Cost: PartialOrd<Cost>> PriorityEntry<Cost> {
    pub(crate) fn new(node_index: usize, cost: Cost) -> Self {
        Self { node_index, cost }
    }
}

impl<Cost: PartialOrd<Cost>> PartialEq for PriorityEntry<Cost> {
    fn eq(&self, other: &Self) -> bool {
        self.cost == other.cost
    }
}

impl<Cost: PartialOrd<Cost>> PartialOrd for PriorityEntry<Cost> {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        other.cost.partial_cmp(&self.cost)
    }
}

impl<Cost: PartialOrd<Cost>> Eq for PriorityEntry<Cost> {}

impl<Cost: PartialOrd<Cost>> Ord for PriorityEntry<Cost> {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .partial_cmp(&self.cost)
            .unwrap_or(Ordering::Equal)
    }
}

#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
#[derive(Debug, Clone)]
pub struct SearchContext<Cost: PartialOrd<Cost>> {
    pub(crate) seq: u64,
    pub(crate) priority_queue: BinaryHeap<PriorityEntry<Cost>>,
    pub(crate) node_grid: Grid<SearchNode<Cost>>,
}

impl<Cost: PartialOrd<Cost> + Zero> SearchContext<Cost> {
    pub fn new(size: Size) -> Self {
        Self {
            seq: 0,
            node_grid: Grid::new_fn(size, SearchNode::new),
            priority_queue: BinaryHeap::new(),
        }
    }
    pub fn width(&self) -> u32 {
        self.node_grid.width()
    }
    pub fn height(&self) -> u32 {
        self.node_grid.height()
    }
    pub fn size(&self) -> Size {
        self.node_grid.size()
    }
}

impl<Cost: Copy + Add<Cost> + PartialOrd<Cost> + Zero> SearchContext<Cost> {
    pub(crate) fn init<G, F>(
        &mut self,
        start: Coord,
        predicate: F,
        grid: &G,
        config: SearchConfig,
        path: &mut Vec<Direction>,
    ) -> Result<PriorityEntry<Cost>, Result<SearchMetadata<Cost>, Error>>
    where
        G: SolidGrid,
        F: Fn(Coord) -> bool,
    {
        if let Some(solid) = grid.is_solid(start) {
            let index = if let Some(index) = self.node_grid.index_of_coord(start) {
                index
            } else {
                return Err(Err(Error::VisitOutsideContext));
            };

            if solid && !config.allow_solid_start {
                return Err(Err(Error::StartSolid));
            };

            if predicate(start) {
                path.clear();
                return Err(Ok(SearchMetadata {
                    num_nodes_visited: 0,
                    cost: Zero::zero(),
                    length: 0,
                }));
            }

            self.seq += 1;
            self.priority_queue.clear();

            let node = &mut self.node_grid[index];
            node.from_parent = None;
            node.seen = self.seq;
            node.cost = Zero::zero();

            Ok(PriorityEntry::new(index, Zero::zero()))
        } else {
            Err(Err(Error::StartOutsideGrid))
        }
    }

    pub(crate) fn search_general<G, V, D, H>(
        &mut self,
        grid: &G,
        start: Coord,
        goal: Coord,
        directions: D,
        heuristic_fn: H,
        config: SearchConfig,
        path: &mut Vec<Direction>,
    ) -> Result<SearchMetadata<Cost>, Error>
    where
        G: CostGrid<Cost = Cost>,
        V: Into<Direction>,
        D: Copy + IntoIterator<Item = V>,
        H: Fn(Coord, Coord) -> Cost,
    {
        let initial_entry = match self.init(start, |c| c == goal, grid, config, path) {
            Ok(initial_entry) => initial_entry,
            Err(result) => return result,
        };

        self.priority_queue.push(initial_entry);

        let goal_index = self
            .node_grid
            .index_of_coord(goal)
            .ok_or(Error::VisitOutsideContext)?;

        let mut num_nodes_visited = 0;

        while let Some(current_entry) = self.priority_queue.pop() {
            num_nodes_visited += 1;

            if current_entry.node_index == goal_index {
                let node = &self.node_grid[goal_index];

                path::make_path_all_adjacent(&self.node_grid, goal_index, path);
                return Ok(SearchMetadata {
                    num_nodes_visited,
                    cost: node.cost,
                    length: path.len(),
                });
            }

            let (current_coord, current_cost) = {
                let node = &mut self.node_grid[current_entry.node_index];
                if node.visited == self.seq {
                    continue;
                }
                node.visited = self.seq;
                (node.coord, node.cost)
            };

            for d in directions {
                let direction = d.into();
                let neighbour_coord = current_coord + direction.coord();

                let neighbour_cost =
                    if let Some(CostCell::Cost(cost)) = grid.cost(neighbour_coord, direction) {
                        cost
                    } else {
                        continue;
                    };

                self.see_successor(
                    current_cost + neighbour_cost,
                    neighbour_coord,
                    direction,
                    &heuristic_fn,
                    goal,
                )?;
            }
        }

        Err(Error::NoPath)
    }

    pub(crate) fn see_successor<H>(
        &mut self,
        cost: Cost,
        successor_coord: Coord,
        direction: Direction,
        heuristic_fn: H,
        goal: Coord,
    ) -> Result<(), Error>
    where
        H: Fn(Coord, Coord) -> Cost,
    {
        let index = self
            .node_grid
            .index_of_coord(successor_coord)
            .ok_or(Error::VisitOutsideContext)?;

        let node = &mut self.node_grid[index];

        if node.seen != self.seq || node.cost > cost {
            node.from_parent = Some(direction);
            node.seen = self.seq;
            node.cost = cost;

            let heuristic = cost + heuristic_fn(successor_coord, goal);
            let entry = PriorityEntry::new(index, heuristic);
            self.priority_queue.push(entry);
        }

        Ok(())
    }
}

impl<Cost> SearchContext<Cost>
where
    Cost: Copy + Add + PartialOrd + Zero + One,
{
    pub fn populate_distance_map<G, V, D>(
        &mut self,
        grid: &G,
        start: Coord,
        directions: D,
        config: SearchConfig,
        distance_map: &mut DistanceMap<Cost>,
    ) -> Result<DistanceMapMetadata, Error>
    where
        G: CostGrid<Cost = Cost>,
        V: Into<Direction>,
        D: Copy + IntoIterator<Item = V>,
    {
        if let Some(solid) = grid.is_solid(start) {
            if solid && !config.allow_solid_start {
                return Err(Error::StartSolid);
            };

            let index = distance_map
                .grid
                .index_of_coord(start)
                .ok_or(Error::VisitOutsideDistanceMap)?;

            self.priority_queue.clear();
            self.priority_queue
                .push(PriorityEntry::new(index, Zero::zero()));

            distance_map.seq += 1;
            distance_map.origin = start;
            let cell = &mut distance_map.grid[index];
            cell.seen = distance_map.seq;
            cell.cost = Zero::zero();
        } else {
            return Err(Error::StartOutsideGrid);
        }

        let mut num_nodes_visited = 0;

        while let Some(current_entry) = self.priority_queue.pop() {
            num_nodes_visited += 1;

            let (current_coord, current_cost) = {
                let cell = &mut distance_map.grid[current_entry.node_index];
                if cell.visited == distance_map.seq {
                    continue;
                }
                cell.visited = distance_map.seq;
                (cell.coord, cell.cost)
            };

            for d in directions {
                let direction = d.into();
                let neighbour_coord = current_coord + direction.coord();

                let neighbour_cost =
                    if let Some(CostCell::Cost(cost)) = grid.cost(neighbour_coord, direction) {
                        cost
                    } else {
                        continue;
                    };

                let cost = current_cost + neighbour_cost;

                let index = distance_map
                    .grid
                    .index_of_coord(neighbour_coord)
                    .ok_or(Error::VisitOutsideDistanceMap)?;

                let cell = &mut distance_map.grid[index];

                if cell.seen != distance_map.seq || cell.cost > cost {
                    cell.direction = direction.opposite();
                    cell.seen = distance_map.seq;
                    cell.cost = cost;

                    let entry = PriorityEntry::new(index, cost);
                    self.priority_queue.push(entry);
                }
            }
        }

        Ok(DistanceMapMetadata { num_nodes_visited })
    }
}

impl<Cost> SearchContext<Cost>
where
    Cost: Copy + Add + PartialOrd + Zero + One + Sub<Output = Cost>,
{
    pub fn best_search_uniform_distance_map<G, V, D>(
        &mut self,
        grid: &G,
        start: Coord,
        config: SearchConfig,
        max_depth: Cost,
        distance_map: &UniformDistanceMap<Cost, D>,
        path: &mut Vec<Direction>,
    ) -> Result<SearchMetadata<Cost>, Error>
    where
        G: SolidGrid,
        V: Into<Direction>,
        D: Copy + IntoIterator<Item = V>,
    {
        let mut initial_entry =
            match self.init(start, |_| max_depth == Zero::zero(), grid, config, path) {
                Ok(initial_entry) => initial_entry,
                Err(result) => return result,
            };

        initial_entry.cost = distance_map
            .cost(start)
            .ok_or(Error::InconsistentDistanceMap)?;

        let mut best_map = BestMapNonEmpty::new(initial_entry.cost, initial_entry.node_index);
        self.priority_queue.push(initial_entry);

        let mut num_nodes_visited = 0;

        while let Some(current_entry) = self.priority_queue.pop() {
            num_nodes_visited += 1;

            let (current_coord, current_depth) = {
                let node = &self.node_grid[current_entry.node_index];
                (node.coord, node.cost)
            };

            if current_depth >= max_depth {
                continue;
            }

            let remaining_depth = max_depth - current_depth;
            if *best_map.key() + remaining_depth <= current_entry.cost {
                continue;
            }

            let next_depth = current_depth + One::one();

            for v in distance_map.directions {
                let direction = v.into();
                let offset: Coord = direction.coord();
                let neighbour_coord = current_coord + offset;

                if let Some(false) = grid.is_solid(neighbour_coord) {
                } else {
                    continue;
                }

                let cost = distance_map
                    .cost(neighbour_coord)
                    .ok_or(Error::InconsistentDistanceMap)?;

                let index = self
                    .node_grid
                    .index_of_coord(neighbour_coord)
                    .ok_or(Error::VisitOutsideContext)?;

                {
                    let node = &mut self.node_grid[index];
                    if node.seen != self.seq {
                        node.seen = self.seq;
                        node.from_parent = Some(direction);
                        node.cost = next_depth;
                        self.priority_queue.push(PriorityEntry::new(index, cost));
                    }
                }

                best_map.insert_lt(cost, index);
            }
        }

        let (cost, index) = best_map.into_key_and_value();
        path::make_path_all_adjacent(&self.node_grid, index, path);
        let length = path.len();
        Ok(SearchMetadata {
            num_nodes_visited,
            length,
            cost,
        })
    }
}
