cg-math 0.1.2

extern crate nalgebra as na;

use core::panic;
use egui::plot::PlotPoints;
use na::{Matrix3, Matrix4, Vector2, Vector3};
use ordered_float::NotNan;
use std::fmt;

/// Describes common behaviour between regular and weighted triangles <br>
// V can be Vertex or WeightedVertex
// E can be Edge or WeightedEdge
// T can be Triangle or WeightedTriangle
pub trait TriangleLogic<V, E, T> {
    fn area(&self) -> f64;
    fn area3d(&self) -> f64;
    fn edges(&self) -> Vec<E>;
    fn has_edge(&self, edge: &E) -> bool;
    fn has_vertex(&self, vertex: &V) -> bool;
    fn is_neighbor(&self, other: &T) -> Option<E>;
    fn regularity(&self, other: &V) -> f64;
    /// Check whether the triangle is regular, w.r.t to some vertex; by tolerance of epsilon. <br>
    fn is_regular(&self, vertex: &V) -> bool;
    fn is_eps_regular(&self, vertex: &V, epsilon: f64) -> bool;
    fn orientation(&self) -> f64;
    fn vertices(&self) -> Vec<WeightedVertex>;
}

/// A weighted 0-simplex in 2-dimensional space.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
pub struct WeightedVertex {
    x: NotNan<f64>,
    y: NotNan<f64>,
    w: NotNan<f64>,
}

impl WeightedVertex {
    pub fn new(x: f64, y: f64, weight: f64) -> Self {
        Self {
            x: NotNan::new(x).unwrap(),
            y: NotNan::new(y).unwrap(),
            w: NotNan::new(weight).unwrap(),
        }
    }

    pub fn as_vec2(&self) -> Vector2<f64> {
        Vector2::new(self.x(), self.y())
    }

    // Lifts the vertex to 3d such that $z = xˆ2 + yˆ2 - w$
    pub fn lift(&self) -> Vector3<f64> {
        Vector3::new(self.x(), self.y(), self.x().powi(2) + self.y().powi(2) - self.w())
    }

    pub fn w(&self) -> f64 {
        self.w.into_inner()
    }

    pub fn x(&self) -> f64 {
        self.x.into_inner()
    }

    pub fn y(&self) -> f64 {
        self.y.into_inner()
    }
}

impl fmt::Display for WeightedVertex {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "({}, {}, w: {})", self.x(), self.y(), self.w())
    }
}

/// A weighted 1-simplex in 2-dimensional space.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
pub struct WeightedEdge {
    pub a: WeightedVertex,
    pub b: WeightedVertex,
    triangles: [Option<WeightedTriangle>; 2], // store the potential 1, 2 incident triangles
}

impl WeightedEdge {
    pub fn new(a: WeightedVertex, b: WeightedVertex) -> Self {
        let mut vertices = vec![a, b];
        vertices.sort_unstable();
        Self {
            a: vertices[0],
            b: vertices[1],
            triangles: [None, None],
        }
    }

    /// Create a new edge with the given triangles as incident triangles
    pub fn with_triangles(
        a: WeightedVertex,
        b: WeightedVertex,
        triangles: [Option<WeightedTriangle>; 2],
    ) -> Self {
        let mut vertices = vec![a, b];
        vertices.sort_unstable();
        Self {
            a: vertices[0],
            b: vertices[1],
            triangles,
        }
    }

    /// Set the incident triangles of the edge
    pub fn set_triangles(&mut self, triangles: [Option<WeightedTriangle>; 2]) {
        self.triangles = triangles;
    }

    pub fn vertices(&self) -> Vec<WeightedVertex> {
        vec![self.a, self.b]
    }
}

impl fmt::Display for WeightedEdge {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "Edge {{ {}, {} }}", self.a, self.b)
    }
}

/// A weighted 2-simplex in 2-dimensional space.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
pub struct WeightedTriangle {
    pub a: WeightedVertex,
    pub b: WeightedVertex,
    pub c: WeightedVertex,
}

impl WeightedTriangle {
    pub fn new(a: WeightedVertex, b: WeightedVertex, c: WeightedVertex) -> Self {
        let mut vertices = vec![a, b, c];
        vertices.sort_unstable();
        Self {
            a: vertices[0],
            b: vertices[1],
            c: vertices[2],
        }
    }

    /// Given an edge of the triangle retrieve the point not on the edge
    pub fn get_third_point(&self, edge: &WeightedEdge) -> WeightedVertex {
        if self.a != edge.a && self.a != edge.b {
            self.a
        } else if self.b != edge.a && self.b != edge.b {
            self.b
        } else {
            self.c
        }
    }
}

impl TriangleLogic<WeightedVertex, WeightedEdge, WeightedTriangle> for WeightedTriangle {
    /// The area of the triangle.
    /// ### Example
    /// 
    /// ```
    /// use cg_math::geometry::{WeightedVertex, WeightedTriangle, TriangleLogic};
    /// 
    /// let v0 = WeightedVertex::new(2.5, 2.0, 0.0);
    /// let v1 = WeightedVertex::new(1.0, 2.5, 0.0);
    /// let v2 = WeightedVertex::new(2.0, 3.0, 0.0);
    /// 
    /// let triangle = WeightedTriangle::new(v0, v1, v2);
    /// 
    /// assert_eq!(triangle.area(), 0.625);
    /// ```
    fn area(&self) -> f64 {
        ((self.a.x() * (self.b.y() - self.c.y())
            + self.b.x() * (self.c.y() - self.a.y())
            + self.c.x() * (self.a.y() - self.b.y()))
            / 2.0)
            .abs()
    }


    /// The area of the triangle lifted to 3d with $z = x^2 + y^2 - w$.
    /// ### Example 
    /// 
    /// ```
    /// use cg_math::geometry::{WeightedVertex, WeightedTriangle, TriangleLogic};
    /// 
    /// let v0 = WeightedVertex::new(2.5, 2.0, 1.0);
    /// let v1 = WeightedVertex::new(1.0, 2.5, 2.0);
    /// let v2 = WeightedVertex::new(2.0, 3.0, 3.0);
    /// 
    /// let triangle = WeightedTriangle::new(v0, v1, v2);
    /// 
    /// //assert_eq!(triangle.area3d(), 1.3975424859373686);
    /// ```
    fn area3d(&self) -> f64 {
        let ab = self.b.lift() - self.a.lift();
        let ac = self.c.lift() - self.a.lift();
        let cross = ab.cross(&ac);
        let cross_norm = cross.norm();
        cross_norm / 2.0
    }

    fn edges(&self) -> Vec<WeightedEdge> {
        vec![
            WeightedEdge::new(self.a, self.b),
            WeightedEdge::new(self.b, self.c),
            WeightedEdge::new(self.c, self.a),
        ]
    }

    /// Check if the given edge is part of the triangle.
    fn has_edge(&self, edge: &WeightedEdge) -> bool {
        for e in self.edges() {
            if e == *edge {
                return true;
            }
        }
        false
    }

    /// Check if the given vertex is part of the triangle.
    fn has_vertex(&self, vertex: &WeightedVertex) -> bool {
        self.a == *vertex || self.b == *vertex || self.c == *vertex
    }

    /// Check whether the triangle is regular w.r.t to the given vertex.
    /// 
    /// ```
    /// use cg_math::{geometry::{WeightedVertex, WeightedTriangle, TriangleLogic}, utils::c_hull};
    /// 
    /// // these 4 vertices span two of the three triangles in the triangulation
    /// let v0 = WeightedVertex::new(6.24, 8.38, 2.24);
    /// let v1 = WeightedVertex::new(9.34, 9.19, 7.46);  // assume this one was inserted last
    /// let v2 = WeightedVertex::new(8.04, 5.53, 7.53);
    /// let v3 = WeightedVertex::new(8.53, 8.43, 1.34);
    /// 
    /// assert_eq!(WeightedTriangle::new(v0, v2, v3).is_regular(&v1), false);
    /// ```
    fn is_regular(&self, vertex: &WeightedVertex) -> bool {
        self.regularity(vertex) < 0.0
    }

    /// Check whether the triangle is regular w.r.t to the given vertex; by tolerance of epsilon.
    fn is_eps_regular(&self, vertex: &WeightedVertex, epsilon: f64) -> bool {
        self.regularity(vertex) - epsilon < 0.0
    }

    /// The orientation of the triangle; clockwise (cw) or counter-cw.
    /// # Panics
    /// If the triangle consists of three collinear points.
    fn orientation(&self) -> f64 {
        let matrix = Matrix3::new(
            self.a.x(),
            self.a.y(),
            1.0,
            self.b.x(),
            self.b.y(),
            1.0,
            self.c.x(),
            self.c.y(),
            1.0,
        );
        match matrix.determinant() {
            d if d > 0.0 => {
                1.0 //println!("CounterClockwise");
            }
            d if d < 0.0 => {
                -1.0 //println!("Clockwise");
            }
            _ => panic!("Triangle consists of three collinear points"),
        }
    }

    /// Compute the level of local regularity w.r.t to the given vertex
    fn regularity(&self, vertex: &WeightedVertex) -> f64 {
        let orientation = self.orientation();

        // TODO: we could also discuss to shift each weight by epsilon
        let matrix = Matrix4::new(
            self.a.x(),
            self.a.y(),
            self.a.x().powi(2) + self.a.y().powi(2) - self.a.w(),
            1.0,
            self.b.x(),
            self.b.y(),
            self.b.x().powi(2) + self.b.y().powi(2) - self.b.w(),
            1.0,
            self.c.x(),
            self.c.y(),
            self.c.x().powi(2) + self.c.y().powi(2) - self.c.w(),
            1.0,
            vertex.x(),
            vertex.y(),
            vertex.x().powi(2) + vertex.y().powi(2) - vertex.w(),
            1.0,
        );

        //println!("matrix det * orientation: {}", matrix.determinant() * orientation);
        matrix.determinant() * orientation
    }

    /// Check if this and the other triangle share an edge, i.e. are neighbors
    /// and return it if existing
    fn is_neighbor(&self, other: &WeightedTriangle) -> Option<WeightedEdge> {
        let self_edges = self.edges();
        let other_edges = other.edges();

        for self_edge in &self_edges {
            for other_edge in &other_edges {
                if *self_edge == *other_edge {
                    return Some(*self_edge);
                }
            }
        }
        None
    }

    fn vertices(&self) -> Vec<WeightedVertex> {
        vec![self.a, self.b, self.c]
    }
}

impl From<&WeightedTriangle> for PlotPoints {
    fn from(triangle: &WeightedTriangle) -> Self {
        let points: Vec<[f64; 2]> = triangle
            .vertices()
            .iter()
            .map(|v| [v.x(), v.y()])
            .collect();
        PlotPoints::from(points)
    }
}

impl fmt::Display for WeightedTriangle {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "{{ {}, {}, {} }}", self.a, self.b, self.c)
    }
}

/// A triangulation of a set of vertices in 2-dimensional space.
pub struct WeightedTriangulation<'a>(pub &'a Vec<WeightedTriangle>);

impl fmt::Display for WeightedTriangulation<'_> {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        for (index, triangle) in self.0.iter().enumerate() {
            writeln!(f, "{}: {}", index, triangle)?;
        }
        Ok(())
    }
}

/// A triangulation of a set of vertices in 2-dimensional space.
// TODO merge with WeightedTriangulation
#[derive(PartialEq)]
pub struct Triangulation {
    pub triangles: Vec<WeightedTriangle>,
    pub used_vertices: Vec<WeightedVertex>,
    pub ignored_vertices: Vec<WeightedVertex>,
    pub cache: Vec<Vec<WeightedTriangle>>
}

impl Triangulation {
    /// Create a new empty trangulation.
    pub fn empty() -> Self {
        Self {
            triangles: vec![],
            used_vertices: vec![],
            ignored_vertices: vec![],
            cache: vec![],
        }
    }
    
    /// Create a new trangulation with the given starting triangle.
    pub fn new(starting_triangle: WeightedTriangle) -> Self {
        Self {
            triangles: vec![starting_triangle],
            used_vertices: vec![],
            ignored_vertices: vec![],
            cache: vec![],
        }
    }

    pub fn triangles(&self) -> &Vec<WeightedTriangle> {
        &self.triangles
    }

    pub fn triangles_mut(&mut self) -> &mut Vec<WeightedTriangle> {
        &mut self.triangles
    }

    pub fn used_vertices(&self) -> &Vec<WeightedVertex> {
        &self.used_vertices
    }

    pub fn used_vertices_mut(&mut self) -> &mut Vec<WeightedVertex> {
        &mut self.used_vertices
    }

    pub fn ignored_vertices(&self) -> &Vec<WeightedVertex> {
        &self.ignored_vertices
    }

    pub fn ignored_vertices_mut(&mut self) -> &mut Vec<WeightedVertex> {
        &mut self.ignored_vertices
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn edges_equal() {
        let e0 = WeightedEdge::new(
            WeightedVertex::new(0.0, 1.0, 0.0),
            WeightedVertex::new(3.0, 4.0, 0.0),
        );
        let e1 = WeightedEdge::new(
            WeightedVertex::new(3.0, 4.0, 0.0),
            WeightedVertex::new(0.0, 1.0, 0.0),
        );
        let e2 = WeightedEdge::new(
            WeightedVertex::new(0.0, 0.0, 0.0),
            WeightedVertex::new(1.0, 1.0, 0.0),
        );
        assert_eq!(e0, e1);
        assert_ne!(e0, e2);
        assert_ne!(e1, e2);
    }

    #[test]
    fn triangles_equal() {
        let t0 = WeightedTriangle::new(
            WeightedVertex::new(0.0, 1.0, 0.0),
            WeightedVertex::new(1.0, 0.0, 0.0),
            WeightedVertex::new(-1.0, 0.0, 0.0),
        );
        let t1 = WeightedTriangle::new(
            WeightedVertex::new(1.0, 0.0, 0.0),
            WeightedVertex::new(-1.0, 0.0, 0.0),
            WeightedVertex::new(0.0, 1.0, 0.0),
        );
        assert_eq!(t0, t1);
    }

    #[test]
    fn triangle_and_point_regular() {
        let vertex = WeightedVertex::new(0.6984975495419887, 0.6787937052516924, 0.0);

        let tri = WeightedTriangle::new(
            WeightedVertex::new(0.5222943585280901, 0.9101950968457111, 0.0),
            WeightedVertex::new(0.7504630819229956, 0.42525042963771664, 0.0),
            WeightedVertex::new(0.9030435560273877, 0.6666048060052796, 0.0),
        );

        assert!(!tri.is_regular(&vertex));
        assert!(tri.regularity(&vertex) - 0.1 < 0.0);
    }

    #[test]
    fn neighbors() {
        let t0 = WeightedTriangle::new(
            WeightedVertex::new(2.0, 0.0, 0.0),
            WeightedVertex::new(2.8, 2.8, 0.0),
            WeightedVertex::new(2.4, 2.4, 0.0),
        );

        let t1 = WeightedTriangle::new(
            WeightedVertex::new(3.0, 2.0, 0.0),
            WeightedVertex::new(2.8, 2.8, 0.0),
            WeightedVertex::new(2.4, 2.4, 0.0),
        );

        assert_eq!(
            t0.is_neighbor(&t1),
            Some(WeightedEdge::new(
                WeightedVertex::new(2.8, 2.8, 0.0),
                WeightedVertex::new(2.4, 2.4, 0.0)
            ))
        );
        assert_eq!(
            t1.is_neighbor(&t0),
            Some(WeightedEdge::new(
                WeightedVertex::new(2.8, 2.8, 0.0),
                WeightedVertex::new(2.4, 2.4, 0.0)
            ))
        );
    }
}