/**
 * C++ fast marching on point cloud Library
 * Copyright (c) 2000-2026 Eric Bechet <eric at bechet dot ca>
 * 
 * This file is part of the C++ Mesh Generation Library.
 *  See the NOTICE & LICENSE files for conditions.
 * 
 * Version 0.2 (July 2024)
 */

#ifndef FMPOINTCLOUD_H
#define FMPOINTCLOUD_H

#include <set>
#include <map>
#include <vector>
#include <string>
#include <limits>
#include <queue>
#include <functional>
#include <Eigen/Dense>
#include "nutil.h"
#include "ANN.h"
#include "GmshGlobal.h"
#include "gmshElement.h"


#include <random>

// #include <pcl/point_types.h>
// #include <pcl/features/normal_3d.h>
// #include <pcl/features/principal_curvatures.h>
// #define USE_PCL

// numerical error allowed (considered as 0)
#ifndef EPS_NUM
#define EPS_NUM 1e-11
#endif

namespace FM
{
    typedef std::function<bool(double,double,double)> geoFunction;

    geoFunction make_circle(npoint3 center, npoint3 normal, double radius, double epsilon);

    geoFunction make_sphere(npoint3 center, double radius, double epsilon);

    geoFunction make_line(npoint3 pt0, npoint3 pt1, double epsilon);

    struct edge {
        ANNpoint p0;
        ANNpoint p1;
        npoint3 vec; // vector pointing inside the surface
        npoint3 edgeVec;
        double length;

        edge(ANNpoint p0_, ANNpoint p1_, npoint3 vec_){
            p0 = p0_;
            p1 = p1_;
            vec = vec_;
            npoint3 vecE(p1[0]-p0[0],p1[1]-p0[1],p1[2]-p0[2]);
            length = vecE.normalize();
            edgeVec = vecE;
        }

        bool checkNotInside(ANNpoint pt){
            double prod = 0;
            for(int i=0; i<3; i++) prod += (pt[i]-p0[i])*vec[i];
            return prod < -1e-10;
        }

    };

    struct vertex {
        edge e0;
        edge e1;

        // vertex(edge e0_, edge e1_){
        //     e0 = e0_; 
        //     e1 =e1_;
        // }

        bool checkNotInside(ANNpoint pt){
            // pour cas concave (mais général si grande distance entre bord)
            std::cout << "check not inside vertex : ";
            std::cout << e0.checkNotInside(pt) << " || " << e1.checkNotInside(pt) << " = " << (e0.checkNotInside(pt) || e1.checkNotInside(pt)) << std::endl;

            return e0.checkNotInside(pt) || e1.checkNotInside(pt);
        }

    };

    enum topology {
        isNone = 0, onEdge = 1, isVertex = 2
    };

    struct curvatures {
        double n[3];
        double pcv[3];
        double pc1;
        double pc2;
    };



    /// FMPointCloud countains the point cloud and its geometrical data
    class FMPointCloud
    {
        protected:
            /* CLASS DATA */

            /// @brief Nodes of the point clouds
            ANNpointArray nodes; // = table of ANNpoint (typedef ANNpoint*)

            /// @brief Number of nodes inside ANNpointArray nodes
            int nbr_nodes;

            /// @brief Number of neighbour per node to build graph
            int nbr_neighbour;

            /// @brief IDs of the neighbours nodes
            ///(neighbourhood[IDPC] gives vector of the ID of the neighbours of IDPC)
            std::vector<std::vector<int> > neighbourhood;

            /// @brief Curvatures and normals of nodes
            std::vector<FM::curvatures> curvs;

            std::vector<FM::vertex> vertices;

            std::vector<FM::edge> edges;

            std::vector<std::pair<FM::topology,void*> > topol;

            std::vector<double> keyRadii;

            /// @brief KD-tree
            ANNkd_tree* kdTree;

            /// @brief Mesh which the PC is extracted from (if it exists)
            FM::FMModel *model;

            /* PROTECTED FUNCTIONS */

            /// @brief Create new node inside point cloud
            /// @param x X coord.
            /// @param y Y coord.
            /// @param z Z coord.
            void addNode(double x, double y, double z);

            /// @brief Set all the nodes at once.
            /// @param pts Vector of all the node to add
            void setNodes(std::vector<std::vector<double>> &pts);

            void deleteLink(int idx1, int idx2);

            // void computeRadiusFirstRing();

            void computeKeyRadius();

        public:

            /* CONSTRUCTORS */

            /// Default constructor
            FMPointCloud() {nbr_nodes=0; kdTree=nullptr; nodes=nullptr; model=nullptr;}

            /// @brief Full constructor (from a file). Graph builder included.
            /// @param fname Relative path to file (extension: csv, txt, stl or msh)
            /// @param maxnb Maximum number of neighbour per node
            FMPointCloud(std::string fname, int maxnb=12);

            /* DESTRUCTORS */

            /// Clean destructor
            ~FMPointCloud() { annDeallocPts(nodes); neighbourhood.clear(); if(kdTree) delete kdTree; if(model) delete model; annClose();}

            /* GET/SET FUNCTIONS */

            /// @return Number of nodes
            int getNbrNodes() const {return nbr_nodes;}

            /// @brief Get a node as a point
            /// @param i Index of the node
            /// @param pt npoint3 of the point
            void getNode(int i, npoint3 &pt) const;

            /// Copy the pointer to the msh model
            void getModel(FM::FMModel*& model_) const {model_= (model) ? model : nullptr;}

            /// @brief Get normal at a node
            /// @param idx Index of the node
            /// @param norm Normal returned
            void getNormal(int idx, npoint3& norm) const { norm = npoint3(curvs[idx].n); }

            /// @brief Get main curvature direction
            /// @param idx Index of the node
            /// @param vecCurv Vector returned
            void getMainCurvature(int idx, npoint3& vecCurv) const { vecCurv = npoint3(curvs[idx].pcv);}

            void getBarycentricCoord(npoint3 pt, int i, int j, int k, double* u, double* v) const;

            npoint3 getNormalTriangle(int i, int j, int k);

            npoint3 getNormalTriangle(int i, int j);

            npoint3 getNormalPatch(std::vector<npoint3> pts);

            int getNbrNeighbour() const { return nbr_neighbour; };

            /// @brief Test if point can be projected on the patch
            /// @param idxNodes Vector of index of the node delimiting the patch
            /// @param pt0 Point to test
            /// @param npt0 Projection on patch
            /// @return If projection onto patch is done or not ("if pt0 is inside the patch")
            bool testProjectionPatch(std::vector<int>& idxNodes, npoint3& pt0, npoint3& npt0) const;

            bool isAcute(npoint3& pt1, npoint3& pt2, npoint3& pt3);

            /* READ FILE FUNCTIONS */

            /// @brief Get nodes from CSV file
            /// @param fname Relative path to CSV file
            void readFromCSV(std::string fname);

            /// @brief Get nodes from file with space separation
            /// @param fname Relative path to the file
            void readFromFile(std::string fname);

            /// @brief Get nodes from a STL file
            /// @param fname Relative path to the file
            void readFromSTL(std::string fname);

            /// @brief Get nodes from a mesh file (MSH)
            /// @param fname Relative path to the file
            void readFromMSH(std::string fname);

            void importGraphCSR(std::string fname);

            /* SET TOPOLOGY FUNCTIONS */

            void findNodesEdge(npoint3& pt1, npoint3& pt2, std::vector<int>& idxEdge);

            void findNodesEdge(npoint3& pt1, npoint3& pt2, double radius, std::vector<int>& idxEdge);

            void setTopology(std::vector<npoint3> pts, std::vector<npoint3> vects);

            void setEdge(int idx0, int idx1, npoint3 vec);

            void setVertex(int idx, FM::edge e0, FM::edge e1);


            /* SEARCH NODE FUNCTIONS (KD-TREE) */

            /// @brief Build the KD-tree required for searching
            void buildTree();

            /// @brief Fing k nearest neighbour of a point
            /// @param pt Query point
            /// @param closest Vector to be filled with closest points found
            /// @param k Number of points to look for
            /// @param keep Maximum number of points inside vector closest
            /// @param dist Distance maximum allowed for neighbours (r-search if positive)
            /// @return Mean distance between the points found and the query points
            double findKClosest(npoint3 pt,std::vector<std::pair<int,double> > &closest,int k,int keep, double dist);

            /// @brief Brute force search to get the closest point
            /// @param pt Query point
            /// @param closest Index of the closest point found
            /// @return Distance between the query point and the point found
            double findClosest(npoint3 pt, int &closest);

            /* BUILD GRAPH FUNCTIONS */

            /// @brief Build graph (node relations)
            /// @param maxnb Maximum number of neighbour per node
            void buildGraph(int maxnb) { 
                buildAdj(maxnb, true); symmetrizeAdj(true); nbr_neighbour=maxnb;
                computeKeyRadius();
            }

            /// @brief Find and fill neighbourhoods
            /// @param maxnb Maximum number of neighbour per node (can be less)
            /// @param chk Allow limitation of neighbourhood wrt. distance
            void buildAdj(int maxnb, bool chk=false);

            /// @brief Symmetrize the neighbourhoods
            void symmetrizeAdj(bool keep=true);

            /// @brief Increase the neighbourhoods with the closest neighbourhoods
            /// @param nblayers Number of neighbourhood to consider relatively
            void augmentAdj(int nblayers);

            /// @brief Remove adjacents whose connection is outside surface
            /// @warning setTopology required !
            void trimOutsideAdj();

            /// @brief quick coded function for the 3D corner case
            void quickCornerTopolCorrect(npoint3& nor1, npoint3& nor2);

            /* READ GRAPH FUNCTIONS */

            /// @brief Get neighbourhood of a node
            /// @param i Index of the node
            /// @param adj Vector to fill with indexes of the neighbourhood
            void getAdj(int i, std::vector<int> &adj) const { adj = std::vector<int>( neighbourhood[i] ); }

            /// @brief Find the node (or the closest) from coordinates
            /// @param x X coordinate
            /// @param y Y coordinate
            /// @param z Z coordinate
            /// @param idx idx of the node found 
            /// @param dist distance from the node found and the given coordinates
            /// @return true if the node returned correspond to the corrdinate (false means the node returned is the closest node found)
            bool findNode(double x, double y, double z, int* idx, double* dist);

            /// @brief Find nodes along a line
            /// @param pt1 1st point of line
            /// @param pt2 2nd point of line
            /// @param fDist distance of sampling along the line
            /// @param pts vector of indexes to fill
            void findNodesLine(npoint3 pt1, npoint3 pt2, double fDist, vector<int> &pts);

            /* GEOMETICAL FUNCTION FIND */

            void findNodesGeo(geoFunction fnc, vector<int> &pts);

            /* SET CURVATURES AND NORMALS */

            /// @brief Compute normals and curvatures using PCL
            /// @warning PCL library required
            void computeNormCurv();

            void computeApproximatedNormals(std::vector<npoint3>& normals);

            npoint3 computeApproximatedNormal(int& ptIdx);

            /// @brief Compute analytically normals and curvatures : plane case
            /// @param norm Normal to the plane
            /// @param tang A tangeant vector to the plane
            void applyNormCurvPlane(npoint3 norm, npoint3 tang);

            /// @brief Compute analytically normals and curvatures : cylinder case
            /// @param axis Axis of the cylinder
            /// @param ptOnAxis A point on the axis of the cylinder (for reference)
            /// @param radius Radius of the cylinder
            void applyNormCurvCylinder(npoint3 axis, npoint3 ptOnAxis, double radius);

            void applyNormCurvCylinder(npoint3 axis, npoint3 ptOnAxis, double radius, double noise);

            void applyNormCurvSphere(npoint3 center, npoint3 refAxis, double radius);

            /* GET CURVATURES AND NORMALS */

            /// @brief Interpolate the normal curvature
            /// @param idx Index of the node concerned
            /// @param vec Vector along the curvature must be computed
            /// @return Normal curvature (Kappa)
            double interpolateCurv(int idx, npoint3 vec);

            /* DISPLAY FUNCTIONS (FLTK) */

            void displayNodes(data_container &data) const;
            
            void displayNodesKeyRadii(data_container &data) const;

            /// @brief Display nodes with their neighbourhoods
            /// @param data data container to fill
            /// @param rep sampling periodicity for drawing (nodes not concerned)
            void displayTree(data_container &data, int rep=1) const;

            void displayNodeRelations(data_container &data, int idx) const;

            void displayEdgeNodes(data_container &data) const;

            /// @brief Display the curvatures and normals
            /// @param data data container to fill
            /// @param addNodes true if nodes should be added too
            /// @param rep sampling periodicity for drawing (node not concerned)
            void displayCurvatures(data_container &data, bool addNodes=true, int rep=0) const;

            std::string printNode(int idx) const;

            std::string printNode(npoint3& pt) const;

            /* EXPORT FUNCTIONS */

            void exportGraphCSR(std::string fileName) const;

            void exportNodesCSV(std::string fileName) const;

            void exportGraph(std::string filePath) const {exportNodesCSV(filePath + "_nodes"); exportGraphCSR(filePath + "_graph");}

            double getKeyRadius(int idx) { return keyRadii[idx]; }

    };
}

#endif //FMPOINTCLOUD_H