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

#include "ct_math/ct_mathpoint.h"

//Inclusion of actions
#include "actions/onf_actionmodifyclustersgroups02.h"

#include <QtConcurrent/QtConcurrentMap>

#include <QMessageBox>

ONF_StepMergeClustersFromPositions02::ONF_StepMergeClustersFromPositions02() : SuperClass()
{
    _interactiveMode = true;
    _hRef = 1.3;
    _dMax = 2.0;
    m_doc = nullptr;

    setManual(true);
}

QString ONF_StepMergeClustersFromPositions02::description() const
{
    return tr("Isoler les houppiers à partir de positions (et de clusters)");
}

QString ONF_StepMergeClustersFromPositions02::detailledDescription() const
{
    return tr("Cette étape permet de générer une scène pour chaque positions 2D fournie.<br>"
              "A partir de chaque position, les clusters fournis en entrée sont agrégés pas à pas au plus proche voisin.<br>"
              "<br>"
              "Ensuite une action interactive permet de corriger cette attribution automatique.");
}

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

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

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

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

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

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

void ONF_StepMergeClustersFromPositions02::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inRclusters, "", "", true);
    manager.setZeroOrMoreRootGroup(_inRclusters, _inZeroOrMoreRootGroupClusters);
    manager.addGroup(_inZeroOrMoreRootGroupClusters, _inGrpClusters, tr("Groupe"));
    manager.addItem(_inGrpClusters, _inCluster, tr("Cluster"));

    manager.addResult(_inRPos, tr("Positions 2D"), "", true);
    manager.setZeroOrMoreRootGroup(_inRPos, _inZeroOrMoreRootGroupPos);
    manager.addGroup(_inZeroOrMoreRootGroupPos, _inGrpPos, tr("Groupe"));
    manager.addItem(_inGrpPos, _inPosition, tr("Position 2D"));

    manager.addResult(_inRMNT, tr("MNT (Raster)"), "", true);
    manager.setZeroOrMoreRootGroup(_inRMNT, _inZeroOrMoreRootGroupMNT);
    manager.addGroup(_inZeroOrMoreRootGroupMNT, _inGrpMNT, tr("Groupe"));
    manager.addItem(_inGrpMNT, _inMNT, tr("Modèle Numérique de Terrain"));
}

void ONF_StepMergeClustersFromPositions02::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inRPos);
    manager.addItem(_inGrpPos, _outScene, tr("Scène segmentée"));
    manager.addItemAttribute(_outScene, _outSceneZRef, CT_AbstractCategory::DATA_Z, tr("Z MNT"));
}

void ONF_StepMergeClustersFromPositions02::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Hauteur de référence"), "", -99999, 99999, 2, _hRef);
    postInputConfigDialog->addDouble(tr("Distance maximum entre clusters d'un même groupe"), "", 0, 99999, 2, _dMax);
    postInputConfigDialog->addBool(tr("Correction interactive ?"), "", "", _interactiveMode);
}

void ONF_StepMergeClustersFromPositions02::compute()
{
    setManual(_interactiveMode);

    // Récupération du MNT
    const CT_Image2D<float>* mnt = _inMNT.firstInput(_inRMNT);

    QMap<const CT_Point2D*, double> positionsZRef;

    // Création de la liste des positions 2D
    for(CT_StandardItemGroup* grpPos : _inGrpPos.iterateOutputs(_inRPos))
    {
        if(isStopped())
            break;

        const CT_Point2D* position = grpPos->singularItem(_inPosition);
        if (position != nullptr)
        {
            CT_PointCloudIndexVector* cloudIndexVector = new CT_PointCloudIndexVector();
            cloudIndexVector->setSortType(CT_AbstractCloudIndex::NotSorted);
            _positionsData.insert(position, QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* >(cloudIndexVector, new QList<const CT_PointCluster*>()));

            double mntVal = 0;
            if (mnt != nullptr) {mntVal = mnt->valueAtCoords(position->centerX(), position->centerY());}
            if (mntVal == mnt->NA()) {mntVal = 0;}

            positionsZRef.insert(position, mntVal + _hRef);
        }
    }

    setProgress(1);

    // Création de la correspondance clusters / groupes
    for(const CT_StandardItemGroup* grp_inGrpClusters : _inGrpClusters.iterateInputs(_inRclusters))
    {
        if(isStopped())
            break;
        const CT_PointCluster* cluster = grp_inGrpClusters->singularItem(_inCluster);
        if (cluster != nullptr)
        {
            _clustersGroups.insert(cluster, grp_inGrpClusters);
        }
    }

    setProgress(3);

    // Création des correspondance clusters / positions
    // Phase 1 : recherche du cluster le plus proche de chaque position
    QList<const CT_PointCluster*> clustersPhase1;

    QMutableMapIterator<const CT_Point2D*, QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > > itPos(_positionsData);
    while (itPos.hasNext() && !isStopped())
    {
        itPos.next();
        Eigen::Vector3d posCenter = itPos.key()->centerCoordinate();
        posCenter(2) = positionsZRef.value(itPos.key());

        const CT_PointCluster* bestCluster = nullptr;
        double minDist = std::numeric_limits<double>::max();

        for(const CT_PointCluster* cluster : _inCluster.iterateInputs(_inRclusters))
        {
            if(isStopped())
                break;

            if (!clustersPhase1.contains(cluster))
            {
                const Eigen::Vector3d &center = cluster->centerCoordinate();
                double distance = squareDist(posCenter, center);
                if (distance <= _dMax && distance < minDist)
                {
                    minDist = distance;
                    bestCluster = cluster;
                }
            }
        }

        if (bestCluster != nullptr)
        {
            clustersPhase1.append(bestCluster);
            QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > &pair = itPos.value();
            pair.second->append(bestCluster);
        }
    }

    setProgress(5);

    // Création des correspondance clusters / positions
    // Phase 2 : création de la map des distances
    QList<ClusterData> clusterDataList;

    for(const CT_PointCluster* cluster : _inCluster.iterateInputs(_inRclusters))
    {
        if(isStopped())
            break;

        if (!clustersPhase1.contains(cluster))
        {
            const Eigen::Vector3d &center = cluster->centerCoordinate();
            const CT_PointCluster* bestCluster = nullptr;
            const CT_Point2D* bestPosition = nullptr;
            double minDist = std::numeric_limits<double>::max();

            QMapIterator<const CT_Point2D*, QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > > itPos(_positionsData);
            while (itPos.hasNext())
            {
                itPos.next();
                const QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > &pair = itPos.value();

                if (pair.second->size() > 0)
                {
                    const CT_PointCluster* clusterForPos = pair.second->first();
                    const Eigen::Vector3d &clusterForPosCenter = clusterForPos->centerCoordinate();

                    double distance = squareDist(clusterForPosCenter, center);
                    if (distance < minDist)
                    {
                        minDist = distance;
                        bestPosition = itPos.key();
                        bestCluster = clusterForPos;
                    }
                }
            }

            if (bestPosition != nullptr)
            {
                clusterDataList.append(ClusterData(cluster, minDist, bestPosition, bestCluster));
            }
        }
    }

    setProgress(10);

    // Création des correspondance clusters / positions
    // Phase 3 : aggrégation des clusters par proximité relative
    int cpt = -1;
    int nbClust = clusterDataList.size();
    double distance = 0;
    bool changed = true;

    while (!clusterDataList.isEmpty() && !isStopped())
    {
        if (changed)
        {
            std::sort(clusterDataList.begin(), clusterDataList.end());
            changed = false;
        }

        ClusterData clusterData = clusterDataList.takeFirst();

        if (clusterData._distance > _dMax)
        {
            _trash.append(clusterData._cluster);
        } else {
            // Affectation du cluster à la position la plus proche
            const QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > &pair = _positionsData.value(clusterData._position);
            pair.second->append(clusterData._cluster);

            _clusterToCluster.insert(clusterData._positionCluster, clusterData._cluster);

            // Mise à jour de la map des distances à l'aide de ce cluster
            for (int i = 0 ; i < clusterDataList.size() ; i++)
            {
                ClusterData& clusterD = clusterDataList[i];

                distance = squareDist(clusterData.center(), clusterD.center());
                if (distance < _dMax && distance < clusterD._distance)
                {
                    clusterD._position = clusterData._position;
                    clusterD._distance = distance;
                    clusterD._positionCluster = clusterData._cluster;
                    changed = true;
                }
            }
        }

        if (++cpt % 500 == 0) {setProgress(10.0f + (float(cpt) / float(nbClust))*70.0f);}
    }

    setProgress(80);

    // Début de la partie interactive
    if (_interactiveMode)
    {
        m_doc = nullptr;

        m_status = 0;
        requestManualMode();

        m_status = 1;
        requestManualMode();
    }
    // Fin de la partie interactive

    // Ajout des points aux nuages d'indices
    QList<QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > > cloudIndices = _positionsData.values();

    QFuture<void> futur = QtConcurrent::map(cloudIndices, ONF_StepMergeClustersFromPositions02::addPointsToScenes);
    int progressMin = futur.progressMinimum();
    int progressTotal = futur.progressMaximum() - futur.progressMinimum();
    while (!futur.isFinished())
    {
        if(isStopped())
            futur.cancel();

        setProgress(80.0f + 19.0f*(futur.progressValue() - progressMin)/progressTotal);
    }

    setProgress(99);

    // Création des scènes
    for(CT_StandardItemGroup* grpPos : _inGrpPos.iterateOutputs(_inRPos))
    {
        if(isStopped())
            break;

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

        if (position != nullptr)
        {
            double zRef = positionsZRef.value(position);

            const QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > &pair = _positionsData.value(position);
            CT_PointCloudIndexVector* cloudIndexVector = pair.first;

            if (cloudIndexVector->size() > 0)
            {
                cloudIndexVector->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder);

                CT_Scene* scene = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(cloudIndexVector));
                scene->updateBoundingBox();
                grpPos->addSingularItem(_outScene, scene);

                scene->addItemAttribute(_outSceneZRef, _outSceneZRef.createInstance(CT_AbstractCategory::DATA_Z, (zRef - _hRef)));
            } else {
                delete cloudIndexVector;
            }

            pair.second->clear();
            delete pair.second;
        }
    }

    setProgress(100);
}

void ONF_StepMergeClustersFromPositions02::addPointsToScenes(QPair<CT_PointCloudIndexVector*, QList<const CT_PointCluster*>* > &pair)
{
    CT_PointCloudIndexVector* cloudIndexVector = pair.first;
    QList<const CT_PointCluster*> *list = pair.second;

    for (int i = 0 ; i < list->size() ; i++)
    {
        const CT_AbstractCloudIndex* clIndex = list->at(i)->pointCloudIndex();
        for (size_t p = 0 ; p < clIndex->size() ; p++)
        {
            cloudIndexVector->addIndex(clIndex->constIndexAt(p));
        }
    }
}

void ONF_StepMergeClustersFromPositions02::initManualMode()
{
    // create a new 3D document
    if(m_doc == nullptr)
        m_doc = guiContext()->documentManager()->new3DDocument();

    m_itemDrawableSelected.clear();
    m_doc->removeAllItemDrawable();

    // set the action (a copy of the action is added at all graphics view, and the action passed in parameter is deleted)
    m_doc->setCurrentAction(new ONF_ActionModifyClustersGroups02(&_positionsData, &_clusterToCluster, &_trash));

    QMessageBox::information(nullptr, tr("Mode manuel"), tr("Bienvenue dans le mode manuel de cette "
                                                         "étape de filtrage."), QMessageBox::Ok);
}

void ONF_StepMergeClustersFromPositions02::useManualMode(bool quit)
{
    if(m_status == 0)
    {
        if(quit)
        {
            m_itemDrawableSelected = m_doc->getSelectedItemDrawable();
        }
    }
    else if(m_status == 1)
    {
        if(!quit)
        {
            m_doc = nullptr;

            quitManualMode();
        }
    }
}