/****************************************************************************
 Copyright (C) 2010-2019 the Office National des Forêts (ONF), France
                         All rights reserved.

 Contact : alexandre.piboule@onf.fr

 Developers : Alexandre PIBOULE (ONF)

 This file is part of PluginONF library.

 PluginONF is free library: you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 PluginONF is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with PluginONF.  If not, see <http://www.gnu.org/licenses/lgpl.html>.
*****************************************************************************/

#include "onf_stepextractreferencegroundpoints.h"

#include "ct_log/ct_logmanager.h"

#define EPSILON 0.000001

ONF_StepExtractReferenceGroundPoints::ONF_StepExtractReferenceGroundPoints() : SuperClass()
{
    _gridsize   = 0.10;

    _gridMode = 2;
    _xBase = 0;
    _yBase = 0;

    _deltaZThreshold = 0.1;
}

QString ONF_StepExtractReferenceGroundPoints::description() const
{
    return tr("Extraire des points sol de référence");
}

QString ONF_StepExtractReferenceGroundPoints::detailledDescription() const
{
    return tr("Cette étape permet d'extraire des points de réfénce sol à la résolution indiquée.");
}

QString ONF_StepExtractReferenceGroundPoints::inputDescription() const
{
    return SuperClass::inputDescription() + tr("");
}

QString ONF_StepExtractReferenceGroundPoints::outputDescription() const
{
    return tr("");
}

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

CT_VirtualAbstractStep* ONF_StepExtractReferenceGroundPoints::createNewInstance() const
{
    // cree une copie de cette etape
    return new ONF_StepExtractReferenceGroundPoints();
}

/////////////////////// PROTECTED ///////////////////////

void ONF_StepExtractReferenceGroundPoints::declareInputModels(CT_StepInModelStructureManager& manager)
{

    manager.addResult(_inResult, tr("Points"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Points"));
    manager.addItem(_inGroup, _inArea, tr("Emprise"));
}

void ONF_StepExtractReferenceGroundPoints::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Résolution d'échantillonnage :"), "cm", 1, 1000, 0, _gridsize, 100);
    postInputConfigDialog->addDouble(tr("Epaiseur de calcul de la densité :"), "cm", 1, 1000, 0, _deltaZThreshold, 100);

    postInputConfigDialog->addSeparationLine();
    postInputConfigDialog->addText(tr("Quelle emprise utiliser ?"),"", "", tr("La section suivante définit comment l'extension selon les axes X et Y du raster est déterminée."));

    CT_ButtonGroup &bg_gridMode = postInputConfigDialog->addButtonGroup(_gridMode);

    postInputConfigDialog->addExcludeValue("", "", tr("La boite englobante de la scène"), bg_gridMode, 0, tr("Dans ce cas l'extension est directement calculée à partir des X et Y maximum et minimum des points de la scène d'entrée. "));
    postInputConfigDialog->addExcludeValue("", "", tr("L'emprise précédement sélectionnée "), bg_gridMode, 2, tr("Cette option ne fonctionne que si une emprise a été séléctionnée dans les résultats d'entrée (optionnel). Dans ce cas c'est cette emprise qui détermine l'extension du raster. Cette option est utilisée dans le cas de dallages pré-définis. "));
    postInputConfigDialog->addExcludeValue("", "", tr("Recaler par rapport aux coordonnées suivantes :"), bg_gridMode, 1, tr("Cette option calcule d'abord l'entension des points de la scène (comme la première option), mais l'agrandit de façon à ce que les coordonnées du coin en bas à gauche du raster tombe \"juste\" par rapport aux coordonnées indiquées (cette à dire qu'elles soient égales à ces coordonnées de référence plus un multiple de la résolution). C'est l'option par défaut, permettant des rasters cohérents entre eux. "));
    postInputConfigDialog->addDouble(tr("Coordonnée X :"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _xBase, 1, tr("Coordonnée X de référence pour la troisième option. "));
    postInputConfigDialog->addDouble(tr("Coordonnée Y :"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _yBase, 1, tr("Coordonnée Y de référence pour la troisième option. "));
}

void ONF_StepExtractReferenceGroundPoints::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outMinZ, "MinZ");
    manager.addItem(_inGroup, _outDensity0, "Density0");
    manager.addItem(_inGroup, _outDensity1, "Density1");
}

void ONF_StepExtractReferenceGroundPoints::compute()
{
    for (CT_StandardItemGroup* group : _inGroup.iterateOutputs(_inResult))
    {
        for (const CT_AbstractItemDrawableWithPointCloud* inScene : group->singularItems(_inScene))
        {
            if (isStopped()) {return;}

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

            if (pointCloudIndex->size() > 0)
            {
                const CT_AbstractGeometricalItem *emprise = group->singularItem(_inArea);

                // Creation du raster
                double minX = std::floor(inScene->minX()*100.0) / 100.0;
                double minY = std::floor(inScene->minY()*100.0) / 100.0;
                double maxX = std::ceil(inScene->maxX()*100.0) / 100.0;
                double maxY = std::ceil(inScene->maxY()*100.0) / 100.0;


                if (_gridMode == 2 && emprise != nullptr && emprise->hasBoundingBox())
                {
                    Eigen::Vector3d min, max;
                    emprise->boundingBox(min, max);

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

                } else if (_gridMode == 1)
                {
                    minX = std::floor((inScene->minX() - _xBase - 1) / _gridsize) * _gridsize + _xBase;
                    minY = std::floor((inScene->minY() - _yBase - 1) / _gridsize) * _gridsize + _yBase;

                    while (minX < inScene->minX()) {minX += _gridsize;};
                    while (minY < inScene->minY()) {minY += _gridsize;};

                    while (minX > inScene->minX()) {minX -= _gridsize;};
                    while (minY > inScene->minY()) {minY -= _gridsize;};
                }

                CT_Image2D<float>*  minZ = CT_Image2D<float>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);
                CT_Image2D<double>* minZ_X = CT_Image2D<double>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);
                CT_Image2D<double>* minZ_Y = CT_Image2D<double>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);

                // Création MNT (version Zmin)
                CT_PointIterator itP(pointCloudIndex);
                while(itP.hasNext() && !isStopped())
                {
                    const CT_Point &point =itP.next().currentPoint();

                    float zpoint = static_cast<float>(point(2));
                    float minZVal = minZ->valueAtCoords(point(0), point(1));

                    if (qFuzzyCompare(minZVal, minZ->NA()) || zpoint < minZVal)
                    {
                        minZ->setValueAtCoords(point(0), point(1), zpoint);
                        minZ_X->setValueAtCoords(point(0), point(1), point(0));
                        minZ_Y->setValueAtCoords(point(0), point(1), point(1));
                    }
                }

                setProgress(25.0f);


                // Filtrer MNT en fonction de la différence de hauteur locale
                bool finished = false;
                while (!finished)
                {
                    finished = true;
                    for (int xx = 0 ; xx < minZ->xdim() ; ++xx)
                    {
                        for (int yy = 0 ; yy < minZ->ydim() ; ++yy)
                        {
                            float value = minZ->value(xx, yy);

                            if (!qFuzzyCompare(value, minZ->NA()))
                            {
                                QList<float> neighbours = minZ->neighboursValues(xx, yy, 3, false, CT_Image2D<float>::CM_DropCenter);

                                if (neighbours.isEmpty())
                                {
                                    minZ->setValue(xx, yy, minZ->NA());
                                    finished = false;
                                } else {
                                    double delta = value - *std::min_element(neighbours.begin(), neighbours.end());

                                    if (delta > 0.5)
                                    {
                                        minZ->setValue(xx, yy, minZ->NA());
                                        finished = false;
                                    }
                                }
                            }
                        }
                    }
                }

                setProgress(50.0f);

                // Calcul de la densité sol et sursol
                CT_Image2D<int>* density0 = CT_Image2D<int>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, 0);
                CT_Image2D<int>* density1 = CT_Image2D<int>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, 0);

                itP.toFront();
                while(itP.hasNext() && !isStopped())
                {
                    const CT_Point &point =itP.next().currentPoint();

                    float zpoint = static_cast<float>(point(2));
                    float minZVal = minZ->valueAtCoords(point(0), point(1));
                    float delta = zpoint - minZVal;

                    if (!qFuzzyCompare(minZVal, minZ->NA()))
                    {
                        if (delta < _deltaZThreshold)
                        {
                            density0->addValueAtCoords(point(0), point(1), 1);
                        } else if (delta < 2.0*_deltaZThreshold)
                        {
                            density1->addValueAtCoords(point(0), point(1), 1);
                        }
                    }

                }


                setProgress(75.0f);

                // ajout du raster MinZ
                minZ->computeMinMax();
                group->addSingularItem(_outMinZ, minZ);

                // ajout du raster Density
                density0->computeMinMax();
                group->addSingularItem(_outDensity0, density0);

                // ajout du raster Density
                density1->computeMinMax();
                group->addSingularItem(_outDensity1, density1);

                delete minZ_X;
                delete minZ_Y;
            }
        }
        setProgress(100.0f);
    }
}