#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/iostream.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/range_image/range_image.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/features/range_image_border_extractor.h>
#include <memory>


#define STRINGIFY(x) #x
#define MACRO_STRINGIFY(x) STRINGIFY(x)

class CloudFuncs
{
private:
    pcl::PointXYZ* pt;
    pcl::PointCloud<pcl::PointXYZ>* cloud;
public:
    CloudFuncs(float a, float b, float c, int d);

    float add_all_pts();

    bool add_array(std::vector<float> nums);

    int loadXYZCloud(std::vector<std::vector<float>> xyz);
};

CloudFuncs::CloudFuncs(float a, float b, float c, int d)
{
    cloud = new pcl::PointCloud<pcl::PointXYZ>();
    for (int i = 0; i < d; ++i)
        cloud->push_back({a, b, c});
}

float CloudFuncs::add_all_pts()
{
    float total{0.f};
    for (const auto& pt: *cloud)
        total += (pt.x + pt.y + pt.z);
    return total;
}

bool CloudFuncs::add_array(std::vector<float> nums)
{
    if (nums.size() == 0) return false;
    for (const auto& num: nums)
        if (num != 2.0) return false;
    return true;
}


int CloudFuncs::loadXYZCloud(std::vector<std::vector<float>> xyz)
{
    if (xyz.size() < 3) return 1;
    size_t length{xyz[0].size()};
    if (length == 0 || length != xyz[1].size() || length != xyz[2].size()) return 1;
    cloud->erase(cloud->begin(), cloud->end());
    for (size_t i = 0; i < length; ++i)
        cloud->push_back({xyz[0][i], xyz[1][i], xyz[2][i]});

    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity());

    float angular_res{0.5f};
    float support_size{0.2f};
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
    bool setUnseenToMaxRange{false};
    float noise_level{0.0f};
    float min_range{0.0f};
    int border_size{1};
    pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
    const pcl::RangeImage* range_image_raw(range_image_ptr.get());
    pcl::RangeImage& range_image = *range_image_ptr;
    range_image.createFromPointCloud(*cloud, angular_res,
                                     pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),
                                     scene_sensor_pose, coordinate_frame,
                                     noise_level, min_range, border_size);
    range_image.integrateFarRanges(far_ranges);
    if (setUnseenToMaxRange)
        range_image.setUnseenToMaxRange();

    pcl::RangeImageBorderExtractor range_image_border_extractor(range_image_raw);
    pcl::NarfKeypoint narf_Keypoint_detector(&range_image_border_extractor);
    narf_Keypoint_detector.setRangeImage(&range_image);
    narf_Keypoint_detector.getParameters().support_size = support_size;
//
    pcl::PointCloud<int> keypoint_indices;
    narf_Keypoint_detector.compute(keypoint_indices);

    return keypoint_indices.size();
}



int add(int i, int j) {
    pcl::PointCloud<pcl::PointXYZ> cloud;
    cloud.push_back({float(i), float(j), float(j)});
    pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
    return cloud[0].x + cloud[0].y + cloud[0].z;
}

namespace py = pybind11;

//py::call_guard<py::scoped_ostream_redirect, py::scoped_estream_redirect>()

PYBIND11_MODULE(tiledbpypcl, m) {
    m.doc() = R"pbdoc(
        TileDB: Pybind11 with PCL
        -----------------------

        .. currentmodule:: cmake_example

        .. autosummary::
           :toctree: _generate

           Cloud Funcs
           add
           subtract
    )pbdoc";

    m.def("add", &add, R"pbdoc(
        Add two numbers

        Some other explanation about the add function.
    )pbdoc");

    m.def("subtract", [](int i, int j) { return i - j; }, R"pbdoc(
        Subtract two numbers

        Some other explanation about the subtract function.
    )pbdoc");

    py::class_<CloudFuncs>(m, "CloudFuncs")
            .def(py::init<float, float, float, int>())
            .def("add_all_pts", &CloudFuncs::add_all_pts)
            .def("add_array", &CloudFuncs::add_array)
            .def("loadXYZCloud", &CloudFuncs::loadXYZCloud);

#ifdef VERSION_INFO
    m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);
#else
    m.attr("__version__") = "dev";
#endif
}
