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

// Local libraries
#include "Frames/FrameField.hpp"
#include "PDS/PDS.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>

// ========================================================================== //
// Remove insignificant surfaces

// Mini surf removal
void StreamSurfaces::clean_surfaces() {

    // Compute statistics
    double N_avg  = 0.0;
    double N_nonZ = 0.0;
    for (auto surf : surfaces) {
        N_avg += surf.pcl.size() < 2 ? 0.0 : surf.pcl.size();
        N_nonZ += surf.pcl.size() < 2 ? 0.0 : 1.0;
    }
    N_avg /= N_nonZ;

    // Remove surfaces with less than 1% of average amount of points.
    for (size_t i = 0; i < surfaces.size(); i++)
        if (surfaces[i].pcl.size() < 0.01 * N_avg)
            surfaces[i] = StreamSurface();
}

// ========================================================================== //
// Smoothing operations

// Setup a PDS and run smoothing
void StreamSurfaces::smooth_single(StreamSurface &surf, const double pds_r) {

    if (surf.pcl.size() < 1) return;

    PDS pds(pds_r, bbox);
    for (size_t idx = 0; idx < surf.pcl.size(); idx++)
        pds.add(idx, surf.pcl.points(idx));
    smooth_single(surf, pds);
}
// Smoothing operation for a single surface
void StreamSurfaces::smooth_single(StreamSurface &surf, PDS &pds) {

    if (surf.pcl.size() < 1) return;

    const double pds_r = pds.get_radius();

    // Re-estimate all points
    for (int smooth_iter = 0; smooth_iter < smooth_max_iter; smooth_iter++) {
        for (size_t idx = 0; idx < surf.pcl.size(); idx++) {
            CGLA::Vec3d p0 = surf.pcl.points(idx);
            CGLA::Vec3d N0 = surf.pcl.normals(idx);
            pds.remove(p0);

            // Compute new point
            CGLA::Vec3d p =
                estimate_position(p0, pds, surf, 2.0 * pds_r, pds_r);
            CGLA::Vec3d N = frame->interpolate_frame_nearest(p, N0);

            if (!point_is_valid(p, N, pds, surf, 5.0 * pds_r)) {
                pds.add(idx, p0);
                continue;
            }

            // Save the new point
            pds.add(idx, p);
            surf.pcl.points(idx)  = p;
            surf.pcl.normals(idx) = N;
            surf.pcl.colors(idx)  = frame->interpolate_layer_nearest(p, N);
        }
    }
}

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