gst 0.1.3

use ordered_float::OrderedFloat;
use std::cmp::{min, max, Ordering};
use std::convert::Into;
use gst::{GstKey, Gst};

/// An [R* Tree](http://dbs.mathematik.uni-marburg.de/publications/myPapers/1990/BKSS90.pdf)
pub type RTree<T> = Gst<Rect, T>;

/// A two dimensional point which supports `Ord`.
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
pub struct Point { 
    x: OrderedFloat<f32>,
    y: OrderedFloat<f32>
}

impl Point {
    /// A new point.
    pub fn new(x: f32, y: f32) -> Point {
        Point{x: OrderedFloat(x), y: OrderedFloat(y)}
    }

    /// Find the distance between two points.
    pub fn distance(&self, other: &Point) -> OrderedFloat<f32> {
        let dist = ((self.x.0 - other.x.0).powi(2) + (self.y.0- other.y.0).powi(2)).sqrt();
        OrderedFloat(dist)
    }
}

impl Into<Rect> for Point {
    fn into(self) -> Rect {
        Rect{xmin: self.x, xmax: self.x, ymin: self.y, ymax: self.y}
    }
}

pub enum Axis { X, Y }

/// A Rectangle.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Rect { 
    pub xmin: OrderedFloat<f32>,
    pub xmax: OrderedFloat<f32>,
    pub ymin: OrderedFloat<f32>,
    pub ymax: OrderedFloat<f32>
}

impl Rect {
    /// Create a `Rect` from floats.
    pub fn from_float(xmin: f32, xmax: f32, ymin: f32, ymax: f32) -> Self {
        assert!(xmax >= xmin);
        assert!(ymax >= ymin);

        Rect{ 
            xmin: OrderedFloat(xmin), 
            xmax: OrderedFloat(xmax), 
            ymin: OrderedFloat(ymin), 
            ymax: OrderedFloat(ymax), 
        }
    }

    /// Return the intersection of two `Rect`s. Return None if they do not intersect.
    /// Also called 'overlap' in the R* tree paper.
    pub fn intersection(&self, other: &Rect) -> Option<Rect> {
        let right = min(self.xmax, other.xmax);
        let left = max(self.xmin, other.xmin);
        let top = min(self.ymax, other.ymax);
        let bot = max(self.ymin, other.ymin);
        if bot > top || left > right {
            None
        }
        else {
            Some(Rect { xmin: left, xmax: right, ymin: bot, ymax: top})
        }
    }

    /// Return the area of a `Rect`
    pub fn area(&self) -> f32 {
        (self.xmax.0 - self.xmin.0) * (self.ymax.0 - self.ymin.0)
    }

    /// Return the difference in area of the `Rect` if it were to merge with another `Rect`
    pub fn growth(&self, other: &Rect) -> f32 {
        self.expand(other).area() - self.area()
    }

    /// Return the perimeter of a `Rect`
    /// Also called 'margin' in the R* tree paper.
    pub fn perimeter(&self) -> f32 {
        2. * ((self.xmax.0 - self.xmin.0) + (self.ymax.0 - self.ymin.0))
    }

    /// Comparison function if sorting by the x axis.
    pub fn cmp_x(lhs: &Rect, rhs: &Rect) -> Ordering {
        match lhs.xmin.cmp(&rhs.xmin) {
            Ordering::Greater => Ordering::Greater,
            Ordering::Less => Ordering::Less,
            Ordering::Equal => lhs.xmax.cmp(&rhs.xmax)
        }
    }

    /// Comparison function if sorting by the y axis.
    pub fn cmp_y(lhs: &Rect, rhs: &Rect) -> Ordering {
        match lhs.ymin.cmp(&rhs.ymin) {
            Ordering::Greater => Ordering::Greater,
            Ordering::Less => Ordering::Less,
            Ordering::Equal => lhs.ymax.cmp(&rhs.ymax)
        }
    }
}


impl GstKey for Rect {
    fn consistent(&self, r: &Rect) -> bool {
        self.intersection(&r).is_some()
    }

    fn expand(&self, other: &Rect) -> Self {
        Rect {
            xmin: min(self.xmin, other.xmin),
            xmax: max(self.xmax, other.xmax),
            ymin: min(self.ymin, other.ymin),
            ymax: max(self.ymax, other.ymax),
        }
    }

    /// Vanilla R tree algorithm: 1. choose subtree based on min growth.
    /// R* tree algorithm: 1. choose subtree based on min overlap growth. 
    ///                    2. use min growth as a tiebreaker
    /// We use R* tree algorithm below.
    fn penalty(bounds: &[Rect], key: &Rect) -> (usize, f32) {
        assert!(bounds.len() != 0, "penalty called when there are no children.");

        let mut min_overlap_growth = ::std::f32::MAX;
        let mut min_overlap_growth_idx = 0;

        for (i, bounds_a) in bounds.iter().enumerate() {
            let overlap_growth = bounds.iter()
                .filter(|b| *b != bounds_a)
                .map(|bounds_b| {
                    let bound = bounds_a.intersection(&bounds_b);
                    let expanded_bounds = bounds_a.expand(&key).intersection(&bounds_b);
                    match (bound, expanded_bounds) {
                        (Some(rect_a), Some(rect_b)) => (rect_b.area() - rect_a.area()),
                        (Some(rect_a), None)         => rect_a.area(),
                        (None, Some(rect_b))         => rect_b.area(),
                        (None, None)                 => 0.
                    }
                })
                .fold(0., |accu, i| accu + i);

            if overlap_growth == 0. {
                return (i, overlap_growth);
            }

            if overlap_growth < min_overlap_growth {
                min_overlap_growth = overlap_growth;
                min_overlap_growth_idx = i;
            } else if (overlap_growth - min_overlap_growth).abs() < ::std::f32::EPSILON {
                let prev_growth = bounds[min_overlap_growth_idx].growth(key);
                if bounds_a.growth(key) < prev_growth {
                    min_overlap_growth = overlap_growth;
                    min_overlap_growth_idx = i;
                }
            }
        }
        (min_overlap_growth_idx, min_overlap_growth)
    }

    fn pick_split(bounds: &[Rect], min_split_size: usize) -> Vec<usize> {
        let (axis, split_index) = choose_split_axis_and_index(&bounds, min_split_size);
        let (_, mut split_indices) = match axis {
            Axis::X => sort_rects_by_x(&bounds),
            Axis::Y => sort_rects_by_y(&bounds)
        };
        split_indices.truncate(split_index);
        split_indices.sort();

        assert!(split_indices.len() >= min_split_size);
        assert!(bounds.len() - split_indices.len() >= min_split_size);
        split_indices
    }
}

fn sort_rects_by_x(bounds: &[Rect]) -> (Vec<Rect>, Vec<usize>) {
    let mut bounds_indices = bounds.iter().enumerate().collect::<Vec<_>>();
    bounds_indices.sort_by(|lhs, rhs| Rect::cmp_x(&lhs.1, &rhs.1));
    let (indices, sorted_bounds) = bounds_indices.into_iter().unzip();
    (sorted_bounds, indices)
}

fn sort_rects_by_y(bounds: &[Rect]) -> (Vec<Rect>, Vec<usize>) {
    let mut bounds_indices = bounds.iter().enumerate().collect::<Vec<_>>();
    bounds_indices.sort_by(|lhs, rhs| Rect::cmp_y(&lhs.1, &rhs.1));
    let (indices, sorted_bounds) = bounds_indices.into_iter().unzip();
    (sorted_bounds, indices)
}

/// return the min margin and the index of partition that yields it.
fn margin_value(bounds: &[Rect], min_split_size: usize) -> (f32, usize) {
    assert!(min_split_size > 0);

    let mut min_margin = ::std::f32::MAX;
    let mut min_margin_idx = 0;
    for i in min_split_size..(bounds.len()-(min_split_size-1)) {
        let (low, high) = bounds.split_at(i);
        let low_bb_margin = match Rect::union(&low) {
            Some(rect) => rect.perimeter(),
            None => 0.
        };
        let high_bb_margin = match Rect::union(&high) {
            Some(rect) => rect.perimeter(),
            None => 0.
        };
        let new_margin =  low_bb_margin + high_bb_margin;
        if new_margin < min_margin {
            min_margin = new_margin;
            min_margin_idx = i;
        }
    }
    (min_margin, min_margin_idx)
}

fn overlap_value(bounds: &[Rect], min_split_size: usize) -> (f32, usize) {
    assert!(min_split_size > 0);

    let mut min_overlap = ::std::f32::MAX;
    let mut min_overlap_idx = 0;
    let mut min_area = ::std::f32::MAX;
    for i in min_split_size..(bounds.len()-(min_split_size-1)) {
        let (low, high) = bounds.split_at(i);
        let low_bb = Rect::union(&low).unwrap();
        let high_bb = Rect::union(&high).unwrap();
        let new_overlap =  match low_bb.intersection(&high_bb) {
            Some(overlap) => overlap.area(),
            None => 0.
        };
        let new_area = low_bb.area() + high_bb.area();
        if new_overlap < min_overlap || 
           ((min_overlap - new_overlap).abs() < ::std::f32::EPSILON && new_area < min_area) {
            min_overlap = new_overlap;
            min_overlap_idx = i;
            min_area = new_area;
        }
    }
    (min_overlap, min_overlap_idx)
}

fn choose_split_axis_and_index(bounds: &[Rect], min_split_size: usize) -> (Axis, usize) {
    // Generate distributions: sort entries. 
    // CSA1 for each asix, sort entries by lower then by the upper value of their rectangles 
    //      Compute S, the sum of all margin values of the different distributions
    // CSA2 choose the axis with the minimum S as split axis
    let (x_bounds, _) = sort_rects_by_x(&bounds);
    let (min_margin_val_x, _) = margin_value(&x_bounds, min_split_size);
    let (y_bounds, _) = sort_rects_by_y(&bounds);
    let (min_margin_val_y, _) = margin_value(&y_bounds, min_split_size);
    if min_margin_val_x < min_margin_val_y {
        (Axis::X, overlap_value(&x_bounds, min_split_size).1)
    } else {
        (Axis::Y, overlap_value(&y_bounds, min_split_size).1)
    }
}

// For bench::rtreebad::RTreeBad. This is otherwise worthless.
impl PartialOrd for Rect {
    fn partial_cmp(&self, other: &Rect) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

impl Ord for Rect {
    fn cmp(&self, other: &Rect) -> Ordering {
        match Rect::cmp_x(self, other) {
            Ordering::Greater => Ordering::Greater,
            Ordering::Less => Ordering::Less,
            Ordering::Equal => Rect::cmp_y(self, other)
        }
    }
}

#[cfg(test)]
mod test {
    use gst::{GstKey};
    use rtree::{Rect, Point};
    use rtree;

    #[test]
    fn test_rect_perimeter() {
        let r = Rect::from_float(-10., 10., -20., 5.);
        assert!((r.perimeter() - 90.).abs() < ::std::f32::EPSILON);
    }

    #[test]
    fn test_rect_area() {
        let r = Rect::from_float(-10., 10., -20., 5.);
        assert!((r.area() - 500.).abs() < ::std::f32::EPSILON);
    }

    #[test]
    fn test_rect_growth() {
        let r0 = Rect::from_float(-10., 10., -20., 5.);
        let r_x = Rect::from_float(0., 20., -20., 5.);
        let r_y = Rect::from_float(-10., 10., 0., 10.);
        let r_xy = Rect::from_float(0., 20., 0., 10.);
        assert!((r0.growth(&r_x) - 250.).abs() < ::std::f32::EPSILON);
        assert!((r0.growth(&r_y) - 100.).abs() < ::std::f32::EPSILON);
        assert!((r0.growth(&r_xy) - 400.).abs() < ::std::f32::EPSILON);
    }

    #[test]
    fn test_rect_intersection() {
        let r_0 = Rect::from_float(0., 0., 0., 0.);
        let r_1 = Rect::from_float(-10., 10., -10., 10.);
        let r_2 = Rect::from_float(-5., 5., -20., 0.);
        let r_3 = Rect::from_float(-5., -5., -5., -5.);
        assert_eq!(r_0.intersection(&r_0).unwrap(), r_0);
        assert_eq!(r_0.intersection(&r_3), None);
        assert_eq!(r_0.intersection(&r_1).unwrap(), r_0);
        assert_eq!(r_0.intersection(&r_1).unwrap(), r_1.intersection(&r_0).unwrap());
        assert_eq!(r_1.intersection(&r_1).unwrap(), r_1);
        assert_eq!(r_1.intersection(&r_2).unwrap(), Rect::from_float(-5., 5., -10., 0.));
    }

    #[test]
    fn test_union_rects() {
        let v = vec![
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float(-1., 0.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.)
        ];
        assert_eq!(Rect::union(&v).unwrap(), Rect::from_float(-1., 1., -1., 1.));
    }

    #[test]
    fn test_union_point() {
        let v = vec![
            Rect::from(Point::new(0., 1.).into()),
            Rect::from(Point::new(1., 1.).into()),
            Rect::from(Point::new(1., -1.).into()),
            Rect::from(Point::new(-1., -1.).into())
        ];
        assert_eq!(Rect::union(&v).unwrap(), Rect::from_float(-1., 1., -1., 1.));
    }

    #[test]
    fn test_rect_penalty_no_overlap() {
        let v = vec![
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float(-1., 0.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.)
        ];
        assert_eq!(Rect::penalty(&v, &Rect::from_float(0., 1., 0., 0.9)), (0, 0.));
        assert_eq!(Rect::penalty(&v, &Rect::from_float(-0.9, 0., 0., 1.)), (1, 0.));
        assert_eq!(Rect::penalty(&v, &Rect::from_float(0., 1., -0.9, 1.)), (2, 0.));
    }

    #[test]
    fn test_rect_penalty_overlap() {
        let v = vec![
            Rect::from_float( 0., 10.,  0., 5.),
            Rect::from_float( 5., 10.,  0., 10.),
        ];
        assert_eq!(Rect::penalty(&v, &Rect::from_float(2.0, 3.0, 6., 7.)), (0, 10.));
    }

    #[test]
    fn test_margin_value() {
        let v = vec![
            Rect::from_float(-1., 0.,  0., 1.),
            Rect::from_float(-1., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.),
            Rect::from_float( 0., 1.,  0., 1.),
        ];
        // TODO: Assert something.
    }

    #[test]
    fn test_overlap_value() {
        let v = vec![
            Rect::from_float(-1., 0.,  0., 1.),
            Rect::from_float(-1., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.),
            Rect::from_float( 0., 1.,  0., 1.),
        ];
        // TODO: Assert something.
    }

    #[test]
    fn test_sort_rects_by_x() {
        let v = vec![
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.),
            Rect::from_float(-1., 1.,  0., 1.),
            Rect::from_float(-1., 0.,  0., 1.),
        ];
        let expect_rects = vec![
            Rect::from_float(-1., 0.,  0., 1.),
            Rect::from_float(-1., 1.,  0., 1.),
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 1.),
        ];
        let expect_idx = vec![3,2,0,1];
        let result = rtree::sort_rects_by_x(&v);
        assert_eq!(result.0, expect_rects);
        assert_eq!(result.1, expect_idx);
    }

    #[test]
    fn test_sort_rects_by_y() {
        let v = vec![
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 2.),
            Rect::from_float(-1., 1.,  0., 2.),
            Rect::from_float(-1., 0.,  1., 3.),
        ];
        let expect_rects = vec![
            Rect::from_float( 0., 1., -1., 2.),
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float(-1., 1.,  0., 2.),
            Rect::from_float(-1., 0.,  1., 3.),
        ];
        let expect_idx = vec![1, 0, 2, 3];
        let result = rtree::sort_rects_by_y(&v);
        assert_eq!(result.0, expect_rects);
        assert_eq!(result.1, expect_idx);
    }

    #[test]
    fn test_rect_pick_split() {
        let v = vec![
            Rect::from_float( 0., 1.,  0., 1.),
            Rect::from_float( 0., 1., -1., 2.),
            Rect::from_float(-1., 1.,  0., 2.),
            Rect::from_float(-1., 0.,  1., 3.),
        ];
        assert_eq!(Rect::pick_split(&v, 1), vec![0,1,2]);
    }
}