// ========================================================================== //
// Class inclusion
#include "StreamSurfaces/StreamSurfaces.hpp"

// Local libraries
#include "Frames/FrameField.hpp"
#include "Pointcloud/Pointcloud.hpp"
#include "Utilities/math_operations.hpp"
#include "Utilities/mesh_operations.hpp"
#include "Utilities/string_operations.hpp"

// External libraries
#include <GEL/CGLA/CGLA.h>
#include <GEL/Geometry/Graph.h>
#include <GEL/Geometry/KDTree.h>
#include <GEL/HMesh/Delaunay_triangulate.h>
#include <GEL/HMesh/HMesh.h>

// Standard libraries
#include <algorithm>
#include <filesystem>
#include <random>
#include <vector>

// Parallelisation
#include <omp.h>
#if USE_MPI == 1
    #include <mpi.h>
#endif

// Macros
#ifndef MPI_VERSION
    #define MPI_VERSION 0
#endif

// ========================================================================== //
// Main hexing function

// Simple intuitive approach to constructing the mesh
void StreamSurfaces::triangle_converter() {
    // Function used to convert Point Clouds to Triangle Meshes

    Utilities::debug_info(__FILE__, __LINE__, "Converting to triangle meshes",
                          debug);

    // Setup variables
    const int N = this->surfaces.size();

#if MPI_VERSION == 0
    #ifdef _OPENMP
        // OpenMP settings.
        #pragma omp parallel for schedule(dynamic)
    #endif /* _OPENMP */
#endif     /* MPI_VERSION */
    for (int s_id = 0; s_id < N; s_id++) {
        if (this->surfaces[s_id].mesh.no_faces() > 0) continue;

        this->surfaces[s_id].mesh = this->triangulate(this->surfaces[s_id].pcl);
    }
    Utilities::debug_info(__FILE__, __LINE__, "Conversion complete", debug);
}

HMesh::Manifold StreamSurfaces::triangulate(PointCloud &pcl) {

    if (pcl.size() < 3) return HMesh::Manifold();

    // General settings
    const double search_radius =
        3.0 * this->super_sample_pds_r * this->frame->element_size;

    // Construct the KDTree
    Geometry::KDTree<CGLA::Vec3d, size_t> kdtree;
    for (size_t idx = 0; idx < pcl.size(); idx++)
        kdtree.insert(pcl.points(idx), idx);
    kdtree.build();

    // ================================================================== //
    // Build the list of possible triangles used for voting

    // Build a list of faces generated from the delaunay triangulator
    std::vector<std::vector<size_t>> face_list; // The list of faces
    std::vector<std::vector<size_t>>
        face_sorted; // Sorted version for voting scheme

    for (size_t idx = 0; idx < pcl.size(); idx++) {
        CGLA::Vec3d p = pcl.points(idx);
        CGLA::Vec3d n = pcl.normals(idx);

        // Compute the tangent space used for triangulation
        CGLA::Vec3d v1 = normalize(cross(n, CGLA::Vec3d(M_PI, n[2], n[0])));
        CGLA::Vec3d v2 = normalize(cross(n, v1));

        // Triangulate small neighbourhood with Delaunay
        std::vector<CGLA::Vec3d> neighbour_pos;
        std::vector<size_t>      neighbour_ids;
        kdtree.in_sphere(p, search_radius, neighbour_pos, neighbour_ids);

        if (neighbour_pos.size() < 3) continue;

        HMesh::Manifold m2 = HMesh::Delaunay_triangulate(neighbour_pos, v1, v2);

        // Construct the list of surface indices for faces
        for (auto fid : m2.faces()) {
            std::vector<size_t>      face_idx;
            std::vector<CGLA::Vec3d> face_pos;

            if (dot(normal(m2, fid), n) < 0.0) continue;

            // Determine corner positions
            for (HMesh::Walker w = m2.walker(fid); !w.full_circle();
                 w               = w.circulate_face_ccw()) {
                face_pos.push_back(m2.pos(w.vertex()));
            }

            // Find surface index for the corners
            for (CGLA::Vec3d p_face : face_pos) {
                size_t      p_idx = 0;
                CGLA::Vec3d p_surf;
                double      dist = 1e-3;
                kdtree.closest_point(p_face, dist, p_surf, p_idx);
                face_idx.push_back(p_idx);
            }

            // Save the face and the sorted version
            face_list.push_back(face_idx);
            sort(face_idx.begin(), face_idx.end());
            face_sorted.push_back(face_idx);
        }
    }

    if (face_list.size() < 2) return HMesh::Manifold();

    // ================================================================== //
    // Vote for which faces to keep. If the same face is found our
    // current idx is incremented, and the compared one is decremented
    // to allow skips and make it clear which are duplicates.

    std::vector<int> face_voting(face_list.size(), 0);
    for (size_t idx = 0; idx < face_list.size(); idx++) {
        if (face_voting[idx] < 0) continue;
        std::vector<size_t> face_tmp = face_sorted[idx];

        for (size_t j = idx + 1; j < face_list.size(); j++) {
            if (face_voting[j] < 0) continue;
            if (face_tmp[0] != face_sorted[j][0]) continue;
            if (face_tmp[1] != face_sorted[j][1]) continue;
            if (face_tmp[2] != face_sorted[j][2]) continue;

            // Make sure we can tell the difference between the original
            // face and the duplicate
            face_voting[idx]++;
            face_voting[j]--;
        }
    }

    // ================================================================== //
    // Construct the surface mesh

    HMesh::Manifold mesh;

    // Add all faces where atleast all corners agree.
    for (size_t idx = 0; idx < face_list.size(); idx++) {
        if (face_voting[idx] < 5 || face_list[idx].size() != 3) continue;
        mesh.add_face({pcl.points(face_list[idx][0]),
                       pcl.points(face_list[idx][1]),
                       pcl.points(face_list[idx][2])});
    }
    stitch_mesh(mesh, 0.01 * search_radius);
    mesh.cleanup();

    // Cleanup the mesh
    HMesh::remove_valence_two_vertices(mesh), mesh.cleanup();
    HMesh::remove_valence_one_vertices(mesh), mesh.cleanup();
    HMesh::close_holes(mesh, 100);
    HMesh::triangulate(mesh, HMesh::SHORTEST_EDGE);
    mesh.cleanup();
    HMesh::maximize_min_angle(mesh, 0.1);

    // Utilities::smooth_boundary(mesh);
    return mesh;
}

// End of File
// ========================================================================== //