#ifndef WCP_TOYPOINTCLOUD_H
#define WCP_TOYPOINTCLOUD_H

#include "WCPNanoflann/nanoflann.h"
#include "WCPData/WCPointCloud.h"
#include "WCPQuickhull/QuickHull.h"
#include "WCPQuickhull/MathUtils.h"

#include "TVector3.h"

#include <map>

typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, WCP::WCPointCloud<double>>,
                                            WCP::WCPointCloud<double>, 3 /* dim */
                                            >
    my_kd_tree_t;

typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, WCP::WC2DPointCloud<double>>,
                                            WCP::WC2DPointCloud<double>, 2 /* dim */
                                            >
    my_kd_tree_2d_t;

namespace WCP {

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

        // build point cloud
        void AddPoint(WCPointCloud<double>::WCPoint& wcp, WC2DPointCloud<double>::WC2DPoint& wcp_u,
                      WC2DPointCloud<double>::WC2DPoint& wcp_v, WC2DPointCloud<double>::WC2DPoint& wcp_w);
        void AddPoint(WCP::Point& p, std::tuple<int, int, int>& wires_index, WCP::SlimMergeGeomCell* mcell);
        void AddPoint(WCPointCloud<double>::WCPoint& wcp);

        void AddPoints(WCP::PointVector& ps, std::vector<std::tuple<int, int, int>>& wires_indices,
                       WCP::SlimMergeGeomCell* mcell);
        void AddPoints(WCP::PointVector& ps);
        void build_kdtree_index();

        // find 3D oiubts
        std::tuple<int, int, double> get_closest_points(ToyPointCloud* point_could);
        std::pair<int, double> get_closest_point_along_vec(Point& p_test, TVector3& dir, double test_dis,
                                                           double dis_step, double angle_cut, double dis_cut);

        // hull, not useful ...
        std::vector<WCPointCloud<double>::WCPoint> get_hull();

        // function to find 3D close points
        std::map<WCP::SlimMergeGeomCell*, WCP::Point> get_closest_mcell(WCP::Point& p, int N);
        std::map<WCP::SlimMergeGeomCell*, WCP::Point> get_closest_mcell(WCP::Point& p, double radius);
        std::vector<std::pair<WCP::SlimMergeGeomCell*, Point>> get_closest_points(WCP::Point& p, int N);
        std::vector<std::pair<WCP::SlimMergeGeomCell*, Point>> get_closest_points(WCP::Point& p, double radius);

        double get_closest_dis(WCP::Point& p);
        double get_closest_2d_dis(double x, double y, int plane);
        std::pair<double, int> get_closest_2d_dis_index(double x, double y, int plane);

        std::pair<double, WCP::Point> get_closest_point(WCP::Point& p);

        std::vector<WCP::WCPointCloud<double>::WCPoint> get_closest_wcpoints(WCP::Point& p, int N);
        std::vector<WCP::WCPointCloud<double>::WCPoint> get_closest_wcpoints(WCP::Point& p, double radius);

        WCP::Point get_center_point_radius(WCP::Point& p, double radius);

        WCP::WCPointCloud<double>::WCPoint& get_closest_wcpoint(WCP::WCPointCloud<double>::WCPoint& wcp);
        WCP::WCPointCloud<double>::WCPoint& get_closest_wcpoint(WCP::Point& p);

        // function to find 2D points ...
        std::vector<size_t> get_closest_2d_index(WCP::Point& p, int N, int plane);
        std::vector<size_t> get_closest_2d_index(WCP::Point& p, double radius, int plane);

        std::pair<int, double> get_closest_2d_dis(WCP::Point& p, int plane);

        int get_num_points() { return cloud.pts.size(); };
        WCP::WCPointCloud<double>& get_cloud() { return cloud; };
        WCP::WC2DPointCloud<double>& get_cloud_u() { return cloud_u; };
        WCP::WC2DPointCloud<double>& get_cloud_v() { return cloud_v; };
        WCP::WC2DPointCloud<double>& get_cloud_w() { return cloud_w; };

        std::vector<int>& get_mcell_indices(WCP::SlimMergeGeomCell* mcell) { return map_mcell_indices[mcell]; };

        void Print();

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

       protected:
        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_kd_tree_t* index;
        WCP::WC2DPointCloud<double> cloud_u;
        my_kd_tree_2d_t* index_u;
        WCP::WC2DPointCloud<double> cloud_v;
        my_kd_tree_2d_t* index_v;
        WCP::WC2DPointCloud<double> cloud_w;
        my_kd_tree_2d_t* index_w;

        // map of mcells to WCPoints
        std::map<WCP::SlimMergeGeomCell*, std::vector<int>> map_mcell_indices;
    };

    typedef std::vector<ToyPointCloud*> ToyPointCloudSelection;

}  // namespace WCP

#endif
