/**
 * 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 2 (March 2025)
 */

#ifndef FMPCAlgo_H
#define FMPCAlgo_H

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

//#define DBUG

#include <iostream>
#include <set>
#include <map>
#include <utility>
#include <vector>
#include "ndisplay.h"
#include "FMPointCloud.h"
#include "FMPCField.h"

namespace FM
{

enum status { fix = 0, inNB = 1, ukn = 2 };

enum methodComp {initiated, edge, marching, marchingEdge, undefined};

// structure to store computation for each node
struct infoComp {
    methodComp methodUsed = methodComp::undefined;
    double frontCurvature = 0.;
    std::vector<int> nodeLinked;
    double error = 0.;
};

struct resultComputation {
    double distance;
    npoint3 grad;
    npoint3 normal;
    infoComp infos;
    double error = 0.;
};

class FMPCAlgo
{
    protected:

    /// Point cloud data
    FMPointCloud* PC;

    /// Data fields computed
    FMPCField* data;

    /// Narrow band
    std::set<std::pair<double,int>> ptsInNB;

    /// Status of the node
    std::vector<status> node_status;

    /// computation informations
    std::vector<infoComp> node_info;

    /// Apply correction due to the curvature in computing distances
    bool curvCorrection;

    /// Enable printing of computations (log)
    bool printComputations = false;

    double distanceCurvatureCorrection(int idx, npoint3 vec, double dist);

    //double distanceCorrection(int idx1, int idx2, double xi, double dist);

    double distanceCurvatureCorrection2(int idx3, int idx1, npoint3 vec, double dist);

    virtual bool validSelection(int iRef, resultComputation& pCand){
        double distRef = data->getDistance(iRef);
        return distRef > pCand.distance;
    };

    virtual bool updatable(int idx1, int idx3) { return true; }

    public :

    /* CONSTRUCTORS / SET FUNCTIONS */

    FMPCAlgo(FMPointCloud* PC_, FMPCField* data_, bool correctCurv) {
        // Get point cloud
        PC = PC_;
        node_status.assign(PC_->getNbrNodes(),status::ukn);

        // Get data field
        data = data_;

        // Curvature correction enable
        curvCorrection = correctCurv;

        // comuptation data stored
        node_info.resize(PC_->getNbrNodes());
    }

    void preFixData(FMPCField* data_, int orderCalled) {
        std::vector<int> setIdxs = data_->getSetNodeIndexes();
        int orderData = data_->getOrder();
        if(orderData < orderCalled && setIdxs.size() > 0){
            std::cerr << "Import data field failed. Orders did not matched !" << std::endl;
        } else {
            for(int idx : setIdxs) node_status[idx] = status::fix;
        }
    }

    void printLog(bool enable=true)
    { printComputations = enable;  std::cout << setprecision(numeric_limits<double>::max_digits10);}

    /* INITIALISATION FUNCTIONS */

    /// @brief Initiate for case source point. Set all neighbours as the point source is a node.
    /// @param pt0 Source point
    void initiateFM(npoint3 pt0);

    /// @brief Initiate source point. It sets the minimal amount of node.
    /// @param pt0 
    void initiateFMsmall(npoint3 pt0);

    /// @brief Initiate nodes from a plane (planar wavefront)
    /// @param pt0 Point on the plane on the surface
    /// @param norm Normal to the plane
    /// @param wholePC until edge (false, by default) or whole PC (true)
    void initiateFM(npoint3 pt0, npoint3 norm, bool wholePC=false){
        if(wholePC) initiateFMplane2(pt0, norm);
        else initiateFMplane1(pt0,norm);
    }

    /// @brief Initiate the nodes from a plane (planar wavefront) limited by neighbours
    /// @param pt0 Point on the plane
    /// @param norm Normal to the plane
    void initiateFMplane1(npoint3 pt0, npoint3 norm);

    /// @brief Initiate the nodes from a plane (planar wavefront) on whole PC
    /// @param pt0 Point on the plane
    /// @param norm Normal to the plane
    void initiateFMplane2(npoint3 pt0, npoint3 norm);

    // initiate point on a curve as 0
    // FOR DEVELOPMENT. OK FOR FM0 or 1, NEVER 2 !
    void initiateCurve(geoFunction linFunc, npoint3 norm);

    void initiateCurve(geoFunction linFunc, npoint3 norm(double,double,double));

    /// @brief Initiate from a node
    /// @param pt0 Point of the node
    void initiateFMNode(npoint3 pt0){
        if(printComputations) std::cout << "== Initialisation (initiateFMNode from coord) ==" << std::endl;
        int idx0;
        double dist;
        PC->findNode(pt0[0],pt0[1],pt0[2],&idx0,&dist);
        if(printComputations){
            std::cout << "closest Node found : " << idx0 << " " << PC->printNode(idx0)
                << " / error distance : " << dist << std::endl;
        }

        initiateFMNode(idx0);
    }

    /// @brief Initiate from a node
    /// @param idx Index of node that is the source
    void initiateFMNode(int idx);

    void initiateFMEdge(npoint3 pt1, npoint3 pt2);

    void initNB(std::vector<int>& idxs);

    void initNB(std::map<int,double>& pts);

    inline void setValues(std::map<int,double>& pts);

    virtual void setGradients(std::vector<int>& idxs, std::vector<npoint3>& grads) {}

    virtual void setGradients(std::map<int,double>& pts, npoint3& grad) {}

    virtual void setGradients(std::map<int,npoint3>& pts) {}

    virtual void setNormals(std::vector<int>& idxs, npoint3 pt0) {}

    virtual void setNormals(std::vector<int>& idxs) {}

    virtual void setNormals(std::map<int,double>& pts, npoint3& normVec) {}

    /* PROPAGATION FUNCTIONS */

    /// @brief Propagate the wave on the surface
    /// @warning An initialization must be performed before
    void propagateFM();

    /// @brief Update a node of narrowband or add if new
    /// @param idx Index of the node to update
    /// @param idx1 Index of the node that has just been set
    void addPtInNB(int idx, int idx1);

    virtual void setInfoPtNB(int& idx, resultComputation& infos) = 0;

    virtual resultComputation computeFM(int idx, int idx1) = 0;

    /* INTERPOLATION FUNCTION */

    /// @brief Interpolate point from computed data according to algorithm used
    /// @param pt Point whose distance msut be computed
    /// @return Distance interpolated at point pt
    virtual double interpolate(npoint3& pt) = 0;

    /* NODE INFO COMPUTATION */

    void setNodeInfo(int idx, methodComp mthd, double WFCurv, std::vector<int> ndLinked){ node_info[idx] = infoComp{mthd,WFCurv,ndLinked}; }

    void setNodeInfo(int idx, infoComp& infos) { node_info[idx] = infoComp(infos); }

    infoComp* getNodeInfo(int idx) { return &node_info[idx]; }

    /* PLOT FLTK */

    void drawNodeMethod(data_container& dataC) const;

    void drawPathes(data_container& dataC) const;

    void drawElementsUsed(int idx, data_container& dataC) const;

    void drawElementsUsed2(int idx, data_container& dataC) const;

    void drawGeodesic(int idx, data_container& dataC) const;

    void getNodeGeodesic(int idx, std::vector<int>& ptIdx) const;

    void getNodeInvolved(int idx, std::vector<int>& ptIdx) const;

};

class FMPCAlgo12 : public FMPCAlgo
{
    protected :

    // virtual double computeDataMain(int& idx1, int& idx2, int& idx3, npoint3& grad, npoint3*& norm) = 0;

    virtual npoint3 computeDataNormal(int& idx1, int& idx2, int& idx3) { return npoint3(); }

    virtual npoint3 getNormal2pts(int& idx1, int& idx2) { return npoint3(); }

    virtual void filterPointsForFM(int& idx3,int& idx1,std::vector<int>& sndPoints);

    public :

    FMPCAlgo12(FMPointCloud* PC_, FMPCField* data_, bool correctCurv = false) : FMPCAlgo(PC_,data_,correctCurv) {};

    /* INITIALISATION FUNCTIONS */

    void setGradients(std::vector<int>& idxs, std::vector<npoint3>& grads) override
    { for(int i=0; i<idxs.size(); i++) data->setGradient(idxs[i],grads[i]); }

    void setGradients(std::map<int,double>& pts, npoint3& grad) override;

    void setGradients(std::map<int,npoint3>& pts) override;

    /* PROPAGATION FUNCTIONS */

    void setInfoPtNB(int& idx, resultComputation& infos) override
    { data->setDistance(idx,infos.distance); data->setGradient(idx,infos.grad);}

    // virtual void categorizeTriangle(int& idx3, int& idx1, std::vector<int>& idxs, std::vector<int>& inside, std::vector<int>& outside) = 0;

    virtual void categorizeTriangle(int& idx3, int& idx1, std::vector<int>& idxs, std::vector<std::pair<double,int>>& candidates) = 0;

    resultComputation computeFM(int idx3, int idx1) override;

    virtual resultComputation computeTriangles(int& idx1, int& idx3, std::vector<int>& sndPoints) = 0;

    virtual bool isFMpossible(int& idx1, int& idx2, int& idx3) = 0;

    /* INTERPOLATION FUNCTION */
   
    /// @brief Function interpoling point from data using FM0 computation
    /// @param pt Point where interpolate distance
    /// @return Distance interpolated
    double interpolate(npoint3& pt) override {
        npoint3 grad;
        return interpolate(pt,grad);
    }

    double interpolate(npoint3& pt, npoint3& grad);

    virtual double computeInterpolation(int& idx1, int& idx2, npoint3& pt, npoint3& grad) = 0;


};

}

#endif // FMPCAlgo_H