#include "onf_stepcomputeroadprobabilityrasters.h"

#include "ct_itemdrawable/ct_image2d_points.h"
#include "ct_math/ct_mathpoint.h"

#include "ct_accessor/ct_pointaccessor.h"


#define EPSILON 0.000001


ONF_StepComputeRoadProbabilityRasters::ONF_StepComputeRoadProbabilityRasters() : SuperClass()
{
    _gndClass = 2;

    _slopeMin = 5.0;
    _slopeMax = 20.0;
    _roughMin = 0.05;
    _roughMax = 0.10;
    _roughPtsMaxHeight = 0.2;
    _roughPtsRadius = 2.0;
    _roughMinPoints = 0.10;
    _roughMaxPoints = 0.10;
    _sobelThMin = 30.0;
    _sobelThMax = 50.0;
    _chmMin = 0.1;
    _chmMax = 0.2;
    _lowVegDensTh = 1.0;
    _lowVegHmin = 0.5;
    _lowVegHmax = 3.0;
    _gndDensityMinPoints = 0.0;
    _gndDensityMaxPoints = 0.4;


    _slopeAdd = -1;
    _roughnessAdd = 1;
    _sobelAdd = -1;
    _chmAdd = 1;
    _intensityAdd = 1;
    _gndDensAdd = 2;
    _lowVegAdd = -1;
}

QString ONF_StepComputeRoadProbabilityRasters::description() const
{
    return tr("Créer des rasters de probabilité de route");
}

QString ONF_StepComputeRoadProbabilityRasters::detailledDescription() const
{
    return tr("Calcul d'un raster de pente (en degrés)"
              "Formula from https://pro.arcgis.com/fr/pro-app/tool-reference/3d-analyst/how-slope-works.htm");
}

QString ONF_StepComputeRoadProbabilityRasters::inputDescription() const
{
    return SuperClass::inputDescription() + tr("<br><br>");
}

QString ONF_StepComputeRoadProbabilityRasters::outputDescription() const
{
    return SuperClass::outputDescription() + tr("<br><br>");
}

QString ONF_StepComputeRoadProbabilityRasters::detailsDescription() const
{
    return tr("");
}

QString ONF_StepComputeRoadProbabilityRasters::URL() const
{
    //return tr("STEP URL HERE");
    return SuperClass::URL(); //by default URL of the plugin
}

CT_VirtualAbstractStep* ONF_StepComputeRoadProbabilityRasters::createNewInstance() const
{
    return new ONF_StepComputeRoadProbabilityRasters();
}

//////////////////// PROTECTED METHODS //////////////////

void ONF_StepComputeRoadProbabilityRasters::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("MNT"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inDTM, tr("MNT"));
    manager.addItem(_inGroup, _inScene, tr("Scène"));
    manager.addItem(_inGroup, _inLASAttributes, tr("Attributs LAS"));
    manager.addItem(_inGroup, _inArea, tr("Emprise"));
}

void ONF_StepComputeRoadProbabilityRasters::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addInt(tr("Classe pour les points sol"), "", 0, 255, _gndClass);

    postInputConfigDialog->addDouble(tr("Pente, seuil minimum"), "°", 0, 90, 2, _slopeMin);
    postInputConfigDialog->addDouble(tr("Pente, seuil maximum"), "°", 0, 90, 2, _slopeMax);
    postInputConfigDialog->addDouble(tr("Rugosité Points, hauteur max"), "m", 0, 99, 2, _roughPtsMaxHeight);
    postInputConfigDialog->addDouble(tr("Rugosité Points, rayon"), "m", 0, 99, 2, _roughPtsRadius);
    postInputConfigDialog->addDouble(tr("Rugosité Points, seuil minimum"), "", 0, 99, 2, _roughMinPoints);
    postInputConfigDialog->addDouble(tr("Rugosité Points, seuil maximum"), "", 0, 99, 2, _roughMaxPoints);
    postInputConfigDialog->addDouble(tr("Vivacité arête, seuil minimum"), "", 0, 500, 2, _sobelThMin);
    postInputConfigDialog->addDouble(tr("Vivacité arête, seuil maximum"), "", 0, 500, 2, _sobelThMax);
    postInputConfigDialog->addDouble(tr("Hauteur CHM, seuil minimum"), "", 0, 99, 2, _chmMin);
    postInputConfigDialog->addDouble(tr("Hauteur CHM, seuil maximum"), "", 0, 99, 2, _chmMax);
    postInputConfigDialog->addDouble(tr("Densité pts sol, seuil minimum"), "", 0, 99, 2, _gndDensityMinPoints);
    postInputConfigDialog->addDouble(tr("Densité pts sol, seuil maximum"), "", 0, 99, 2, _gndDensityMaxPoints);
    postInputConfigDialog->addDouble(tr("Densité végétation, seuil"), "pts", 0, 999, 2, _lowVegDensTh);
    postInputConfigDialog->addDouble(tr("Densité végétation, Hmin"), "m", 0, 999, 2, _lowVegHmin);
    postInputConfigDialog->addDouble(tr("Densité végétation, Hmax"), "m", 0, 999, 2, _lowVegHmax);

    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addTitle(tr("Poids des composants additifs (-1 pour un composant multiplicatif) :"));
    postInputConfigDialog->addDouble(tr("Pente"), "", -1, 10, 2, _slopeAdd);
    postInputConfigDialog->addDouble(tr("Rugosité Points"), "", -1, 10, 2, _roughnessAdd);
    postInputConfigDialog->addDouble(tr("Arêtes vives"), "", -1, 10, 2, _sobelAdd);
    postInputConfigDialog->addDouble(tr("MNH"), "", -1, 10, 2, _chmAdd);
    postInputConfigDialog->addDouble(tr("Amplitude d'intensité"), "", -1, 10, 2, _intensityAdd);
    postInputConfigDialog->addDouble(tr("Densité de points sol"), "", -1, 10, 2, _gndDensAdd);
    postInputConfigDialog->addDouble(tr("Densité de végétation basse"), "", -1, 10, 2, _lowVegAdd);

}

void ONF_StepComputeRoadProbabilityRasters::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outSlopePourc, tr("Pente (Degrés)"));
    manager.addItem(_inGroup, _outRoughnessPoints, tr("Rugosité Points"));
    manager.addItem(_inGroup, _outSobel, tr("Arêtes vives"));
    manager.addItem(_inGroup, _outCHM, tr("MNH"));
    manager.addItem(_inGroup, _outIntensityRange, tr("Amplitude d'intensité"));
    manager.addItem(_inGroup, _outGndPtsDensity, tr("Densité de points sol"));
    manager.addItem(_inGroup, _outLowVegDensity, tr("Densité de végétation basse"));

    manager.addItem(_inGroup, _outSlopePourcConductivity, tr("Conductivité Pente (Degrés)"));
    manager.addItem(_inGroup, _outRoughnessPointsConductivity, tr("Conductivité Rugosité Points"));
    manager.addItem(_inGroup, _outSobelConductivity, tr("Conductivité Arêtes vives"));
    manager.addItem(_inGroup, _outCHMConductivity, tr("Conductivité MNH"));
    manager.addItem(_inGroup, _outIntensityRangeConductivity, tr("Conductivité Amplitude d'intensité"));
    manager.addItem(_inGroup, _outGndPtsDensityConductivity, tr("Conductivité Densité de points sol"));
    manager.addItem(_inGroup, _outLowVegDensityConductivity, tr("Conductivité Densité de végétation basse"));

    manager.addItem(_inGroup, _outConductivity, tr("Conductivité"));
}


void ONF_StepComputeRoadProbabilityRasters::compute()
{
    for (CT_StandardItemGroup* grp : _inGroup.iterateOutputs(_inResult))
    {
        const CT_Image2D<float>* dtm = grp->singularItem(_inDTM);
        const CT_AbstractItemDrawableWithPointCloud* scene = grp->singularItem(_inScene);
        const CT_StdLASPointsAttributesContainer* attributeLAS = grp->singularItem(_inLASAttributes);

        const CT_AbstractGeometricalItem *emprise = grp->singularItem(_inArea);


        if (isStopped()) {return;}

        if (dtm != nullptr && scene != nullptr && attributeLAS != nullptr && scene->pointCloudIndex() != nullptr && emprise != nullptr && scene->pointCloudIndexSize() != 0)
        {

            Eigen::Vector3d min, max;
            emprise->boundingBox(min, max);

            double minX = min(0);
            double minY = min(1);
            double maxX = max(0) - EPSILON;
            double maxY = max(1) - EPSILON;

            CT_AbstractPointAttributesScalar* attributeIntensity     = dynamic_cast<CT_AbstractPointAttributesScalar*>(attributeLAS->pointsAttributesAt(CT_LasDefine::Intensity));
            CT_AbstractPointAttributesScalar* attributeLineOfFlight  = dynamic_cast<CT_AbstractPointAttributesScalar*>(attributeLAS->pointsAttributesAt(CT_LasDefine::Point_Source_ID));
            CT_AbstractPointAttributesScalar* attributeClassification  = dynamic_cast<CT_AbstractPointAttributesScalar*>(attributeLAS->pointsAttributesAt(CT_LasDefine::Classification));

            CT_Image2D<float>* slopeDeg = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* roughness = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* roughnessPoints = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* sobel = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* chm = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());

            CT_Image2D<double>* intensityMin = new CT_Image2D<double>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<double>* intensityMax = new CT_Image2D<double>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());

            CT_Image2D<float>* intensityRange = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* gndPtsDensity = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* gndPtsDensityTot = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* gndPtsDensityStd = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* gndPtsDensity2 = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* gndPtsDensityStd2 = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* lowVegDensity = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), 0);
            CT_Image2D<float>* deltaDTM = new CT_Image2D<float>(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());


            CT_Image2D<float>* slopeDegClipped = CT_Image2D<float>::createImage2DFromXYCoords(minX, minY, maxX, maxY, dtm->resolution(), dtm->level(), dtm->NA(), dtm->NA());
            CT_Image2D<float>* roughnessClippedPoints = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* sobelClipped = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* chmClipped = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* intensityRangeClipped = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* gndPtsDensityClipped = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* lowVegDensityClipped = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), 0);


            CT_Image2D<float>* slopeDegCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* roughnessPointsCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* sobelCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* chmCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* intensityRangeCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* gndPtsDensityCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());
            CT_Image2D<float>* lowVegDensityCond = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());

            CT_Image2D<float>* conductivity = new CT_Image2D<float>(slopeDegClipped->minX(), slopeDegClipped->minY(), slopeDegClipped->xdim(), slopeDegClipped->ydim(), slopeDegClipped->resolution(), slopeDegClipped->level(), slopeDegClipped->NA(), slopeDegClipped->NA());

            grp->addSingularItem(_outSlopePourc, slopeDegClipped);
            grp->addSingularItem(_outRoughnessPoints, roughnessClippedPoints);
            grp->addSingularItem(_outSobel, sobelClipped);
            grp->addSingularItem(_outCHM, chmClipped);
            grp->addSingularItem(_outIntensityRange, intensityRangeClipped);
            grp->addSingularItem(_outGndPtsDensity, gndPtsDensityClipped);
            grp->addSingularItem(_outLowVegDensity, lowVegDensityClipped);

            grp->addSingularItem(_outSlopePourcConductivity, slopeDegCond);
            grp->addSingularItem(_outRoughnessPointsConductivity, roughnessPointsCond);
            grp->addSingularItem(_outSobelConductivity, sobelCond);
            grp->addSingularItem(_outCHMConductivity, chmCond);
            grp->addSingularItem(_outIntensityRangeConductivity, intensityRangeCond);
            grp->addSingularItem(_outGndPtsDensityConductivity, gndPtsDensityCond);
            grp->addSingularItem(_outLowVegDensityConductivity, lowVegDensityCond);
            grp->addSingularItem(_outConductivity, conductivity);

            std::vector<QList<quint16> > linesOfFlight(dtm->nCells());
            float surfacePixel = dtm->resolution() * dtm->resolution();

            const CT_AbstractPointCloudIndex* pointCloudIndex = scene->pointCloudIndex();

            QMap<quint16, QList<double>* > intensityByLine;
            QMap<quint16, int > nByLine;
            QList<double> intensities;

            CT_PointIterator itP0(pointCloudIndex);
            while(itP0.hasNext())
            {
                size_t index = itP0.next().currentGlobalIndex();

                bool hasBeenSet, hasBeenSet2;
                double intensity = attributeIntensity->scalarAsDoubleAt(index, &hasBeenSet);
                quint16 lineOfFlight = quint16(attributeLineOfFlight->scalarAsDoubleAt(index, &hasBeenSet2));

                if (hasBeenSet && hasBeenSet2)
                 {
                    if (!intensityByLine.contains(lineOfFlight))
                    {
                        intensityByLine.insert(lineOfFlight, new QList<double>());
                    }

                    QList<double> *list = intensityByLine.value(lineOfFlight);
                    list->append(intensity);

                    nByLine.insert(lineOfFlight, nByLine.value(lineOfFlight, 0) + 1);

                    intensities.append(intensity);
                 }
            }

            setProgress(10.0);

            double intensityThreshold = computePercentile(intensities, 0.98);


            QMap<quint16, double> intensityByLineMedian;
            QMapIterator<quint16, QList<double>* > it(intensityByLine);

            int maxN = 0;
            double maxI = 0;

            while (it.hasNext())
            {
                it.next();
                QList<double>* list = it.value();
                double median = computePercentile(*list, 0.5);
                intensityByLineMedian.insert(it.key(), median);

                int n = nByLine.value(it.key());
                if (n > maxN)
                {
                    maxN = n;
                    maxI = median;
                }

                delete list;
            }
            if (maxI == 0) {maxI = 1;}

            setProgress(20.0);

            CT_Image2D_Points* optiGrid2D = new CT_Image2D_Points(dtm->minX(), dtm->minY(), dtm->xdim(), dtm->ydim(), dtm->resolution());
            CT_PointIterator itP(pointCloudIndex);
            while(itP.hasNext())
            {
                size_t index = itP.next().currentGlobalIndex();
                const CT_Point& point  = itP.currentPoint();


                float dtmVal = dtm->valueAtCoords(point(0), point(1));

                if (!qFuzzyCompare(dtmVal, dtm->NA()))
                {
                    float h = point(2) - dtmVal;
                    chm->setMaxValueAtCoords(point(0), point(1), h);

                    if (h < _roughPtsMaxHeight) {optiGrid2D->addPoint(index);}

                    bool hasBeenSet = false;
                    bool hasBeenSet2 = false;
                    double intensity = attributeIntensity->scalarAsDoubleAt(index, &hasBeenSet);

                    if (intensity > intensityThreshold) {intensity = intensityThreshold;}

                    quint16 lineOfFlight = quint16(attributeLineOfFlight->scalarAsDoubleAt(index, &hasBeenSet2));

                    if (hasBeenSet && hasBeenSet2)
                    {
                        intensity = intensity / (intensityByLineMedian.value(lineOfFlight, 1) / maxI);

                        if (h < 1.0)
                        {
                            intensityMin->setMinValueAtCoords(point(0), point(1), intensity);
                            intensityMax->setMaxValueAtCoords(point(0), point(1), intensity);
                        }
                    }

                   quint8 classification = quint8(attributeClassification->scalarAsDoubleAt(index, &hasBeenSet));
                   if (hasBeenSet)
                   {
                        if (classification == _gndClass)
                        {
                            gndPtsDensity->addValueAtCoords(point(0), point(1), 1);

                            quint16 lineOfFlight = quint16(attributeLineOfFlight->scalarAsDoubleAt(index, &hasBeenSet));
                            if (hasBeenSet)
                             {
                                size_t cellIndex;
                                gndPtsDensity->indexAtCoords(point(0), point(1), cellIndex);
                                linesOfFlight[cellIndex].append(lineOfFlight);
                             }
                        }
                    }
                   gndPtsDensityTot->addValueAtCoords(point(0), point(1), 1);


                   if (h > _lowVegHmin && h < _lowVegHmax)
                   {
                       lowVegDensity->addValueAtCoords(point(0), point(1), 1);
                   }
                }
            }

            setProgress(40.0);

            for (int xx = 0 ; xx < dtm->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < dtm->ydim() ; yy++)
                {
                    computeNeighbourhood(dtm, xx, yy);

                    if (!qFuzzyCompare(_e, dtm->NA()))
                    {
                        slopeDeg->setValue(xx, yy, computeSlope(dtm));
                        //sobel->setValue(xx, yy, computeSobel(dtm));

                        deltaDTM->setValue(xx, yy, _e - smooth5x5(dtm, xx, yy));

                        if (qFuzzyCompare(chm->value(xx, yy), chm->NA()))
                        {
                            chm->setValue(xx, yy, 0);
                        }

                        double imin = intensityMin->value(xx, yy);
                        double imax = intensityMax->value(xx, yy);

                        if (!qFuzzyCompare(imin, intensityMin->NA()) && !qFuzzyCompare(imax, intensityMax->NA()))
                        {
                            //intensityRange->setValue(xx, yy, imax);
                            intensityRange->setValue(xx, yy, imax - imin);
                        }


                        if (qFuzzyCompare(intensityRange->value(xx, yy), intensityRange->NA()))
                        {
                            intensityRange->setValue(xx, yy, 0);
                        }
                        if (qFuzzyCompare(gndPtsDensity->value(xx, yy), gndPtsDensity->NA()))
                        {
                            gndPtsDensity->setValue(xx, yy, 0);
                            gndPtsDensityStd->setValue(xx, yy, 0);
                        } else {                                                        
                            float val = gndPtsDensity->value(xx, yy) / surfacePixel;
                            float valTot = gndPtsDensityTot->value(xx, yy);
                            if (valTot > 0)
                            {
                                gndPtsDensityStd->setValue(xx, yy, gndPtsDensity->value(xx, yy) / valTot);
                            } else {
                                gndPtsDensityStd->setValue(xx, yy, 0);
                            }

                            gndPtsDensity->setValue(xx, yy, val);
//                            gndPtsDensityStd->setValue(xx, yy, val);

                            size_t cellIndex;
                            gndPtsDensity->index(xx, yy, cellIndex);

                            QList<quint16> &list = linesOfFlight[cellIndex];
                            std::sort(list.begin(), list.end());
                            int nLineOfFlight = std::unique(list.begin(), list.end()) - list.begin();

//                            if (nLineOfFlight > 1)
//                            {
//                                gndPtsDensityStd->setValue(xx, yy, gndPtsDensityStd->value(xx, yy) / nLineOfFlight);
//                            }

                        }
                        if (qFuzzyCompare(lowVegDensity->value(xx, yy), lowVegDensity->NA()))
                        {
                            lowVegDensity->setValue(xx, yy, 0);
                        }


                        Eigen::Vector3d center;
                        dtm->getCellCenterCoordinates(xx, yy, center);

                        QList<size_t> neighboursIndices;
                        if (optiGrid2D->getPointsInCircle(center, _roughPtsRadius, neighboursIndices) < 3)
                        {
                            roughnessPoints->setValue(xx, yy, 0);
                        } else {
                            CT_PointAccessor pointAccessor;
                            QList<Eigen::Vector3d> neighboursIndicesCoords;

                            for (int neigh = 0 ; neigh < neighboursIndices.size() ; neigh++)
                            {
                                neighboursIndicesCoords.append(pointAccessor.constPointAt(neighboursIndices.at(neigh)));
                            }

                            size_t cellIndex;
                            optiGrid2D->index(xx, yy, cellIndex);
                            const QList<size_t>* neighboursIndices2 = optiGrid2D->getConstPointIndexList(cellIndex);
                            QList<Eigen::Vector3d> neighboursIndices2Coords;

                            for (int neigh = 0 ; neigh < neighboursIndices2->size() ; neigh++)
                            {
                                neighboursIndices2Coords.append(pointAccessor.constPointAt(neighboursIndices2->at(neigh)));
                            }

                            Eigen::Vector3d direction;
                            Eigen::Vector3d centroid;
                            if (CT_MathPoint::fitPlaneFromPoints(neighboursIndicesCoords, direction, centroid))
                            {
                                double sumDist2 = 0;

                                int npts = neighboursIndices2Coords.size();
                                for (int neigh = 0 ; neigh < neighboursIndices2Coords.size() ; neigh++)
                                {
                                    const Eigen::Vector3d& pt = neighboursIndices2Coords.at(neigh);

                                    double dist = CT_MathPoint::distancePointPlane(pt, direction, centroid);

                                    sumDist2 += dist*dist / npts;
                                }

                                roughnessPoints->setValue(xx, yy, sqrt(sumDist2));
                            } else {
                                roughnessPoints->setValue(xx, yy, 0);
                            }
                        }
                    }
                }
            }

            setProgress(60.0);


            for (int xx = 0 ; xx < slopeDeg->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < slopeDeg->ydim() ; yy++)
                {
                    computeNeighbourhood(slopeDeg, xx, yy);

                    if (!qFuzzyCompare(_e, slopeDeg->NA()))
                    {
                        sobel->setValue(xx, yy, computeSobel(slopeDeg));
                    }
                }
            }

            setProgress(65.0);

            for (int xx = 0 ; xx < gndPtsDensity->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < gndPtsDensity->ydim() ; yy++)
                {
                    computeNeighbourhood(gndPtsDensity, xx, yy);

                    if (!qFuzzyCompare(_e, gndPtsDensity->NA()))
                    {
                        gndPtsDensity2->setValue(xx, yy, smooth(gndPtsDensity));
                    }
                }
            }

            setProgress(70.0);

            for (int xx = 0 ; xx < gndPtsDensityStd->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < gndPtsDensityStd->ydim() ; yy++)
                {
                    computeNeighbourhood(gndPtsDensityStd, xx, yy);

                    if (!qFuzzyCompare(_e, gndPtsDensityStd->NA()))
                    {
                        gndPtsDensityStd2->setValue(xx, yy, smooth(gndPtsDensityStd));
                    }
                }
            }

            setProgress(75.0);

            for (int xx = 0 ; xx < deltaDTM->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < deltaDTM->ydim() ; yy++)
                {
                    computeNeighbourhood(deltaDTM, xx, yy);

                    if (!qFuzzyCompare(_e, deltaDTM->NA()))
                    {
                        roughness->setValue(xx, yy, computeRoughness(deltaDTM));
                    }
                }
            }     

            setProgress(80.0);


            // clip rasters
            for (int xx = 0 ; xx < slopeDegClipped->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < slopeDegClipped->ydim() ; yy++)
                {
                    Eigen::Vector3d center;
                    slopeDegClipped->getCellCenterCoordinates(xx, yy, center);

                    slopeDegClipped->setValue(xx, yy, slopeDeg->valueAtCoords(center(0), center(1)));
                    roughnessClippedPoints->setValue(xx, yy, roughnessPoints->valueAtCoords(center(0), center(1)));
                    sobelClipped->setValue(xx, yy, sobel->valueAtCoords(center(0), center(1)));
                    chmClipped->setValue(xx, yy, chm->valueAtCoords(center(0), center(1)));
                    intensityRangeClipped->setValue(xx, yy, intensityRange->valueAtCoords(center(0), center(1)));
                    gndPtsDensityClipped->setValue(xx, yy, gndPtsDensityStd->valueAtCoords(center(0), center(1)));
                    lowVegDensityClipped->setValue(xx, yy, lowVegDensity->valueAtCoords(center(0), center(1)));
                }
            }

            slopeDegClipped->computeMinMax();
            roughnessClippedPoints->computeMinMax();
            sobelClipped->computeMinMax();
            chmClipped->computeMinMax();
            intensityRangeClipped->computeMinMax();
            gndPtsDensityClipped->computeMinMax();
            lowVegDensityClipped->computeMinMax();

            // remove useless rasters
            delete slopeDeg;
            delete roughness;
            delete roughnessPoints;
            delete sobel;
            delete chm;
            delete intensityMin;
            delete intensityMax;
            delete intensityRange;
            delete gndPtsDensity;
            delete gndPtsDensityTot;
            delete gndPtsDensityStd;
            delete gndPtsDensity2;
            delete gndPtsDensityStd2;
            delete lowVegDensity;
            delete deltaDTM;

            delete optiGrid2D;


            setProgress(90.0);

            // Conductivités

            QList<double> intensityQuant;
            QList<double> densityQuant;

            for (int xx = 0 ; xx < slopeDegClipped->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < slopeDegClipped->ydim() ; yy++)
                {
                    if (!qFuzzyCompare(slopeDegClipped->value(xx, yy), slopeDegClipped->NA()))
                    {
                        float intensityVal = intensityRangeClipped->value(xx, yy);
                        if (intensityVal > 0)
                        {
                            intensityQuant.append(intensityVal);
                        }

                        float dens = gndPtsDensityClipped->value(xx, yy);
                        if (dens > 0)
                        {
                            densityQuant.append(dens);
                        }
                    }
                }
            }

            float percI1 = computePercentile(intensityQuant, 0.10);
            float percI2 = computePercentile(intensityQuant, 0.50);

//            float percD1 = computePercentile(densityQuant, 0.25);
//            float percD2 = computePercentile(densityQuant, 0.90);

            for (int xx = 0 ; xx < slopeDegClipped->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < slopeDegClipped->ydim() ; yy++)
                {
                    if (qFuzzyCompare(slopeDegClipped->value(xx, yy), slopeDegClipped->NA()))
                    {
                        slopeDegCond->setValue(xx, yy, 0.0);
                        roughnessPointsCond->setValue(xx, yy, 0.0);
                        sobelCond->setValue(xx, yy, 0.0);
                        chmCond->setValue(xx, yy, 0.0);
                        intensityRangeCond->setValue(xx, yy, 0.0);
                        gndPtsDensityCond->setValue(xx, yy, 0.0);
                        lowVegDensityCond->setValue(xx, yy, 0.0);
                    } else {
                        slopeDegCond->setValue(xx, yy, activationPiecewiseLinear(slopeDegClipped->value(xx, yy), _slopeMin, _slopeMax, 0, 1, false));
                        roughnessPointsCond->setValue(xx, yy, activationPiecewiseLinear(roughnessClippedPoints->value(xx, yy), _roughMinPoints, _roughMaxPoints, 0, 1, false));
                        sobelCond->setValue(xx, yy, activationPiecewiseLinear(sobelClipped->value(xx, yy), _sobelThMin, _sobelThMax, 0, 1, false));
                        chmCond->setValue(xx, yy, activationPiecewiseLinear(chmClipped->value(xx, yy), _chmMin, _chmMax, 0, 1, false));
                        intensityRangeCond->setValue(xx, yy, activationPiecewiseLinear(intensityRangeClipped->value(xx, yy), percI1, percI2, 0, 1, false));
                        gndPtsDensityCond->setValue(xx, yy, activationPiecewiseLinear(gndPtsDensityClipped->value(xx, yy), _gndDensityMinPoints, _gndDensityMaxPoints, 0.5, 1, true));
                        lowVegDensityCond->setValue(xx, yy, activationThresholds(lowVegDensityClipped->value(xx, yy), _lowVegDensTh));
                    }
                }
            }


            for (int xx = 0 ; xx < slopeDegClipped->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < slopeDegClipped->ydim() ; yy++)
                {
                    computeNeighbourhood(slopeDegClipped, xx, yy);

                    if (!qFuzzyCompare(_e, slopeDegClipped->NA()))
                    {
                        float condSlope = slopeDegCond->value(xx, yy);
                        float condRoughness = roughnessPointsCond->value(xx, yy);
                        float condSobel = sobelCond->value(xx, yy);
                        float condCHM = chmCond->value(xx, yy);
                        float condIntensity = intensityRangeCond->value(xx, yy);
                        float condGNDDensity = gndPtsDensityCond->value(xx, yy);
                        float condLowVeg = lowVegDensityCond->value(xx, yy);

                        float cond = 1.0;
                        float sum = 0.0;
                        float weight = 0.0;

                        if (qFuzzyCompare(_slopeAdd, -1.0)) {cond *= condSlope;} else {sum += _slopeAdd*condSlope; weight += _slopeAdd;}
                        if (qFuzzyCompare(_roughnessAdd, -1.0)) {cond *= condRoughness;} else {sum += _roughnessAdd*condRoughness; weight += _roughnessAdd;}
                        if (qFuzzyCompare(_sobelAdd, -1.0)) {cond *= condSobel;} else {sum += _sobelAdd*condSobel; weight += _sobelAdd;}
                        if (qFuzzyCompare(_chmAdd, -1.0)) {cond *= condCHM;} else {sum += _chmAdd*condCHM; weight += _chmAdd;}
                        if (qFuzzyCompare(_intensityAdd, -1.0)) {cond *= condIntensity;} else {sum += _intensityAdd*condIntensity; weight += _intensityAdd;}
                        if (qFuzzyCompare(_gndDensAdd, -1.0)) {cond *= condGNDDensity;} else {sum += _gndDensAdd*condGNDDensity; weight += _gndDensAdd;}
                        if (qFuzzyCompare(_lowVegAdd, -1.0)) {cond *= condLowVeg;} else {sum += _lowVegAdd*condLowVeg; weight += _lowVegAdd;}

                        if (weight > 0)
                        {
                            cond *= sum / weight;
                        }

                        if (cond < 0.1f) {cond = 0.1f;}

                        conductivity->setValue(xx, yy, cond);
                    }
                }
            }

            slopeDegCond->computeMinMax();
            roughnessPointsCond->computeMinMax();
            sobelCond->computeMinMax();
            chmCond->computeMinMax();
            intensityRangeCond->computeMinMax();
            gndPtsDensityCond->computeMinMax();
            lowVegDensityCond->computeMinMax();

            conductivity->computeMinMax();
        }
    }
}

void ONF_StepComputeRoadProbabilityRasters::computeNeighbourhood(const CT_Image2D<float>* dtm, int xx, int yy)
{
    _a = dtm->value(xx - 1, yy - 1);
    _b = dtm->value(xx    , yy - 1);
    _c = dtm->value(xx + 1, yy - 1);
    _d = dtm->value(xx - 1, yy    );
    _e = dtm->value(xx    , yy    );
    _f = dtm->value(xx + 1, yy    );
    _g = dtm->value(xx - 1, yy + 1);
    _h = dtm->value(xx    , yy + 1);
    _i = dtm->value(xx + 1, yy + 1);
}


float ONF_StepComputeRoadProbabilityRasters::computeSlope(const CT_Image2D<float>* dtm) const
{
    float aa = _a;
    float bb = _b;
    float cc = _c;
    float dd = _d;
    float ff = _f;
    float gg = _g;
    float hh = _h;
    float ii = _i;

    if (qFuzzyCompare(aa, dtm->NA())) {aa = _e;}
    if (qFuzzyCompare(bb, dtm->NA())) {bb = _e;}
    if (qFuzzyCompare(cc, dtm->NA())) {cc = _e;}
    if (qFuzzyCompare(dd, dtm->NA())) {dd = _e;}
    if (qFuzzyCompare(ff, dtm->NA())) {ff = _e;}
    if (qFuzzyCompare(gg, dtm->NA())) {gg = _e;}
    if (qFuzzyCompare(hh, dtm->NA())) {hh = _e;}
    if (qFuzzyCompare(ii, dtm->NA())) {ii = _e;}

    float dzdx = ((cc + 2.0f*ff + ii) - (aa + 2.0f*dd + gg)) / (8.0f * float(dtm->resolution()));
    float dzdy = ((gg + 2.0f*hh + ii) - (aa + 2.0f*bb + cc)) / (8.0f * float(dtm->resolution()));
    double slope_radians = std::atan(std::sqrt(double(dzdx)*double(dzdx) + double(dzdy)*double(dzdy)));


    return slope_radians * 180.0 / M_PI;

}

float ONF_StepComputeRoadProbabilityRasters::computeRoughness(const CT_Image2D<float>* dtm) const
{
    float min = _e;
    float max = _e;

    if (!qFuzzyCompare(_a, dtm->NA()))
    {
        if (_a < min) {min = _a;}
        if (_a > max) {max = _a;}
    }
    if (!qFuzzyCompare(_b, dtm->NA()))
    {
        if (_b < min) {min = _b;}
        if (_b > max) {max = _b;}
    }
    if (!qFuzzyCompare(_c, dtm->NA()))
    {
        if (_c < min) {min = _c;}
        if (_c > max) {max = _c;}
    }
    if (!qFuzzyCompare(_d, dtm->NA()))
    {
        if (_d < min) {min = _d;}
        if (_d > max) {max = _d;}
    }
    if (!qFuzzyCompare(_f, dtm->NA()))
    {
        if (_f < min) {min = _f;}
        if (_f > max) {max = _f;}
    }
    if (!qFuzzyCompare(_g, dtm->NA()))
    {
        if (_g < min) {min = _g;}
        if (_g > max) {max = _g;}
    }
    if (!qFuzzyCompare(_h, dtm->NA()))
    {
        if (_h < min) {min = _h;}
        if (_h > max) {max = _h;}
    }
    if (!qFuzzyCompare(_i, dtm->NA()))
    {
        if (_i < min) {min = _i;}
        if (_i > max) {max = _i;}
    }

    return max - min;
}


float ONF_StepComputeRoadProbabilityRasters::computeSobel(const CT_Image2D<float>* raster) const
{
    float fx = 0;
    float fy = 0;

    if (!qFuzzyCompare(_a, raster->NA())) {fx +=  1*_a; fy += -1*_a;}
    if (!qFuzzyCompare(_b, raster->NA())) {fx +=  2*_b;            }
    if (!qFuzzyCompare(_c, raster->NA())) {fx +=  1*_c; fy +=  1*_c;}
    if (!qFuzzyCompare(_d, raster->NA())) {            fy += -2*_d;}
    if (!qFuzzyCompare(_f, raster->NA())) {            fy +=  2*_f;}
    if (!qFuzzyCompare(_g, raster->NA())) {fx += -1*_g; fy += -1*_g;}
    if (!qFuzzyCompare(_h, raster->NA())) {fx += -2*_h;            }
    if (!qFuzzyCompare(_i, raster->NA())) {fx += -1*_i; fy +=  1*_i;}

    return sqrt(fx*fx + fy*fy);
}

float ONF_StepComputeRoadProbabilityRasters::smooth(const CT_Image2D<float>* raster) const
{
    float mean = 0;
    float nb = 0;

    if (!qFuzzyCompare(_a, raster->NA())) {mean += _a; nb++;}
    if (!qFuzzyCompare(_b, raster->NA())) {mean += _b; nb++;}
    if (!qFuzzyCompare(_c, raster->NA())) {mean += _c; nb++;}
    if (!qFuzzyCompare(_d, raster->NA())) {mean += _d; nb++;}
    if (!qFuzzyCompare(_e, raster->NA())) {mean += 2*_e; nb += 2;}
    if (!qFuzzyCompare(_f, raster->NA())) {mean += _f; nb++;}
    if (!qFuzzyCompare(_g, raster->NA())) {mean += _g; nb++;}
    if (!qFuzzyCompare(_h, raster->NA())) {mean += _h; nb++;}
    if (!qFuzzyCompare(_i, raster->NA())) {mean += _i; nb++;}

    if (nb > 0)
    {
        mean /= nb;
    }
    return mean;
}

float ONF_StepComputeRoadProbabilityRasters::smooth5x5(const CT_Image2D<float>* raster, int xx, int yy) const
{
    float mean = 0;
    float nb = 0;

    float val = raster->value(xx - 2, yy - 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 1, yy - 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx, yy - 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 1, yy - 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 2, yy - 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 2, yy - 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 1, yy - 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx, yy - 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 1, yy - 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 2, yy - 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 2, yy);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 1, yy);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx, yy);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 1, yy);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 2, yy);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 2, yy + 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 1, yy + 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx, yy + 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 1, yy + 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 2, yy + 1);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 2, yy + 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx - 1, yy + 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx, yy + 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 1, yy + 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}

    val = raster->value(xx + 2, yy + 2);
    if (!qFuzzyCompare(val, raster->NA())) {mean += val; nb++;}


    if (nb > 0)
    {
        mean /= nb;
    }
    return mean;
}


double ONF_StepComputeRoadProbabilityRasters::computePercentile(QList<double> &array, const double &p)
{
    std::sort(array.begin(), array.end());

    int arraySize = array.size();

    // Second Variant, show wikipedia "Percentile"
    double v = ((double)(arraySize-1)) * p;
    int ip1 = (int)v;
    double f = (v-ip1); // (arraySize-1)*p = ip1+f

    int ip2 = ip1 + 1;

    if(ip2 == arraySize)
        return array[ip1];

    if(f == 0)
        return array[ip1];

    return array[ip1] + (f * (array[ip2] - array[ip1]));
}

float ONF_StepComputeRoadProbabilityRasters::activationPiecewiseLinear(float x, float th1, float th2, float minValue, float maxValue, bool ascending)
{
    float y = 0;

    if (x <= th1)
    {
        if (ascending)
        {
            y = minValue;
        } else {
            y = maxValue;
        }
    } else if (x >= th2)
    {
        if (ascending)
        {
            y = maxValue;
        } else {
            y = minValue;
        }
    } else {

        float a = (maxValue - minValue) / (th2 - th1);

        if (ascending)
        {
            y = a*(x - th1) + minValue;
        } else {
            y = a*(th2 - x) + minValue;
        }
    }

    return std::round(y * 1000.0) / 1000.0;
}

float ONF_StepComputeRoadProbabilityRasters::activationThresholds(float x, float th)
{
    if (x >= th) {return 0;}
    return 1;
}