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

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

ONF_StepFilterMaximaByClusterPositions::ONF_StepFilterMaximaByClusterPositions() : SuperClass()
{
}

QString ONF_StepFilterMaximaByClusterPositions::description() const
{
    return tr("Filtrer les maxima à partir de clusters de référence");
}

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

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

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

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

QString ONF_StepFilterMaximaByClusterPositions::URL() const
{
    //return tr("STEP URL HERE");
    return SuperClass::URL(); //by default URL of the plugin
}

CT_VirtualAbstractStep* ONF_StepFilterMaximaByClusterPositions::createNewInstance() const
{
    return new ONF_StepFilterMaximaByClusterPositions();
}

//////////////////// PROTECTED METHODS //////////////////

void ONF_StepFilterMaximaByClusterPositions::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Maxima"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGrp);
    manager.addItem(_inGrp, _inImage, tr("Image (hauteurs)"));
    manager.addItem(_inGrp, _inMaxima, tr("Maxima"));

    manager.addGroup(_inGrp, _inGrpPos);
    manager.addItem(_inGrpPos, _inPosition, tr("Cluster Position"));
}

void ONF_StepFilterMaximaByClusterPositions::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGrp, _filteredMaxima, tr("Maxima filtrés"));

    manager.addItem(_inGrpPos, _attMaximaItem, tr("MaximaID"));
    manager.addItemAttribute(_attMaximaItem, _attMaxima, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_ID), tr("MaximaID"));
    manager.addItemAttribute(_attMaximaItem, _attClusterID, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_ID), tr("PointClusterID"));
}

void ONF_StepFilterMaximaByClusterPositions::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addFileChoice(tr("Rayons de recherche"),CT_FileChoiceButton::OneExistingFile , "Fichier de paramètres (*.*)", _fileNameSearchRadii);
    postInputConfigDialog->addFileChoice(tr("Rayons d'exclusion"),CT_FileChoiceButton::OneExistingFile , "Fichier de paramètres (*.*)", _fileNameExclusionRadii);
}

void ONF_StepFilterMaximaByClusterPositions::readRadii(QString fileName, QMap<double, double> &radii)
{
    QFile parameterFile(fileName);
    if (parameterFile.exists() && parameterFile.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        QTextStream stream(&parameterFile);

        while (!stream.atEnd())
        {
            QString line = stream.readLine();
            if (!line.isEmpty())
            {
                QStringList values = line.split("\t");

                if (values.size() > 1)
                {
                    bool ok1, ok2;
                    double height = values.at(0).toDouble(&ok1);
                    double radius = values.at(1).toDouble(&ok2);

                    if (ok1 && ok2)
                    {
                        radii.insert(height, radius);
                    }
                }
            }
        }
        parameterFile.close();
    }

    if (!radii.contains(0)) {radii.insert(0, radii.first());}
    radii.insert(std::numeric_limits<double>::max(), radii.last());
}

void ONF_StepFilterMaximaByClusterPositions::compute()
{
    QMap<double, double> searchRadii;
    if (_fileNameSearchRadii.size() > 0) {readRadii(_fileNameSearchRadii.first(), searchRadii);}

    QMap<double, double> exclusionRadii;
    if (_fileNameExclusionRadii.size() > 0) {readRadii(_fileNameExclusionRadii.first(), exclusionRadii);}


    for (CT_StandardItemGroup* grp : _inGrp.iterateOutputs(_inResult))
    {
        CT_Image2D<qint32>* maximaIn = const_cast<CT_Image2D<qint32>*>(grp->singularItem(_inMaxima));
        const CT_Image2D<float>* imageIn = grp->singularItem(_inImage);

        if (maximaIn != nullptr)
        {
            // List of positions
            QMultiMap<size_t, const CT_PointCluster*> positions;
            QMap<const CT_PointCluster*, CT_StandardItemGroup*> positionsGroups;

            for (const CT_StandardItemGroup* grpPos : grp->groups(_inGrpPos))
            {
                if (isStopped()) {return;}

                const CT_PointCluster* position = grpPos->singularItem(_inPosition);

                if (position != nullptr)
                {
                    positions.insert(position->pointCloudIndexSize(), position);
                    positionsGroups.insert(position, const_cast<CT_StandardItemGroup*>(grpPos));
                }
            }

            Eigen::Vector2d min;
            maximaIn->getMinCoordinates(min);
            CT_Image2D<qint32>* filteredMaxima = new CT_Image2D<qint32>(min(0), min(1), maximaIn->xdim(), maximaIn->ydim(), maximaIn->resolution(), maximaIn->level(), maximaIn->NA(), 0);
            grp->addSingularItem(_filteredMaxima, filteredMaxima);

            filteredMaxima->getMat().release();
            filteredMaxima->getMat() = maximaIn->getMat().clone();

            setProgress(20);

            // Get maxima coordinates list
            QMultiMap<qint32, Eigen::Vector3d*> maximaCoords;
            QMultiMap<double, qint32> maximaHeights;

            for (int xx = 0 ; xx < maximaIn->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < maximaIn->ydim() ; yy++)
                {
                    qint32 maximaID = maximaIn->value(xx, yy);

                    if (maximaID > 0 && maximaID != maximaIn->NA())
                    {
                        Eigen::Vector3d* coords = new Eigen::Vector3d();
                        if (maximaIn->getCellCenterCoordinates(xx, yy, *coords))
                        {
                            (*coords)(2) = double(imageIn->value(xx, yy));
                            maximaCoords.insert(maximaID, coords);
                            maximaHeights.insert((*coords)(2), maximaID);
                        }
                    }
                }
            }

            setProgress(25);

            // Compute ordered vector of maxima ids
            QList<qint32> validMaxima;

            QMapIterator<double, qint32> itH(maximaHeights);
            itH.toBack();
            while (itH.hasPrevious())
            {
                itH.previous();
                qint32 cl = itH.value();
                if (!validMaxima.contains(cl)) {validMaxima.append(cl);}
            }

            QVector<qint32> orderedMaxima = validMaxima.toVector();
            int mxSize = orderedMaxima.size();
            validMaxima.clear();

            // Create maxima coords vector
            QVector<Eigen::Vector3d> coords(mxSize);
            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 id = orderedMaxima.at(i);

                QList<Eigen::Vector3d*> coordinates = maximaCoords.values(id);
                coords[i] = *(coordinates.at(0));

                // Compute position of the current maxima if more than one pixel
                int size = coordinates.size();
                if (size > 1)
                {
                    for (int j = 1 ; j < size ; j++)
                    {
                        Eigen::Vector3d* pos = coordinates.at(j);
                        coords[i](0) += (*pos)(0);
                        coords[i](1) += (*pos)(1);
                        if ((*pos)(2) > coords[i](2)) {coords[i](2) = (*pos)(2);}
                    }

                    coords[i](0) /= size;
                    coords[i](1) /= size;
                }
            }

            setProgress(30);

            // affect clusters positions to maxima
            QMap<qint32, const CT_PointCluster*> maximaClusters;

            QMapIterator<size_t, const CT_PointCluster*> itPos(positions);
            itPos.toBack();
            while (itPos.hasPrevious())
            {
                itPos.previous();
                const CT_PointCluster* position = itPos.value();
                const CT_PointClusterBarycenter &barycenter = position->getBarycenter();

                bool found = false;
                for (int i = 0 ; i < mxSize && !found; i++)
                {
                    qint32 id = orderedMaxima.at(i);
                    if (id > 0)
                    {
                        double x = coords[i](0);
                        double y = coords[i](1);
                        double z = coords[i](2);
                        double radius = getRadius(z, searchRadii);

                        double dist = sqrt(pow(barycenter.x() - x, 2) + pow(barycenter.y() - y, 2));

                        if (dist < radius)
                        {
                            found = true;
                            maximaClusters.insert(id, position);
                        }
                    }
                }

                if (!found)
                {
                    CT_StandardItemGroup *grp = positionsGroups.value(position);
                    if (grp != nullptr)
                    {
                        CT_ItemAttributeList *item = new CT_ItemAttributeList();
                        grp->addSingularItem(_attMaximaItem, item);
                        item->addItemAttribute(_attMaxima, new CT_StdItemAttributeT<qint32>(CT_AbstractCategory::DATA_ID, 0));
                        item->addItemAttribute(_attClusterID, new CT_StdItemAttributeT<quint64>(CT_AbstractCategory::DATA_ID, position->id()));
                    }
                }
            }

            // For each radius, test others
            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 id = orderedMaxima.at(i);

                if (id > 0)
                {
                    double x = coords[i](0);
                    double y = coords[i](1);
                    double z = coords[i](2);
                    double radius = getRadius(z, exclusionRadii);

                    // detect the maximum to remove
                    for (int j = 0 ; j < mxSize ; j++)
                    {
                        qint32 idTested = orderedMaxima.at(j);

                        if (i != j && idTested > 0 && !maximaClusters.contains(idTested))
                        {
                            double dist = sqrt(pow(x - coords[j](0), 2) + pow(y - coords[j](1), 2));
                            if (dist < radius)
                            {
                                orderedMaxima[j] = 0;
                            }
                        }
                    }
                }

                setProgress(29.0f*float(i) / float(mxSize) + 30.0f);
            }

            setProgress(60);

            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 cl = orderedMaxima.at(i);
                if (cl > 0)
                {
                    validMaxima.append(cl);
                }
            }

            setProgress(70);

            QMap<qint32, qint32> newIds;
            qint32 cpt = 1;
            // effectively delete toRemove maximum and numbers them in a continuous way
            for (int xx = 0 ; xx < filteredMaxima->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < filteredMaxima->ydim() ; yy++)
                {
                    qint32 maximaID = filteredMaxima->value(xx, yy);

                    if (maximaID > 0)
                    {
                        if (validMaxima.contains(maximaID))
                        {
                            qint32 newId = newIds.value(maximaID, 0);
                            if (newId == 0)
                            {
                                newId = cpt++;
                                newIds.insert(maximaID, newId);

                                const CT_PointCluster *position = maximaClusters.value(maximaID, nullptr);
                                if (position != nullptr)
                                {
                                    CT_StandardItemGroup *grpPos = positionsGroups.value(position);
                                    if (grpPos != nullptr)
                                    {
                                        CT_ItemAttributeList *item = new CT_ItemAttributeList();
                                        grpPos->addSingularItem(_attMaximaItem, item);
                                        item->addItemAttribute(_attMaxima, new CT_StdItemAttributeT<qint32>(CT_AbstractCategory::DATA_ID, newId));
                                        item->addItemAttribute(_attClusterID, new CT_StdItemAttributeT<quint64>(CT_AbstractCategory::DATA_ID, position->id()));
                                    }
                                }
                            }
                            filteredMaxima->setValue(xx, yy, newId);
                        } else {
                            filteredMaxima->setValue(xx, yy, 0);
                        }
                    }
                }
            }

            newIds.clear();
            setProgress(90);

            filteredMaxima->computeMinMax();

            qDeleteAll(maximaCoords.values());

            positions.clear();
            positionsGroups.clear();

            setProgress(99);
        }
    }
    setProgress(100);
}

double ONF_StepFilterMaximaByClusterPositions::getRadius(double height, const QMap<double, double> &radii)
{
    double radius = 0;
    bool stop = false;
    QMapIterator<double, double> it(radii);
    while (it.hasNext() && !stop)
    {
        it.next();
        double h = it.key();
        double r = it.value();

        if (height >= h)
        {
            radius = r;
        } else {
            stop = true;
        }
    }

    return radius;
}