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

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

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

// Standard libraries
#include <vector>

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

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

// ========================================================================== //
// Extract the voxel structure

void StreamSurfaces::indicator(StreamSurface &surf, const int s_id) {

    // Set search radius
    const double search_radius =
        sqrt(0.25 * pow(desired_spacing, 2.0) +
             (1.0 / 3.0) * pow(1.5 * surf_pds_r * frame->element_size, 2.0));

    // Construct KDTree for the surface
    Geometry::KDTree<CGLA::Vec3d, size_t> kdtree;

    for (size_t idx = 0; idx < surfaces[s_id].pcl.size(); idx++)
        kdtree.insert(surfaces[s_id].pcl.points(idx), idx);
    kdtree.build();

    // Compute the indicator response.
    for (size_t pr_id = 0; pr_id < probe_points.size(); pr_id++) {

        double      D = 2.0 * search_radius; // Distance to nearest point
        size_t      i = 0;                   // Index of nearest point
        CGLA::Vec3d p;                       // Nearest surface point

        kdtree.closest_point(probe_points[pr_id], D, p, i);

        if (D > search_radius) continue;

        const size_t indicator =
            1 + frame->lookup_frame_index(probe_points[pr_id],
                                          surfaces[s_id].pcl.normals(i));

        // If the indicator return 0
        if (indicator == 0) continue;

        indicator_response.push_back(s_id);      // Surface ID
        indicator_response.push_back(pr_id);     // Probe ID
        indicator_response.push_back(indicator); // Indicator
    }
}

// Probe point extraction.
void StreamSurfaces::compute_indicator() {
    // This function is used to extract the voxels where a probe point is close
    // to the surfaces.

    // Debug: We are starting the computation
    Utilities::debug_info(__FILE__, __LINE__, "Computing Adjacency", debug);

    // Loop options
    int id_0 = 0, stride = 1;
#if MPI_VERSION > 0
    // Setup MPI options
    int mpi_rank, mpi_size;
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);

    id_0   = mpi_rank;
    stride = mpi_size;
#else
    #ifdef _OPENMP
        // OpenMP settings.
        #pragma omp parallel for schedule(dynamic)
    #endif /* _OPENMP */
#endif     /* MPI_VERSION */
    for (size_t id = id_0; id < surfaces.size(); id += stride) {
        if (surfaces[id].pcl.size() < 2) continue;
        indicator(surfaces[id], id);
    }

    // Debug: We have completed the computation
    Utilities::debug_info(__FILE__, __LINE__, "Adjacency Complete", debug);
}