path-planning 0.1.0

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

pub mod rrt_2d_f32;
pub mod rrt_2d_f64;
pub mod rrt_3d_f32;
pub mod rrt_3d_f64;

pub mod rrtstar_2d_f32;
pub mod rrtstar_2d_f64;
pub mod rrtstar_3d_f32;
pub mod rrtstar_3d_f64;

pub mod rrtx_2d_f32;
pub mod rrtx_2d_f64;
pub mod rrtx_3d_f32;
pub mod rrtx_3d_f64;

pub mod rrtx_2d_f32_dual;
pub mod rrtx_2d_f64_dual;
pub mod rrtx_3d_f32_dual;
pub mod rrtx_3d_f64_dual;

pub mod rrtx_2d_f32_dual_polar;
pub mod rrtx_2d_f64_dual_polar;

pub mod rrtx_3d_f32_dual_spherical;
pub mod rrtx_3d_f64_dual_spherical;

pub use rrt_2d_f32::Rrt2df32;
pub use rrt_2d_f64::Rrt2df64;
pub use rrt_3d_f32::Rrt3df32;
pub use rrt_3d_f64::Rrt3df64;

pub use rrtstar_2d_f32::RrtStar2df32;
pub use rrtstar_2d_f64::RrtStar2df64;
pub use rrtstar_3d_f32::RrtStar3df32;
pub use rrtstar_3d_f64::RrtStar3df64;

pub use rrtx_2d_f32::Rrtx2df32;
pub use rrtx_2d_f64::Rrtx2df64;
pub use rrtx_3d_f32::Rrtx3df32;
pub use rrtx_3d_f64::Rrtx3df64;

pub use rrtx_2d_f32_dual::Rrtx2df32Dual;
pub use rrtx_2d_f64_dual::Rrtx2df64Dual;
pub use rrtx_3d_f32_dual::Rrtx3df32Dual;
pub use rrtx_3d_f64_dual::Rrtx3df64Dual;

pub use rrtx_2d_f32_dual_polar::Rrtx2df32DualPolar;
pub use rrtx_2d_f64_dual_polar::Rrtx2df64DualPolar;

pub use rrtx_3d_f32_dual_spherical::Rrtx3df32DualSpherical;
pub use rrtx_3d_f64_dual_spherical::Rrtx3df64DualSpherical;

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

use std::convert::{From, Into};
use std::fmt::{Display, Error as FmtError, Formatter};
use std::result::Result as StdResult;
use std::str::FromStr;

use serde::{Deserialize, Serialize};

use crate::error::Result;
use crate::run::{self, Settings};

pub fn run_and_save(settings: Settings) -> Result<()> {
  let algo = Algo::from_str(&settings.algorithm).unwrap();

  match (algo, settings.double) {
    (Algo::Rrt2d, false) => {
      run::run_and_save::<Rrt2df32>(settings)?;
    }
    (Algo::Rrt2d, true) => {
      run::run_and_save::<Rrt2df64>(settings)?;
    }
    (Algo::Rrt3d, false) => {
      run::run_and_save::<Rrt3df32>(settings)?;
    }
    (Algo::Rrt3d, true) => {
      run::run_and_save::<Rrt3df64>(settings)?;
    }
    (Algo::RrtStar2d, false) => {
      run::run_and_save::<RrtStar2df32>(settings)?;
    }
    (Algo::RrtStar2d, true) => {
      run::run_and_save::<RrtStar2df64>(settings)?;
    }
    (Algo::RrtStar3d, false) => {
      run::run_and_save::<RrtStar3df32>(settings)?;
    }
    (Algo::RrtStar3d, true) => {
      run::run_and_save::<RrtStar3df64>(settings)?;
    }
    (Algo::Rrtx2d, false) => {
      run::run_and_save::<Rrtx2df32>(settings)?;
    }
    (Algo::Rrtx2d, true) => {
      run::run_and_save::<Rrtx2df64>(settings)?;
    }
    (Algo::Rrtx3d, false) => {
      run::run_and_save::<Rrtx3df32>(settings)?;
    }
    (Algo::Rrtx3d, true) => {
      run::run_and_save::<Rrtx3df64>(settings)?;
    }
    (Algo::Rrtx2dDual, false) => {
      run::run_and_save::<Rrtx2df32DualPolar>(settings)?;
    }
    (Algo::Rrtx2dDual, true) => {
      run::run_and_save::<Rrtx2df64DualPolar>(settings)?;
    }
    (Algo::Rrtx3dDual, false) => {
      run::run_and_save::<Rrtx3df32DualSpherical>(settings)?;
    }
    (Algo::Rrtx3dDual, true) => {
      run::run_and_save::<Rrtx3df64DualSpherical>(settings)?;
    }
  };
  Ok(())
}

/// Enumeration of all different algorithms implemented
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum Algo {
  Rrt2d,
  Rrt3d,
  RrtStar2d,
  RrtStar3d,
  Rrtx2d,
  Rrtx3d,
  Rrtx2dDual,
  Rrtx3dDual,
}

impl Display for Algo {
  fn fmt(&self, f: &mut Formatter<'_>) -> StdResult<(), FmtError> {
    f.write_str(self.into())
  }
}

impl From<&Algo> for &'_ str {
  fn from(algo: &Algo) -> &'static str {
    (*algo).into()
  }
}

impl From<Algo> for &'_ str {
  fn from(algo: Algo) -> &'static str {
    match algo {
      Algo::Rrt2d => "rrt-2d",
      Algo::Rrt3d => "rrt-3d",
      Algo::RrtStar2d => "rrtstar-2d",
      Algo::RrtStar3d => "rrtstar-3d",
      Algo::Rrtx2d => "rrtx-2d",
      Algo::Rrtx3d => "rrtx-3d",
      Algo::Rrtx2dDual => "rrtx-2d-dual",
      Algo::Rrtx3dDual => "rrtx-3d-dual",
    }
  }
}

impl FromStr for Algo {
  type Err = ();
  fn from_str(s: &str) -> StdResult<Self, Self::Err> {
    match &s.to_lowercase()[..] {
      "rrt-2d" => Ok(Algo::Rrt2d),
      "rrt-3d" => Ok(Algo::Rrt3d),
      "rrtstar-2d" => Ok(Algo::RrtStar2d),
      "rrtstar-3d" => Ok(Algo::RrtStar3d),
      "rrtx-2d" => Ok(Algo::Rrtx2d),
      "rrtx-3d" => Ok(Algo::Rrtx3d),
      "rrtx-2d-dual" => Ok(Algo::Rrtx2dDual),
      "rrtx-3d-dual" => Ok(Algo::Rrtx3dDual),
      _ => Err(()),
    }
  }
}

struct Iter(Option<Algo>);

impl Algo {
  fn next(&self) -> Option<Self> {
    match self {
      Algo::Rrt2d => Some(Algo::Rrt3d),
      Algo::Rrt3d => Some(Algo::RrtStar2d),
      Algo::RrtStar2d => Some(Algo::RrtStar3d),
      Algo::RrtStar3d => Some(Algo::Rrtx2d),
      Algo::Rrtx2d => Some(Algo::Rrtx3d),
      Algo::Rrtx3d => Some(Algo::Rrtx2dDual),
      Algo::Rrtx2dDual => Some(Algo::Rrtx3dDual),
      Algo::Rrtx3dDual => None,
    }
  }
}

impl Iterator for Iter {
  type Item = Algo;
  fn next(&mut self) -> Option<Self::Item> {
    let this = self.0?;
    self.0 = this.next();
    Some(this)
  }
}

impl Algo {
  pub fn long_help() -> String {
    let mut s = String::from("Which algorithm to run. Supported algorithms:\n");

    for ref algo in Algo::all() {
      s.push_str(algo.into());
      s.push_str(" ");
    }
    s
  }

  fn all() -> Iter {
    Iter(Some(Algo::Rrt2d))
  }
}