/****************************************************************************
 Copyright (C) 2010-2012 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_stepremoveuppernoisev2.h"

#include "ct_accessor/ct_pointaccessor.h"

#include "ct_log/ct_logmanager.h"

#include <math.h>
#include <iostream>
#include <QList>

ONF_StepRemoveUpperNoiseV2::ONF_StepRemoveUpperNoiseV2() : SuperClass()
{
    _mode = 2;

    _minHeight = 15.0;
    _maxHeight = 100.0;
    _filterUnderground = true;

    _resolution = 2.0;
    _searchDistInCells = 5;
    _threshold = 2;
    _maxNEmptyCells = 1;
}

QString ONF_StepRemoveUpperNoiseV2::description() const
{
    return tr("Supprimer les points aberrants en hauteur v2");
}

QString ONF_StepRemoveUpperNoiseV2::detailledDescription() const
{
    return tr("");
}

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

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

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

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

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

void ONF_StepRemoveUpperNoiseV2::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scène à débruiter"), tr("Par exemple pour des scènes où les filtres matériels sont désactivés"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scène à débruiter"));

    if (_mode == 0)
    {
        manager.addItem(_inGroup, _inDTM, tr("MNT"));
    } else if (_mode == 1)
    {
        manager.addItem(_inGroup, _inTIN, tr("TIN"));
    }
}

// Création et affiliation des modèles OUT
void ONF_StepRemoveUpperNoiseV2::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outScene, tr("Scène débruitée"));
}

void ONF_StepRemoveUpperNoiseV2::fillPreInputConfigurationDialog(CT_StepConfigurableDialog* preInputConfigDialog)
{
    CT_ButtonGroup &bg_mode = preInputConfigDialog->addButtonGroup(_mode);
    preInputConfigDialog->addExcludeValue("", "", tr("Filtrer une scène en Altitude (MNT)"), bg_mode, 0);
    preInputConfigDialog->addExcludeValue("", "", tr("Filtrer une scène en Altitude (TIN)"), bg_mode, 1);
    preInputConfigDialog->addExcludeValue("", "", tr("Filtrer une scène en Hauteur"), bg_mode, 2);

}

void ONF_StepRemoveUpperNoiseV2::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Hauteur de sécurité"), "m", 0, 99999, 2, _minHeight, 1, tr("Aucun point ne seras éliminé entre le sol et cette hauteur"));
    postInputConfigDialog->addDouble(tr("Hauteur maximale autorisée"), "m", 0, 99999, 2, _maxHeight, 1, tr("Tous les points au dessus de cette hauteur seront éliminés"));
    postInputConfigDialog->addBool(tr("Filtrer les points situés sous le sol"), "", "", _filterUnderground, tr("Si cette option est activée, tous les points situés sous le sols seront éliminés"));
    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addDouble(tr("Résolution de la grille"), "m", 0, 9999.99, 2, _resolution, 1, tr("Résolution de filtrage. L'espace vertical maximum entre les derniers points à conserver et ceux à éliminer sera un multiple de cette résultion. "));
    postInputConfigDialog->addInt(tr("Distance de recherche horizontale"), "cellules", 1, 999, _searchDistInCells, tr("Pour qu'une cellule soit considérée comme vide, aucune cellule pleine dans le même plan horizontal ne doit se situer à moins de cette distance."));
    postInputConfigDialog->addInt(tr("Nombre minimum de points pour considérer une cellule remplie"), "points", 1, 999999, _threshold, tr("Les cellules dont la densité de points est inférieure à ce seuil sont considérées comme vide pour les calculs d'espacement vertical."));
    postInputConfigDialog->addInt(tr("Espacement maximal vertical entre points à conserver"), "cellules", 1, 999, _maxNEmptyCells, tr("Espace vertical, exprimé en multiples de la résolution, entre les points à conserver et les points à éliminer."));
}


void ONF_StepRemoveUpperNoiseV2::compute()
{
    for (CT_StandardItemGroup* group : _inGroup.iterateOutputs(_inResult))
    {
        if (isStopped()) {return;}

        const CT_AbstractItemDrawableWithPointCloud* inScene = group->singularItem(_inScene);
        const CT_Image2D<float>* inDTM = nullptr;
        const CT_Triangulation2D* inTIN = nullptr;
        CT_DelaunayTriangulation *triangulation =  nullptr;
        CT_DelaunayTriangle* refTri = nullptr;

        if (_mode == 0) {inDTM = group->singularItem(_inDTM);}
        if (_mode == 1)
        {
            inTIN = group->singularItem(_inTIN);
            triangulation = inTIN->getDelaunayT();
        }

        if (inScene != nullptr && (_mode != 0 || inDTM != nullptr) && (_mode != 1 || inTIN != nullptr)) // on vérifie que ça existe
        {
            const CT_AbstractPointCloudIndex* pointCloudIndexVector = inScene->pointCloudIndex();
            size_t n_points = pointCloudIndexVector->size(); // nombre de points du nuage
            PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène d'entrée comporte %1 points.")).arg(n_points));

            // Création du nuage de points pour la scène de sortie
            CT_PointCloudIndexVector *resPointCloudIndex = new CT_PointCloudIndexVector();
            resPointCloudIndex->setSortType(CT_AbstractCloudIndex::NotSorted);

            QVector<size_t> indices;
            QVector<float>  heights;
            QVector<bool>   keep;
            QVector<size_t> densityIndices;

            float maxHeight = 0;

            indices.reserve(n_points);

            CT_PointIterator itP(pointCloudIndexVector);
            while(itP.hasNext() && !isStopped())
            {
                itP.next(); // point suivant
                const CT_Point &point = itP.currentPoint();

                indices.append(itP.currentGlobalIndex());

                // compute point heights
                float height = -9999;
                if (_mode == 0)
                {
                    float zDTM = inDTM->valueAtCoords(point(0), point(1));
                    if (zDTM != inDTM->NA())
                    {
                        height = point(2) - zDTM;
                    } else
                    {
                        height = -9999;
                    }
                } else if (_mode == 1)
                {
                    double zTIN = -9999;

                    refTri = triangulation->getZCoordForXY(point(0), point(1), zTIN, refTri);
                    if (zTIN != NAN)
                    {
                        height = point(2) - zTIN;
                    } else
                    {
                        height = -9999;
                    }

                } else {
                    height = point(2);
                }

                heights.append(height);

                if ((_filterUnderground && height < 0) || height > _maxHeight)
                {
                    keep.append(false);
                }
                else
                {
                    keep.append(true);
                    if (height > maxHeight) {maxHeight = height;}
                }
            }

            setProgress(20);

            double minX = inScene->minX();
            double minY = inScene->minY();
            double maxX = inScene->maxX();
            double maxY = inScene->maxY();

            if (_mode == 0)
            {
                minX = inDTM->minX();
                minY = inDTM->minY();
                maxX = inDTM->maxX();
                maxY = inDTM->maxY();
            }

            // compute point density for each cell
            CT_Grid3D<int>* densityGrd = CT_Grid3D<int>::createGrid3DFromXYZCoords(minX, minY, 0, maxX, maxY, maxHeight, _resolution, -1, 0);

            CT_PointAccessor ptAccessor;
            for (int i = 0 ; i < keep.size() ; i++)
            {
                if (keep[i])
                {
                    CT_Point point = ptAccessor.constPointAt(indices[i]);

                    size_t index;
                    densityGrd->indexAtXYZ(point(0), point(1), heights[i], index);
                    densityIndices.append(index);

                    densityGrd->addValueAtIndex(index, 1);
                } else {
                    densityIndices.append(0);
                }
            }

            setProgress(30);

            // binarize grid with 0 if < threshold, and 1 if >= threshold
            for (int xx = 0 ; xx < densityGrd->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < densityGrd->ydim() ; yy++)
                {
                    for (int zz = 0 ; zz < densityGrd->zdim() ; zz++)
                    {
                        // to reduce voxel limit impacts on density test
                        if (densityGrd->value(xx, yy, zz) > 0 && zz > 0 && densityGrd->value(xx, yy, zz - 1) > 0)
                        {
                            densityGrd->setValue(xx, yy, zz, 1);
                        } else {
                            if (densityGrd->value(xx, yy, zz) >= _threshold)
                            {
                                densityGrd->setValue(xx, yy, zz, 1);
                            } else {
                                densityGrd->setValue(xx, yy, zz, 0);
                            }
                        }
                    }
                }
            }

            setProgress(40);

            // horizontal dilatation
            for (int xx = 0 ; xx < densityGrd->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < densityGrd->ydim() ; yy++)
                {
                    for (int zz = 0 ; zz < densityGrd->zdim() ; zz++)
                    {
                        int value = densityGrd->value(xx, yy, zz);

                        if (value < 1)
                        {
                            size_t index;
                            densityGrd->index(xx, yy, zz, index);

                            Eigen::Vector3d center;
                            densityGrd->getCellCenterCoordinates(index, center);

                            int firstColx = xx - _searchDistInCells;
                            int lastColx = xx + _searchDistInCells;
                            int firstLiny = yy - _searchDistInCells;
                            int lastLiny = yy + _searchDistInCells;

                            if (firstColx < 0)     {firstColx = 0;}
                            if (lastColx >= densityGrd->xdim()) {lastColx = densityGrd->xdim() - 1;}
                            if (firstLiny < 0)     {firstLiny = 0;}
                            if (lastLiny >= densityGrd->ydim()) {lastLiny = densityGrd->ydim() - 1;}

                            double searchDist = _searchDistInCells * _resolution * 1.01;

                            bool found = false;
                            for (int xxn = firstColx ; xxn <= lastColx && !found; xxn++)
                            {
                                for (int yyn = firstLiny ; yyn <= lastLiny && !found ; yyn++)
                                {
                                    size_t indexN;
                                    densityGrd->index(xxn, yyn, zz, indexN);

                                    Eigen::Vector3d centerN;
                                    densityGrd->getCellCenterCoordinates(indexN, centerN);

                                    double dist = sqrt(pow(center(0) - centerN(0), 2) + pow(center(1) - centerN(1), 2));

                                    if (dist <= searchDist && densityGrd->value(xxn, yyn, zz) == 1)
                                    {
                                        found = true;
                                    }
                                }
                            }

                            if (found)
                            {
                                densityGrd->setValue(xx, yy, zz, 2);
                            }
                        }
                    }
                }
            }

            setProgress(50);

            // Compute vertical continuity to get safe height
            for (int xx = 0 ; xx < densityGrd->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < densityGrd->ydim() ; yy++)
                {
                    int lastIndex = 0;
                    for (int zz = 0 ; zz < densityGrd->zdim() ; zz++)
                    {
                        int val = densityGrd->value(xx, yy, zz);

                        int deltaIndex = zz - lastIndex - 1;
                        if (val >= 1 && (deltaIndex <= _maxNEmptyCells || (zz*_resolution <= _minHeight)))
                        {
                            densityGrd->setValue(xx, yy, zz, 3);
                            lastIndex = zz;
                        }
                    }
                }
            }

            setProgress(80);

            for (int i = 0 ; i < indices.size() ; i++)
            {
                if (keep[i] && densityGrd->valueAtIndex(densityIndices[i]) == 3)
                {
                    resPointCloudIndex->addIndex(indices[i]);
                }
            }

            delete densityGrd;

            setProgress(90);

            if (resPointCloudIndex->size() > 0)
            {
                // Optimisation : on réactive de tri à la volée (du coup déclenche un tri complet)
                resPointCloudIndex->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder);

                // Création de l'item scène et ajout au résultat
                // Au passage le nuage d'indices de points est enregistré auprès du dépôt
                CT_Scene *outScene = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(resPointCloudIndex));
                outScene->updateBoundingBox();

                group->addSingularItem(_outScene, outScene);

                PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène de densité réduite comporte %1 points.")).arg(outScene->pointCloudIndex()->size()));
                PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("Nombre de points filtrés : %1")).arg(n_points - outScene->pointCloudIndex()->size()));
            } else {
                delete resPointCloudIndex;
                PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Aucun point conservé pour cette scène"));
            }

        }

        setProgress(99);
    }

}