use crate::common::{System, SystemSerde};
use core::cmp::Ordering;
use csv::StringRecord;
use fnv::{FnvHashMap, FnvHashSet};
use humantime::format_duration;
use permutohedron::LexicalPermutation;
use pyo3::prelude::*;
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::time::Instant;

#[derive(Debug)]
pub struct SearchState {
    pub mode: String,
    pub system: String,
    pub body: String,
    pub from: String,
    pub to: String,
    pub depth: usize,
    pub queue_size: usize,
    pub d_rem: f32,
    pub d_total: f32,
    pub prc_done: f32,
    pub n_seen: usize,
    pub prc_seen: f32,
}

#[derive(Debug, Clone)]
pub enum SysEntry {
    ID(u32),
    Name(String),
}

impl SysEntry {
    pub fn parse(s: &str) -> SysEntry {
        match s.parse() {
            Ok(n) => SysEntry::ID(n),
            _ => SysEntry::Name(s.to_owned()),
        }
    }
}

pub struct RouteOpts {
    pub range: Option<f32>,
    pub file_path: PathBuf,
    pub precomp_file: Option<PathBuf>,
    pub precompute: bool,
    pub permute: bool,
    pub primary: bool,
    pub keep_first: bool,
    pub keep_last: bool,
    pub factor: Option<f32>,
    pub mode: Mode,
    pub prune: Option<(usize, f32)>,
    pub systems: Vec<SysEntry>,
    pub callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
}

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

impl Mode {
    pub fn parse(s: &str) -> Result<Mode, String> {
        let s = s.to_lowercase();
        match s.as_ref() {
            "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) -> Option<Self> {
        let idx_path = path.with_extension("idx");
        let cache =
            bincode::deserialize_from(&mut BufReader::new(File::open(idx_path).ok()?)).ok()?;
        let mut reader = BufReader::new(File::open(path).ok()?);
        let header = Self::read_record(&mut reader);
        let ret = Self {
            file: reader,
            cache,
            header,
        };
        Some(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)).ok()?;
        let rec = Self::read_record(&mut self.file)?;
        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,
    prune: Option<(usize, f32)>,
    callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
}

impl Router {
    pub fn new(
        path: &PathBuf,
        range: f32,
        prune: Option<(usize, f32)>,
        primary_only: bool,
        callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
    ) -> Result<Self, String> {
        let mut scoopable = FnvHashSet::default();
        let mut reader = match csv::ReaderBuilder::new().from_path(path) {
            Ok(rdr) => rdr,
            Err(e) => {
                return Err(format!("Error opening {}: {}", path.to_str().unwrap(), e));
            }
        };
        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();
        let ret = Self {
            tree: RTree::bulk_load(systems),
            scoopable,
            route_tree: None,
            range,
            primary_only,
            cache: LineCache::new(path),
            path: path.clone(),
            callback,
            prune,
        };
        println!(
            "{} Systems loaded in {}",
            ret.tree.size(),
            format_duration(t_load.elapsed())
        );
        Ok(ret)
    }

    pub fn from_file(
        filename: &PathBuf,
        callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
    ) -> Result<(PathBuf, Self), String> {
        let mut reader = BufReader::new(match File::open(&filename) {
            Ok(fh) => fh,
            Err(e) => {
                return Err(format!(
                    "Error opening file {}: {}",
                    filename.to_str().unwrap(),
                    e
                ))
            }
        });
        println!("Loading {}", filename.to_str().unwrap());
        let (primary, range, file_hash, path, route_tree): (
            bool,
            f32,
            Vec<u8>,
            String,
            FnvHashMap<u32, u32>,
        ) = match bincode::deserialize_from(&mut reader) {
            Ok(res) => res,
            Err(e) => {
                return Err(format!(
                    "Error loading file {}: {}",
                    filename.to_str().unwrap(),
                    e
                ))
            }
        };
        let path = PathBuf::from(path);
        if hash_file(&path) != file_hash {
            return Err("File hash mismatch!".to_string());
        }
        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,
                callback,
                prune: None,
            },
        ))
    }

    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 {
        let scoopable = self.scoopable.contains(&sys.id);
        return scoopable;
    }

    pub fn best_multiroute(
        &self,
        waypoints: &[System],
        range: f32,
        keep: (bool, bool),
        mode: Mode,
        factor: f32,
    ) -> Result<Vec<System>, String> {
        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();
        println!("Finding best permutation of hops...");
        while waypoints.prev_permutation() {}
        loop {
            let c_first = waypoints.first().cloned();
            let c_last = waypoints.last().cloned();
            let valid = (keep.0 && (c_first == first)) && (keep.1 && (c_last == last));
            if valid {
                let mut total_d = 0.0;
                for pair in waypoints.windows(2) {
                    match pair {
                        [src, dst] => {
                            total_d += src.distp2(dst);
                        }
                        _ => return Err("Invalid routing parameters!".to_string()),
                    }
                }
                if total_d < best_score {
                    best_score = total_d;
                    best_permutation_waypoints = waypoints.to_owned();
                }
            }
            if !waypoints.next_permutation() {
                break;
            }
        }
        println!("Best permutation: {:?}", best_permutation_waypoints);
        self.multiroute(&best_permutation_waypoints, range, mode, factor)
    }

    pub fn multiroute(
        &self,
        waypoints: &[System],
        range: f32,
        mode: Mode,
        factor: f32,
    ) -> Result<Vec<System>, String> {
        let mut route: Vec<System> = Vec::new();
        for pair in waypoints.windows(2) {
            match pair {
                [src, dst] => {
                    let d_total=dist(&src.pos,&dst.pos);
                    println!("Plotting route from [{}] to [{}]...", src.system, dst.system);
                    println!(
                        "Jump Range: {} Ly, Distance: {} Ly, Estimated Jumps: {}",
                        range,
                        d_total,
                        d_total / range
                    );
                    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());
                        }
                    }
                }
                _ => {
                    return Err("Invalid routing parameters!".to_owned());
                }
            }
        }
        Ok(route)
    }

    fn resolve_systems(&self, systems: &[SysEntry]) -> Result<Vec<System>, String> {
        let mut ret = Vec::new();
        let mut sys_by_id: FnvHashMap<u32, &System> = FnvHashMap::default();
        let mut sys_by_name: FnvHashMap<String, &System> = FnvHashMap::default();
        for sys in &self.tree {
            for ent in systems {
                match ent {
                    SysEntry::ID(i) => {
                        let i = *i;
                        if sys.id == i {
                            sys_by_id.insert(i, sys);
                        }
                    }
                    SysEntry::Name(n) => {
                        if &sys.body == n || &sys.system == n {
                            sys_by_name.insert(n.to_string(), sys);
                        }
                    }
                }
            }
        }
        for ent in systems {
            match ent {
                SysEntry::ID(i) => match sys_by_id.get(i) {
                    Some(sys) => ret.push((*sys).clone()),
                    None => {
                        return Err(format!("System: {:?} not found", ent));
                    }
                },
                SysEntry::Name(n) => match sys_by_name.get(n) {
                    Some(sys) => ret.push((*sys).clone()),
                    None => {
                        return Err(format!("System: {:?} not found", ent));
                    }
                },
            }
        }
        Ok(ret)
    }

    pub fn route_astar(
        &self,
        src: &System,
        dst: &System,
        range: f32,
        factor: f32,
    ) -> Result<Vec<System>, String> {
        if factor == 0.0 {
            return self.route_bfs(src, dst, range);
        }
        let src_name = src.system.clone();
        let dst_name = dst.system.clone();
        let start_sys = src;
        let goal_sys = dst;
        let d_total = dist(&start_sys.pos, &goal_sys.pos);
        let mut d_rem = d_total;

        let mut state = SearchState {
            mode: "A-Star".into(),
            depth: 0,
            queue_size: 0,
            d_rem: d_total,
            d_total,
            prc_done: 0.0,
            n_seen: 0,
            prc_seen: 0.0,
            from: src_name.clone(),
            to: dst_name.clone(),
            body: start_sys.body.clone(),
            system: start_sys.system.clone(),
        };
        let total = self.tree.size() as f32;
        let mut t_last = Instant::now();
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let mut found = false;
        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 !found {
            while let Some((depth, _, sys)) = queue.pop() {
                if t_last.elapsed().as_millis() > 100 {
                    t_last = Instant::now();
                    state.depth = depth;
                    state.queue_size = queue.len();
                    state.prc_done = ((d_total - d_rem) * 100f32) / d_total;
                    state.d_rem = d_rem;
                    state.n_seen = seen.len();
                    state.prc_seen = ((seen.len() * 100) as f32) / total;
                    state.body = sys.body.clone();
                    state.system = sys.system.clone();
                    match (self.callback)(&state) {
                        Ok(_) => (),
                        Err(e) => {
                            return Err(format!("{:?}", e).to_string());
                        }
                    };
                }
                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);
                            if d_g < d_rem {
                                d_rem = d_g;
                            }
                            (depth + 1, (d_g / range) as usize, 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();
            }
            if queue.is_empty() {
                break;
            }
        }
        println!();

        println!();
        if !found {
            return Err(format!("No route from {} to {} found!", src_name, dst_name));
        }
        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();
        Ok(v)
    }

    pub fn route_greedy(
        &self,
        src: &System,
        dst: &System,
        range: f32,
    ) -> Result<Vec<System>, String> {
        let src_name = src.system.clone();
        let dst_name = dst.system.clone();
        let start_sys = src;
        let goal_sys = dst;
        let d_total = dist(&start_sys.pos, &goal_sys.pos);
        let mut d_rem = d_total;
        let mut state = SearchState {
            mode: "Greedy".into(),
            depth: 0,
            queue_size: 0,
            d_rem: d_total,
            d_total,
            prc_done: 0.0,
            n_seen: 0,
            prc_seen: 0.0,
            from: src_name.clone(),
            to: dst_name.clone(),
            body: start_sys.body.clone(),
            system: start_sys.system.clone(),
        };
        let total = self.tree.size() as f32;
        let mut t_last = Instant::now();
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let mut found = false;
        let mut queue: Vec<(f32, usize, &System)> = Vec::new();
        queue.push((start_sys.distp(goal_sys), 0, &start_sys));
        seen.insert(start_sys.id);
        while !found {
            while let Some((_, depth, sys)) = queue.pop() {
                if t_last.elapsed().as_millis() > 100 {
                    t_last = Instant::now();
                    state.depth = depth;
                    state.queue_size = queue.len();
                    state.prc_done = ((d_total - d_rem) * 100f32) / d_total;
                    state.d_rem = d_rem;
                    state.n_seen = seen.len();
                    state.prc_seen = ((seen.len() * 100) as f32) / total;
                    state.body = sys.body.clone();
                    state.system = sys.system.clone();
                    match (self.callback)(&state) {
                        Ok(_) => (),
                        Err(e) => {
                            return Err(format!("{:?}", e).to_string());
                        }
                    };
                }
                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);
                            if d_g < d_rem {
                                d_rem = d_g;
                            }
                            (d_g, depth + 1, nb)
                        }),
                );
                queue.sort_by(|a, b| fcmp(a.0, b.0).then(a.1.cmp(&b.1)));
                queue.reverse();
            }
            if queue.is_empty() {
                break;
            }
        }
        if !found {
            return Err(format!("No route from {} to {} found!", src_name, dst_name));
        }
        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();
        Ok(v)
    }

    pub fn precompute(&mut self, src: &System) -> Result<(), String> {
        let total = self.tree.size() as f32;
        let t_start = Instant::now();
        let mut prev = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        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, &src));
        seen.insert(src.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.system.replace("*", "").replace(" ", "_"),
            self.range,
            if self.primary_only { "_primary" } else { "" }
        );
        let mut out_fh = BufWriter::new(File::create(&ofn).unwrap());
        let data = (
            self.primary_only,
            self.range,
            hash_file(&self.path),
            String::from(self.path.to_str().unwrap()),
            self.route_tree.as_ref().unwrap(),
        );
        match bincode::serialize_into(&mut out_fh, &data) {
            Ok(_) => Ok(()),
            Err(e) => Err(format!("Error: {}", e).to_string()),
        }
    }

    fn get_systems_by_ids(
        &mut self,
        path: &PathBuf,
        ids: &[u32],
    ) -> Result<FnvHashMap<u32, System>, String> {
        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 => {
                        missing = true;
                        break;
                    }
                }
            }
            if !missing {
                return Ok(ret);
            }
        }
        let mut reader = match csv::ReaderBuilder::new().from_path(path) {
            Ok(reader) => reader,
            Err(e) => {
                return Err(format!("Error opening {}: {}", path.to_str().unwrap(), e));
            }
        };
        reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .filter(|sys| ids.contains(&sys.id))
            .map(|sys| {
                ret.insert(sys.id, sys.build());
            })
            .count();
        for id in ids {
            if !ret.contains_key(&id) {
                return Err(format!("ID {} not found", id));
            }
        }
        Ok(ret)
    }

    pub fn route_to(
        &mut self,
        dst: &System,
        systems_path: &PathBuf,
    ) -> Result<Vec<System>, String> {
        let prev = self.route_tree.as_ref().unwrap();
        if !prev.contains_key(&dst.id) {
            return Err(format!("System-ID {} not found", dst.id).to_string());
        };
        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 = match id_map.get(&sys_id) {
                Some(sys) => sys,
                None => {
                    return Err(format!("System-ID {} not found!", sys_id));
                }
            };
            v.push(sys.clone())
        }
        Ok(v)
    }

    pub fn route_bfs(
        &self,
        start_sys: &System,
        goal_sys: &System,
        range: f32,
    ) -> Result<Vec<System>, String> {
        println!("Running BFS");
        let min_improvement = self.prune.map(|v| (v.0,v.1*range)).unwrap_or_else(|| (0,0.0));
        let src_name = start_sys.system.clone();
        let dst_name = goal_sys.system.clone();
        let d_total = dist(&start_sys.pos, &goal_sys.pos);
        let mut d_rem = d_total;
        let mut state = SearchState {
            mode: "BFS".into(),
            depth: 0,
            queue_size: 0,
            d_rem,
            d_total,
            prc_done: 0.0,
            n_seen: 0,
            prc_seen: 0.0,
            from: src_name.clone(),
            to: dst_name.clone(),
            system: start_sys.system.clone(),
            body: start_sys.body.clone(),
        };
        let total = self.tree.size() as f32;
        let mut prev: FnvHashMap<u32, &System> = FnvHashMap::default();
        let mut prune_map: FnvHashMap<u32, (usize, f32)> = FnvHashMap::default();
        let mut seen = FnvHashSet::default();
        let mut depth = 0;
        let mut found = false;
        let mut t_last = Instant::now();
        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 !found {
            while let Some((d, sys)) = queue.pop_front() {
                if sys.id == goal_sys.id {
                    found = true;
                    break;
                }
                if t_last.elapsed().as_millis() > 100 {
                    state.depth = depth;
                    state.queue_size = queue.len();
                    state.prc_done = ((d_total - d_rem) * 100f32) / d_total;
                    state.d_rem = d_rem;
                    state.n_seen = seen.len();
                    state.prc_seen = ((seen.len() * 100) as f32) / total;
                    {
                        let s = queue.get(0).unwrap().1;
                        state.system = s.system.clone();
                        state.body = s.body.clone();
                    }
                    match (self.callback)(&state) {
                        Ok(_) => (),
                        Err(e) => {
                            return Err(format!("{:?}", e).to_string());
                        }
                    };
                    t_last = Instant::now();
                }
                if self.prune.is_some() {
                    let best_dist = if let Some(p_sys) = prev.get(&sys.id) {
                        dist2(&p_sys.pos, &goal_sys.pos).min(
                            prune_map
                                .get(&p_sys.id)
                                .map(|v| v.1)
                                .unwrap_or(std::f32::MAX),
                        )
                    } else {
                        dist2(&sys.pos, &goal_sys.pos)
                    };
                    prune_map.insert(sys.id, (depth, best_dist));
                }
                // TODO: check improvement, if too small: continue
                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);
                            let dist = nb.distp(goal_sys);
                            if dist < d_rem {
                                d_rem = dist;
                            }
                            (d + 1, nb)
                        }),
                );
            }
            std::mem::swap(&mut queue, &mut queue_next);
            if queue.is_empty() {
                break;
            }
            depth += 1;
        }
        println!();
        println!();
        if !found {
            return Err(format!("No route from {} to {} found!", src_name, dst_name));
        }
        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();
        Ok(v)
    }
}
pub fn route(opts: RouteOpts) -> Result<Option<Vec<System>>, String> {
    // TODO: implement pruning (check if dist to target improved by at least $n*jump_range$ Ly in the last $m$ steps)
    if opts.systems.is_empty() {
        if opts.precomp_file.is_some() {
            return Err("Error: Please specify exatly one system".to_owned());
        } else if opts.precompute {
            return Err("Error: Please specify at least one system".to_owned());
        } else {
            return Err("Error: Please specify at least two systems".to_owned());
        };
    }
    let mut path = opts.file_path;
    let mut router: Router = if opts.precomp_file.is_some() {
        let (path_, ret) =
            Router::from_file(&opts.precomp_file.clone().unwrap(), Box::new(opts.callback))?;
        path = path_;
        ret
    } else if opts.range.is_some() {
        Router::new(
            &path,
            opts.range.unwrap(),
            opts.prune,
            opts.primary,
            Box::new(opts.callback),
        )?
    } else {
        Router::new(
            &path,
            opts.range.unwrap(),
            opts.prune,
            opts.primary,
            opts.callback,
        )?
    };
    let systems: Vec<System> = router.resolve_systems(&opts.systems)?.to_vec();
    if opts.precompute {
        for sys in systems {
            router.route_tree = None;
            router.precompute(&sys)?;
        }
        return Ok(None);
    }
    let route = if router.route_tree.is_some() {
        router.route_to(systems.first().unwrap(), &path)?
    } else if opts.permute {
        router.best_multiroute(
            &systems,
            opts.range.unwrap(),
            (opts.keep_first, opts.keep_last),
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )?
    } else {
        router.multiroute(
            &systems,
            opts.range.unwrap(),
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )?
    };
    if route.is_empty() {
        return Err("No route found!".to_string());
    }
    Ok(Some(route))
}