polyhedron 0.1.0

use std::ops::Mul;

use linear_isomorphic::{InnerSpace, RealField, VectorSpace};
use nalgebra::{Matrix4, Vector4};

use super::queue::PQueue;
use crate::*;
use crate::{MeshStorage, PolyHedron, VertData};

/// Quadric error metric simplifiation.
pub fn quadric_simplify<S, C: MeshStorage, const MANIFOLD: bool>(
    mesh: &mut PolyHedron<C, MANIFOLD>,
    mut simplify_count: usize,
) where
    VertData<C>: linear_isomorphic::InnerSpace<S>,
    S: linear_isomorphic::RealField
        + Mul<VertData<C>, Output = VertData<C>>
        + ordered_float::FloatCore
        + nalgebra::ComplexField,
{
    let (mut quadrics, mut queue) = initialize_vertex_quadrics(mesh);

    while !queue.is_empty() && simplify_count > 0
    {
        let (eid, _) = queue.pop().unwrap();

        let hedge_handle = mesh.half_edge_handle(eid);

        // TODO(high): handle boundary edges as well.
        if hedge_handle.id().is_void()
            || hedge_handle.is_in_boundary()
            || hedge_handle
                .source()
                .iter_star_half_edges()
                .any(|e| e.is_in_boundary())
            || hedge_handle
                .dest()
                .iter_star_half_edges()
                .any(|e| e.is_in_boundary())
            || hedge_handle.iterate_butterfly().any(|e| e.is_in_boundary())
            || !hedge_handle.can_collapse()
        {
            continue;
        }

        simplify_count -= 1;

        // Remove all edges that touch the current edge from the queue.
        for e in hedge_handle.source().iter_star_half_edges()
        {
            let id = e.id();
            queue.remove(id);
        }

        for e in hedge_handle.dest().iter_star_half_edges()
        {
            let id = e.id();
            queue.remove(id);
        }

        let (_cost, point) = compute_edge_weight(hedge_handle.replicate(), &quadrics);
        let new_quadric = quadrics[hedge_handle.source().id().to_index()]
            + quadrics[hedge_handle.dest().id().to_index()];

        let v = hedge_handle.collapse();

        let mut v_handle = mesh.vert_handle(v);
        quadrics[v_handle.id().to_index()] = new_quadric;

        // Update the position of the collapsed vertex to that wich minimizes
        // the quadric error.
        let mut pos = VertData::<C>::default();
        pos[0] = point.x;
        pos[1] = point.y;
        pos[2] = point.z;
        v_handle.update_data(|v| *v = pos.clone());

        // Add all edges touching the collapsed vertex with update costs.
        for e in v_handle.iter_star_half_edges()
        {
            debug_assert!(!e.id().is_void());
            let (cost, _) = compute_edge_weight(e.replicate(), &quadrics);
            queue.push(e.id(), cost);
        }
    }
}

fn initialize_vertex_quadrics<S, C: MeshStorage, const MANIFOLD: bool>(
    mesh: &PolyHedron<C, MANIFOLD>,
) -> (Vec<Matrix4<S>>, PQueue<HalfEdgeId, S>)
where
    VertData<C>: linear_isomorphic::InnerSpace<S>,
    S: linear_isomorphic::RealField
        + Mul<VertData<C>, Output = VertData<C>>
        + ordered_float::FloatCore
        + nalgebra::ComplexField,
{
    let mut vertex_quadrics = vec![Matrix4::<S>::zeros(); mesh.vert_count()];

    for face in mesh.iter_faces()
    {
        let normal = face.naive_normal().normalized();

        let position = face.half_edge().source().read_data().clone();
        let d = position.dot(&normal);

        let q = Vector4::<S>::new(normal[0], normal[1], normal[2], -d);
        let quadric = q * q.transpose();

        for v in face.vertices()
        {
            vertex_quadrics[v.id().to_index()] += quadric;
        }
    }

    let mut queue = PQueue::new();
    for edge in mesh.iter_edges()
    {
        let eid = edge.id();
        let (cost, _) = compute_edge_weight(edge, &vertex_quadrics);
        queue.push(eid, cost);
    }

    (vertex_quadrics, queue)
}

fn compute_edge_weight<S, C: MeshStorage, const MANIFOLD: bool>(
    edge: HalfEdgeHandle<C, MANIFOLD>,
    quadrics: &Vec<Matrix4<S>>,
) -> (S, Vector4<S>)
where
    S: RealField + nalgebra::ComplexField,
    VertData<C>: VectorSpace<Scalar = S>,
{
    debug_assert!(!edge.id().is_void());
    let cost_matrix =
        quadrics[edge.source().id().0 as usize] + quadrics[edge.dest().id().0 as usize];

    let intermediate = match cost_matrix.try_inverse()
    {
        None =>
        {
            let half = S::from(0.5).unwrap();
            (edge.dest().read_data().clone() + edge.source().read_data().clone()) * half
        }
        Some(inverse) =>
        {
            let b = Vector4::<S>::new(
                S::from(0.).unwrap(),
                S::from(0.).unwrap(),
                S::from(0.).unwrap(),
                S::from(1.).unwrap(),
            );
            // Find the point that minimizes the quadric error.
            let sol = inverse * b;

            // Homogenize the coordinates.
            let mut res = VertData::<C>::default();
            res[0] = sol.x / sol.w;
            res[1] = sol.y / sol.w;
            res[2] = sol.z / sol.w;

            res
        }
    };

    let new_point = Vector4::<S>::new(
        intermediate[0],
        intermediate[1],
        intermediate[2],
        S::from(1.0).unwrap(),
    );

    (
        -(new_point.transpose() * cost_matrix * new_point)[(0, 0)],
        new_point,
    )
}

// +| Tests |+ =======================================================
#[cfg(test)]
mod tests
{
    use wavefront_loader::ObjData;

    use super::*;
    use crate::verification::verify_correctness;
    use crate::BasicPolyHedron;

    #[test]
    fn test_quadric_simplify()
    {
        let ObjData {
            vertices,
            vertex_face_indices,
            ..
        } = ObjData::from_disk_file("../../../Assets/armadillo.obj");

        let mut mesh = BasicPolyHedron::<_, false>::new(
            vertices.clone(),
            (),
            vertex_face_indices
                .iter()
                .map(|l| l.iter().map(|i| *i as usize)),
        );
        verify_correctness(&mesh).unwrap();

        let count = mesh.vert_count() as f64 * (1.0 - 0.1);
        quadric_simplify(&mut mesh, count as usize);
        verify_correctness(&mesh).unwrap();

        let (vs, fs) = mesh.face_list();
        ObjData::export(&(&vs, &fs), "tmp/simple_armadillo.obj");
    }
}