use crate::common::{System, SystemSerde, TreeNode};
use crate::edsm::build_index;
use crate::ship::Ship;
use core::cmp::Ordering;
use crossbeam_channel::{bounded, unbounded, Receiver, SendError, Sender, TryIter};
use derivative::Derivative;
use dict_derive::IntoPyObject;
use fnv::{FnvHashMap, FnvHashSet};
use humantime::format_duration;
use permutohedron::LexicalPermutation;
use pyo3::prelude::*;
use rstar::{PointDistance, RStarInsertionStrategy, RTree, RTreeObject, RTreeParams, AABB};
use sha3::{Digest, Sha3_256};
use std::collections::VecDeque;
use std::fs::File;
use std::hash::{Hash, Hasher};
use std::io::{BufReader, BufWriter, Write};
use std::path::PathBuf;
use std::sync::{Arc, Mutex};
use std::thread;
use std::thread::JoinHandle;
use std::time::Instant;

const STATUS_INVERVAL: u128 = 5000; //ms

struct Weight {
    dist_from_start: f32,
    dist_to_goal: f32,
    dist_to_point: Vec<(f32, [f32; 3])>,
}

impl Weight {
    fn calc(&self, node: &TreeNode, dst: &TreeNode, src: &TreeNode) -> f32 {
        let d_total = dist(&src.pos, &dst.pos);
        let d_start = (dist(&node.pos, &src.pos) / d_total) * self.dist_from_start;
        let d_goal = (dist(&node.pos, &dst.pos) / d_total) * self.dist_to_goal;
        let points: f32 = self
            .dist_to_point
            .iter()
            .map(|&(f, p)| dist(&p, &node.pos) * f)
            .sum();
        return d_start + d_goal + points;
    }
}

#[derive(Debug, Clone, IntoPyObject)]
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,
}

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 systems: Vec<u32>,
    pub callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
    pub workers: usize,
}

#[derive(Debug)]
#[allow(non_camel_case_types)]
pub enum Mode {
    BFS,
    BiDir, // TODO: implement bidirectional BFS
    Greedy,
    AStar,
    Shortest, // TODO: A-Star with distance as weight
}

#[derive(Debug)]
#[allow(non_camel_case_types)]
pub enum PrecomputeMode {
    Full,
    Route_From,
    Route_To,
    None,
}

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),
            "bidir" => Ok(Mode::BiDir),
            val => Err(format!("Invalid Mode: {}", val)),
        }
    }
}

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 TreeNode {
    pub fn dist2(&self, p: &[f32; 3]) -> f32 {
        dist2(&self.pos, p)
    }

    pub fn distp(&self, p: &System) -> f32 {
        dist(&self.pos, &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 TreeNode {
    type Envelope = AABB<[f32; 3]>;

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

impl PointDistance for TreeNode {
    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.finalize().iter().copied().collect()
}

pub struct LineCache {
    cache: Vec<u64>,
    reader: csv::Reader<File>,
}

impl LineCache {
    pub fn new(path: &PathBuf) -> Option<Arc<Mutex<Self>>> {
        //TODO: Verify match between index file and csv file
        let idx_path = path.with_extension("idx");
        if !idx_path.exists() {
            eprintln!("No index found for {:?}, building...", path);
            build_index(path)
                .map_err(|e| {
                    eprintln!("Error creating index for {:?}: {}", path, e);
                })
                .ok()?;
        }
        let cache = bincode::deserialize_from(&mut BufReader::new(
            File::open(idx_path)
                .map_err(|e| {
                    eprintln!("Error opening index for {:?}: {}", path, e);
                })
                .ok()?,
        ))
        .map_err(|e| {
            eprintln!("Reading index for {:?}: {}", path, e);
        })
        .ok()?;
        let reader = csv::Reader::from_path(path).ok()?;
        let ret = Self { reader, cache };
        Some(Arc::new(Mutex::new(ret)))
    }

    fn read_sys(&mut self) -> Option<SystemSerde> {
        Some(
            self.reader
                .deserialize()
                .next()
                .expect("Failed to read SystemSerde")
                .expect("Failed to parse SystemSerde"),
        )
    }

    pub fn get(&mut self, id: u32) -> Option<System> {
        let mut pos = csv::Position::new();
        pos.set_byte(self.cache[id as usize]);
        self.reader.seek(pos).ok()?;
        self.read_sys().map(SystemSerde::build)
    }
}

pub struct LargeNodeParameters;
impl RTreeParams for LargeNodeParameters {
    const MIN_SIZE: usize = 200;
    const MAX_SIZE: usize = 400;
    const REINSERTION_COUNT: usize = 100;
    type DefaultInsertionStrategy = RStarInsertionStrategy;
}

// Optional but helpful: Define a type alias for the new r-tree
pub type LargeNodeRTree<T> = RTree<T, LargeNodeParameters>;

#[derive(Debug, Clone)]
struct WorkUnit {
    node: TreeNode,
    depth: usize,
    parent_id: Option<u32>,
    range: f32,
}

#[derive(Debug)]
enum WorkerSet {
    Empty,
    Workers {
        handles: Vec<JoinHandle<()>>,
        tx: Sender<Option<WorkUnit>>,
        rx: Receiver<WorkUnit>,
    },
}

impl WorkerSet {
    fn new(tree: Arc<LargeNodeRTree<TreeNode>>, num_workers: usize) -> Self {
        if num_workers == 0 {
            return WorkerSet::Empty;
        }
        let (jobs_tx, jobs_rx) = unbounded();
        let (result_tx, result_rx) = bounded(100_000);
        let handles = (0..num_workers)
            .map(|_| {
                thread::spawn({
                    let rx = jobs_rx.clone();
                    let tx = result_tx.clone();
                    let tree = tree.clone();
                    move || {
                        Self::work(&tree, rx, tx);
                    }
                })
            })
            .collect();
        return WorkerSet::Workers {
            handles,
            tx: jobs_tx,
            rx: result_rx,
        };
    }

    fn work(tree: &LargeNodeRTree<TreeNode>, rx: Receiver<Option<WorkUnit>>, tx: Sender<WorkUnit>) {
        while let Ok(Some(unit)) = rx.recv() {
            let range = unit.range * unit.node.mult;
            tree.locate_within_distance(unit.node.pos, range * range)
                .cloned()
                .for_each(|nb| {
                    let wu = WorkUnit {
                        node: nb,
                        depth: unit.depth + 1,
                        parent_id: Some(unit.node.id),
                        range: unit.range,
                    };
                    tx.send(wu).unwrap();
                });
        }
        drop(tx);
    }

    fn resize(self, tree: Arc<LargeNodeRTree<TreeNode>>, num: usize) -> Result<Self, String> {
        self.close()?;
        return Ok(WorkerSet::new(tree, num));
    }

    // fn replace(self, tree: Arc<LargeNodeRTree<TreeNode>>) -> Result<Self, String> {
    //     let num=self.num();
    //     return self.resize(tree.clone(),num);
    // }

    fn close(self) -> Result<(), String> {
        if let WorkerSet::Workers {
            mut handles,
            tx,
            rx,
        } = self
        {
            let t_start = Instant::now();
            loop {
                if rx.is_empty() && tx.is_empty() {
                    break;
                }
                rx.try_iter().for_each(|_| {});
            }
            for _ in &handles {
                match tx.send(None) {
                    Ok(_) => {}
                    Err(e) => {
                        return Err(format!("{:?}", e));
                    }
                }
            }
            drop(tx);
            while let Some(handle) = handles.pop() {
                handle.join().unwrap();
            }
            drop(rx);
            println!(
                "workerset cleared in {}",
                format_duration(t_start.elapsed())
            );
        }
        return Ok(());
    }

    fn queue_size(&self) -> usize {
        match self {
            WorkerSet::Empty => 0,
            WorkerSet::Workers { rx, tx, .. } => tx.len() + rx.len(),
        }
    }

    fn queue_empty(&self) -> bool {
        return self.queue_size() == 0;
    }

    fn send(&self, wu: WorkUnit) -> Result<(), SendError<Option<WorkUnit>>> {
        match self {
            WorkerSet::Empty => {
                panic!("send() on empty WorkerSet");
            }
            WorkerSet::Workers { tx, .. } => {
                return tx.send(Some(wu));
            }
        }
    }

    fn num(&self) -> usize {
        match self {
            WorkerSet::Empty => 1,
            WorkerSet::Workers { handles, .. } => handles.len(),
        }
    }

    fn is_empty(&self) -> bool {
        match self {
            WorkerSet::Empty => true,
            WorkerSet::Workers { handles, .. } => handles.len() == 0,
        }
    }

    // impl Iterator<Item = &TreeNode>

    fn iter(&self) -> Result<TryIter<WorkUnit>, String> {
        match self {
            WorkerSet::Empty => Err("can't iterate on empty WorkerSet".to_string()),
            WorkerSet::Workers { rx, .. } => Ok(rx.try_iter()),
        }
    }

    // fn join(mut self) -> thread::Result<()> {
    //     drop(self.tx);
    //     drop(self.rx);
    //     let ret: thread::Result<Vec<_>> = self.handles.drain(..).map(|v| v.join()).collect();
    //     ret?;
    //     return Ok(());
    // }
}

#[derive(Derivative)]
#[derivative(Debug)]
pub struct Router {
    #[derivative(Debug = "ignore")]
    tree: Arc<LargeNodeRTree<TreeNode>>,
    #[derivative(Debug = "ignore")]
    scoopable: FnvHashSet<u32>,
    #[derivative(Debug = "ignore")]
    pub route_tree: Option<FnvHashMap<u32, u32>>,

    pub ship: Option<Ship>,
    #[derivative(Debug = "ignore")]
    pub cache: Option<Arc<Mutex<LineCache>>>,

    pub path: PathBuf,
    pub primary_only: bool,

    #[derivative(Debug = "ignore")]
    workers: WorkerSet,
    #[derivative(Debug = "ignore")]
    pub callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
}

impl Router {
    pub fn new(callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>) -> Self {
        Self {
            tree: Arc::new(LargeNodeRTree::default()),
            scoopable: FnvHashSet::default(),
            route_tree: None,
            cache: None,
            ship: None,
            callback,
            primary_only: false,
            workers: WorkerSet::Empty,
            path: PathBuf::from(""),
        }
    }

    pub fn load(&mut self, path: &PathBuf, primary_only: bool) -> Result<(), String> {
        if self.path == path.to_path_buf() && self.primary_only == primary_only {
            return Ok(());
        }
        self.tree = Arc::new(LargeNodeRTree::default()); // clear R*-Tree
        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.display(), e));
            }
        };
        let t_load = Instant::now();
        println!("Loading {}...", path.display());
        let systems: Vec<TreeNode> = reader
            .deserialize::<SystemSerde>()
            .filter_map(|res| {
                let sys = res.expect("Failed to read");
                if primary_only && sys.distance != 0.0 {
                    return None;
                }
                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;
                        }
                    }
                }
                Some(sys.to_node())
            })
            .collect();
        println!(
            "{} Systems loaded in {}",
            systems.len(),
            format_duration(t_load.elapsed())
        );
        let t_load = Instant::now();
        self.tree = Arc::new(LargeNodeRTree::bulk_load_with_params(systems));
        self.path = path.to_path_buf();
        self.cache = LineCache::new(&path.to_path_buf());
        self.scoopable = scoopable;
        println!("R*-Tree built in {}", format_duration(t_load.elapsed()));
        Ok(())
    }

    pub fn set_ship(&mut self, ship: Ship) {
        self.ship = Some(ship);
    }

    fn start_workers(&mut self, num: usize) -> Result<(), String> {
        let mut w = WorkerSet::Empty;
        std::mem::swap(&mut self.workers, &mut w);
        self.workers = w.resize(self.tree.clone(), num)?;
        Ok(())
    }

    pub fn from_file(
        filename: &PathBuf,
        callback: Box<dyn Fn(&SearchState) -> PyResult<PyObject>>,
    ) -> Result<(PathBuf, f32, Self), String> {
        let mut reader = BufReader::new(match File::open(&filename) {
            Ok(fh) => fh,
            Err(e) => return Err(format!("Error opening file {}: {}", filename.display(), e)),
        });
        println!("Loading {}", filename.display());
        let (primary, range, file_hash, path, route_tree): (
            bool,
            f32,
            Vec<u8>,
            PathBuf,
            FnvHashMap<u32, u32>,
        ) = match bincode::deserialize_from(&mut reader) {
            Ok(res) => res,
            Err(e) => return Err(format!("Error loading file {}: {}", filename.display(), e)),
        };
        if hash_file(&path) != file_hash {
            return Err("File hash mismatch!".to_string());
        }
        let cache = LineCache::new(&path);
        Ok((
            path.clone(),
            range,
            Self {
                tree: Arc::new(RTree::default()),
                scoopable: FnvHashSet::default(),
                route_tree: Some(route_tree),
                cache,
                path,
                callback,
                ship: None,
                primary_only: primary,
                workers: WorkerSet::Empty,
            },
        ))
    }

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

    fn neighbours(&self, node: &TreeNode, range: f32) -> impl Iterator<Item = &TreeNode> {
        self.points_in_sphere(&node.pos, node.mult * range)
    }

    fn neighbours_r(&self, node: &TreeNode, range: f32) -> impl Iterator<Item = &TreeNode> {
        let pos = node.pos;
        self.points_in_sphere(&node.pos, range * 4.0)
            .filter(move |s| {
                return s.dist2(&pos) < (range * s.mult) * (range * s.mult);
            })
    }

    fn valid(&self, id: u32) -> bool {
        let scoopable = self.scoopable.contains(&id);
        return scoopable;
    }

    fn make_jump(&mut self, d: f32) -> Option<f32> {
        let ship = self.ship.as_mut().expect("Tried to jump without a ship!");
        return ship.make_jump(d);
    }

    fn best_multiroute(
        &mut self,
        waypoints: &[System],
        range: f32,
        keep: (bool, bool),
        factor: f32,
        beam_width: usize,
        num_workers: usize,
    ) -> 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,
            factor,
            beam_width,
            num_workers,
        )
    }

    fn multiroute(
        &mut self,
        waypoints: &[System],
        range: f32,
        factor: f32,
        beam_width: usize,
        num_workers: usize,
    ) -> Result<Vec<System>, String> {
        if self.tree.size() == 0 {
            return Err("No Systems loaded, pleased load some with the 'load' method!".to_string());
        }
        if factor < 0.0 || factor > 1.0 {
            return Err("Factor needs to be between 0.0 (BFS) and 1.0 (Greedy Search)".to_string());
        }
        let mut route: Vec<System> = Vec::new();
        if factor == 0.0 {
            self.start_workers(num_workers)?;
        }
        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 = self.route_astar(&src, &dst, factor, beam_width, range)?;
                    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 route_astar(
        &self,
        src: &System,
        dst: &System,
        factor: f32,
        beam_width: usize,
        range: f32,
    ) -> Result<Vec<System>, String> {
        if factor == 0.0 {
            return self.route_bfs(src, dst, range, beam_width);
        }
        if (factor - 1.0).abs() < 1e-3 {
            if beam_width != 0 {
                eprintln!("Usign greedy algorithm, ignorimg beam width!")
            }
            return self.route_greedy(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: FnvHashMap<u32, f32> = FnvHashMap::default();
        let mut found = false;
        let mut queue: Vec<(usize, usize, TreeNode)> = Vec::new();
        queue.push((
            0,                                            // depth
            (start_sys.distp(goal_sys) / range) as usize, // h
            start_sys.to_node(),
        ));
        seen.insert(start_sys.id, 0.0);
        while !found {
            while let Some((depth, _, node)) = queue.pop() {
                if t_last.elapsed().as_millis() > STATUS_INVERVAL {
                    let sys = node
                        .get(&self)
                        .unwrap_or_else(|| panic!("System-ID {} not found!", node.id));
                    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));
                        }
                    };
                }
                if node.id == goal_sys.id {
                    queue.clear();
                    found = true;
                    break;
                }

                let new_nodes: Vec<_> = self
                    .neighbours(&node, range)
                    .filter(|nb| (self.valid(nb.id) || (nb.id == goal_sys.id)))
                    .filter(|nb| !seen.contains_key(&nb.id))
                    .map(|nb| {
                        prev.insert(nb.id, node);
                        let d_g = nb.distp(goal_sys);
                        if d_g < d_rem {
                            d_rem = d_g;
                        }
                        (depth + 1, (d_g / (range * 4.0)) as usize, *nb)
                    })
                    .collect();
                for node in new_nodes {
                    seen.insert(node.2.id, 0.0);
                    queue.push(node);
                }
                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 * (1.0 - factor) + a_1 * factor;
                    let v_b = b_0 * (1.0 - factor) + 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.clone();
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = sys.get(&self).unwrap(),
                None => {
                    break;
                }
            }
        }
        v.reverse();
        Ok(v)
    }

    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: FnvHashMap<u32, f32> = FnvHashMap::default();
        let mut found = false;
        let mut queue: Vec<(f32, usize, TreeNode)> = Vec::new();
        queue.push((start_sys.distp(goal_sys), 0, start_sys.to_node()));
        seen.insert(start_sys.id, 0.0);
        while !found {
            while let Some((_, depth, node)) = queue.pop() {
                if t_last.elapsed().as_millis() > STATUS_INVERVAL {
                    let sys = node
                        .get(&self)
                        .unwrap_or_else(|| panic!("System-ID {} does not exist!", &node.id));
                    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));
                        }
                    };
                }
                if node.id == goal_sys.id {
                    queue.clear();
                    found = true;
                    break;
                }
                let new_nodes: Vec<_> = self
                    .neighbours(&node, range)
                    .filter(|nb| (self.valid(nb.id) || (nb.id == goal_sys.id)))
                    .filter(|nb| !seen.contains_key(&nb.id))
                    .map(|nb| {
                        prev.insert(nb.id, node);
                        let d_g = nb.distp(goal_sys);
                        if d_g < d_rem {
                            d_rem = d_g;
                        }
                        (d_g, depth + 1, *nb)
                    })
                    .collect();
                for node in new_nodes {
                    seen.insert(node.2.id, 0.0);
                    queue.push(node);
                }
                queue.sort_by(|a, b| fcmp(b.0, a.0).then(b.1.cmp(&a.1)));
            }
            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.clone();
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = sys.get(&self).unwrap(),
                None => {
                    break;
                }
            }
        }
        v.reverse();
        Ok(v)
    }

    fn precompute_all(&mut self, range: f32) -> Result<(), String> {
        // TODO: implement all-pairs shortest path based on optimized BFS
        unimplemented!();
    }

    fn precompute_to(&mut self, dst: &System, range: f32) -> Result<(), String> {
        // TODO: -> precompute to
        unimplemented!();
    }

    fn precompute(&mut self, src: &System, range: f32) -> Result<(), String> {
        // TODO: -> precompute from
        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, TreeNode)> = VecDeque::new();
        let mut queue_next: VecDeque<(usize, TreeNode)> = VecDeque::new();
        queue.push_front((0, src.to_node()));
        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, 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 file_hash = hash_file(&self.path);
        let file_hash_hex = file_hash
            .iter()
            .map(|v| format!("{:02x}", v))
            .collect::<Vec<String>>()
            .join("");
        let ofn = format!(
            "{}_{}_{}.router",
            src.system.replace("*", "").replace(" ", "_"),
            range,
            file_hash_hex
        );
        let mut out_fh = BufWriter::new(File::create(&ofn).unwrap());
        let data = (
            self.tree.size(),
            range,
            file_hash,
            self.path.clone(),
            self.route_tree.as_ref().unwrap(),
        );
        match bincode::serialize_into(&mut out_fh, &data) {
            Ok(_) => Ok(()),
            Err(e) => Err(format!("Error: {}", e)),
        }
    }

    fn get_sys(&self, id: u32) -> Result<Option<System>, String> {
        let path = &self.path;
        if let Some(c) = &self.cache {
            if let Some(sys) = c.lock().unwrap().get(id) {
                return Ok(Some(sys));
            };
        }
        let mut reader = match csv::ReaderBuilder::new().from_path(path) {
            Ok(reader) => reader,
            Err(e) => {
                return Err(format!("Error opening {}: {}", path.display(), e));
            }
        };
        println!("Running serial search for ID: {:?}", id);
        return Ok(reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .filter(|sys| sys.id == id)
            .map(|sys| sys.build())
            .last());
    }

    fn get_systems_by_ids(&self, ids: &[u32]) -> Result<FnvHashMap<u32, System>, String> {
        let path = &self.path;
        let mut ret = FnvHashMap::default();
        if let Some(c) = &self.cache {
            let mut c = c.lock().unwrap();
            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.display(), e));
            }
        };
        println!("Running serial search for IDs: {:?}", ids);
        reader
            .deserialize::<SystemSerde>()
            .map(|res| res.unwrap())
            .filter(|sys| ids.contains(&sys.id))
            .for_each(|sys| {
                ret.insert(sys.id, sys.build());
            });
        for id in ids {
            if !ret.contains_key(&id) {
                return Err(format!("ID {} not found", id));
            }
        }
        Ok(ret)
    }

    fn route_to(&self, dst: &System) -> Result<Vec<System>, String> {
        if self.route_tree.is_none() {
            return Err("Can't computer route without a precomputed route-tree".to_owned());
        }
        let prev = self.route_tree.as_ref().unwrap();
        if !prev.contains_key(&dst.id) {
            return Err(format!("System-ID {} not found", dst.id));
        };
        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(&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)
    }

    fn route_bfs(
        &self,
        start_sys: &System,
        goal_sys: &System,
        range: f32,
        beam_width: usize,
    ) -> Result<Vec<System>, String> {
        if self.workers.is_empty() {
            return self.route_bfs_serial(start_sys, goal_sys, range, beam_width);
        }
        println!("Running BFS with {} worker(s)", self.workers.num());
        let t_start = Instant::now();
        let mut t_last = Instant::now();
        let mut prev = FnvHashMap::default();
        let mut seen: FnvHashMap<u32, f32> = FnvHashMap::default();
        let src_name = start_sys.system.clone();
        let dst_name = goal_sys.system.clone();
        let workers = &self.workers;
        let wu = WorkUnit {
            node: start_sys.to_node(),
            parent_id: None,
            depth: 0,
            range,
        };
        if wu.node.id == goal_sys.id {
            return Ok(vec![goal_sys.clone()]);
        }
        let mut found = false;
        let total = self.tree.size() as f32;
        let d_total = dist(&start_sys.pos, &goal_sys.pos);
        let mut d_rem = d_total;
        let mut state = SearchState {
            mode: format!("BFS_parallel({})", self.workers.num()),
            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(),
        };
        seen.insert(wu.node.id, 0.0);
        workers.send(wu).unwrap();
        loop {
            if found {
                break;
            }
            let num_seen = seen.len();
            let mut nbs: Vec<_> = workers
                .iter()?
                .filter(|wu| !found && !seen.contains_key(&wu.node.id))
                .filter(|wu| wu.parent_id.is_some())
                .inspect(|wu| {
                    if t_last.elapsed().as_millis() > STATUS_INVERVAL {
                        let dist = wu.node.distp(goal_sys);
                        if dist < d_rem {
                            d_rem = dist;
                        };
                        state.depth = wu.depth;
                        state.queue_size = workers.queue_size();
                        state.prc_done = ((d_total - d_rem) * 100f32) / d_total;
                        state.d_rem = d_rem;
                        state.n_seen = num_seen;
                        state.prc_seen = ((num_seen * 100) as f32) / total;
                        {
                            let s = wu.node.get(&self).unwrap();
                            state.system = s.system;
                            state.body = s.body;
                        }
                        match (self.callback)(&state) {
                            Ok(_) => (),
                            Err(e) => {
                                println!("CB_ERROR: {:?}", e);
                            }
                        };
                        t_last = Instant::now();
                    }
                })
                .collect();
            if nbs.is_empty() && workers.queue_empty() && seen.len() > 1 {
                break;
            }
            if beam_width != 0 && nbs.len() > beam_width {
                nbs.sort_by(|a, b| {
                    let d_a = a.node.dist2(&goal_sys.pos);
                    let d_b = b.node.dist2(&goal_sys.pos);
                    return fcmp(d_a, d_b);
                });
                nbs = nbs.iter().take(beam_width).cloned().collect();
            }

            while let Some(wu) = nbs.pop() {
                if let Some(parent_id) = wu.parent_id {
                    prev.insert(wu.node.id, parent_id);
                }
                seen.insert(wu.node.id, 0.0);
                if wu.node.id == goal_sys.id {
                    found = true;
                    println!("FOUND!");
                    break;
                }
                workers.send(wu).unwrap();
            }
        }
        println!();
        println!();
        println!("Took: {}", format_duration(t_start.elapsed()));
        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.clone();
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => {
                    curr_sys = self
                        .get_sys(*sys)?
                        .ok_or(format!("System id {} not found", sys))?
                }
                None => {
                    break;
                }
            }
        }
        v.reverse();
        Ok(v)
    }

    fn route_bfs_serial(
        &self,
        start_sys: &System,
        goal_sys: &System,
        range: f32,
        beam_width: usize,
    ) -> Result<Vec<System>, String> {
        if start_sys.id == goal_sys.id {
            return Ok(vec![goal_sys.clone()]);
        }
        let t_start = Instant::now();
        println!("Running BFS");
        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_serial".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::default();
        let mut seen: FnvHashMap<u32, f32> = FnvHashMap::default();
        let mut depth = 0;
        let mut found = false;
        let mut t_last = Instant::now();
        let mut queue: VecDeque<TreeNode> = VecDeque::new();
        let mut queue_next: VecDeque<TreeNode> = VecDeque::new();
        queue.push_front(start_sys.to_node());
        seen.insert(start_sys.id, 0.0);
        while !found {
            while let Some(node) = queue.pop_front() {
                if node.id == goal_sys.id {
                    queue.clear();
                    found = true;
                    break;
                }
                if t_last.elapsed().as_millis() > STATUS_INVERVAL {
                    state.depth = depth;
                    state.queue_size = queue.len() + queue_next.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;
                    if !queue.is_empty() {
                        let s = queue.get(0).unwrap().get(&self).unwrap();
                        state.system = s.system.clone();
                        state.body = s.body.clone();
                    }
                    match (self.callback)(&state) {
                        Ok(_) => (),
                        Err(e) => {
                            return Err(format!("{:?}", e));
                        }
                    };
                    t_last = Instant::now();
                }
                let valid_nbs = self
                    .neighbours(&node, range)
                    .filter(|nb| (self.valid(nb.id) || (nb.id == goal_sys.id)))
                    .filter(|nb| seen.insert(nb.id, 0.0).is_none())
                    .map(|nb| {
                        prev.insert(nb.id, node);
                        let dist = nb.distp(goal_sys);
                        if dist < d_rem {
                            d_rem = dist;
                        };
                        *nb
                    });
                queue_next.extend(valid_nbs);
            }

            if beam_width != 0 {
                let mut q = Vec::new();
                while let Some(v) = queue_next.pop_front() {
                    q.push(v);
                }
                q.sort_by(|a, b| {
                    let d_a = a.dist2(&goal_sys.pos);
                    let d_b = b.dist2(&goal_sys.pos);
                    return fcmp(d_a, d_b);
                });
                queue.clear();
                for v in q.iter().take(beam_width).cloned() {
                    queue.push_back(v);
                }
                queue_next.clear();
            } else {
                std::mem::swap(&mut queue, &mut queue_next);
            }
            if found {
                break;
            }
            if queue.is_empty() {
                break;
            }
            depth += 1;
        }
        println!();
        println!();
        println!("Took: {}", format_duration(t_start.elapsed()));
        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.clone();
        loop {
            v.push(curr_sys.clone());
            match prev.get(&curr_sys.id) {
                Some(sys) => curr_sys = sys.get(&self).unwrap(),
                None => {
                    break;
                }
            }
        }
        v.reverse();
        Ok(v)
    }
}

impl Router {
    pub fn compute_route(
        &mut self,
        sys_ids: &[u32],
        range: Option<f32>,
        factor: f32,
        beam_width: usize,
        num_workers: usize,
    ) -> Result<Vec<System>, String> {
        if range.is_none() && self.ship.is_none() {
            return Err("Need either a jump range or a ship to compute a route with!".to_owned());
        }
        let range = range.ok_or("Dynamic range calculation is not yet implemented, sorry!")?;
        let id_map = self.get_systems_by_ids(sys_ids)?;
        let hops: Vec<System> = sys_ids
            .iter()
            .map(|id| id_map.get(&id).unwrap())
            .cloned()
            .collect();
        self.multiroute(&hops, range, factor, beam_width, num_workers)
    }
}