path-planning 0.1.0

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

pub use petgraph::stable_graph::{EdgeIndex, NodeIndex};

////////////////////////////////////////////////////////////////////////////////

use std::collections::VecDeque;
use std::marker::PhantomData;

use std::cmp::Ordering;
use std::collections::HashSet;
use std::ops::{Add, Index, IndexMut};

use nalgebra::{SVector, Scalar};
use num_traits::{Float, Zero};
use petgraph::stable_graph::{
  DefaultIx, EdgeIndices, Edges, NodeIndices, StableDiGraph, WalkNeighbors,
};
use petgraph::visit::EdgeRef;
use petgraph::{Directed, Direction};
use serde::{de::DeserializeOwned, Deserialize, Serialize};

use crate::trajectories::{FullTrajRefOwned, FullTrajectory, Trajectory};

/// The cost at a node
#[derive(Debug, Clone, Copy, Default, PartialEq, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct Cost<X> {
  /// The look-ahead estimate of cost-to-goal
  pub lmc: X,
  /// The (e-consistent) cost-to-goal of reaching v_goal from v through the optimal tree
  pub g: X,
}

impl<X> Cost<X> {
  pub fn new(lmc: X, g: X) -> Self {
    Self { lmc, g }
  }
}

impl<X: Float> Cost<X> {
  pub fn infinity() -> Self {
    Self {
      lmc: X::infinity(),
      g: X::infinity(),
    }
  }

  pub fn priority(&self) -> Priority<X> {
    Priority(self.g.min(self.lmc), self.g)
  }
}

impl<X: Add<X, Output = X>> Add for Cost<X> {
  type Output = Self;

  fn add(self, other: Self) -> Self::Output {
    Self {
      lmc: self.lmc + other.lmc,
      g: self.g + other.g,
    }
  }
}

impl<X: Zero + PartialEq> Zero for Cost<X> {
  fn zero() -> Self {
    Self::new(X::zero(), X::zero())
  }

  fn is_zero(&self) -> bool {
    self == &Self::zero()
  }
}

/// The priority of a node in the queue
/// priority should give the opposite of min cost because the priority queue
/// sorts the maximum value first
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct Priority<X>(X, X);

impl<X: Float> Priority<X> {
  fn is_nan(&self) -> bool {
    self.0.is_nan() || self.1.is_nan()
  }
}

impl<X: Float> PartialEq for Priority<X> {
  fn eq(&self, other: &Self) -> bool {
    debug_assert!(!(self.is_nan() || other.is_nan()));
    self.0 == other.0 && self.1 == other.1
  }
}

/// Because of assertion in PartialEq impl
impl<X: Float> Eq for Priority<X> {}

impl<X: Float> PartialOrd for Priority<X> {
  fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
    if self == other {
      return Some(Ordering::Equal);
    }

    let (a, b) = (self.0, self.1);
    let (c, d) = (other.0, other.1);

    // Flip the order because PriorityQueue sorts the maximum items first
    match a == c {
      true => b.partial_cmp(&d).map(|ord| ord.reverse()),
      false => a.partial_cmp(&c).map(|ord| ord.reverse()),
    }
  }
}

impl<X: Float> Ord for Priority<X> {
  fn cmp(&self, other: &Self) -> Ordering {
    self.partial_cmp(other).unwrap()
  }
}

/// A node in the Rrtx graph. Stores information about a particular state in the state space.
#[derive(PartialEq, Clone, Debug, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct Node<X: Scalar, const N: usize> {
  /// The point in the state space this node is at (immutable)
  state: SVector<X, N>,
  /// The cost data for this node (mutable)
  pub cost: Cost<X>,
  /// The index of the edge that leads to the parent
  parent_edge: Option<EdgeIndex>,
}

impl<X: Scalar + Float, const N: usize> Node<X, N> {
  pub fn new(state: SVector<X, N>) -> Self {
    Self {
      state,
      cost: Cost::infinity(),
      parent_edge: None,
    }
  }
}

impl<X: Scalar, const N: usize> Node<X, N> {
  fn with_cost(state: SVector<X, N>, cost: Cost<X>) -> Self {
    Self {
      state,
      cost,
      parent_edge: None,
    }
  }

  pub fn state(&self) -> &SVector<X, N> {
    &self.state
  }

  fn clear_parent_edge(&mut self) {
    self.parent_edge = None;
  }

  fn set_parent_edge(&mut self, edge_idx: EdgeIndex) {
    self.parent_edge = Some(edge_idx);
  }

  pub fn parent_edge(&self) -> Option<EdgeIndex> {
    self.parent_edge
  }
}

/// In Rrtx, all outgoing edges are either classified as Original or Running
#[derive(PartialEq, Eq, Clone, Copy, Debug, Serialize, Deserialize)]
pub enum NeighborType {
  Original,
  Running,
}

/// An edge in the Rrtx graph.
/// Stores information about what type of connection this is between two nodes (directed)
/// All information is from the perspective of node a, assuming this edge represents the directed edge a -> b.
/// An outgoing edge of a in the directed edge a -> b
/// An incoming edge of a in the directed edge b -> a
#[derive(PartialEq, Eq, Clone, Copy, Debug, Serialize, Deserialize)]
pub struct Edge<X, T, const N: usize>
where
  T: Trajectory<X, N>,
{
  bits: u8,
  trajectory: T,
  phantom_data: PhantomData<X>,
}

impl<X, T, const N: usize> Edge<X, T, N>
where
  T: Trajectory<X, N>,
{
  fn new(
    outgoing_neighbor_type: NeighborType,
    incoming_neighbor_type: NeighborType,
    parent: bool,
    trajectory: T,
  ) -> Self {
    let mut new = Self {
      bits: 0,
      trajectory,
      phantom_data: PhantomData,
    };
    new.set_outgoing_neighbor_type(Some(outgoing_neighbor_type));
    new.set_incoming_neighbor_type(Some(incoming_neighbor_type));
    new.set_is_parent(parent);
    new.set_is_trajectory_valid_bit(true);
    new
  }

  pub fn bits(&self) -> u8 {
    self.bits
  }

  /// Check at least one of the directions is valid
  /// if false, then the edge should be deleted
  fn is_valid(&self) -> bool {
    self.is_valid_dir(Direction::Outgoing)
      || self.is_valid_dir(Direction::Incoming)
    // || self.is_parent()
  }

  /// Check if the specified direction is still valid (i.e. not None)
  fn is_valid_dir(&self, direction: Direction) -> bool {
    match direction {
      Direction::Outgoing => self.outgoing_neighbor_type().is_some(),
      Direction::Incoming => self.incoming_neighbor_type().is_some(),
    }
  }

  /// For the directed edge a -> b,
  /// - outgoing: removes a's knowledge of b
  /// - incoming: removes b's knowledge of a
  /// Returns whether the edge is still valid,
  /// if false, then the edge should be deleted
  fn remove_connection(&mut self, to_remove: Direction) -> bool {
    match to_remove {
      Direction::Outgoing => {
        self.set_outgoing_neighbor_type(None);
        self.is_valid_dir(Direction::Incoming)
      }
      Direction::Incoming => {
        self.set_incoming_neighbor_type(None);
        self.is_valid_dir(Direction::Outgoing)
      }
    }
  }

  /// Returns true if along the directed edge a -> b,
  /// - outgoing: if b is a original neighbor of a
  /// - incoming: if a is a original neighbor of b
  fn is_original(&self, direction: Direction) -> bool {
    match direction {
      Direction::Outgoing => match self.outgoing_neighbor_type() {
        Some(t) => t == NeighborType::Original,
        None => false,
      },
      Direction::Incoming => match self.incoming_neighbor_type() {
        Some(t) => t == NeighborType::Original,
        None => false,
      },
    }
  }

  /// Returns true if along the directed edge a -> b,
  /// - outgoing: if b is a running neighbor of a
  /// - incoming: if a is a running neighbor of b
  fn is_running(&self, direction: Direction) -> bool {
    match direction {
      Direction::Outgoing => match self.outgoing_neighbor_type() {
        Some(t) => t == NeighborType::Running,
        None => false,
      },
      Direction::Incoming => match self.incoming_neighbor_type() {
        Some(t) => t == NeighborType::Running,
        None => false,
      },
    }
  }

  /* Getters */

  pub fn outgoing_neighbor_type(&self) -> Option<NeighborType> {
    match self.get_is_outgoing_valid_bit() {
      true => match self.get_is_outgoing_original_bit() {
        true => Some(NeighborType::Original),
        false => Some(NeighborType::Running),
      },
      false => None,
    }
  }

  pub fn incoming_neighbor_type(&self) -> Option<NeighborType> {
    match self.get_is_incoming_valid_bit() {
      true => match self.get_is_incoming_original_bit() {
        true => Some(NeighborType::Original),
        false => Some(NeighborType::Running),
      },
      false => None,
    }
  }

  /// Returns true if along the directed edge a -> b, b is the parent of a
  pub fn is_parent(&self) -> bool {
    self.get_is_parent_bit()
  }

  /// The if the trajectory to get from a to b is valid
  pub fn trajectory(&self) -> Option<&T> {
    match self.get_is_trajectory_valid_bit() {
      true => Some(&self.trajectory),
      false => None,
    }
  }

  /* Setters */

  fn set_outgoing_neighbor_type(&mut self, outgoing: Option<NeighborType>) {
    match outgoing {
      Some(NeighborType::Original) => {
        self.set_is_outgoing_valid_bit(true);
        self.set_is_outgoing_original_bit(true);
      }
      Some(NeighborType::Running) => {
        self.set_is_outgoing_valid_bit(true);
        self.set_is_outgoing_original_bit(false);
      }
      None => self.set_is_outgoing_valid_bit(false),
    }
  }

  fn set_incoming_neighbor_type(&mut self, incoming: Option<NeighborType>) {
    match incoming {
      Some(NeighborType::Original) => {
        self.set_is_incoming_valid_bit(true);
        self.set_is_incoming_original_bit(true);
      }
      Some(NeighborType::Running) => {
        self.set_is_incoming_valid_bit(true);
        self.set_is_incoming_original_bit(false);
      }
      None => self.set_is_incoming_valid_bit(false),
    }
  }

  fn set_is_parent(&mut self, is_parent: bool) {
    self.set_is_parent_bit(is_parent)
  }

  fn set_trajectory(&mut self, trajectory: Option<T>) {
    match trajectory {
      Some(trajectory) => {
        self.set_is_trajectory_valid_bit(true);
        self.trajectory = trajectory;
      }
      None => self.set_is_trajectory_valid_bit(false),
    }
  }

  const IS_OUTGOING_VALID_BIT: u8 = 5;
  const IS_OUTGOING_ORIGINAL_BIT: u8 = 4;
  const IS_INCOMING_VALID_BIT: u8 = 3;
  const IS_INCOMING_ORIGINAL_BIT: u8 = 2;
  const IS_PARENT_BIT: u8 = 1;
  const IS_TRAJECTORY_VALID_BIT: u8 = 0;

  fn get_is_outgoing_valid_bit(&self) -> bool {
    self.get_bit(Self::IS_OUTGOING_VALID_BIT)
  }

  fn get_is_outgoing_original_bit(&self) -> bool {
    self.get_bit(Self::IS_OUTGOING_ORIGINAL_BIT)
  }

  fn get_is_incoming_valid_bit(&self) -> bool {
    self.get_bit(Self::IS_INCOMING_VALID_BIT)
  }

  fn get_is_incoming_original_bit(&self) -> bool {
    self.get_bit(Self::IS_INCOMING_ORIGINAL_BIT)
  }

  fn get_is_parent_bit(&self) -> bool {
    self.get_bit(Self::IS_PARENT_BIT)
  }

  fn get_is_trajectory_valid_bit(&self) -> bool {
    self.get_bit(Self::IS_TRAJECTORY_VALID_BIT)
  }

  fn get_bit(&self, bit_pos: u8) -> bool {
    let mask = 1 << bit_pos;
    (self.bits & mask) != 0
  }

  fn set_is_outgoing_valid_bit(&mut self, is_valid: bool) {
    self.set_bit(Self::IS_OUTGOING_VALID_BIT, is_valid)
  }

  fn set_is_outgoing_original_bit(&mut self, is_original: bool) {
    self.set_bit(Self::IS_OUTGOING_ORIGINAL_BIT, is_original)
  }

  fn set_is_incoming_valid_bit(&mut self, is_valid: bool) {
    self.set_bit(Self::IS_INCOMING_VALID_BIT, is_valid)
  }

  fn set_is_incoming_original_bit(&mut self, is_original: bool) {
    self.set_bit(Self::IS_INCOMING_ORIGINAL_BIT, is_original)
  }

  fn set_is_parent_bit(&mut self, is_parent: bool) {
    self.set_bit(Self::IS_PARENT_BIT, is_parent)
  }

  fn set_is_trajectory_valid_bit(&mut self, is_valid: bool) {
    self.set_bit(Self::IS_TRAJECTORY_VALID_BIT, is_valid)
  }

  fn set_bit(&mut self, bit_pos: u8, value: bool) {
    let mask = 1 << bit_pos;
    match value {
      true => self.bits |= mask,
      false => self.bits &= !mask,
    }
  }
}

/// Different subsets for neighbors relative to a node
pub enum NeighborSet {
  Outgoing,
  Incoming,
  OriginalOutgoing,
  OriginalIncoming,
  RunningOutgoing,
  RunningIncoming,
}

/// An iterator over different Neighbor sets of a node
pub struct NeighborSetIter<'a, X, T, const N: usize>
where
  T: Trajectory<X, N>,
{
  outgoing: Edges<'a, Edge<X, T, N>, Directed>,
  incoming: Edges<'a, Edge<X, T, N>, Directed>,
  set_type: NeighborSet,
}

impl<'a, X, T, const N: usize> NeighborSetIter<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  fn new<NT>(
    graph: &'a StableDiGraph<NT, Edge<X, T, N>>,
    node: NodeIndex,
    set_type: NeighborSet,
  ) -> Self {
    let outgoing = graph.edges_directed(node, Direction::Outgoing);
    let incoming = graph.edges_directed(node, Direction::Incoming);
    Self {
      outgoing,
      incoming,
      set_type,
    }
  }
}

impl<'a, X, T, const N: usize> NeighborSetIter<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  pub fn next_edge(&mut self) -> Option<EdgeIndex> {
    Some(self.next()?.0)
  }

  pub fn next_node(&mut self) -> Option<NodeIndex> {
    Some(self.next()?.1)
  }
}

impl<'a, X, T, const N: usize> Iterator for NeighborSetIter<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  type Item = (EdgeIndex, NodeIndex);

  fn next(&mut self) -> Option<Self::Item> {
    match self.set_type {
      NeighborSet::Outgoing => loop {
        let e = self.outgoing.next()?;
        match e.weight().is_valid_dir(Direction::Outgoing) {
          true => break Some((e.id(), e.target())),
          false => continue,
        }
      },
      NeighborSet::Incoming => loop {
        let e = self.incoming.next()?;
        match e.weight().is_valid_dir(Direction::Incoming) {
          true => break Some((e.id(), e.source())),
          false => continue,
        }
      },
      NeighborSet::OriginalOutgoing => loop {
        let e = self.outgoing.next()?;
        match e.weight().is_original(Direction::Outgoing) {
          true => break Some((e.id(), e.target())),
          false => continue,
        }
      },
      NeighborSet::OriginalIncoming => loop {
        let e = self.incoming.next()?;
        match e.weight().is_original(Direction::Incoming) {
          true => break Some((e.id(), e.source())),
          false => continue,
        }
      },
      NeighborSet::RunningOutgoing => loop {
        let e = self.outgoing.next()?;
        match e.weight().is_running(Direction::Outgoing) {
          true => break Some((e.id(), e.target())),
          false => continue,
        }
      },
      NeighborSet::RunningIncoming => loop {
        let e = self.incoming.next()?;
        match e.weight().is_running(Direction::Incoming) {
          true => break Some((e.id(), e.source())),
          false => continue,
        }
      },
    }
  }
}

/// An walker over different Neighbor sets of a node
pub struct NeighborSetWalker {
  outgoing: WalkNeighbors<DefaultIx>,
  incoming: WalkNeighbors<DefaultIx>,
  set_type: NeighborSet,
}

impl NeighborSetWalker {
  fn new<X, T, NT, const N: usize>(
    graph: &StableDiGraph<NT, Edge<X, T, N>>,
    node: NodeIndex,
    set_type: NeighborSet,
  ) -> Self
  where
    T: Trajectory<X, N>,
  {
    let outgoing = graph.neighbors_directed(node, Direction::Outgoing).detach();
    let incoming = graph.neighbors_directed(node, Direction::Incoming).detach();
    Self {
      outgoing,
      incoming,
      set_type,
    }
  }

  /// Returns the node in the set along with the associated edge that connects that node
  pub fn next<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<(EdgeIndex, NodeIndex)>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    match self.set_type {
      NeighborSet::Outgoing => loop {
        let item = self.outgoing.next(&g.graph)?;
        match g.graph[item.0].is_valid_dir(Direction::Outgoing) {
          true => break Some(item),
          false => continue,
        }
      },
      NeighborSet::Incoming => loop {
        let item = self.incoming.next(&g.graph)?;
        match g.graph[item.0].is_valid_dir(Direction::Incoming) {
          true => break Some(item),
          false => continue,
        }
      },
      NeighborSet::OriginalOutgoing => loop {
        let item = self.outgoing.next(&g.graph)?;
        match g.graph[item.0].is_original(Direction::Outgoing) {
          true => break Some(item),
          false => continue,
        }
      },
      NeighborSet::OriginalIncoming => loop {
        let item = self.incoming.next(&g.graph)?;
        match g.graph[item.0].is_original(Direction::Incoming) {
          true => break Some(item),
          false => continue,
        }
      },
      NeighborSet::RunningOutgoing => loop {
        let item = self.outgoing.next(&g.graph)?;
        match g.graph[item.0].is_running(Direction::Outgoing) {
          true => break Some(item),
          false => continue,
        }
      },
      NeighborSet::RunningIncoming => loop {
        let item = self.incoming.next(&g.graph)?;
        match g.graph[item.0].is_running(Direction::Incoming) {
          true => break Some(item),
          false => continue,
        }
      },
    }
  }

  /// Returns the next neighbor edge in the set
  /// Note: might return
  pub fn next_edge<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<EdgeIndex>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    Some(self.next(g)?.0)
  }

  /// Returns the next neighbor node in the set
  /// Note: only returns each node a maximum of once
  pub fn next_node<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<NodeIndex>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    Some(self.next(g)?.1)
  }
}

/// An iterator over the children in the optimal subtree
pub struct Childern<'a, X, T, const N: usize>
where
  T: Trajectory<X, N>,
{
  edges: Edges<'a, Edge<X, T, N>, Directed>,
}

impl<'a, X, T, const N: usize> Childern<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  fn new<NT>(
    graph: &'a StableDiGraph<NT, Edge<X, T, N>>,
    node: NodeIndex,
  ) -> Self {
    let edges = graph.edges_directed(node, Direction::Incoming);
    Self { edges }
  }
}

impl<'a, X, T, const N: usize> Childern<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  pub fn next_edge(&mut self) -> Option<EdgeIndex> {
    Some(self.next()?.0)
  }

  pub fn next_node(&mut self) -> Option<NodeIndex> {
    Some(self.next()?.1)
  }
}

impl<'a, X, T, const N: usize> Iterator for Childern<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  type Item = (EdgeIndex, NodeIndex);

  fn next(&mut self) -> Option<Self::Item> {
    loop {
      let e = self.edges.next()?;
      match e.weight().is_parent() {
        true => return Some((e.id(), e.source())),
        false => continue,
      }
    }
  }
}

/// An walker object over the children in the optimal subtree
pub struct ChildernWalker {
  neighbors: WalkNeighbors<DefaultIx>,
}

impl ChildernWalker {
  fn new<X, T, NT, const N: usize>(
    graph: &StableDiGraph<NT, Edge<X, T, N>>,
    node: NodeIndex,
  ) -> Self
  where
    T: Trajectory<X, N>,
  {
    let neighbors =
      graph.neighbors_directed(node, Direction::Incoming).detach();
    Self { neighbors }
  }

  pub fn next<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<(EdgeIndex, NodeIndex)>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    loop {
      let item = self.neighbors.next(&g.graph)?;
      match g.graph[item.0].is_parent() {
        true => return Some(item),
        false => continue,
      }
    }
  }

  pub fn next_edge<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<EdgeIndex>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    Some(self.next(g)?.0)
  }

  pub fn next_node<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<NodeIndex>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    Some(self.next(g)?.1)
  }
}

/// Iterator over all the node indices of the graph
pub struct NodeIter<'a, X: Scalar, const N: usize> {
  nodes: NodeIndices<'a, Node<X, N>>,
}

impl<'a, X: Scalar, const N: usize> NodeIter<'a, X, N> {
  fn new<ET>(graph: &'a StableDiGraph<Node<X, N>, ET>) -> Self
where {
    Self {
      nodes: graph.node_indices(),
    }
  }
}

impl<'a, X: Scalar, const N: usize> Iterator for NodeIter<'a, X, N> {
  type Item = NodeIndex;

  fn next(&mut self) -> Option<Self::Item> {
    self.nodes.next()
  }
}

/// Iterator over all the edge indices of the graph
pub struct EdgeIter<'a, X, T, const N: usize>
where
  T: Trajectory<X, N>,
{
  edges: EdgeIndices<'a, Edge<X, T, N>>,
}

impl<'a, X, T, const N: usize> EdgeIter<'a, X, T, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
{
  fn new(graph: &'a StableDiGraph<Node<X, N>, Edge<X, T, N>>) -> Self {
    Self {
      edges: graph.edge_indices(),
    }
  }
}

impl<'a, X, T, const N: usize> Iterator for EdgeIter<'a, X, T, N>
where
  T: Trajectory<X, N>,
{
  type Item = EdgeIndex;

  fn next(&mut self) -> Option<Self::Item> {
    self.edges.next()
  }
}

/// Iterator over edge indices of the graph in the optimal subtree
pub struct OptimalPathIter<'a, X, T, const N: usize>
where
  X: Scalar,
  T: Trajectory<X, N>,
{
  graph: &'a RrtxGraph<X, T, N>,
  next_node: Option<NodeIndex>,
}

impl<'a, X, T, const N: usize> OptimalPathIter<'a, X, T, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
{
  fn new(graph: &'a RrtxGraph<X, T, N>, node: NodeIndex) -> Self {
    Self {
      graph,
      next_node: Some(node),
    }
  }

  pub fn detach(self) -> OptimalPathWalker {
    OptimalPathWalker::new(self.next_node)
  }
}

impl<'a, X, T, const N: usize> Iterator for OptimalPathIter<'a, X, T, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
{
  type Item = NodeIndex;

  fn next(&mut self) -> Option<Self::Item> {
    let node = self.next_node?;
    match self.graph.parent(node) {
      Some(parent) => self.next_node = Some(parent),
      None => self.next_node = None,
    }

    Some(node)
  }
}

/// Iterator over edge indices of the graph in the optimal subtree
pub struct OptimalPathWalker {
  next_node: Option<NodeIndex>,
}

impl OptimalPathWalker {
  fn new(node: Option<NodeIndex>) -> Self {
    Self { next_node: node }
  }

  pub fn next<X, T, const N: usize>(
    &mut self,
    g: &RrtxGraph<X, T, N>,
  ) -> Option<NodeIndex>
  where
    X: Scalar,
    T: Trajectory<X, N>,
  {
    let node = self.next_node?;
    match g.parent(node) {
      Some(parent) => self.next_node = Some(parent),
      None => self.next_node = None,
    }

    Some(node)
  }
}

#[derive(Clone, Debug, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, T: Serialize",
  deserialize = "X: DeserializeOwned, T: DeserializeOwned",
))]
pub struct RrtxGraph<X: Scalar, T: Trajectory<X, N>, const N: usize> {
  /// The index of the goal node in the graph
  goal_idx: NodeIndex,

  /// The actual graph structure
  graph: StableDiGraph<Node<X, N>, Edge<X, T, N>>,

  /// All the nodes in the graph, used to test for existance faster
  deleted: HashSet<NodeIndex>,

  /// A temporary location to store orphaned nodes
  orphans: HashSet<NodeIndex>,
}

impl<X, T, const N: usize> RrtxGraph<X, T, N>
where
  X: Scalar + Zero,
  T: Trajectory<X, N>,
{
  /// Creates new graph with goal as the root node with cost zero
  pub fn new(goal: SVector<X, N>) -> Self {
    // Graph
    let mut graph = StableDiGraph::new();
    let goal_node = Node::with_cost(goal, Cost::zero());
    let goal_idx = graph.add_node(goal_node);

    // Nodes
    let deleted = HashSet::new();

    // Orphans
    let orphans = HashSet::new();

    Self {
      goal_idx,
      graph,
      deleted,
      orphans,
    }
  }
}

impl<X, T, const N: usize> RrtxGraph<X, T, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
{
  /// Returns the number of nodes in the graph
  pub fn node_count(&self) -> usize {
    self.graph.node_count()
  }

  /// Returns the index of the goal node
  pub fn get_goal_idx(&self) -> NodeIndex {
    self.goal_idx
  }

  /// Returns reference to the goal node
  pub fn get_goal(&self) -> &Node<X, N> {
    self.get_node(self.goal_idx)
  }

  /// Returns iterator over all nodes in the graph
  pub fn all_nodes(&self) -> NodeIter<X, N> {
    NodeIter::new(&self.graph)
  }

  /// Returns iterator over all edges in the graph
  pub fn all_edges(&self) -> EdgeIter<X, T, N> {
    EdgeIter::new(&self.graph)
  }

  /// Returns an iterator over the neighbors to a in the described set
  pub fn neighbor_set(
    &self,
    a: NodeIndex,
    set: NeighborSet,
  ) -> NeighborSetIter<X, T, N> {
    NeighborSetIter::new(&self.graph, a, set)
  }

  /// Returns an walker over the neighbors to a in the described set
  pub fn neighbor_set_walker(
    &self,
    node: NodeIndex,
    set: NeighborSet,
  ) -> NeighborSetWalker {
    NeighborSetWalker::new(&self.graph, node, set)
  }

  /// Returns the NodeIndex of the parent of a in the optimal subtree if exists
  /// Returns None for the root node
  pub fn parent(&self, node: NodeIndex) -> Option<NodeIndex> {
    let parent_edge_idx = self.parent_edge(node)?;
    debug_assert!(self.get_edge(parent_edge_idx).is_parent());
    let (child, parent) = self.get_endpoints(parent_edge_idx);
    debug_assert_eq!(child, node);
    Some(parent)
  }

  /// Returns the edge directed at the nodes parent in the optimal subtree if exists
  fn parent_edge(&self, node: NodeIndex) -> Option<EdgeIndex> {
    self.get_node(node).parent_edge()
  }

  /// Looks up edge from node -> parent to see if parent is the parent of node
  pub fn is_parent(&self, node: NodeIndex, parent: NodeIndex) -> bool {
    match self.parent(node) {
      Some(true_parent) => true_parent == parent,
      None => false,
    }
  }

  /// Returns iterator over all children of a in the optimal subtree.
  /// Iterator will be empty for leaf nodes
  pub fn children(&self, node: NodeIndex) -> Childern<X, T, N> {
    Childern::new(&self.graph, node)
  }

  /// Returns walker over all children of a in the optimal subtree.
  /// Iterator will be empty for leaf nodes
  pub fn children_walker(&self, node: NodeIndex) -> ChildernWalker {
    ChildernWalker::new(&self.graph, node)
  }

  /// Looks up edge from child -> node to see if node is the parent of child
  /// Returns None if they aren't connected
  pub fn is_child(&self, node: NodeIndex, child: NodeIndex) -> bool {
    self.is_parent(child, node)
  }

  /// Add node and children of node to the internal set of orphans
  ///
  /// This garentees that the children of any orphan node are also marked as orphans
  /// as soon as they are added
  pub fn add_orphan(&mut self, node: NodeIndex) {
    // Add all childern node as orphans
    let mut queue = VecDeque::new();
    queue.push_back(node);

    // While there are still nodes to process
    while let Some(node) = queue.pop_front() {
      // Set node as orphan
      if self.orphans.insert(node) {
        // if newly inserted, add childern to queue
        let mut children = self.children_walker(node);
        while let Some(child_idx) = children.next_node(&self) {
          queue.push_back(child_idx);
        }
      }
    }
  }

  /// Remove node from the internal list of orphans, doesn't modifiy the graph
  pub fn remove_orphan(&mut self, node: NodeIndex) {
    let result = self.orphans.remove(&node);
    assert!(result);
  }

  /// Checks if node is in the internal set of orphans
  pub fn is_orphan(&self, node: NodeIndex) -> bool {
    self.orphans.contains(&node)
  }

  /// Get iterator over all orphans
  pub fn orphans(&self) -> impl Iterator<Item = NodeIndex> + '_ {
    self.orphans.iter().map(|&x| x)
  }

  /// Adds a node to the graph and returns it's index
  pub fn add_node(&mut self, node: Node<X, N>) -> NodeIndex {
    self.graph.add_node(node)
  }

  /// Adds a edge to the graph from a -> b with the associated data
  pub fn add_edge(
    &mut self,
    a: NodeIndex,
    b: NodeIndex,
    outgoing_neighbor_type: NeighborType,
    incoming_neighbor_type: NeighborType,
    is_parent: bool,
    trajectory: T,
  ) -> EdgeIndex {
    let edge = Edge::new(
      outgoing_neighbor_type,
      incoming_neighbor_type,
      is_parent,
      trajectory,
    );
    let edge_idx = self.graph.add_edge(a, b, edge);

    if is_parent {
      let node = self.get_node_mut(a);
      node.set_parent_edge(edge_idx);
    }

    edge_idx
  }

  /// Switches the unique parent edge from node to the new_parent
  /// Still keeps a connection to the old parent
  /// Returns Ok if the new_parent was set correctly, otherwise Err
  /// Either way, returns the old_parent if was found
  pub fn change_parent(
    &mut self,
    node: NodeIndex,
    new_parent: NodeIndex,
  ) -> Result<Option<NodeIndex>, Option<NodeIndex>> {
    match self.remove_parent(node) {
      Some(old_parent) => match self.set_parent(node, new_parent) {
        Some(_) => Ok(Some(old_parent)),
        None => Err(Some(old_parent)),
      },
      None => match self.set_parent(node, new_parent) {
        Some(_) => Ok(None),
        None => Err(None),
      },
    }
  }

  /// Blindly set the edge from node -> parent as a parent connection
  /// Returns none if edge wasn't found
  fn set_parent(
    &mut self,
    node_idx: NodeIndex,
    parent_idx: NodeIndex,
  ) -> Option<EdgeIndex> {
    let edge_idx = self.graph.find_edge(node_idx, parent_idx)?;
    let edge = self.get_edge_mut(edge_idx);
    edge.set_is_parent(true);

    let node = self.get_node_mut(node_idx);
    node.set_parent_edge(edge_idx);

    if node_idx.index() == 9091 {
      log::debug!(
        "{} is now parent of {}, edge_idx: {}",
        parent_idx.index(),
        node_idx.index(),
        edge_idx.index(),
      );
      assert_eq!(self.parent(node_idx).unwrap(), parent_idx);
    }

    if parent_idx.index() == 9091 {
      log::debug!(
        "{} is now parent of {}",
        parent_idx.index(),
        node_idx.index()
      );
      log::debug!(
        "Parent of {}: {:?}",
        parent_idx.index(),
        self.parent(parent_idx)
      );
      log::debug!("{}: {:?}", parent_idx.index(), self.get_node(parent_idx));
    }
    Some(edge_idx)
  }

  /// Blindly sets edge connected to parent node as not a parent
  /// Return Some parent index if found
  pub fn remove_parent(&mut self, node_idx: NodeIndex) -> Option<NodeIndex> {
    let old_parent = self.parent(node_idx)?;

    let edge_idx = self.parent_edge(node_idx)?;
    let edge = self.get_edge_mut(edge_idx);
    edge.set_is_parent(false);

    let node = self.get_node_mut(node_idx);
    node.clear_parent_edge();

    if node_idx.index() == 9091 {
      log::debug!(
        "Removing parent of {}, old_parent: {:?}",
        node_idx.index(),
        old_parent.index()
      );
    }

    Some(old_parent)
  }

  /// Removes any outgoing or incoming running neighbor connection on the edge from node -> neighbor
  /// Returns None if the edge wasn't found
  pub fn remove_running_neighbor(
    &mut self,
    node: NodeIndex,
    neighbor: NodeIndex,
  ) -> Option<()> {
    let edge_idx = self.graph.find_edge(node, neighbor)?;
    let edge = self.get_edge_mut(edge_idx);

    if edge.is_running(Direction::Outgoing) {
      edge.remove_connection(Direction::Outgoing);
    }

    if edge.is_running(Direction::Incoming) {
      edge.remove_connection(Direction::Incoming);
    }

    if !edge.is_valid() {
      assert!(!edge.is_parent());
      self.graph.remove_edge(edge_idx);
    }

    Some(())
  }

  /// Returns an iterator of edges over the optimal path to the goal node
  /// Returns None if no such path exists
  pub fn get_optimal_path(
    &self,
    node: NodeIndex,
  ) -> Option<OptimalPathIter<X, T, N>> {
    match self.is_orphan(node) {
      true => None,
      false => Some(OptimalPathIter::new(self, node)),
    }
  }

  /// Returns a refernce the specified node
  ///
  /// panics if `idx` is invalid
  pub fn get_node(&self, idx: NodeIndex) -> &Node<X, N> {
    self.graph.index(idx)
  }

  /// Returns a mutable refernce the specified node
  ///
  /// panics if `idx` is invalid
  pub fn get_node_mut(&mut self, idx: NodeIndex) -> &mut Node<X, N> {
    self.graph.index_mut(idx)
  }

  /// Checks if the specified node exists
  pub fn check_node(&self, idx: NodeIndex) -> bool {
    !self.deleted.contains(&idx)
  }

  /// Irreversibly delete the specified node
  ///
  /// panics if `idx` is invalid
  pub fn delete_node(&mut self, idx: NodeIndex) {
    // if the node has any children, remove the parent connection stored in the node,
    // the edge will be removed by petgraph
    let mut children = self.children_walker(idx);
    while let Some(child_idx) = children.next_node(&self) {
      self.get_node_mut(child_idx).clear_parent_edge();
    }

    self.graph.remove_node(idx);
    self.deleted.insert(idx);
    self.orphans.remove(&idx);
  }

  /// Returns a refernce the specified edge
  ///
  /// panics if `idx` is invalid
  pub fn get_edge(&self, idx: EdgeIndex) -> &Edge<X, T, N> {
    self.graph.index(idx)
  }

  /// Returns a mutable refernce the specified edge
  ///
  /// panics if `idx` is invalid
  fn get_edge_mut(&mut self, idx: EdgeIndex) -> &mut Edge<X, T, N> {
    self.graph.index_mut(idx)
  }

  /// Returns source and target endpoints of an edge
  ///
  /// panics if `idx` is invalid
  pub fn get_endpoints(&self, idx: EdgeIndex) -> (NodeIndex, NodeIndex) {
    self.graph.edge_endpoints(idx).unwrap()
  }

  /// Returns the trajectory stored at the specified edge if exists
  ///
  /// panics if `idx` is invalid
  pub fn get_trajectory(
    &self,
    idx: EdgeIndex,
  ) -> Option<FullTrajRefOwned<X, T, N>> {
    let (start_idx, end_idx) = self.get_endpoints(idx);
    let start = self.get_node(start_idx).state();
    let end = self.get_node(end_idx).state();
    let traj_data = self.get_edge(idx).trajectory()?;
    Some(FullTrajRefOwned::new(start, end, traj_data))
  }

  /// Updates the trajectory along the specified edge
  ///
  /// panics if `idx` is invalid
  pub fn update_trajectory(&mut self, idx: EdgeIndex, new_trajectory: T) {
    let edge = self.graph.index_mut(idx);
    edge.set_trajectory(Some(new_trajectory));
  }

  /// Sets the cost of the given edge to infinity
  ///
  /// panics if `idx` is invalid
  pub fn set_infinite_edge_cost(&mut self, idx: EdgeIndex) {
    let edge = self.graph.index_mut(idx);
    edge.set_trajectory(None);
  }
}

impl<X, T, const N: usize> RrtxGraph<X, T, N>
where
  X: Scalar + Float,
  T: Trajectory<X, N>,
{
  pub fn get_trajectory_cost(&self, idx: EdgeIndex) -> X {
    match self.get_trajectory(idx) {
      Some(trajectory) => trajectory.cost(),
      None => X::infinity(),
    }
  }
}

#[cfg(test)]
mod tests {

  use crate::trajectories::EuclideanTrajectory;

  use super::*;

  #[test]
  fn test_rrtx_graph_neighbor_set_iter() {
    let goal_coord = [1.5, 1.5].into();

    let mut g = RrtxGraph::new(goal_coord);
    let goal = g.get_goal_idx();

    let n1_coord = [2.0, 2.0].into();
    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
    let n1 = g.graph.add_node(n1);

    g.add_edge(
      n1,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n1,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    let n2_coord = [-2.0, -2.0].into();
    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
    let n2 = g.graph.add_node(n2);

    g.add_edge(
      n2,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n2,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    g.add_edge(
      n2,
      n1,
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );
    // No edge possible from n1 -> n2

    // All Outgoing neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    // All Incoming neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::Incoming);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::Incoming);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::Incoming);
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    // Original Outgoing neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    // Running Outgoing neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    // Original Incoming neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(), Some(goal));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    // Running Incoming neighbors
    let mut iter = g.neighbor_set(goal, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n1, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.neighbor_set(n2, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(), None);
    assert_eq!(iter.next_node(), None);
  }

  #[test]
  fn test_rrtx_graph_neighbor_set_walker() {
    let goal_coord = [1.5, 1.5].into();

    let mut g = RrtxGraph::new(goal_coord);
    let goal = g.get_goal_idx();

    let n1_coord = [2.0, 2.0].into();
    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
    let n1 = g.graph.add_node(n1);

    g.add_edge(
      n1,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n1,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    let n2_coord = [-2.0, -2.0].into();
    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
    let n2 = g.graph.add_node(n2);

    g.add_edge(
      n2,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n2,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    g.add_edge(
      n2,
      n1,
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );
    // No edge possible from n1 -> n2

    // All Outgoing neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::Outgoing);
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    // All Incoming neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::Incoming);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::Incoming);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::Incoming);
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    // Original Outgoing neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::OriginalOutgoing);
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    // Running Outgoing neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::RunningOutgoing);
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    // Original Incoming neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::OriginalIncoming);
    assert_eq!(iter.next_node(&g), Some(goal));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    // Running Incoming neighbors
    let mut iter = g.neighbor_set_walker(goal, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n1, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.neighbor_set_walker(n2, NeighborSet::RunningIncoming);
    assert_eq!(iter.next_node(&g), None);
    assert_eq!(iter.next_node(&g), None);
  }

  #[test]
  fn test_rrtx_graph_parent() {
    let goal_coord = [1.5, 1.5].into();

    let mut g = RrtxGraph::new(goal_coord);
    let goal = g.get_goal_idx();

    let n1_coord = [2.0, 2.0].into();
    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
    let n1 = g.graph.add_node(n1);

    g.add_edge(
      n1,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n1,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    let n2_coord = [-2.0, -2.0].into();
    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
    let n2 = g.graph.add_node(n2);

    g.add_edge(
      n2,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n2,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    g.add_edge(
      n2,
      n1,
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );
    // No edge possible from n1 -> n2

    // Parents
    assert_eq!(g.parent(goal), None);
    assert_eq!(g.parent(n1), Some(goal));
    assert_eq!(g.parent(n2), Some(goal));
  }

  #[test]
  fn test_rrtx_graph_children() {
    let goal_coord = [1.5, 1.5].into();

    let mut g = RrtxGraph::new(goal_coord);
    let goal = g.get_goal_idx();

    let n1_coord = [2.0, 2.0].into();
    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
    let n1 = g.graph.add_node(n1);

    g.add_edge(
      n1,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n1,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    let n2_coord = [-2.0, -2.0].into();
    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
    let n2 = g.graph.add_node(n2);

    g.add_edge(
      n2,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n2,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    g.add_edge(
      n2,
      n1,
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );
    // No edge possible from n1 -> n2

    // Childern
    let mut iter = g.children(goal);
    assert_eq!(iter.next_node(), Some(n2));
    assert_eq!(iter.next_node(), Some(n1));
    assert_eq!(iter.next_node(), None);

    let mut iter = g.children(n1);
    assert_eq!(iter.next_node(), None);

    let mut iter = g.children(n2);
    assert_eq!(iter.next_node(), None);
  }

  #[test]
  fn test_rrtx_graph_children_walker() {
    let goal_coord = [1.5, 1.5].into();

    let mut g = RrtxGraph::new(goal_coord);
    let goal = g.get_goal_idx();

    let n1_coord = [2.0, 2.0].into();
    let n1 = Node::with_cost(n1_coord, Cost::new(0.1, 0.2));
    let n1 = g.graph.add_node(n1);

    g.add_edge(
      n1,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n1,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    let n2_coord = [-2.0, -2.0].into();
    let n2 = Node::with_cost(n2_coord, Cost::new(0.5, 0.6));
    let n2 = g.graph.add_node(n2);

    g.add_edge(
      n2,
      goal,
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    g.add_edge(
      goal,
      n2,
      NeighborType::Running,
      NeighborType::Original,
      false,
      EuclideanTrajectory::new(),
    );

    g.add_edge(
      n2,
      n1,
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );
    // No edge possible from n1 -> n2

    // Childern Walker
    let mut iter = g.children_walker(goal);
    assert_eq!(iter.next_node(&g), Some(n2));
    assert_eq!(iter.next_node(&g), Some(n1));
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.children_walker(n1);
    assert_eq!(iter.next_node(&g), None);

    let mut iter = g.children_walker(n2);
    assert_eq!(iter.next_node(&g), None);
  }

  #[test]
  fn test_get_bit() {
    let edge_1 = Edge::<f32, _, 3>::new(
      NeighborType::Original,
      NeighborType::Running,
      true,
      EuclideanTrajectory::new(),
    );
    let edge_2 = Edge::<f32, _, 3>::new(
      NeighborType::Original,
      NeighborType::Running,
      false,
      EuclideanTrajectory::new(),
    );

    assert_eq!(edge_1.is_parent(), true);
    assert_eq!(edge_2.is_parent(), false);
  }
}