path-planning 0.1.0

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

use std::collections::HashSet;

use kdtree::KdTree;
use nalgebra::storage::Storage;
use nalgebra::{Const, SVector, VectorSlice};
use num_traits::Float;
use petgraph::stable_graph::NodeIndex;
use priority_queue::PriorityQueue;
use rayon::iter::{IntoParallelIterator, ParallelExtend, ParallelIterator};
use serde::{de::DeserializeOwned, Deserialize, Serialize};

use crate::cspace::CSpace;
use crate::error::{InvalidParamError, Result};
use crate::obstacles::{Obstacle, ObstacleSpace};
use crate::path_planner::{MoveGoal, PathPlanner, RobotSpecs};
use crate::scalar::Scalar;
use crate::trajectories::{FullTrajOwned, FullTrajRefOwned, FullTrajectory};
use crate::util::math::unit_d_ball_vol;

use super::{Cost, NeighborSet, NeighborType, Node, Priority, RrtxGraph};

/// Rrtx generic parameters
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct RrtxParams<X> {
  pub min_cost: X,
  pub portion: X,
  pub delta: X,
  pub gamma: X,
  pub eps: X,
}

/// Rrtx Serializable State information
#[derive(Debug, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, CS::Traj: Serialize, OS: Serialize",
  deserialize = "X: DeserializeOwned, CS::Traj: DeserializeOwned, OS: DeserializeOwned",
))]
pub struct RrtxState<X, CS, OS, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  /// All the nodes with their associated costs and edges that connect them
  pub graph: RrtxGraph<X, CS::Traj, N>,

  /// The current path being travelled from some point to a node in the graph
  /// and the trajectory of how to get there
  pub current_path: Option<(SVector<X, N>, CS::Traj, NodeIndex)>,

  /// The last know current pose of the system
  pub pose: SVector<X, N>,

  /// Tracks the obstacles in the space
  pub obs_space: OS,

  /// Shrinking ball radius
  pub radius: X,

  /// Robot specifications
  pub robot_specs: RobotSpecs<X>,

  // Algorithm Parameters
  pub params: RrtxParams<X>,
}

impl<X, CS, OS, const N: usize> Clone for RrtxState<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  fn clone(&self) -> Self {
    Self {
      graph: self.graph.clone(),
      current_path: self.current_path.clone(),
      pose: self.pose.clone(),
      obs_space: self.obs_space.clone(),
      radius: self.radius.clone(),
      robot_specs: self.robot_specs.clone(),
      params: self.params.clone(),
    }
  }
}

/// Rrtx implementation
pub struct Rrtx<X, CS, OS, const N: usize>
where
  X: Scalar,
  CS: CSpace<X, N>,
  OS: ObstacleSpace<X, CS, N>,
{
  /// All the serializable state information.
  state: RrtxState<X, CS, OS, N>,

  /// Priority queue
  queue: PriorityQueue<NodeIndex, Priority<X>>,

  /// For fast nearest neighbor searching
  kdtree: KdTree<X, NodeIndex, [X; N]>,

  /// Describes the configuration space
  cspace: CS,

  /// The number of sampled points
  sampled: usize,
}

impl<X, CS, OS, const N: usize> PathPlanner<X, CS, OS, N> for Rrtx<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N> + Send + Sync,
  CS::Traj: Send + Sync,
  OS: ObstacleSpace<X, CS, N> + Send + Sync,
  OS::Obs: Send + Sync,
{
  type Params = RrtxParams<X>;
  type State = RrtxState<X, CS, OS, N>;

  fn new(
    init: SVector<X, N>,
    goal: SVector<X, N>,
    robot_specs: RobotSpecs<X>,
    cspace: CS,
    obs_space: OS,
    params: Self::Params,
  ) -> Result<Self> {
    let graph = RrtxGraph::new(goal.clone());
    let current_path = None;
    let pose = init.clone();

    let queue = PriorityQueue::new();

    let mut kdtree: KdTree<X, NodeIndex, [X; N]> = KdTree::new(N.into());
    kdtree.add(goal.into(), graph.get_goal_idx())?;

    let radius = params.delta;

    // Validate robot_radius are greater than 0
    if !(X::zero() < robot_specs.robot_radius) {
      Err(InvalidParamError {
        parameter_name: "robot_specs.robot_radius",
        parameter_value: format!("{:?}", robot_specs.robot_radius),
      })?;
    }

    // Validate sensor_radius are greater than 0
    if !(X::zero() < robot_specs.sensor_radius) {
      Err(InvalidParamError {
        parameter_name: "robot_specs.sensor_radius",
        parameter_value: format!("{:?}", robot_specs.sensor_radius),
      })?;
    }

    // Validate min_cost is in [0, delta)
    if !(X::zero() <= params.min_cost && params.min_cost < params.delta) {
      Err(InvalidParamError {
        parameter_name: "params.min_cost",
        parameter_value: format!("{:?}", params.min_cost),
      })?;
    }

    // Validate portion is in (0, 1)
    if !(X::zero() < params.portion && params.portion < X::one()) {
      Err(InvalidParamError {
        parameter_name: "params.portion",
        parameter_value: format!("{:?}", params.portion),
      })?;
    }

    let state = RrtxState {
      graph,
      current_path,
      pose,
      obs_space,
      radius,
      robot_specs,
      params,
    };

    let mut new = Self {
      state,
      queue,
      kdtree,
      cspace,
      sampled: 0,
    };

    new.state.radius = new.shrinking_ball_radius();
    new.check_gamma();

    // Check that the init and goal locations are in free space
    if !new.is_free(&init) {
      Err(InvalidParamError {
        parameter_name: "init",
        parameter_value: format!("{:?}", init),
      })?;
    }
    if !new.is_free(&goal) {
      Err(InvalidParamError {
        parameter_name: "goal",
        parameter_value: format!("{:?}", goal),
      })?;
    }

    Ok(new)
  }

  fn create_node(&mut self) -> &Self::State {
    // Continue to sample points until one is found
    loop {
      if let Some(()) = self.try_create_node() {
        break;
      }
      self.log_sampling_ratio();
    }

    // Update the new radius now that we changed self.count()
    self.state.radius = self.shrinking_ball_radius();
    log::debug!("Shrinking ball radius: {:?}", self.state.radius);
    self.get_state()
  }

  fn sample_node(&mut self) -> Option<&Self::State> {
    self.try_create_node()?;

    // Update the new radius now that we changed self.count()
    self.state.radius = self.shrinking_ball_radius();
    log::debug!("Shrinking ball radius: {:?}", self.state.radius);
    Some(self.get_state())
  }

  fn check_sensors(&mut self) {
    let added = self
      .state
      .obs_space
      .check_sensors(&self.state.pose, self.state.robot_specs.sensor_radius);

    if !added.is_empty() {
      log::info!("Found {:?} obstacles", added.len());

      // Add newly added obstacles to the environment possibly creating orphans
      let added = self.state.obs_space.get_obstacles_as_obstacle_space(&added);

      // Add to environment possibly creating orphans
      self.add_obstacle_to_environment(&added);

      log::info!("Added {:?} obstacles to the environment", added.count());

      // Cleanup
      self.propagate_descendants();
      if let Some((_, _, idx)) = self.state.current_path {
        self.verrify_queue(idx);
      }
      log::info!("Reducing inconsistency");
      self.reduce_inconsistency();

      // TODO: validate path is still viable
    }

    // if let Some(current_path) = self.state.current_path.as_ref() {
    //   let path_to_goal = self
    //     .state
    //     .graph
    //     .get_optimal_path(current_path.2)
    //     .unwrap()
    //     .collect::<Vec<_>>();

    //   let last_idx = path_to_goal.last().unwrap().clone();

    //   if last_idx != self.state.graph.get_goal_idx() {
    //     self.state.current_path = None;
    //   }
    // }
  }

  fn get_obs_space(&self) -> &OS {
    &self.state.obs_space
  }

  fn get_obs_space_mut(&mut self) -> &mut OS {
    &mut self.state.obs_space
  }

  fn get_cspace(&self) -> &CS {
    &self.cspace
  }

  fn get_cspace_mut(&mut self) -> &mut CS {
    &mut self.cspace
  }

  fn check_nodes(&mut self, check_obs_space: bool, check_cspace: bool) {
    if !(check_obs_space || check_cspace) {
      // Nothing to do
      return;
    }

    // Set of edges that have been invalidated by either the cspace or obs_space
    let mut orphaned = false;

    if check_cspace {
      let mut invalidated_edges = Vec::new();

      // Check that all nodes are valid in the cspace
      let all_nodes = self.state.graph.all_nodes().collect::<Vec<_>>();
      let graph_ref = &self.state.graph;
      let cspace_ref = &self.cspace;

      let iter = all_nodes.into_par_iter().filter_map(|u_idx| {
        let u = graph_ref.get_node(u_idx).state();

        // Check cspace
        if !cspace_ref.is_free(u) {
          // Return list ouer all edges with u
          let edges = graph_ref
            .neighbor_set(u_idx, NeighborSet::Incoming)
            // .chain(graph_ref.neighbor_set(u_idx, NeighborSet::Outgoing))
            .map(|(edge_idx, _)| {
              let (v_idx, u_idx) = self.state.graph.get_endpoints(edge_idx);
              (v_idx, edge_idx, u_idx)
            });

          Some(edges)
        } else {
          None
        }
      });
      invalidated_edges.par_extend(iter.flatten_iter());

      // Proccess invalidated edges
      for &(v_idx, edge_idx, u_idx) in &invalidated_edges {
        self.state.graph.set_infinite_edge_cost(edge_idx);

        // if the parent of v is u, make v an orphan
        if self.state.graph.is_parent(v_idx, u_idx) {
          self.verrify_orphan(v_idx);
          orphaned = true;
        }

        // if u is the current target, remove it
        if let Some((_, _, move_goal_idx)) = self.state.current_path {
          if move_goal_idx == u_idx {
            self.state.current_path = None;
          }
        }
      }

      // Permanetly delete non-free nodes from the graph
      for &(_v_idx, _edge_idx, u_idx) in &invalidated_edges {
        if self.state.graph.check_node(u_idx) {
          self.delete_node(u_idx);
        }
      }
    }

    if check_obs_space {
      let mut invalidated_edges = HashSet::new();

      // Check that all edges are valid in the obs_space
      let all_edges = self.state.graph.all_edges().collect::<Vec<_>>();
      let graph_ref = &self.state.graph;
      let obs_space_ref = &self.state.obs_space;

      let iter = all_edges
        .into_par_iter()
        .filter_map(|edge_idx| {
          let trajectory = graph_ref.get_trajectory(edge_idx)?;
          match obs_space_ref.trajectory_free(&trajectory) {
            true => None,
            false => Some(edge_idx),
          }
        })
        .map(|edge_idx| {
          let (v_idx, u_idx) = self.state.graph.get_endpoints(edge_idx);
          (v_idx, edge_idx, u_idx)
        });
      invalidated_edges.par_extend(iter);

      // Proccess invalidated edges
      for &(v_idx, edge_idx, u_idx) in &invalidated_edges {
        self.state.graph.set_infinite_edge_cost(edge_idx);

        // if the parent of v is u, make v an orphan
        if self.state.graph.is_parent(v_idx, u_idx) {
          self.verrify_orphan(v_idx);
          orphaned = true;
        }

        // if u is the current target, remove it
        if let Some((_, _, move_goal_idx)) = self.state.current_path {
          if move_goal_idx == u_idx {
            self.state.current_path = None;
          }
        }
      }

      // invalidated_edges.into_par_iter()
    }

    // if nodes were orphaned
    if orphaned {
      self.propagate_descendants();
      if let Some((_, _, idx)) = self.state.current_path {
        self.verrify_queue(idx);
      }
      log::info!("Reducing inconsistency");
      self.reduce_inconsistency();
    }
  }

  fn update_pose(
    &mut self,
    pose: SVector<X, N>,
    nearest: bool,
  ) -> Option<MoveGoal<X, N>> {
    self.state.pose = pose;
    log::info!(
      "Updating pose to {:?}, cost to local goal: {:?}",
      <[X; N]>::from(pose),
      self.get_cost_to_move_goal()
    );

    // Check sensors from this new pose
    self.check_sensors();

    if !nearest {
      // Check that the next move goal is still valid for this new pose,
      // if so, return it
      // otherwise, determine a new move goal
      if let Some((start_pose, _, move_goal_idx)) = self.state.current_path {
        let move_goal = self.state.graph.get_node(move_goal_idx).state();
        let pose_ref = pose.index((.., 0));
        let mg_ref = move_goal.index((.., 0));

        if let Some(new_trajectory) = self.cspace.trajectory(pose_ref, mg_ref) {
          if self.trajectory_free(&new_trajectory) {
            // Trajectory is valid
            let init_dist_err = self.cspace.cost(&start_pose, move_goal);
            let rel_dist_err = self.cspace.cost(&pose, move_goal);

            if init_dist_err > rel_dist_err {
              // We are closer to the move goal than initially

              if init_dist_err - rel_dist_err
                > init_dist_err * self.state.params.portion
                || rel_dist_err < self.state.params.min_cost
              {
                // We have reached the goal we were aiming for

                // Check if we have reach the global goal node
                if self.state.graph.get_goal_idx() == move_goal_idx {
                  log::info!("  - Reached the finish!");
                  return Some(MoveGoal::Finished);
                }

                // Find the first viable parent in the optimal path that is more
                // than min_cost away
                log::info!(
                  "  - Reached move goal, looking for next along path"
                );
                let res = self.find_move_goal_along_path(&pose, move_goal_idx);

                // Update if found
                if let Some((new_trajectory, new_move_goal_idx)) = res {
                  self.state.current_path =
                    Some((pose, new_trajectory, new_move_goal_idx));
                  log::info!("  - New move goal found");
                  return Some(MoveGoal::New(*self.get_current_path()?.end()));
                } else {
                  log::info!("  - No valid move goal along path");
                }
              } else {
                // Keep the same move goal
                // log::info!("  - Keeping same move goal");
                return Some(MoveGoal::Old(*self.get_current_path()?.end()));
              }
            } else {
              // We are farther from the move goal than initial

              if rel_dist_err - init_dist_err
                > init_dist_err * self.state.params.portion
              {
                // We are out of range of the goal we were aiming for
                // Falling through to look for a new path
                log::info!("  - Out of range of move goal");
              } else {
                // Keep the same move goal
                log::info!("  - Keeping same move goal");
                return Some(MoveGoal::Old(*self.get_current_path()?.end()));
              }
            }
          } else {
            // Trajectory is blocked by an obstacle
            log::info!("  - Current move goal blocked by obstacle");
          }
        } else {
          // The trajectory is invalid
          log::info!("  - Trajectory to current move goal infesible");
        }
      }
    }

    // The current move goal is invalid, find a new one
    log::info!("  - Move goal invalid, looking for a new one");
    self.state.current_path = self.find_new_path(pose);
    Some(MoveGoal::New(*self.get_current_path()?.end()))
  }

  fn get_tree(&self) -> Vec<FullTrajRefOwned<X, CS::Traj, N>> {
    self
      .state
      .graph
      .all_edges()
      .filter(|&edge_idx| self.state.graph.get_edge(edge_idx).is_parent())
      .filter_map(|edge_idx| self.state.graph.get_trajectory(edge_idx))
      .collect()
  }

  fn get_current_path(&self) -> Option<FullTrajOwned<X, CS::Traj, N>> {
    let path = self.state.current_path.as_ref()?.clone();
    let start = path.0;
    let end = self.state.graph.get_node(path.2).state().clone();
    let traj = path.1;

    Some(FullTrajOwned::new(start, end, traj))
  }

  fn get_path_to_goal(&self) -> Option<Vec<SVector<X, N>>> {
    let path_to_goal = self
      .state
      .graph
      .get_optimal_path(self.state.current_path.as_ref()?.2)?
      .collect::<Vec<_>>();

    let last_idx = path_to_goal.last().unwrap().clone();

    if last_idx != self.state.graph.get_goal_idx() {
      log::error!(
        "Invalid node in path to goal: {} at {:?}",
        last_idx.index(),
        <[X; N]>::from(self.state.graph.get_node(last_idx).state().clone())
      );
      panic!("If path to goal is valid, it should lead to the goal");
    }

    Some(
      path_to_goal
        .into_iter()
        .map(|node_idx| self.state.graph.get_node(node_idx).state().clone())
        .collect(),
    )
  }

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

  fn get_state(&self) -> &Self::State {
    &self.state
  }

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

  fn count(&self) -> usize {
    // assert_eq!(self.kdtree.size(), self.state.graph.node_count());
    self.state.graph.node_count() + 1
  }
}

impl<X, CS, OS, const N: usize> Rrtx<X, CS, OS, N>
where
  X: Scalar,
  CS: CSpace<X, N> + Send + Sync,
  CS::Traj: Send + Sync,
  OS: ObstacleSpace<X, CS, N> + Send + Sync,
  OS::Obs: Send + Sync,
{
  /// Try to place a new node in the graph, returns None when unsuccessful
  fn try_create_node(&mut self) -> Option<()> {
    let mut v = self.cspace.sample();
    self.sampled += 1;

    let (cost, v_nearest) = self.nearest(&v);

    if cost > self.state.params.delta {
      self
        .cspace
        .saturate(&mut v, v_nearest, self.state.params.delta);
    }

    match self.is_free(&v) {
      true => {
        let v_idx = self.extend(v)?;
        self.rewire_neighbors(v_idx);
        self.reduce_inconsistency();
        Some(())
      }
      false => None,
    }
  }

  fn log_sampling_ratio(&self) {
    log::debug!(
      "sampled: {:?}, added: {:?}, sampling_ratio: {:?}",
      self.sampled,
      self.count() - 1, // Don't count goal node
      self.sampling_ratio()
    );
  }

  fn sampling_ratio(&self) -> f64 {
    let sampled = self.sampled;
    let added = self.count() - 1; // Don't count goal node
    (sampled as f64) / (added as f64)
  }

  /// Returns the updated shrinking ball radius
  fn shrinking_ball_radius(&self) -> X {
    let dims = X::from(N).unwrap();
    let num_vertices = X::from(self.count()).unwrap();

    let temp = <X as Float>::ln(num_vertices) / num_vertices;
    let temp = <X as Float>::powf(temp, <X as Float>::recip(dims));

    <X as Float>::min(self.state.params.gamma * temp, self.state.params.delta)
  }

  /// Warn if gamma is too low for a connected graph
  fn check_gamma(&self) {
    let d = X::from(N).unwrap();
    let one = X::one();
    let two = one + one;
    let one_over_d = <X as Float>::recip(d);
    let volume = self.cspace.volume();

    let temp1 = <X as Float>::powf(two * (one + one_over_d), one_over_d);
    let temp2 = <X as Float>::powf(volume / unit_d_ball_vol(N), one_over_d);

    let min_gamma = temp1 * temp2;

    log::info!(
      "Gamma: {:?}, Minimum Gamma: {:?}, volume: {:?}",
      self.state.params.gamma,
      min_gamma,
      volume
    );

    // Assert
    if !(self.state.params.gamma > min_gamma) {
      log::warn!("Gamma is too small to gaurantee a conected graph");
    }
  }

  /// Find the nearest node to v and the cost to get to it
  fn nearest(&self, v: &SVector<X, N>) -> (X, &SVector<X, N>) {
    let (cost, v_nearest_idx) = self
      .kdtree
      .iter_nearest(v.into(), &|a, b| {
        let a = VectorSlice::<X, Const<N>>::from_slice(a);
        let b = VectorSlice::<X, Const<N>>::from_slice(b);
        self.cspace.cost(&a, &b)
      })
      .expect("kdtree error")
      .filter_map(|(cost, &idx)| match self.state.graph.check_node(idx) {
        true => Some((cost, idx)),
        false => None,
      })
      .next()
      .unwrap();
    // let (cost, v_nearest_idx) = iter.next().unwrap();
    let v_nearest = self.state.graph.get_node(v_nearest_idx).state();
    (cost, v_nearest)
  }

  /// Try to extend the graph to include the given node
  /// Setup original and running neighbors
  fn extend(&mut self, v: SVector<X, N>) -> Option<NodeIndex> {
    let v_near = self.near(&v);

    let mut v = Node::new(v);
    let (_, parent_idx) = self.find_parent(&mut v, &v_near)?;

    let v_idx = self.state.graph.add_node(v);
    self
      .kdtree
      .add(
        self.state.graph.get_node(v_idx).state().clone().into(),
        v_idx,
      )
      .expect("kdtree error");

    // Add edges
    for u_idx in v_near {
      // v -> u
      let v_ref = self.state.graph.get_node(v_idx).state().index((.., 0));
      let u_ref = self.state.graph.get_node(u_idx).state().index((.., 0));
      let t_v_u = self.cspace.trajectory(v_ref, u_ref);

      if let Some(trajectory) = t_v_u {
        if self.trajectory_free(&trajectory) {
          let traj_data = trajectory.to_trajectory();
          // v -> u
          self.state.graph.add_edge(
            v_idx,
            u_idx,
            NeighborType::Original,
            NeighborType::Running,
            u_idx == parent_idx,
            traj_data.clone(),
          );

          // TODO: Make this optional based on a function in CSpace
          // u -> v
          self.state.graph.add_edge(
            u_idx,
            v_idx,
            NeighborType::Running,
            NeighborType::Original,
            false,
            traj_data,
          );
        }
      }

      // // u -> v
      // let u = self.state.graph.get_node(u_idx);
      // let v = self.state.graph.get_node(v_idx);
      // let t_u_v = self.cspace.trajectory(u.state(), v.state());

      // if let Some(traj_data) = t_u_v {
      //   let trajectory = FullTrajRef::new(u.state(), v.state(), &traj_data)v_ref, u_ref, trajectory;
      //   if self.trajectory_free(&trajectory) {

      //   }
      // }
    }

    Some(v_idx)
  }

  /// Return vector of nodes that are within self.state.radius
  fn near(&self, v: &SVector<X, N>) -> Vec<NodeIndex> {
    let result = self.kdtree.within(v.into(), self.state.radius, &|a, b| {
      let a = VectorSlice::<X, Const<N>>::from_slice(a);
      let b = VectorSlice::<X, Const<N>>::from_slice(b);
      self.cspace.cost(&a, &b)
    });

    match result {
      Ok(vec) => vec
        .into_iter()
        .filter_map(|(_, &idx)| match self.state.graph.check_node(idx) {
          true => Some(idx),
          false => None,
        })
        .collect(),
      Err(kdtree::ErrorKind::ZeroCapacity) => vec![],
      _ => panic!("kdtree error"),
    }
  }

  /// Searchs u_near for the best selection of a parent for v that minimzes the look-ahead cost estimate
  /// Returns Some(trajectory_to_parent, parent_idx) if a fesiable parent was found
  fn find_parent(
    &self,
    v: &mut Node<X, N>,
    u_near: &Vec<NodeIndex>,
  ) -> Option<(CS::Traj, NodeIndex)> {
    let mut parent = None;

    for &u_idx in u_near {
      // Check each node for a valid trajectory, keeping track of minimum viable path
      let u = self.state.graph.get_node(u_idx);
      let v_ref = v.state().index((.., 0));
      let u_ref = u.state().index((.., 0));

      // Check System dynamics
      if let Some(trajectory) = self.cspace.trajectory(v_ref, u_ref) {
        // Check if cost is lower through u
        let cost = trajectory.cost();
        if v.cost.lmc > cost + u.cost.lmc {
          // Check trajectory for intersections with obstacles
          if self.trajectory_free(&trajectory) {
            parent = Some((trajectory.to_trajectory(), u_idx));
            v.cost.lmc = cost + u.cost.lmc
          }
        }
      }
    }
    parent
  }

  /// Remove all edges that are shorter than self.state.radius
  /// Except original neighbors and edges part of the optimal subtree
  fn cull_neighbors(&mut self, v_idx: NodeIndex) {
    // Iterate over all outgoing running neighbors
    let mut out_run = self
      .state
      .graph
      .neighbor_set_walker(v_idx, NeighborSet::RunningOutgoing);
    while let Some((edge_idx, u_idx)) = out_run.next(&self.state.graph) {
      let neighbor_edge = self.state.graph.get_edge(edge_idx);
      let neighbor_edge_cost = self.state.graph.get_trajectory_cost(edge_idx);

      if self.state.radius < neighbor_edge_cost && !neighbor_edge.is_parent() {
        // remove running connections
        self.state.graph.remove_running_neighbor(v_idx, u_idx);
      }
    }
  }

  /// Rewires incoming neighbors to v if doing so lowers the cost of u
  fn rewire_neighbors(&mut self, v_idx: NodeIndex) {
    let v = self.state.graph.get_node(v_idx);
    if v.cost.g - v.cost.lmc > self.state.params.eps {
      // Cull neighbors of v
      self.cull_neighbors(v_idx);

      // Iterate over all incoming neighbors
      let mut incoming = self
        .state
        .graph
        .neighbor_set_walker(v_idx, NeighborSet::Incoming);
      while let Some((edge_idx, u_idx)) = incoming.next(&self.state.graph) {
        // Skip the parent of v
        if self.state.graph.is_parent(v_idx, u_idx) {
          continue;
        }

        // Gather data
        let v_cost = self.state.graph.get_node(v_idx).cost;
        let edge_cost = self.state.graph.get_trajectory_cost(edge_idx);

        // Check if u should be rewired
        let u = self.state.graph.get_node_mut(u_idx);
        if u.cost.lmc > edge_cost + v_cost.lmc {
          // Update cost and new parent of u
          u.cost.lmc = edge_cost + v_cost.lmc;
          self.state.graph.change_parent(u_idx, v_idx).unwrap();

          // Add u to queue if eps-inconsistent
          let u = self.state.graph.get_node(u_idx);
          if u.cost.g - u.cost.lmc > self.state.params.eps {
            self.verrify_queue(u_idx)
          }
        }
      }
    }
  }

  /// Iterate through the queue, reducing the inconsistency of the graph
  fn reduce_inconsistency(&mut self) {
    while let Some((_, &priority_peek)) = self.queue.peek() {
      // Empty queue if no valid move exists
      if let Some((_, _, idx)) = self.state.current_path {
        // Check that at least one of the other conditions are met on move_goal
        let move_goal = self.state.graph.get_node(idx);
        if !(priority_peek > move_goal.cost.priority()
          || move_goal.cost.lmc != move_goal.cost.g
          || move_goal.cost.g == X::infinity()
          || self.queue.get(&idx).is_some())
        {
          break;
        }
      }

      if let Some((v_idx, _)) = self.queue.pop() {
        let v = self.state.graph.get_node(v_idx);
        if v.cost.g - v.cost.lmc > self.state.params.eps {
          self.update_lmc(v_idx);
          self.rewire_neighbors(v_idx);
        }

        let v = self.state.graph.get_node_mut(v_idx);
        v.cost.g = v.cost.lmc;
      }
    }
  }

  /// Handles the set of orphans that have acumulated as obstacles have been added
  fn propagate_descendants(&mut self) {
    log::info!("Propagating descendants");
    // Orphans are already marked as they were added, see RrtxGraph::add_orphan

    // Add all the non-orphan outgoing neighbors of orphans to the queue
    let orphans: Vec<_> = self.state.graph.orphans().collect();
    for &v_idx in &orphans {
      // Iterate over all outgoing neighbors
      let mut outgoing = self
        .state
        .graph
        .neighbor_set_walker(v_idx, NeighborSet::Outgoing);
      while let Some(u_idx) = outgoing.next_node(&self.state.graph) {
        if !self.state.graph.is_orphan(u_idx) {
          let u = self.state.graph.get_node_mut(u_idx);
          u.cost.g = X::infinity();
          self.verrify_queue(u_idx);
        }
      }
    }

    // Remove all orphans, remove parent connection, set cost to infinity
    for v_idx in orphans {
      self.state.graph.remove_orphan(v_idx);
      let v = self.state.graph.get_node_mut(v_idx);

      v.cost = Cost::infinity();

      self.state.graph.remove_parent(v_idx);
    }
  }

  /// Updates the graph nodes and edges that become blocked by this obstacle
  fn add_obstacle_to_environment<O>(&mut self, obstacle: &O)
  where
    O: Obstacle<X, CS, N> + Send + Sync,
  {
    // Find all the edges that intersect with the obstacle
    let graph_ref = &self.state.graph;
    let iter = graph_ref.all_edges().collect::<Vec<_>>();
    let iter = iter.into_par_iter().filter_map(|edge_idx| {
      let trajectory = graph_ref.get_trajectory(edge_idx)?;
      match obstacle.trajectory_free(&trajectory) {
        true => None,
        false => {
          let (v_idx, u_idx) = graph_ref.get_endpoints(edge_idx);
          Some((v_idx, edge_idx, u_idx))
        }
      }
    });

    // Compute this iterator
    let vec: Vec<_> = iter.collect();

    for (v_idx, edge_idx, u_idx) in vec {
      self.state.graph.set_infinite_edge_cost(edge_idx);

      // if the parent of v is u, make v an orphan
      if self.state.graph.is_parent(v_idx, u_idx) {
        self.verrify_orphan(v_idx);
      }

      // if u is the current target, remove it
      if let Some((_, _, move_goal_idx)) = self.state.current_path {
        if move_goal_idx == u_idx {
          self.state.current_path = None;
        }
      }
    }
  }

  /// Updates the graph nodes and edges that were previously blocked by this obstacle
  fn _remove_obstacle_to_environment<O>(&mut self, obstacle: &O)
  where
    O: Obstacle<X, CS, N> + Send + Sync,
  {
    // Find all the edges that intersect with the obstacle
    let graph_ref = &self.state.graph;
    let iter = graph_ref.all_edges().collect::<Vec<_>>();
    let iter = iter.into_par_iter().filter(|&edge_idx| {
      match graph_ref.get_trajectory(edge_idx) {
        Some(trajectory) => !obstacle.trajectory_free(&trajectory),
        None => false,
      }
    });

    // Filter out edges that intersect with any other obstacles
    let obs_space_ref = &self.state.obs_space;
    let iter =
      iter.filter(|&edge_idx| match graph_ref.get_trajectory(edge_idx) {
        Some(trajectory) => obs_space_ref.trajectory_free(&trajectory),
        None => false,
      });

    // Add respective node indices
    let iter = iter.filter_map(|edge_idx| {
      let (v_idx, u_idx) = graph_ref.get_endpoints(edge_idx);
      Some((v_idx, edge_idx, u_idx))
    });

    // Compute this iterator
    let vec: Vec<_> = iter.collect();

    for (v_idx, _, _) in vec.iter() {
      // Iterate through outgoing edge of v
      let mut outgoing = self
        .state
        .graph
        .neighbor_set_walker(*v_idx, NeighborSet::Outgoing);
      while let Some(neighbor_edge) = outgoing.next_edge(&self.state.graph) {
        // Only check edges that are in vec
        let opt = vec
          .iter()
          .find(|(_, edge_idx, _)| neighbor_edge == *edge_idx);

        if let Some(n) = opt {
          let n1_ref = self.state.graph.get_node(n.0).state().index((.., 0));
          let n2_ref = self.state.graph.get_node(n.2).state().index((.., 0));
          let trajectory = self.cspace.trajectory(n1_ref, n2_ref);
          if let Some(trajectory) = trajectory {
            let trajectory = trajectory.to_trajectory();
            self.state.graph.update_trajectory(n.1, trajectory);
          };
        }
      }

      // Update LMC
      self.update_lmc(*v_idx);

      // Put v in queue if inconsistent
      let v = self.state.graph.get_node(*v_idx);
      if v.cost.lmc != v.cost.g {
        self.verrify_queue(*v_idx)
      }
    }
  }

  /// Check that the given coordinate does not intersect with any obstacles
  fn is_free(&self, v: &SVector<X, N>) -> bool {
    self.cspace.is_free(v) && self.state.obs_space.is_free(v)
  }

  /// Check that the given trajectory does not intersect with any obstacles
  fn trajectory_free<TF, S1, S2>(&self, t: &TF) -> bool
  where
    TF: FullTrajectory<X, CS::Traj, S1, S2, N>,
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>,
  {
    self.state.obs_space.trajectory_free(t)
  }

  /// Verifies v is in the queue and updates it's spot with possibily new cost
  fn verrify_queue(&mut self, v_idx: NodeIndex) {
    let node = self.state.graph.get_node(v_idx);
    self.queue.push(v_idx, node.cost.priority());
  }

  /// Ensures that v becomes an orphan
  fn verrify_orphan(&mut self, v_idx: NodeIndex) {
    self.queue.remove(&v_idx);
    self.state.graph.add_orphan(v_idx);
  }

  /// Update the lmc of v by checking neighbors of v for a better cost path
  fn update_lmc(&mut self, v_idx: NodeIndex) {
    // Possibly remove some neighbors
    self.cull_neighbors(v_idx);

    // Iterate over all outgoing neighbors
    let mut outgoing = self
      .state
      .graph
      .neighbor_set_walker(v_idx, NeighborSet::Outgoing);
    while let Some((edge_idx, u_idx)) = outgoing.next(&self.state.graph) {
      // Skip if u is an orphan
      if self.state.graph.is_orphan(u_idx) {
        continue;
      }

      // Skip if v is the parent of u
      if self.state.graph.is_parent(u_idx, v_idx) {
        continue;
      }

      // Gather data
      let edge_cost = self.state.graph.get_trajectory_cost(edge_idx);
      let u_cost = self.state.graph.get_node(u_idx).cost;

      // Check if v should be rewired
      let v = self.state.graph.get_node_mut(v_idx);
      if v.cost.lmc > edge_cost + u_cost.lmc {
        // Update cost and new parent of v
        v.cost.lmc = edge_cost + u_cost.lmc;
        self.state.graph.change_parent(v_idx, u_idx).unwrap();
      }
    }
  }

  /// Find the first viable parent in the optimal path that is more than min_cost away
  ///
  /// panics if move_goal_idx is an orphan
  fn find_move_goal_along_path(
    &self,
    pose: &SVector<X, N>,
    move_goal_idx: NodeIndex,
  ) -> Option<(CS::Traj, NodeIndex)> {
    // This is an invariant, if the move_goal exists then the optimal path exists
    let mut optimal_path_iter =
      self.state.graph.get_optimal_path(move_goal_idx).unwrap();

    optimal_path_iter.next(); // Pop off the current goal (move_goal_idx)

    // Seach the path for the first viable node that is more than min_cost away
    for node_idx in optimal_path_iter {
      log::info!("  --> Considering {:?}", node_idx);
      let node = self.state.graph.get_node(node_idx).state();

      if let Some(trajectory) =
        self.cspace.trajectory(pose.clone(), node.clone())
      {
        log::info!("  --> CSpace Trajectory Valid");
        if self.trajectory_free(&trajectory) {
          let cost = trajectory.cost();
          log::info!("  --> Trajectory Free, cost: {:?}", cost);

          // Valid trajectory, see if it is longer than min_cost,
          // disregarding min_cost if global goal is found
          if self.state.params.min_cost < cost
            || node_idx == self.state.graph.get_goal_idx()
          {
            return Some((trajectory.to_trajectory(), node_idx));
          }
          continue;
        }
      }
      // Invalid trajectory, cut off search if farther than delta
      let cost = self.cspace.cost(pose, node);
      if self.state.params.delta < cost {
        log::info!("Cutting search along path short");
        return None;
      }
    }
    None
  }

  /// Irreversibly delete the specified node from both the queue and the graph
  ///
  /// panics if `idx` is invalid
  fn delete_node(&mut self, v_idx: NodeIndex) {
    // Remove from queue if in it
    self.queue.remove(&v_idx);

    // remove from graph
    self.state.graph.delete_node(v_idx);
  }

  /// The current move goal is unreachable, this looks for a new one
  /// Returns Some new move goal if one is viable
  fn find_new_path(
    &self,
    pose: SVector<X, N>,
  ) -> Option<(SVector<X, N>, CS::Traj, NodeIndex)> {
    // Create temporary Node as the given pose
    let mut pose_node = Node::new(pose);

    // Create Iterator over all points in cost order
    let cost_func = |a: &[X], b: &[X]| {
      let a = VectorSlice::<X, Const<N>>::from_slice(a);
      let b = VectorSlice::<X, Const<N>>::from_slice(b);
      self.cspace.cost(&a, &b)
    };
    let mut iter = self
      .kdtree
      .iter_nearest(pose.as_slice(), &cost_func)
      .expect("kdtree error")
      .filter(|(_, &idx)| self.state.graph.check_node(idx));

    // Create first batch to determine the initial batch size
    let mut batch = Vec::new();
    loop {
      match iter.next() {
        Some((cost, &idx)) => {
          if cost < self.state.params.min_cost {
            continue;
          }
          if cost >= self.state.radius {
            break;
          }
          batch.push(idx);
        }
        None => break,
      }
    }

    // Search the first batch
    log::info!("  - Searching {:?} nearest nodes", batch.len());
    if let Some((traj, idx)) = self.find_parent(&mut pose_node, &batch) {
      log::info!("  - Found new move goal");
      return Some((pose, traj, idx));
    }

    // Search for a viable target in batches that double in size each loop
    // up until the cost is greater than delta
    loop {
      // Reset and create new batch of double size
      let batch_size = 2 * batch.len();
      batch.clear();
      while let Some((cost, &idx)) = iter.next() {
        if cost < self.state.params.min_cost {
          continue;
        }
        if self.state.params.delta < cost {
          log::warn!(
            "  - Cutting search short, min_cost and delta are too restrictive"
          );
          break;
        }

        batch.push(idx);
        if batch.len() >= batch_size {
          break;
        }
      }

      // Iterator stopped giving elements, we have searched every node
      if batch.is_empty() {
        log::info!("  - End of search, no new move goal found");
        return None;
      }

      // Search the batch
      log::info!("  - Searching next {:?} nearest nodes", batch.len());
      if let Some((traj, idx)) = self.find_parent(&mut pose_node, &batch) {
        log::info!("  - Found new move goal");
        return Some((pose, traj, idx));
      }
    }
  }
}

#[cfg(test)]
mod tests {

  use parry3d::math::Isometry;
  use parry3d::shape::{Ball, Cuboid, SharedShape};
  use rand::SeedableRng;

  use crate::cspace::EuclideanSpace;
  use crate::obstacles::obstacles_3d_f32::{Obstacle3df32, ObstacleSpace3df32};
  use crate::rng::RNG;
  use crate::util::bounds::Bounds;

  use super::*;

  const SEED: u64 = 0xe580e2e93fd6b040;

  #[test]
  fn test_rrtx() {
    let init = [5.0, 0.5, 5.0].into();
    let goal = [-5.0, 0.5, -5.0].into();

    let robot_specs = RobotSpecs {
      robot_radius: 0.1,
      sensor_radius: 2.0,
    };

    let bounds = Bounds::new([-5.0, 0.0, -5.0].into(), [5.0, 1.0, 5.0].into());
    let cspace = EuclideanSpace::new(bounds, RNG::seed_from_u64(SEED)).unwrap();

    let ball = Obstacle3df32::with_offset(
      SharedShape::new(Ball::new(0.5)),
      Isometry::translation(0.0, 0.5, 0.0),
    );

    let cube = Obstacle3df32::with_offset(
      SharedShape::new(Cuboid::new([0.5, 0.5, 0.5].into())),
      Isometry::translation(2.5, 0.5, 2.5),
    );

    let obs_space = ObstacleSpace3df32::from(vec![ball, cube]);

    let params = RrtxParams {
      min_cost: 0.0,
      portion: 0.1,
      delta: 1.0,
      gamma: 1067.0,
      eps: 0.01,
    };

    let mut rrtx =
      Rrtx::new(init, goal, robot_specs, cspace, obs_space, params).unwrap();

    let path = loop {
      rrtx.create_node();
      rrtx.update_pose(init, false);
      if let Some(path) = rrtx.get_path_to_goal() {
        break path;
      }
    };

    let mut cost = 0.0;
    for i in 0..path.len() - 1 {
      cost += path[i].metric_distance(&path[i + 1]);
    }
    println!("{:?}", rrtx.count());
    println!("{:?}", rrtx.state.radius);
    println!("{:?}", path);
    println!("{:?}", cost);
  }

  #[test]
  fn test_serialize_rrtx_state() {
    let init = [5.0, 0.5, 5.0].into();
    let goal = [-5.0, 0.5, -5.0].into();

    let robot_specs = RobotSpecs {
      robot_radius: 0.1,
      sensor_radius: 2.0,
    };

    let bounds = Bounds::new([-5.0, 0.0, -5.0].into(), [5.0, 1.0, 5.0].into());
    let cspace = EuclideanSpace::new(bounds, RNG::seed_from_u64(SEED)).unwrap();

    let ball = Obstacle3df32::with_offset(
      SharedShape::new(Ball::new(0.5)),
      Isometry::translation(0.0, 0.5, 0.0),
    );

    let cube = Obstacle3df32::with_offset(
      SharedShape::new(Cuboid::new([0.5, 0.5, 0.5].into())),
      Isometry::translation(2.5, 0.5, 2.5),
    );

    let obs_space = ObstacleSpace3df32::from(vec![ball, cube]);

    let params = RrtxParams {
      min_cost: 0.0,
      portion: 0.1,
      delta: 1.0,
      gamma: 1067.0,
      eps: 0.01,
    };

    let mut rrtx =
      Rrtx::new(init, goal, robot_specs, cspace, obs_space, params).unwrap();

    loop {
      rrtx.create_node();
      rrtx.update_pose(init, false);
      if let Some(_) = rrtx.get_path_to_goal() {
        break;
      }
    }

    let state = rrtx.get_state();
    let v = bincode::serialize(&state).unwrap();
    let _: RrtxState<f32, EuclideanSpace<f32, 3>, ObstacleSpace3df32, 3> =
      bincode::deserialize(&v).unwrap();
  }
}