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

#include "ct_log/ct_logmanager.h"

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/types_c.h"


#define EPSILON 0.000001

ONF_StepRemoveHighGroundPoints::ONF_StepRemoveHighGroundPoints() : SuperClass()
{
    _gridsize   = 1.0;
    _nbErosion = 2;
    _deltaHMax = 2.0;
}

QString ONF_StepRemoveHighGroundPoints::description() const
{
    return tr("Supprimer les points végétation classés sols");
}

QString ONF_StepRemoveHighGroundPoints::detailledDescription() const
{
    return tr("Cette étape permet de filter les points sol qui sont en fait des points végétation dans des zones où il n'y a pas de points sol.");
}

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

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

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

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

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

void ONF_StepRemoveHighGroundPoints::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Points sol"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Points sol"));
}

void ONF_StepRemoveHighGroundPoints::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Résolution du raster :"), "cm", 1, 1000, 0, _gridsize, 100);
    postInputConfigDialog->addInt(tr("Nombre d'érosions :"), "", 1, 10, _nbErosion);
    postInputConfigDialog->addDouble(tr("DeltaH maximum :"), "m", 1, 1000, 0, _deltaHMax);
}

void ONF_StepRemoveHighGroundPoints::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outSceneGround, tr("Sol (filtré)"));

    manager.addItem(_inGroup, _outMinZ, tr("MinZ"));
    manager.addItem(_inGroup, _outMinZ2, tr("MinZ2"));
    manager.addItem(_inGroup, _outMinZ3, tr("MinZ3"));
    manager.addItem(_inGroup, _outMask01, tr("Mask01"));
    manager.addItem(_inGroup, _outMask02, tr("Mask02"));
    manager.addItem(_inGroup, _outMask03, tr("Mask03"));
    manager.addItem(_inGroup, _outMask04, tr("Mask04"));
}

void ONF_StepRemoveHighGroundPoints::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)
            {
                // fit bounding box
                double minX = (std::floor(inScene->minX() - 1) / _gridsize) * _gridsize;
                double minY = (std::floor(inScene->minY() - 1) / _gridsize) * _gridsize;
                double maxX = std::ceil(inScene->maxX()*100.0) / 100.0;
                double maxY = std::ceil(inScene->maxY()*100.0) / 100.0;

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

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

                // create rasters
                CT_Image2D<float>* minZ = CT_Image2D<float>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);
                CT_Image2D<float>* minZ2 = CT_Image2D<float>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);
                CT_Image2D<float>* minZ3 = CT_Image2D<float>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, inScene->minZ(), -9999, -9999);

                CT_Image2D<quint8>* mask01 = CT_Image2D<quint8>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, 0, 0, 0);
                CT_Image2D<quint8>* mask02 = CT_Image2D<quint8>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, 0, 0, 0);
                CT_Image2D<quint8>* mask03 = CT_Image2D<quint8>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, 0, 0, 0);
                CT_Image2D<quint8>* mask04 = CT_Image2D<quint8>::createImage2DFromXYCoords(minX, minY, maxX, maxY, _gridsize, 0, 0, 0);

                // Create Zmin raster
                CT_PointIterator itP(pointCloudIndex);
                while(itP.hasNext() && !isStopped())
                {
                    const CT_Point &point =itP.next().currentPoint();
                    minZ->setMinValueAtCoords(point(0), point(1), static_cast<float>(point(2)));
                    minZ2->setMinValueAtCoords(point(0), point(1), static_cast<float>(point(2)));
                    minZ3->setMinValueAtCoords(point(0), point(1), static_cast<float>(point(2)));
                    mask01->setValueAtCoords(point(0), point(1), 1);
                    mask02->setValueAtCoords(point(0), point(1), 1);
                }

                setProgress(20.0f);

                cv::Mat_<quint8>& mask02Mat2 = mask02->getMat();

                // Create mask for potential problematic pixels
                for (int i = 0 ; i < _nbErosion ; i++)
                {
                    cv::erode(mask02Mat2, mask02Mat2, cv::getStructuringElement(cv::MORPH_RECT, cv::Size2d(3,3)));
                }

                for (int i = 0 ; i < _nbErosion ; i++)
                {
                    cv::dilate(mask02Mat2, mask02Mat2, cv::getStructuringElement(cv::MORPH_RECT, cv::Size2d(3,3)));
                }

                for (size_t i = 0 ; i < mask03->nCells() ; i++)
                {
                    if ((mask01->valueAtIndex(i) == 1) && (mask02->valueAtIndex(i) == 0))
                    {
                        mask03->setValueAtIndex(i, 1);
                        minZ2->setValueAtIndex(i, minZ2->NA());
                        minZ3->setValueAtIndex(i, minZ3->NA());
                    }
                }

                setProgress(40.0f);

                // Create extrapolated zMin raster (successive dilatations)
                bool filled = true;
                while (filled)
                {
                    filled = false;

                    for (int yy = 0; yy < minZ3->ydim() ; yy++)
                    {
                        for (int xx = 0; xx < minZ3->xdim() ; xx++)
                        {
                            if (qFuzzyCompare(minZ3->value(xx, yy), minZ3->NA()))
                            {
                                float moy = 0;
                                float nn = 0;

                                float val = minZ2->value(xx - 1, yy - 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}
                                val = minZ2->value(xx - 1, yy);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}
                                val = minZ2->value(xx - 1, yy + 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}

                                val = minZ2->value(xx, yy - 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}
                                val = minZ2->value(xx, yy + 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}

                                val = minZ2->value(xx + 1, yy - 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}
                                val = minZ2->value(xx + 1, yy);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}
                                val = minZ2->value(xx + 1, yy + 1);
                                if (!qFuzzyCompare(val, minZ2->NA())) {moy += val; ++nn;}

                                if (nn > 0)
                                {
                                    minZ3->setValue(xx, yy, moy / nn);
                                    filled = true;
                                }
                            }
                        }
                    }

                    for (size_t i = 0 ; i < minZ2->nCells() ; i++)
                    {
                        minZ2->setValueAtIndex(i, minZ3->valueAtIndex(i));
                    }
                }

                setProgress(60.0f);

                // filter cells with too important deltaH
                for (size_t i = 0 ; i < mask04->nCells() ; i++)
                {
                    if ((mask03->valueAtIndex(i) == 1))
                    {
                        float deltaH = minZ->valueAtIndex(i) - minZ3->valueAtIndex(i);
                        if (deltaH > _deltaHMax)
                        {
                            mask04->setMaxValueAtIndex(i, 1);
                        }
                    }
                }

                setProgress(70.0f);

                // Filter ground points using final mask
                CT_PointCloudIndexVector *filteredGroundCloud = new CT_PointCloudIndexVector();
                filteredGroundCloud->setSortType(CT_AbstractCloudIndex::NotSorted);

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

                    if (mask04->valueAtCoords(point(0), point(1)) != 1)
                    {
                        filteredGroundCloud->addIndex(index);
                    }
                }

                setProgress(90.0f);


                // Create ground points scene
                if (pointCloudIndex->size() > 0)
                {
                    // filteredGroundCloud->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder); // commented for best performance
                    CT_Scene *outSceneGround = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(filteredGroundCloud));
                    outSceneGround->updateBoundingBox();
                    group->addSingularItem(_outSceneGround, outSceneGround);
                } else {
                    delete pointCloudIndex;
                }


                // add rasters to results
                minZ->computeMinMax();
                minZ2->computeMinMax();
                minZ3->setMinMax(minZ->dataMin(),minZ->dataMax());
                mask01->setMinMax(0,1);
                mask02->setMinMax(0,1);
                mask03->setMinMax(0,1);
                mask04->setMinMax(0,1);
                group->addSingularItem(_outMinZ, minZ);
                group->addSingularItem(_outMinZ2, minZ2);
                group->addSingularItem(_outMinZ3, minZ3);
                group->addSingularItem(_outMask01, mask01);
                group->addSingularItem(_outMask02, mask02);
                group->addSingularItem(_outMask03, mask03);
                group->addSingularItem(_outMask04, mask04);

//                delete minZ;
//                delete minZ2;
//                delete minZ3;

//                delete mask01;
//                delete mask02;
//                delete mask03;
//                delete mask04;

            }
        }
        setProgress(100.0f);
    }
}