extern crate csv;
extern crate serde;
#[macro_use]
extern crate serde_derive;

extern crate fnv;
extern crate humantime;
// extern crate rayon;
// use rayon::prelude::*;
use fnv::{FnvHashMap, FnvHashSet};
use humantime::format_duration;
use rstar::{PointDistance, RTree, RTreeObject, AABB};
use std::collections::VecDeque;
use std::hash::{Hash, Hasher};
use std::io::Write;
use std::time::Instant;

#[derive(Debug, Deserialize)]
struct Record {
    id: i64,
    star_type: String,
    name: String,
    mult: f32,
    x: f32,
    y: f32,
    z: f32,
}
#[derive(Debug)]
struct System {
    id: i64,
    star_type: String,
    name: String,
    mult: f32,
}

#[derive(Debug)]
struct Point {
    id: i64,
    x: f32,
    y: f32,
    z: f32,
}

impl Point {
    pub fn dist2(&self, p: &[f32; 3]) -> f32 {
        let dx = self.x - p[0];
        let dy = self.y - p[1];
        let dz = self.z - p[2];

        return dx * dx + dy * dy + dz * dz;
    }
    pub fn distp(&self, p: &Point) -> f32 {
        return self.distp2(p).sqrt();
    }
    pub fn distp2(&self, p: &Point) -> f32 {
        return self.dist2(&[p.x, p.y, p.z]);
    }
}
impl PartialEq for Point {
    fn eq(&self, other: &Self) -> bool {
        self.id == other.id
    }
}

impl Eq for Point {}

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

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

    fn envelope(&self) -> Self::Envelope {
        return AABB::from_point([self.x, self.y, self.z]);
    }
}

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

struct Router {
    tree: RTree<Point>,
    systems: FnvHashMap<i64, System>,
    range: f32,
    scoopable: FnvHashSet<i64>,
}

impl Router {
    pub fn new(path: &str, range: f32) -> Self {
        let mut scoopable = FnvHashSet::default();
        let mut systems: FnvHashMap<i64, System> = FnvHashMap::default();
        let mut reader = csv::ReaderBuilder::new()
            .has_headers(false)
            .from_path(path)
            .unwrap();
        println!("Loading {}...", path);
        let points = reader
            .deserialize()
            .map(|res: Result<Record, _>| {
                let sys = res.unwrap();
                systems.insert(
                    sys.id,
                    System {
                        id: sys.id,
                        star_type: sys.star_type.clone(),
                        name: sys.name,
                        mult: sys.mult,
                    },
                );
                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;
                        }
                    }
                }
                return Point {
                    x: sys.x,
                    y: sys.y,
                    z: sys.z,
                    id: sys.id,
                };
            })
            .collect();
        return Self {
            tree: RTree::<Point>::bulk_load(points),
            systems,
            range,
            scoopable,
        };
    }

    fn preload_points(&self) -> FnvHashMap<i64, &Point> {
        let mut ret = FnvHashMap::default();
        for point in &self.tree {
            ret.insert(point.id, point);
        }
        return ret;
    }

    fn closest(&self, point: &[f32; 3]) -> &Point {
        return self.tree.nearest_neighbor(point).unwrap();
    }
    fn points_in_sphere(&self, center: &[f32; 3], radius: f32) -> impl Iterator<Item = &Point> {
        let center: [f32; 3] = *center;
        return self
            .tree
            .locate_in_envelope(&AABB::from_corners(
                [
                    center[0] - radius * 1f32,
                    center[1] - radius * 1f32,
                    center[2] - radius * 1f32,
                ],
                [
                    center[0] + radius * 1f32,
                    center[1] + radius * 1f32,
                    center[2] + radius * 1f32,
                ],
            ))
            .filter(move |p| (p.dist2(&center) < (radius * radius)));
    }

    fn mult(&self, id: i64) -> f32 {
        if let Some(sys) = self.systems.get(&id) {
            return sys.mult;
        };
        return 1.0;
    }
    fn neighbours(&self, sys: &Point) -> impl Iterator<Item = &Point> {
        return self.points_in_sphere(&[sys.x, sys.y, sys.z], self.mult(sys.id) * self.range);
    }

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

    pub fn route(&mut self, src: &[f32; 3], dst: &[f32; 3]) -> Option<Vec<(&System, &Point)>> {
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        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, &Point)> = VecDeque::new();
        let mut queue_next: VecDeque<(usize, &Point)> = VecDeque::new();
        queue.push_front((0, &start_sys));
        seen.insert(start_sys.id);
        while !queue.is_empty() {
            while let Some((d, sys)) = queue.pop_front() {
                if d != depth {
                    depth = d;
                    print!(
                        "\r[{}] Depth: {}, Queue: {}, Seen: {} ({:.2}%)    ",
                        format_duration(t_start.elapsed()),
                        d,
                        queue.len(),
                        prev.len(),
                        ((prev.len() as f32) / total) * 100.0
                    );
                    std::io::stdout().flush().unwrap();
                }
                if sys.id == goal_sys.id {
                    found = true;
                    break;
                }
                let nbs = self
                    .neighbours(&sys)
                    .filter(|&nb| (self.valid(nb) || (nb.id == goal_sys.id)));
                for nb in nbs {
                    if seen.insert(nb.id) {
                        prev.insert(nb.id, sys.id);
                        queue_next.push_back((d + 1, nb));
                    }
                }
            }
            std::mem::swap(&mut queue, &mut queue_next);
        }
        println!();
        if !found {
            return None;
        }
        let points = self.preload_points();
        let mut v: Vec<(&System, &Point)> = Vec::new();
        let mut prev_sys_id = goal_sys.id;
        loop {
            if let Some(sys) = self.systems.get(&prev_sys_id) {
                v.push((sys, points[&sys.id]));
            } else {
                break;
            };
            match prev.get(&prev_sys_id) {
                Some(sys_id) => prev_sys_id = *sys_id,
                None => {
                    break;
                }
            }
        }
        v.reverse();
        return Some(v);
    }
}

fn main() {
    let path = r#"D:\devel\files\python\EDSM\stars.csv"#;
    let t_load = Instant::now();
    let mut router: Router = Router::new(path, 48.0);
    println!("Done in {}!", format_duration(t_load.elapsed()));
    let t_route = Instant::now();
    let route = router.route(
        &[-65.21875, 7.75, -111.03125], // Ix
        &[-1111.5625, -134.21875, 65269.75], // Beagle Point
                                        // &[-7095.375, 401.25, 2396.8125], // V1357 Cygni,
    ); // Ix -> BP 537
    let route = match route {
        Some(r) => r,
        None => Vec::new(),
    };
    println!(
        "Done in {} ({} Jumps)!\n",
        format_duration(t_route.elapsed()),
        route.len()
    );

    let mut total: f32 = 0.0;
    for ((sys1, p1), (_, p2)) in route.iter().zip(route.iter().skip(1)) {
        let dist = p1.distp(p2);
        total += dist;
        println!("{} [{}]: {:.2} Ly", sys1.name, sys1.star_type, dist);
    }
    let sys = route.iter().last().unwrap().0;
    println!("{} [{}]: {:.2} Ly\n", sys.name, sys.star_type, 0.0);
    println!("Total: {:.2} Ly", total);
}