#ifndef WCP_DYNAMICTOYPOINTCLOUD_H
#define WCP_DYNAMICTOYPOINTCLOUD_H

#include "WCPNanoflann/nanoflann.h"
#include "WCPData/WCPointCloud.h"
#include "WCPData/PR3DCluster.h"

#include "TVector3.h"

#include <map>

typedef nanoflann::KDTreeSingleIndexDynamicAdaptor<nanoflann::L2_Simple_Adaptor<double, WCP::WCPointCloud<double>>,
                                                   WCP::WCPointCloud<double>, 3 /* dim */
                                                   >
    my_dynamic_kd_tree_t;

typedef nanoflann::KDTreeSingleIndexDynamicAdaptor<nanoflann::L2_Simple_Adaptor<double, WCP::WC2DPointCloud<double>>,
                                                   WCP::WC2DPointCloud<double>, 2 /* dim */
                                                   >
    my_dynamic_kd_tree_2d_t;

namespace WCP {
    class DynamicToyPointCloud {
       public:
        DynamicToyPointCloud(double angle_u = 1.0472, double angle_v = -1.0472, double angle_w = 0);
        ~DynamicToyPointCloud();

        void AddPoints(PR3DCluster* cluster, int flag = 0,
                       double step = 0.6 * units::cm);                    // flag 1 points, flag 2 scheleton
        void AddPoints(PointVector& pts, double step = 0.6 * units::cm);  // directly add points ...
        void AddPoints(ToyPointCloud* point_cloud, double step = 0.6 * units::cm);

        void AddPoints(PR3DCluster* cluster, Point& p_test, TVector3& dir, double range, double step, double angle);

        std::tuple<double, PR3DCluster*, size_t> get_closest_point_info(WCP::Point& p);
        std::tuple<double, PR3DCluster*, size_t> get_closest_2d_point_info(WCP::Point& p, int plane);
        std::vector<std::tuple<double, PR3DCluster*, size_t>> get_2d_points_info(WCP::Point& p, double radius,
                                                                                 int plane);

        PR3DCluster* get_cluster(int index);

        std::vector<std::pair<WCP::SlimMergeGeomCell*, Point>> get_closest_points(WCP::Point& p, double radius);

        std::pair<double, double> HoughTrans(Point& p, double dis);
        TVector3 VHoughTrans(Point& p, double dis);

        WCP::WCPointCloud<double>& get_cloud() { return cloud; };
        int get_num_points();

       protected:
        std::vector<std::pair<size_t, double>> get_closest_index(WCP::Point& p, int N);
        std::vector<std::pair<size_t, double>> get_closest_index(WCP::Point& p, double radius);

        std::vector<std::pair<size_t, double>> get_closest_2d_index(double x, double y, int N, int plane);
        std::vector<std::pair<size_t, double>> get_closest_2d_index(double x, double y, double radius, int plane);

        double angle_u, angle_v, angle_w;
        WCP::WCPointCloud<double> cloud;
        my_dynamic_kd_tree_t* index;
        WCP::WC2DPointCloud<double> cloud_u;
        my_dynamic_kd_tree_2d_t* index_u;
        WCP::WC2DPointCloud<double> cloud_v;
        my_dynamic_kd_tree_2d_t* index_v;
        WCP::WC2DPointCloud<double> cloud_w;
        my_dynamic_kd_tree_2d_t* index_w;

        std::vector<PR3DCluster*> vec_index_cluster;
    };
}  // namespace WCP

#endif
