use core::cmp::Ordering;
use csv::StringRecord;
use fnv::{FnvHashMap, FnvHashSet};
use humantime::format_duration;
use permutohedron::LexicalPermutation;
use rstar::{PointDistance, RTree, RTreeObject, AABB};
use sha3::{Digest, Sha3_256};
use std::collections::VecDeque;
use std::fs::File;
use std::hash::{Hash, Hasher};
use std::io::Seek;
use std::io::{BufRead, BufReader, BufWriter, Write};
use std::path::PathBuf;
use std::str::FromStr;
use std::time::Instant;
use structopt::StructOpt;

use crate::common::{System, SystemSerde};

#[derive(Debug, StructOpt)]
pub struct RouteOpts {
    #[structopt(short, long = "range")]
    /// Jump Range
    pub range: Option<f32>,
    #[structopt(
        parse(from_os_str),
        short = "i",
        long = "path",
        default_value = "./stars.csv"
    )]
    /// Path to stars.csv
    ///
    /// Generate using ed_lrr_pp
    pub file_path: PathBuf,

    #[structopt(
        parse(from_os_str),
        long = "precomp_file",
        conflicts_with = "precompute"
    )]
    /// Path to precomputed route graph
    ///
    /// Generate using --precompute option
    pub precomp_file: Option<PathBuf>,

    #[structopt(
        short = "c",
        long = "precompute",
        conflicts_with = "full_permute",
        conflicts_with = "permute",
        conflicts_with = "precomp_file"
    )]
    /// Precompute all routes for the specified jump range starting at the specified system and write the result to {system}_{range}.bin
    pub precompute: bool,

    #[structopt(short = "p", long = "permute", conflicts_with = "full_permute")]
    /// Permute intermediate hops to find shortest route
    pub permute: bool,

    #[structopt(short = "o", long = "primary")]
    /// Only route through the primary star of a system
    pub primary: bool,

    #[structopt(short = "f", long = "full_permute", conflicts_with = "permute")]
    /// Permute all hops to find shortest route
    pub full_permute: bool,

    #[structopt(short = "g", long = "factor")]
    /// Greedyness factor for A-Star (0=BFS, inf=Greedy)
    pub factor: Option<f32>,

    #[structopt(
        short,
        long,
        raw(possible_values = "&[\"bfs\", \"greedy\",\"astar\"]"),
        default_value = "bfs"
    )]
    /// Search mode
    ///
    /**
     - BFS is guaranteed to find the shortest route but very slow
    - Greedy is a lot faster but will probably not find the shortest route
    - A-Star is a good middle ground between speed and accuracy
    */
    pub mode: Mode,
    /// Systems to route through
    pub systems: Vec<String>,
}

#[derive(Debug)]
pub enum Mode {
    BFS,
    Greedy,
    AStar,
}

impl FromStr for Mode {
    type Err = String;
    fn from_str(s: &str) -> Result<Mode, String> {
        match s {
            "bfs" => Ok(Mode::BFS),
            "greedy" => Ok(Mode::Greedy),
            "astar" => Ok(Mode::AStar),
            _ => Err("Invalid Mode".to_string()),
        }
    }
}

fn dist2(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
    let dx = p1[0] - p2[0];
    let dy = p1[1] - p2[1];
    let dz = p1[2] - p2[2];

    dx * dx + dy * dy + dz * dz
}

fn dist(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
    dist2(p1, p2).sqrt()
}

fn fcmp(a: f32, b: f32) -> Ordering {
    match (a, b) {
        (x, y) if x.is_nan() && y.is_nan() => Ordering::Equal,
        (x, _) if x.is_nan() => Ordering::Greater,
        (_, y) if y.is_nan() => Ordering::Less,
        (..) => a.partial_cmp(&b).unwrap(),
    }
}

impl System {
    pub fn dist2(&self, p: &[f32; 3]) -> f32 {
        dist2(&self.pos, p)
    }
    pub fn distp(&self, p: &System) -> f32 {
        dist(&self.pos, &p.pos)
    }
    pub fn distp2(&self, p: &System) -> f32 {
        self.dist2(&p.pos)
    }
}
impl PartialEq for System {
    fn eq(&self, other: &Self) -> bool {
        self.id == other.id
    }
}

impl Eq for System {}

impl Hash for System {
    fn hash<H: Hasher>(&self, state: &mut H) {
        self.id.hash(state);
    }
}

impl RTreeObject for System {
    type Envelope = AABB<[f32; 3]>;

    fn envelope(&self) -> Self::Envelope {
        AABB::from_point(self.pos)
    }
}

impl PointDistance for System {
    fn distance_2(&self, point: &[f32; 3]) -> f32 {
        self.dist2(&point)
    }
}

fn hash_file(path: &PathBuf) -> Vec<u8> {
    let mut hash_reader = BufReader::new(File::open(path).unwrap());
    let mut hasher = Sha3_256::new();
    std::io::copy(&mut hash_reader, &mut hasher).unwrap();
    hasher.result().iter().copied().collect()
}

struct LineCache {
    cache: Vec<u64>,
    file: BufReader<File>,
    header: Option<StringRecord>,
}

impl LineCache {
    pub fn new(path: &PathBuf) -> std::io::Result<Self> {
        let idx_path = path.with_extension("idx");
        let t_load = Instant::now();
        println!("Loading {}", path.to_str().unwrap());
        let mut idx_reader = BufReader::new(File::open(idx_path)?);
        let cache = match bincode::deserialize_from(&mut idx_reader) {
            Ok(value) => value,
            err => err.unwrap(),
        };
        let mut reader = BufReader::new(File::open(path)?);
        let header = Self::read_record(&mut reader);
        let ret = Self {
            file: reader,
            cache,
            header,
        };
        println!("Done in {}!", format_duration(t_load.elapsed()));
        Ok(ret)
    }
    fn read_record(reader: &mut BufReader<File>) -> Option<StringRecord> {
        let mut line = String::new();
        reader.read_line(&mut line).ok()?;
        let v: Vec<_> = line.trim_end().split(',').collect();
        let rec = StringRecord::from(v);
        Some(rec)
    }
    pub fn get(&mut self, id: u32) -> Option<System> {
        let pos = self.cache[id as usize];
        self.file.seek(std::io::SeekFrom::Start(pos)).unwrap();
        let rec = Self::read_record(&mut self.file).unwrap();
        let sys: SystemSerde = rec.deserialize(self.header.as_ref()).unwrap();
        Some(sys.build())
    }
}

pub struct Router {
    tree: RTree<System>,
    scoopable: FnvHashSet<u32>,
    pub route_tree: Option<FnvHashMap<u32, u32>>,
    cache: Option<LineCache>,
    range: f32,
    primary_only: bool,
    path: PathBuf,
}

impl Router {
    pub fn new(path: &PathBuf, range: f32, primary_only: bool) -> Self {
        let mut scoopable = FnvHashSet::default();
        let mut reader = csv::ReaderBuilder::new()
            .from_path(path)
            .unwrap_or_else(|e| {
                println!("Error opening {}: {}", path.to_str().unwrap(), e);
                std::process::exit(1);
            });
        let t_load = Instant::now();
        println!("Loading {}...", path.to_str().unwrap());
        let systems: Vec<System> = reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .filter(|sys| {
                if primary_only {
                    sys.distance == 0
                } else {
                    true
                }
            })
            .map(|sys| {
                if sys.mult > 1.0f32 {
                    scoopable.insert(sys.id);
                } else {
                    for c in "KGBFOAM".chars() {
                        if sys.star_type.starts_with(c) {
                            scoopable.insert(sys.id);
                            break;
                        }
                    }
                }
                sys.build()
            })
            .collect();
        println!("Building RTree...");
        let ret = Self {
            tree: RTree::bulk_load(systems),
            scoopable,
            route_tree: None,
            range,
            primary_only,
            cache: LineCache::new(path).ok(),
            path: path.clone(),
        };
        println!(
            "{} Systems loaded in {}",
            ret.tree.size(),
            format_duration(t_load.elapsed())
        );
        ret
    }

    pub fn from_file(filename: &PathBuf) -> (PathBuf, Self) {
        let t_load = Instant::now();
        let mut reader = BufReader::new(File::open(&filename).unwrap());
        println!("Loading {}", filename.to_str().unwrap());
        let (primary, range, file_hash, path, route_tree): (
            bool,
            f32,
            Vec<u8>,
            String,
            FnvHashMap<u32, u32>,
        ) = bincode::deserialize_from(&mut reader).unwrap();
        let path = PathBuf::from(path);
        println!("Done in {}!", format_duration(t_load.elapsed()));
        if hash_file(&path) != file_hash {
            panic!("File hash mismatch!")
        }
        let cache = LineCache::new(&path).ok();
        (
            path.clone(),
            Self {
                tree: RTree::default(),
                scoopable: FnvHashSet::default(),
                route_tree: Some(route_tree),
                range,
                cache,
                primary_only: primary,
                path,
            },
        )
    }

    fn closest(&self, point: &[f32; 3]) -> &System {
        self.tree.nearest_neighbor(point).unwrap()
    }
    fn points_in_sphere(&self, center: &[f32; 3], radius: f32) -> impl Iterator<Item = &System> {
        self.tree.locate_within_distance(*center, radius * radius)
    }

    fn neighbours(&self, sys: &System, r: f32) -> impl Iterator<Item = &System> {
        self.points_in_sphere(&sys.pos, sys.mult * r)
    }

    fn valid(&self, sys: &System) -> bool {
        self.scoopable.contains(&sys.id)
    }

    pub fn best_name_multiroute(
        &self,
        waypoints: &[String],
        range: f32,
        full: bool,
        mode: Mode,
        factor: f32,
    ) -> Vec<System> {
        let mut best_score: f32 = std::f32::MAX;
        let mut waypoints = waypoints.to_owned();
        let mut best_permutation_waypoints = waypoints.to_owned();
        let first = waypoints.first().cloned();
        let last = waypoints.last().cloned();
        let t_start = Instant::now();
        println!("Finding best permutation of hops...");
        while waypoints.prev_permutation() {}
        loop {
            let c_first = waypoints.first().cloned();
            let c_last = waypoints.last().cloned();
            if full || ((c_first == first) && (c_last == last)) {
                let mut total_d = 0.0;
                for pair in waypoints.windows(2) {
                    match pair {
                        [src, dst] => {
                            let (mut src, dst) =
                                (self.name_to_systems(&src), self.name_to_systems(&dst));
                            src.sort_by_key(|&p| (p.mult * 10.0) as u8);
                            let src = src.last().unwrap();
                            let dst = dst.last().unwrap();
                            total_d += src.distp2(dst);
                        }
                        _ => panic!("Invalid routing parameters!"),
                    }
                }
                if total_d < best_score {
                    best_score = total_d;
                    best_permutation_waypoints = waypoints.to_owned();
                }
            }
            if !waypoints.next_permutation() {
                break;
            }
        }

        println!("Done in {}!", format_duration(t_start.elapsed()));
        println!("Best permutation: {:?}", best_permutation_waypoints);
        self.name_multiroute(&best_permutation_waypoints, range, mode, factor)
    }

    pub fn name_multiroute(
        &self,
        waypoints: &[String],
        range: f32,
        mode: Mode,
        factor: f32,
    ) -> Vec<System> {
        let mut coords = Vec::new();
        for p_name in waypoints {
            let mut p_l = self.name_to_systems(p_name);
            p_l.sort_by_key(|&p| (p.mult * 10.0) as u8);
            let p = p_l.last().unwrap();
            coords.push((p_name, p.pos));
        }
        self.multiroute(coords.as_slice(), range, mode, factor)
    }
    pub fn multiroute(
        &self,
        waypoints: &[(&String, [f32; 3])],
        range: f32,
        mode: Mode,
        factor: f32,
    ) -> Vec<System> {
        let mut route: Vec<System> = Vec::new();
        for pair in waypoints.windows(2) {
            match *pair {
                [src, dst] => {
                    let block = match mode {
                        Mode::BFS => self.route_bfs(&src, &dst, range),
                        Mode::Greedy => self.route_greedy(&src, &dst, range),
                        Mode::AStar => self.route_astar(&src, &dst, range, factor),
                    };
                    if route.is_empty() {
                        for sys in block.iter() {
                            route.push(sys.clone());
                        }
                    } else {
                        for sys in block.iter().skip(1) {
                            route.push(sys.clone());
                        }
                    }
                }
                _ => panic!("Invalid routing parameters!"),
            }
        }
        route
    }

    fn name_to_systems(&self, name: &str) -> Vec<&System> {
        for sys in &self.tree {
            if sys.system == name {
                return self.neighbours(&sys, 0.0).collect();
            }
        }
        eprintln!("System not found: \"{}\"", name);
        std::process::exit(1);
    }

    pub fn route_astar(
        &self,
        src: &(&String, [f32; 3]),
        dst: &(&String, [f32; 3]),
        range: f32,
        factor: f32,
    ) -> Vec<System> {
        if factor == 0.0 {
            return self.route_bfs(src, dst, range);
        }
        println!("Running A-Star with greedy factor of {}", factor);
        let (src_name, src) = src;
        let (dst_name, dst) = dst;
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        {
            let d = dist(src, dst);
            println!("Plotting route from {} to {}...", src_name, dst_name);
            println!(
                "Jump Range: {} Ly, Distance: {} Ly, Theoretical Jumps: {}",
                range,
                d,
                d / range
            );
        }
        let total = self.tree.size() as f32;
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let t_start = Instant::now();
        let mut found = false;
        let mut maxd = 0;
        let mut queue: Vec<(usize, usize, &System)> = Vec::new();
        queue.push((
            0,                                            // depth
            (start_sys.distp(goal_sys) / range) as usize, // h
            &start_sys,
        ));
        seen.insert(start_sys.id);

        while !(queue.is_empty() || found) {
            while let Some((depth, _, sys)) = queue.pop() {
                if depth > maxd {
                    maxd = depth;
                    print!(
                        "[{}] Depth: {}, Queue: {}, Seen: {} ({:.02}%)     \r",
                        format_duration(t_start.elapsed()),
                        depth,
                        queue.len(),
                        seen.len(),
                        ((seen.len() * 100) as f32) / total
                    );
                    std::io::stdout().flush().unwrap();
                }
                if sys.id == goal_sys.id {
                    found = true;
                    break;
                }
                queue.extend(
                    self.neighbours(&sys, range)
                        .filter(|&nb| (self.valid(nb) || (nb.id == goal_sys.id)))
                        .filter(|&nb| seen.insert(nb.id))
                        .map(|nb| {
                            prev.insert(nb.id, sys);
                            let d_g = (nb.distp(goal_sys) / range) as usize;
                            (depth + 1, d_g, nb)
                        }),
                );
                queue.sort_by(|b, a| {
                    let (a_0, a_1) = (a.0 as f32, a.1 as f32);
                    let (b_0, b_1) = (b.0 as f32, b.1 as f32);
                    let v_a = a_0 + a_1 * factor;
                    let v_b = b_0 + b_1 * factor;
                    fcmp(v_a, v_b)
                });
                // queue.reverse();
            }
        }
        println!();

        println!();
        if !found {
            eprintln!("No route from {} to {} found!", src_name, dst_name);
            return Vec::new();
        }
        let mut v: Vec<System> = Vec::new();
        let mut curr_sys = goal_sys;
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = *sys,
                None => {
                    break;
                }
            }
        }
        v.reverse();
        v
    }

    pub fn route_greedy(
        &self,
        src: &(&String, [f32; 3]),
        dst: &(&String, [f32; 3]),
        range: f32,
    ) -> Vec<System> {
        println!("Running Greedy-Search");
        let (src_name, src) = src;
        let (dst_name, dst) = dst;
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        {
            let d = dist(src, dst);
            println!("Plotting route from {} to {}...", src_name, dst_name);
            println!(
                "Jump Range: {} Ly, Distance: {} Ly, Theoretical Jumps: {}",
                range,
                d,
                d / range
            );
        }
        let total = self.tree.size() as f32;
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let t_start = Instant::now();
        let mut found = false;
        let mut maxd = 0;
        let mut queue: Vec<(f32, f32, usize, &System)> = Vec::new();
        queue.push((-goal_sys.mult, start_sys.distp2(goal_sys), 0, &start_sys));
        seen.insert(start_sys.id);
        while !(queue.is_empty() || found) {
            while let Some((_, _, depth, sys)) = queue.pop() {
                if depth > maxd {
                    maxd = depth;
                    print!(
                        "[{}] Depth: {}, Queue: {}, Seen: {} ({:.02}%)     \r",
                        format_duration(t_start.elapsed()),
                        depth,
                        queue.len(),
                        seen.len(),
                        ((seen.len() * 100) as f32) / total
                    );
                    std::io::stdout().flush().unwrap();
                }
                if sys.id == goal_sys.id {
                    found = true;
                    break;
                }
                queue.extend(
                    self.neighbours(&sys, range)
                        .filter(|&nb| (self.valid(nb) || (nb.id == goal_sys.id)))
                        .filter(|&nb| seen.insert(nb.id))
                        .map(|nb| {
                            prev.insert(nb.id, sys);
                            (-nb.mult, nb.distp2(goal_sys), depth + 1, nb)
                        }),
                );
                queue.sort_by(|a, b| fcmp(a.0, b.0).then(fcmp(a.1, b.1)));
                queue.reverse();
            }
        }
        println!();
        println!();
        if !found {
            eprintln!("No route from {} to {} found!", src_name, dst_name);
            return Vec::new();
        }
        let mut v: Vec<System> = Vec::new();
        let mut curr_sys = goal_sys;
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = *sys,
                None => {
                    break;
                }
            }
        }
        v.reverse();
        v
    }

    pub fn precompute(&mut self, src: &str) {
        let mut sys_l = self.name_to_systems(src);
        sys_l.sort_by_key(|&sys| (sys.mult * 10.0) as u8);
        let sys = sys_l.last().unwrap();
        println!("Precomputing routes starting at {} ...", sys.system);
        let total = self.tree.size() as f32;
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let t_start = Instant::now();
        let mut depth = 0;
        let mut queue: VecDeque<(usize, &System)> = VecDeque::new();
        let mut queue_next: VecDeque<(usize, &System)> = VecDeque::new();
        queue.push_front((0, &sys));
        seen.insert(sys.id);
        while !queue.is_empty() {
            print!(
                "[{}] Depth: {}, Queue: {}, Seen: {} ({:.02}%)     \r",
                format_duration(t_start.elapsed()),
                depth,
                queue.len(),
                seen.len(),
                ((seen.len() * 100) as f32) / total
            );
            std::io::stdout().flush().unwrap();
            while let Some((d, sys)) = queue.pop_front() {
                queue_next.extend(
                    self.neighbours(&sys, self.range)
                        // .filter(|&nb| self.valid(nb))
                        .filter(|&nb| seen.insert(nb.id))
                        .map(|nb| {
                            prev.insert(nb.id, sys.id);
                            (d + 1, nb)
                        }),
                );
            }
            std::mem::swap(&mut queue, &mut queue_next);
            depth += 1;
        }
        self.route_tree = Some(prev);
        let ofn = format!(
            "{}_{}{}.router",
            src.replace("*", "").replace(" ", "_"),
            self.range,
            if self.primary_only { "_primary" } else { "" }
        );
        println!("\nSaving to {}", ofn);
        let mut out_fh = BufWriter::new(File::create(&ofn).unwrap());
        // (range, path, route_tree)
        let data = (
            self.primary_only,
            self.range,
            hash_file(&self.path),
            String::from(self.path.to_str().unwrap()),
            self.route_tree.as_ref().unwrap(),
        );
        bincode::serialize_into(&mut out_fh, &data).unwrap();
    }

    fn get_systems_by_ids(&mut self, path: &PathBuf, ids: &[u32]) -> FnvHashMap<u32, System> {
        println!("Processing {}", path.to_str().unwrap());
        let mut ret = FnvHashMap::default();
        if let Some(c) = &mut self.cache.as_mut() {
            let mut missing = false;
            for id in ids {
                match c.get(*id) {
                    Some(sys) => {
                        ret.insert(*id, sys);
                    }
                    None => {
                        println!("ID {} not found in cache", id);
                        missing = true;
                        break;
                    }
                }
            }
            if !missing {
                return ret;
            }
        }
        let mut reader = csv::ReaderBuilder::new()
            .from_path(path)
            .unwrap_or_else(|e| {
                println!("Error opening {}: {}", path.to_str().unwrap(), e);
                std::process::exit(1);
            });
        reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .filter(|sys| ids.contains(&sys.id))
            .map(|sys| {
                ret.insert(sys.id, sys.build());
            })
            .last()
            .unwrap_or_else(|| {
                eprintln!("Error: No systems matching {:?} found!", ids);
                std::process::exit(1);
            });
        ret
    }

    fn get_system_by_name(path: &PathBuf, name: &str) -> System {
        let mut reader = csv::ReaderBuilder::new()
            .from_path(path)
            .unwrap_or_else(|e| {
                eprintln!("Error opening {}: {}", path.to_str().unwrap(), e);
                std::process::exit(1);
            });
        let sys = reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .find(|sys| sys.system == name)
            .unwrap_or_else(|| {
                eprintln!("Error: System '{}' not found!", name);
                std::process::exit(1);
            });
        sys.build()
    }

    pub fn route_to(&mut self, dst: &str, systems_path: &PathBuf) -> Vec<System> {
        let prev = self.route_tree.as_ref().unwrap();
        let dst = Self::get_system_by_name(&systems_path, dst);
        if !prev.contains_key(&dst.id) {
            eprintln!("System-ID {} not found", dst.id);
            std::process::exit(1);
        };
        let mut v_ids: Vec<u32> = Vec::new();
        let mut v: Vec<System> = Vec::new();
        let mut curr_sys: u32 = dst.id;
        loop {
            v_ids.push(curr_sys);
            match prev.get(&curr_sys) {
                Some(sys_id) => curr_sys = *sys_id,
                None => {
                    break;
                }
            }
        }
        v_ids.reverse();
        let id_map = self.get_systems_by_ids(&systems_path, &v_ids);
        for sys_id in v_ids {
            let sys = id_map.get(&sys_id).unwrap();
            v.push(sys.clone())
        }
        v
    }

    pub fn route_bfs(
        &self,
        src: &(&String, [f32; 3]),
        dst: &(&String, [f32; 3]),
        range: f32,
    ) -> Vec<System> {
        println!("Running BFS");
        let (src_name, src) = src;
        let (dst_name, dst) = dst;
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        {
            let d = dist(src, dst);
            println!("Plotting route from {} to {}...", src_name, dst_name);
            println!(
                "Jump Range: {} Ly, Distance: {} Ly, Theoretical Jumps: {}",
                range,
                d,
                d / range
            );
        }
        let total = self.tree.size() as f32;
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let t_start = Instant::now();
        let mut depth = 0;
        let mut found = false;
        let mut queue: VecDeque<(usize, &System)> = VecDeque::new();
        let mut queue_next: VecDeque<(usize, &System)> = VecDeque::new();
        queue.push_front((0, &start_sys));
        seen.insert(start_sys.id);
        while !(queue.is_empty() || found) {
            print!(
                "[{}] Depth: {}, Queue: {}, Seen: {} ({:.02}%)     \r",
                format_duration(t_start.elapsed()),
                depth,
                queue.len(),
                seen.len(),
                ((seen.len() * 100) as f32) / total
            );
            std::io::stdout().flush().unwrap();
            while let Some((d, sys)) = queue.pop_front() {
                if sys.id == goal_sys.id {
                    found = true;
                    break;
                }
                queue_next.extend(
                    self.neighbours(&sys, range)
                        .filter(|&nb| (self.valid(nb) || (nb.id == goal_sys.id)))
                        .filter(|&nb| seen.insert(nb.id))
                        .map(|nb| {
                            prev.insert(nb.id, sys);
                            (d + 1, nb)
                        }),
                );
            }
            std::mem::swap(&mut queue, &mut queue_next);
            depth += 1;
        }
        println!();
        println!();
        if !found {
            eprintln!("No route from {} to {} found!", src_name, dst_name);
            return Vec::new();
        }
        let mut v: Vec<System> = Vec::new();
        let mut curr_sys = goal_sys;
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = *sys,
                None => {
                    break;
                }
            }
        }
        v.reverse();
        v
    }
}
pub fn route(opts: RouteOpts) -> std::io::Result<()> {
    if opts.systems.is_empty() {
        if opts.precomp_file.is_some() {
            eprintln!("Error: Please specify exatly one system");
        } else if opts.precompute {
            eprintln!("Error: Please specify at least one system");
        } else {
            eprintln!("Error: Please specify at least two systems");
        }
        std::process::exit(1);
    }
    let mut path = opts.file_path;
    let mut router: Router = if opts.precomp_file.is_some() {
        let ret = Router::from_file(&opts.precomp_file.clone().unwrap());
        path = ret.0;
        ret.1
    } else {
        Router::new(&path, opts.range.unwrap(), opts.primary)
    };
    if opts.precompute {
        for sys in opts.systems {
            router.route_tree = None;
            router.precompute(&sys);
        }
        std::process::exit(0);
    }
    let t_route = Instant::now();
    let route = if router.route_tree.is_some() {
        router.route_to(opts.systems.first().unwrap(), &path)
    } else if opts.permute || opts.full_permute {
        router.best_name_multiroute(
            &opts.systems,
            opts.range.unwrap(),
            opts.full_permute,
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )
    } else {
        router.name_multiroute(
            &opts.systems,
            opts.range.unwrap(),
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )
    };
    println!("Route computed in {}\n", format_duration(t_route.elapsed()));
    if route.is_empty() {
        eprintln!("No route found!");
        return Ok(());
    }
    let mut total: f32 = 0.0;
    for (sys1, sys2) in route.iter().zip(route.iter().skip(1)) {
        let dist = sys1.distp(sys2);
        total += dist;
        println!(
            "{} [{}] ({},{},{}) [{} Ls]: {:.2} Ly",
            sys1.system,
            sys1.star_type,
            sys1.pos[0],
            sys1.pos[1],
            sys1.pos[2],
            sys1.distance,
            dist
        );
    }
    let sys = route.iter().last().unwrap();
    println!(
        "{} [{}] ({},{},{}) [{} Ls]: {:.2} Ly",
        sys.system, sys.star_type, sys.pos[0], sys.pos[1], sys.pos[2], sys.distance, 0.0
    );
    println!("Total: {:.2} Ly ({} Jumps)", total, route.len());
    Ok(())
}