extern crate csv;
extern crate serde;
extern crate structopt;
#[macro_use]
extern crate serde_derive;
extern crate fnv;
extern crate humantime;
extern crate permutohedron;
use core::cmp::Ordering;
use fnv::{FnvHashMap, FnvHashSet};
use humantime::format_duration;
use permutohedron::LexicalPermutation;
use rstar::{PointDistance, RTree, RTreeObject, AABB};
use std::collections::VecDeque;
use std::hash::{Hash, Hasher};
use std::io::Write;
use std::path::PathBuf;
use std::str::FromStr;
use std::time::Instant;
use structopt::StructOpt;

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

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

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

#[derive(Debug, StructOpt)]
#[structopt(
    name = "ed_lrr",
    about = "Elite: Dangerous Long-Range Router",
    rename_all = "kebab-case"
)]
/// Plots a route through multiple systems using breadth-first search (Currently needs a lot of RAM (about 6GB), sorry)
struct Opt {
    #[structopt(short, long = "range")]
    /// Jump Range
    range: f32,
    #[structopt(
        parse(from_os_str),
        short = "f",
        long = "path",
        default_value = "./stars.csv"
    )]
    /// Path to stars.csv
    ///
    /// Generate using process.py (https://gitlab.com/Earthnuker/ed_lrr/raw/master/dumps/process.py)
    file_path: PathBuf,

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

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

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

    #[structopt(
        short,
        long = "mode",
        raw(possible_values = "&[\"bfs\", \"greedy\",\"astar\"]")
    )]
    /// 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
    */
    mode: Mode,
    /// Systems to route through
    systems: Vec<String>,
}
fn dist(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];

    return (dx * dx + dy * dy + dz * dz).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 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>,
    scoopable: FnvHashSet<i64>,
}

impl Router {
    pub fn new(path: &PathBuf) -> 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_or_else(|e| {
                println!("Error loading {}: {}", path.to_str().unwrap(), e);
                std::process::exit(1);
            });
        println!("Loading {}...", path.to_str().unwrap());
        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,
            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> {
        return self.tree.locate_within_distance(*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, r: f32) -> impl Iterator<Item = &Point> {
        return self.points_in_sphere(&[sys.x, sys.y, sys.z], self.mult(sys.id) * r);
    }

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

    pub fn best_name_multiroute(
        &self,
        waypoints: &Vec<String>,
        range: f32,
        full: bool,
        mode: Mode,
        factor: f32,
    ) -> Vec<(&System, &Point)> {
        let mut best_score: f32 = std::f32::MAX;
        let mut waypoints = waypoints.clone();
        let mut best_permutation_waypoints = waypoints.clone();
        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 (src, dst) = (self.name_to_point(&src), self.name_to_point(&dst));
                            total_d += src.distp2(dst);
                        }
                        _ => panic!("Invalid routing parameters!"),
                    }
                }
                if total_d < best_score {
                    best_score = total_d;
                    best_permutation_waypoints = waypoints.clone();
                }
            }
            if !waypoints.next_permutation() {
                break;
            }
        }

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

    pub fn name_multiroute(
        &self,
        waypoints: &Vec<String>,
        range: f32,
        mode: Mode,
        factor: f32,
    ) -> Vec<(&System, &Point)> {
        let mut coords = Vec::new();
        for p in waypoints {
            let p = self.name_to_point(p);
            let s = [p.x, p.y, p.z];
            coords.push(s);
        }
        return self.multiroute(coords.as_slice(), range, mode, factor);
    }
    pub fn multiroute(
        &self,
        waypoints: &[[f32; 3]],
        range: f32,
        mode: Mode,
        factor: f32,
    ) -> Vec<(&System, &Point)> {
        let mut route = 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() {
                        route.extend(block.iter());
                    } else {
                        route.extend(block.iter().skip(1));
                    }
                }
                _ => panic!("Invalid routing parameters!"),
            }
        }
        return route;
    }

    fn sys_to_point(&self, id: i64) -> &Point {
        for p in &self.tree {
            if p.id == id {
                return p;
            }
        }
        eprintln!("Sytem-ID not found: \"{}\"", id);
        std::process::exit(1);
    }

    fn name_to_point(&self, name: &str) -> &Point {
        for sys in self.systems.values() {
            if sys.name == name {
                return self.sys_to_point(sys.id);
            }
        }
        eprintln!("Sytem not found: \"{}\"", name);
        std::process::exit(1);
    }

    pub fn route_astar(
        &self,
        src: &[f32; 3],
        dst: &[f32; 3],
        range: f32,
        factor: f32,
    ) -> Vec<(&System, &Point)> {
        if factor == 0.0 {
            return self.route_bfs(src, dst, range);
        }
        println!("Running A-Star with greedy factor of {}", factor);
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        let start_sys_name = self.systems.get(&start_sys.id).unwrap().name.clone();
        let goal_sys_name = self.systems.get(&goal_sys.id).unwrap().name.clone();
        {
            let d = dist(src, dst);
            println!(
                "Plotting route from {} to {}...",
                start_sys_name, goal_sys_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, &Point)> = 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.id);
                            let d_g = (nb.distp(goal_sys) / range) as usize;
                            return (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;
                    return fcmp(&v_a, &v_b);
                });
                // queue.reverse();
            }
        }
        println!();

        println!();
        if !found {
            eprintln!(
                "No route from {} to {} found!",
                start_sys_name, goal_sys_name
            );
            return Vec::new();
        }
        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 v;
    }

    pub fn route_greedy(
        &self,
        src: &[f32; 3],
        dst: &[f32; 3],
        range: f32,
    ) -> Vec<(&System, &Point)> {
        println!("Running Greedy-Search");
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        let start_sys_name = self.systems.get(&start_sys.id).unwrap().name.clone();
        let goal_sys_name = self.systems.get(&goal_sys.id).unwrap().name.clone();
        {
            let d = dist(src, dst);
            println!(
                "Plotting route from {} to {}...",
                start_sys_name, goal_sys_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, &Point)> = Vec::new();
        queue.push((
            -self.mult(goal_sys.id),
            start_sys.distp2(goal_sys),
            0,
            &start_sys,
        ));
        seen.insert(start_sys.id);
        while !(queue.is_empty() || found) {
            std::io::stdout().flush().unwrap();
            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
                    );
                }
                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.id);
                            return (-self.mult(nb.id), 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!();
        if !found {
            eprintln!(
                "No route from {} to {} found!",
                start_sys_name, goal_sys_name
            );
            return Vec::new();
        }
        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 v;
    }

    pub fn route_bfs(&self, src: &[f32; 3], dst: &[f32; 3], range: f32) -> Vec<(&System, &Point)> {
        println!("Running BFS");
        let start_sys = self.closest(src);
        let goal_sys = self.closest(dst);
        let start_sys_name = self.systems.get(&start_sys.id).unwrap().name.clone();
        let goal_sys_name = self.systems.get(&goal_sys.id).unwrap().name.clone();
        {
            let d = dist(src, dst);
            println!(
                "Plotting route from {} to {}...",
                start_sys_name, goal_sys_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, &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() || 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.id);
                            return (d + 1, nb);
                        }),
                );
            }
            std::mem::swap(&mut queue, &mut queue_next);
            depth += 1;
        }
        println!();
        if !found {
            eprintln!(
                "No route from {} to {} found!",
                start_sys_name, goal_sys_name
            );
            return Vec::new();
        }
        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 v;
    }
}

fn main() {
    let opts = Opt::from_args();
    let path = opts.file_path;
    let t_load = Instant::now();
    let router: Router = Router::new(&path);
    println!("Done in {}!", format_duration(t_load.elapsed()));
    let t_route = Instant::now();
    let route = if opts.permute || opts.full_permute {
        router.best_name_multiroute(
            &opts.systems,
            opts.range,
            opts.full_permute,
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )
    } else {
        router.name_multiroute(
            &opts.systems,
            opts.range,
            opts.mode,
            opts.factor.unwrap_or(1.0),
        )
    };
    println!(
        "Done in {} ({} Jumps)!\n",
        format_duration(t_route.elapsed()),
        route.len(),
    );
    if route.len() == 0 {
        eprintln!("No route found!");
        return;
    }
    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, p1.x, p1.y, p1.z, dist
        );
    }
    let (sys, p) = route.iter().last().unwrap();
    println!(
        "{} [{}] ({},{},{}): {:.2} Ly",
        sys.name, sys.star_type, p.x, p.y, p.z, 0.0
    );
    println!("Total: {:.2} Ly", total);
}