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

#include "ct_log/ct_logmanager.h"

#include <math.h>
#include <stdlib.h>

ONF_StepClassifyGround::ONF_StepClassifyGround() : SuperClass()
{
    _gridsize   = 0.5;
    _groundwidth = 0.32;
    _min_density = 200;
    _dist = 3;
    _filterByDensity = true;
    _filterByNeighourhoud = true;
    _debug = false;
}

QString ONF_StepClassifyGround::description() const
{
    return tr("Classifier les points sol (TLS)");
}

QString ONF_StepClassifyGround::detailledDescription() const
{
    return tr("Cette étape permet de séparer les points Sol et Végétation"
              "<ul>"
              "<li>Une grille Zmin est créée à la <b>résolution</b> spécifiée</li>"
              "<li>La densité de points situés entre Zmin et (Zmin + <b>épaisseur du sol</b>) est calculée pour chaque case</li>"
              "<li>La valeur NA est affectée à toute case dont la densité est inférieure à la <b>densité minimum</b></li>"
              "<li>Un test de cohérence des Zmin restants est réalisé pour chaque case sur le <b>voisinage</b> spécifié (nombre de cases). La valeur NA est affectée aux cases incohérentes</li>"
              "</ul>");
}

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

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

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

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

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

void ONF_StepClassifyGround::declareInputModels(CT_StepInModelStructureManager& manager)
{     
    manager.addResult(_inResult, tr("Scène(s)"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scène(s)"));
}

void ONF_StepClassifyGround::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Résolution de la grille :"), "cm", 1, 1000, 0, _gridsize, 100);
    postInputConfigDialog->addDouble(tr("Epaisseur du sol :"), "cm", 1, 100, 0, _groundwidth, 100);
    postInputConfigDialog->addBool(tr("Filtrage selon la densité"), "", "", _filterByDensity);
    postInputConfigDialog->addDouble(tr("Densité minimum :"), "pts/m2", 0, 99999999, 2, _min_density);
    postInputConfigDialog->addBool(tr("Filtrage selon le voisinnage"), "", "", _filterByNeighourhoud);
    postInputConfigDialog->addInt(tr("Voisinage (points isolés) :"), "Cases", 1, 99999999, _dist);
    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addBool(tr("Conserver les résultats intermédiaires :"), "", "", _debug);
}

void ONF_StepClassifyGround::declareOutputModels(CT_StepOutModelStructureManager& manager)
{   
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outSceneVeg, tr("Points végétation"));
    manager.addItem(_inGroup, _outSceneGround, tr("Points sol"));

    if (_debug)
    {
        manager.addResult(m_hOutResultDTM, tr("Résultats intermédiaires"));
        manager.setRootGroup(m_hOutResultDTM, m_hOutRootGroupDTM);
        manager.addItem(m_hOutRootGroupDTM, m_hOutDTM, tr("MNT (Zmin)"));
        manager.addItem(m_hOutRootGroupDTM, m_hOutDensity, tr("Densité pts sol"));
    }
}

void ONF_StepClassifyGround::compute()
{

    // Determination de l'emprise du MNT, à partir de la bounding box des scènes d'entrée
    double minX = std::numeric_limits<double>::max();
    double minY = std::numeric_limits<double>::max();
    double minZ = std::numeric_limits<double>::max();
    double maxX = -std::numeric_limits<double>::max();
    double maxY = -std::numeric_limits<double>::max();
    double maxZ = -std::numeric_limits<double>::max();

    int nSc = 1;
    for (const CT_AbstractItemDrawableWithPointCloud* scene : _inScene.iterateOutputs(_inResult))
    {
        if (isStopped()) {return;}

        const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();
        size_t n_points = pointCloudIndex->size();

        if (n_points > 0)
        {
            if (scene->minX() < minX) {minX = scene->minX();}
            if (scene->minY() < minY) {minY = scene->minY();}
            if (scene->minZ() < minZ) {minZ = scene->minZ();}
            if (scene->maxX() > maxX) {maxX = scene->maxX();}
            if (scene->maxY() > maxY) {maxY = scene->maxY();}
            if (scene->maxZ() > maxZ) {maxZ = scene->maxZ();}
        }
        PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène d'entrée %2 comporte %1 points.")).arg(n_points).arg(nSc++));
    }

    // Creation des rasters
    int n_mntX = int(abs((maxX - minX)/_gridsize)) + 2;
    int n_mntY = int(abs((maxY - minY)/_gridsize)) + 2;
    size_t tab_mnt_size = size_t(n_mntX)*size_t(n_mntY);

    PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Grille MNT à créer : %1 lignes sur %2 colonnes").arg(n_mntY).arg(n_mntX));

    CT_Image2D<float>* mnt = new CT_Image2D<float>(minX, minY, n_mntX, n_mntY, _gridsize, minZ, -9999, -9999);
    CT_Image2D<int>* densite = new CT_Image2D<int>(minX, minY, n_mntX, n_mntY, _gridsize, minZ - 1, -9999, 0);

    // Création MNT (version Zmin) + MNS
    for (const CT_AbstractItemDrawableWithPointCloud* scene : _inScene.iterateOutputs(_inResult))
    {
        const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();

        CT_PointIterator itP(pointCloudIndex);
        while(itP.hasNext())
        {
            if (isStopped()) {return;}

            const CT_Point &point =itP.next().currentPoint();
            mnt->setMinValueAtCoords(point(0), point(1), float(point(2)));
        }
    }

    size_t cpt = 0;
    // Progression Etape 1
    setProgress(20);
    PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Grille Zmin créée"));

    // Creation raster densité points sol (sur la base de Zmin + _groundWidth)
    for (const CT_AbstractItemDrawableWithPointCloud* scene : _inScene.iterateOutputs(_inResult))
    {
        const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();

        CT_PointIterator itP(pointCloudIndex);
        while(itP.hasNext())
        {
            if (isStopped()) {return;}

            const CT_Point &point =itP.next().currentPoint();
            float value = mnt->valueAtCoords(point(0), point(1)) + float(_groundwidth);
            if (point(2) < double(value))
            {
                densite->addValueAtCoords(point(0), point(1), 1);
            }
        }
    }

    // Progression Etape 2
    setProgress(40);
    PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Filtrage sur la densité terminé"));

    double m_inDensity = _min_density * (_gridsize*_gridsize);

    if (_filterByDensity || _filterByNeighourhoud)
    {
        // Test de cohérence de voisinnage
        for (int xx = 0 ; xx < n_mntX ; ++xx)
        {
            for (int yy = 0 ; yy < n_mntY ; ++yy)
            {
                float value = mnt->value(xx, yy);

                if (!qFuzzyCompare(value, mnt->NA()))
                {
                    if (_filterByDensity && densite->value(xx,yy) < m_inDensity)
                    {
                        mnt->setValue(xx, yy, mnt->NA());
                    } else if (_filterByNeighourhoud)
                    {
                        QList<float> neighbours = mnt->neighboursValues(xx, yy, _dist, false, CT_Image2D<float>::CM_DropCenter);

                        std::sort(neighbours.begin(), neighbours.end());

                        int size_neighbours = neighbours.size();
                        if (size_neighbours > 0)
                        {
                            int med_ind = int(size_neighbours/2);
                            float median = neighbours.at(med_ind);
                            float val_test = std::min(fabs(neighbours.last() - median), fabs(neighbours.first()  - median));

                            if (fabs(value - median) > (val_test*5))
                            {
                                mnt->setValue(xx, yy, mnt->NA());
                            } else
                            {
                                val_test = neighbours.first();

                                if (double(fabs(value - val_test)) > (_dist * _gridsize))
                                {
                                    mnt->setValue(xx, yy, mnt->NA());
                                }
                            }
                        } else {
                            mnt->setValue(xx, yy, mnt->NA());
                        }
                    }
                }

                // Progression Etape 3
                setProgress(float((cpt++/tab_mnt_size)*20.0 + 40.0));
            }
        }

        setProgress(60.0f);
        PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Test de cohérence de voisinnage terminé"));
    }

    CT_PointCloudIndexVector *tab_sol_index = nullptr;
    CT_PointCloudIndexVector *tab_veg_index = nullptr;

    int nscenes = 0;
    for (CT_StandardItemGroup* group : _inGroup.iterateOutputs(_inResult))
    {
        for (const CT_AbstractItemDrawableWithPointCloud* scene : group->singularItems(_inScene))
        {
            if (isStopped()) {return;}

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

            CT_PointIterator itP(pointCloudIndex);
            while(itP.hasNext() && !isStopped())
            {
                const CT_Point &point = itP.next().currentPoint();
                size_t index = itP.currentGlobalIndex();

                float value = mnt->valueAtCoords(point(0), point(1)) + float(_groundwidth);

                if (!qFuzzyCompare(value, mnt->NA()) && point(2) < double(value))
                {
                    if (tab_sol_index == nullptr)
                    {
                        tab_sol_index = new CT_PointCloudIndexVector();
                        tab_sol_index->setSortType(CT_AbstractCloudIndex::NotSorted);
                    }
                    tab_sol_index->addIndex(index);

                } else {
                    if (tab_veg_index == nullptr)
                    {
                        tab_veg_index = new CT_PointCloudIndexVector();
                        tab_veg_index->setSortType(CT_AbstractCloudIndex::NotSorted);
                    }
                    tab_veg_index->addIndex(index);
                }
            }

            setProgress(80);

            size_t numberOfGroundPoints = 0;
            size_t numberOfVegetationPoints = 0;

            if (tab_sol_index != nullptr)
            {
                tab_sol_index->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder);
                numberOfGroundPoints = tab_sol_index->size();

                if (numberOfGroundPoints > 0)
                {
                    // creation de la scene sol
                    CT_Scene *outSceneGround = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(tab_sol_index));
                    outSceneGround->updateBoundingBox();
                    group->addSingularItem(_outSceneGround, outSceneGround);
                }
            }

            if (tab_veg_index != nullptr)
            {
                tab_veg_index->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder);
                numberOfVegetationPoints = tab_veg_index->size();

                if (numberOfVegetationPoints > 0)
                {
                    // creation de la scene vegetation
                    CT_Scene *outSceneVegetation = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(tab_veg_index));
                    outSceneVegetation->updateBoundingBox();
                    group->addSingularItem(_outSceneVeg, outSceneVegetation);
                }
            }

            PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("Scène %3 : Création des scènes sol (%1 points) et végétation (%2 points) terminée")).arg(numberOfGroundPoints).arg(numberOfVegetationPoints).arg(nscenes++));
            tab_sol_index = nullptr;
            tab_veg_index = nullptr;
        }
    }

    // Progression Etape 4
    setProgress(90);

    if (_debug)
    {
        for(CT_ResultGroup* result : m_hOutResultDTM.iterateOutputs())
        {
            // ajout du raster MNT
            CT_StandardItemGroup *outGroupMNT = new CT_StandardItemGroup();
            result->addRootGroup(m_hOutRootGroupDTM, outGroupMNT);

            outGroupMNT->addSingularItem(m_hOutDTM, mnt);
            mnt->computeMinMax();

            // ajout du raster Densité
            outGroupMNT->addSingularItem(m_hOutDensity, densite);
            densite->computeMinMax();

            for (size_t index = 0 ; index < densite->nCells() ; index++)
            {
                if (densite->valueAtIndex(index) == densite->NA()) {densite->setValueAtIndex(index, 0);}
            }
        }
    } else {
        delete mnt;
        delete densite;
    }

    setProgress(100.0f);
}