/****************************************************************************
 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_stepremoveuppernoise.h"

#include "ct_log/ct_logmanager.h"

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

ONF_StepRemoveUpperNoise::ONF_StepRemoveUpperNoise() : SuperClass()
{
    _resolution = 5.0;
    _threshold = 1;
    _thresholdValid = 5;
    _length = 5.0;
}

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

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

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

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

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

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

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

void ONF_StepRemoveUpperNoise::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"));
}

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

void ONF_StepRemoveUpperNoise::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Résolution de la grille :"), "m", 0, 10000, 2, _resolution);
    postInputConfigDialog->addInt(tr("Nombre minimum de points pour considérer une cellule remplie :"), "points", 1, 1000000, _threshold);
    postInputConfigDialog->addInt(tr("Nombre minimum de points pour considérer une cellule valide :"), "points", 1, 1000000, _thresholdValid);
    postInputConfigDialog->addDouble(tr("Espacement maximal :"), "m", 0, 10000, 2, _length);
}

CT_Image2D<float>* ONF_StepRemoveUpperNoise::getFilteredPointIndices(const CT_AbstractItemDrawableWithPointCloud *_inScene, double offset, double progressVal, double progressOffset)
{
    double demiProgressVal = progressVal / 2.0;

    // Réupération de la liste de points du nuage
    const CT_AbstractPointCloudIndex* pointCloudIndexVector = _inScene->pointCloudIndex();

    CT_PointIterator itP(pointCloudIndexVector);
    size_t n_points = itP.size(); // nombre de points du nuage

    CT_Grid3D_Sparse<int>* densityGrd = CT_Grid3D_Sparse<int>::createGrid3DFromXYZCoords(_inScene->minX() - offset, _inScene->minY() - offset, _inScene->minZ() - offset, _inScene->maxX()+_resolution, _inScene->maxY()+_resolution, _inScene->maxZ()+_resolution, _resolution, -1, 0);

    // Boucle sur les points
    int i = 0;
    while(itP.hasNext() && !isStopped())
    {
        itP.next(); // point suivant
        const CT_Point &point = itP.currentPoint(); // accès aux coordonnées du points, sous forme de référence constante

        densityGrd->addValueAtXYZ(point(0), point(1), point(2), 1);
        ++i;
        // Mise à jour de la barre de progression
        setProgress(float(progressOffset + demiProgressVal * i / n_points));
    }

    CT_Image2D<float>* safeMaxHeight = new CT_Image2D<float>(densityGrd->minX(), densityGrd->minY(), densityGrd->xdim(), densityGrd->ydim(), _resolution, 0, NAN, float(_inScene->minZ()));

    for (int x = 0 ; x < densityGrd->xdim() ; x++)
    {
        for (int y = 0 ; y < densityGrd->ydim() ; y++)
        {
            int safez = 0;
            for (int z = 0 ; z < densityGrd->zdim() ; z++)
            {
                int density = densityGrd->value(x, y, z);
                if (density >= _thresholdValid) {safez = z;}
                //if (safez == 0 && density > _threshold) {safez = z;}
            }

            double safeHeight = densityGrd->getCellCenterZ(safez) + _resolution / 2.0;

            for (int z = safez + 1 ; z < densityGrd->zdim() ; z++)
            {
                int density = densityGrd->value(x, y, z);
                safeHeight = densityGrd->getCellCenterZ(safez) + _resolution / 2.0;
                double newHeight = densityGrd->getCellCenterZ(z) + _resolution / 2.0;

                if ((newHeight - safeHeight) < _length && density >= _threshold)
                {
                    safez = z;
                    safeHeight = newHeight;
                }
            }

            safeMaxHeight->setValueAtCoords(densityGrd->getCellCenterX(x), densityGrd->getCellCenterY(y), float(safeHeight));
        }
    }

    delete densityGrd;

    return safeMaxHeight;
}

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

        const CT_AbstractItemDrawableWithPointCloud* inScene = group->singularItem(_inScene);

        if (inScene != 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));

            // Détermination des points à filter
            // Deux fois pour eviter les effets de seuils en bords de voxels
            CT_Image2D<float>* safeMaxHeight1 = getFilteredPointIndices(inScene, 0, 20.0, 0.0);
            CT_Image2D<float>* safeMaxHeight2 = getFilteredPointIndices(inScene, _resolution / 2.0, 20.0, 21.0);

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

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

                if (point(2) <= double(safeMaxHeight1->valueAtCoords(point(0), point(1))) || point(2) <= double(safeMaxHeight2->valueAtCoords(point(0), point(1))))
                {
                    size_t pointIndex = itP.cIndex();
                    resPointCloudIndex->addIndex(pointIndex);
                }

                ++i;
                // Mise à jour de la barre de progression
                setProgress(41 + 50 * i / n_points);
            }

            delete safeMaxHeight1;
            delete safeMaxHeight2;

            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);
    }

}