// Copyright 2004-present Facebook. All Rights Reserved.

#include <vector>

// NB: This differs from the GitHub version due to the different
// location of the nanoflann header when installing from source
#include <pangolin/geometry/geometry.h>
#include <pangolin/pangolin.h>

#include <Eigen/Core>
#include <nanoflann.hpp>

class GeometryNormalizationParameters {
   public:
    Eigen::Vector3f center;
    float max_distance;
};

struct KdVertexList {
   public:
    KdVertexList(const std::vector<Eigen::Vector3f>& points)
        : points_(points) {}

    inline size_t kdtree_get_point_count() const { return points_.size(); }

    inline float kdtree_distance(const float* p1, const size_t idx_p2,
                                 size_t /*size*/) const {
        Eigen::Map<const Eigen::Vector3f> p(p1);
        return (p - points_[idx_p2]).squaredNorm();
    }

    inline float kdtree_get_pt(const size_t idx, int dim) const {
        return points_[idx](dim);
    }

    template <class BBOX>
    bool kdtree_get_bbox(BBOX& /*bb*/) const {
        return false;
    }

   private:
    std::vector<Eigen::Vector3f> points_;
};

using KdVertexListTree = nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<float, KdVertexList>, KdVertexList, 3, int>;

std::vector<Eigen::Vector3f> sample_points_on_unit_sphere(
    const uint num_samples, const float radius);

void find_invisible_faces(const pangolin::Image<Eigen::Vector4f>& image_normals,
                          std::vector<bool>& backface_test);
void find_observed_backfaces(
    const pangolin::Image<Eigen::Vector4f>& image_normals,
    std::vector<Eigen::Vector4f>& face_normal_test,
    std::vector<bool>& observed_backface_test,
    std::vector<int>& face_observation_count);

void get_valid_surface_points_and_normals_from_image(
    const int view_index,
    const pangolin::Image<Eigen::Vector4f>& image_vertices,
    const pangolin::Image<Eigen::Vector4f>& image_normals,
    std::vector<Eigen::Vector4f>& surface_points,
    std::vector<Eigen::Vector4f>& surface_normals,
    std::vector<int>& surface_points_view_index);

float compute_triangle_area(const Eigen::Vector3f& a, const Eigen::Vector3f& b,
                            const Eigen::Vector3f& c);

Eigen::Vector3f sample_point_from_triangle(const Eigen::Vector3f& a,
                                           const Eigen::Vector3f& b,
                                           const Eigen::Vector3f& c);

GeometryNormalizationParameters normalize_geometry(
    pangolin::Geometry& geometry, const bool fit_to_unit_sphere,
    const float buffer = 1.03);

std::vector<Eigen::Vector3i> get_geometry_faces(pangolin::Geometry& geometry);
std::vector<float> compute_face_area(pangolin::Geometry& geometry,
                                     std::vector<bool>& ignore_face);

void sample_from_surface(pangolin::Geometry& geometry,
                         std::vector<bool>& ignore_face,
                         std::vector<Eigen::Vector3f>& face_normals,
                         std::vector<Eigen::Vector3f>& sampled_normals,
                         std::vector<Eigen::Vector3f>& sampled_points,
                         int num_sample);
