/****************************************************************************
 Copyright (C) 2010-2012 the Association de Recherche Technologie et Sciences (ARTS), Ecole Nationale Supérieure d'Arts et Métiers (ENSAM), Cluny, France.
                         All rights reserved.

 Contact : Michael.KREBS@ENSAM.EU

 Developers : Michaël KREBS (ARTS/ENSAM)

 This file is part of PluginARTSFREE library.

 PluginARTSFREE 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.

 PluginARTSFREE 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 PluginARTSFREE.  If not, see <http://www.gnu.org/licenses/lgpl.html>.
*****************************************************************************/

#include "arfr_stephorizontalclustering04.h"

#include "ct_result/model/inModel/ct_inresultmodelgroup.h"
#include "ct_result/model/outModel/ct_outresultmodelgroup.h"
#include "ct_result/ct_resultgroup.h"

#include "ct_itemdrawable/ct_scene.h"
#include "ct_itemdrawable/ct_pointcluster.h"
#include "ct_itemdrawable/tools/ct_standardcontext.h"

#include "ct_math/ct_mathpoint.h"
#include "ct_math/ct_mathboundingshape.h"

#include "ct_pointcloudindex/abstract/ct_abstractpointcloudindex.h"
#include "ct_iterator/ct_pointiterator.h"

#include "ct_global/ct_context.h"

#include "ct_view/ct_stepconfigurabledialog.h"

#if QT_VERSION < QT_VERSION_CHECK(5,0,0)
#include <QtConcurrentMap>
#else
#include <QtConcurrent/QtConcurrentMap>
#endif

#include <QFutureWatcher>

#define DEF_SearchInResult "r"
#define DEF_SearchInGroup   "p"
#define DEF_SearchInScene   "sc"

#define DEF_SearchOutLayerGroup  "gly"
#define DEF_SearchOutClusterGroup  "gcl"
#define DEF_SearchOutCluster  "sc"
#define DEF_SearchOutResult "r"

ARFR_StepHorizontalClustering04::ARFR_StepHorizontalClustering04(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _maxSearchRadiusInMeters = 0.03;
    _thicknessInMeters = 0.02;
}

QString ARFR_StepHorizontalClustering04::getStepDescription() const
{
    return tr("Clustering par tranches horizontales (v4)");
}

QString ARFR_StepHorizontalClustering04::getStepDetailledDescription() const
{
    return tr("Cette étape vise à constituer de petits groupes de points aggrégés. "
              "L'idée est d'obtenir, dans le cas de troncs d'arbres des arcs de cercle peu épais.<br>"
              "Pour ce faire, l'étape fonctionne en deux étapes :"
              "<ul>"
              "<li> La scène est découpée en tranches horizontales (Layers) de l' <b>épaisseur</b> choisie</li>"
              "<li> Dans chacune des tranches, les points sont aggrégés en clusters en fonction de leur espacement en (x,y)</li>"
              "</ul>"
              "<br>La <b>distance maximale séparant deux points d'un même groupe</b> est spécifiée en paramètre.");
}

CT_VirtualAbstractStep* ARFR_StepHorizontalClustering04::createNewInstance(CT_StepInitializeData &dataInit)
{
    // crée une copie de cette étape
    return new ARFR_StepHorizontalClustering04(dataInit);
}

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

void ARFR_StepHorizontalClustering04::createInResultModelListProtected()
{
    CT_InResultModelGroup * resultModel = createNewInResultModel(DEF_SearchInResult, tr("Scène(s)"));
    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_SearchInGroup, CT_AbstractItemGroup::staticGetType(), tr("Groupe"));
    resultModel->addItemModel(DEF_SearchInGroup, DEF_SearchInScene, CT_Scene::staticGetType(), tr("Scène à clusteriser"));
}

void ARFR_StepHorizontalClustering04::createPostConfigurationDialog()
{
    CT_StepConfigurableDialog *configDialog = newStandardPostConfigurationDialog();

    configDialog->addDouble(tr("Distance maximum pour intégrer un point à un groupe :"), "cm", 0, 500, 0, _maxSearchRadiusInMeters, 100);
    configDialog->addDouble(tr("Epaisseur des tranches horizontales :"), "cm", 1, 999, 0, _thicknessInMeters, 100);
}

void ARFR_StepHorizontalClustering04::createOutResultModelListProtected()
{   
    CT_OutResultModelGroup *resultModel = createNewOutResultModel(DEF_SearchOutResult, tr("Scène clusterisée"));

    resultModel->setRootGroup(DEF_SearchOutLayerGroup,new CT_StandardItemGroup(), tr("Niveau Z (Grp)"));
    resultModel->addGroupModel(DEF_SearchOutLayerGroup, DEF_SearchOutClusterGroup, new CT_StandardItemGroup(), tr("Cluster (Grp))"));
    resultModel->addItemModel(DEF_SearchOutClusterGroup, DEF_SearchOutCluster, new CT_PointCluster(), tr("Points"));
}

void ARFR_StepHorizontalClustering04::compute()
{
    // recupere les resultats d'entree
    // les resultats sont dans l'ordre qu'on les a demande dans la methode createInResultModelListProtected()
    CT_ResultGroup *inResult = getInputResults().first();
    _outResult = (CT_ResultGroup*) getOutResultList().first();


    int iThickness = 10;
    float thicknessInCm = _thicknessInMeters*100.0;
    float thicknessDivider = thicknessInCm/10.0;

    if(thicknessInCm < 10)
    {
        iThickness = 100;
        thicknessDivider = thicknessInCm;
    }
    else if(thicknessInCm >= 100)
    {
        iThickness = 1;
        thicknessDivider = thicknessInCm/100;
    }

    QMap<float, CT_StandardItemGroup*> mapMultithread;

    CT_ResultItemIterator itScene(inResult, this, DEF_SearchInScene);
    while (itScene.hasNext() && (!isStopped()))
    {
        const CT_Scene *scene = (CT_Scene*)itScene.next();

        if(scene != NULL)
        {
            const CT_AbstractPointCloudIndex *pointCloudIndex = scene->getPointCloudIndex();
            size_t indexSize = pointCloudIndex->size();

            PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène à clusteriser comporte %1 points.")).arg(indexSize));

            size_t i = 0;

            CT_PointIterator itP(pointCloudIndex);

            // Tout d'abord on range tous les points dans diffrentes couches Z d'épaisseur définie par l'utilisateur
            while(itP.hasNext() && (!isStopped()))
            {
                const CT_Point &point = itP.next().currentPoint();
                size_t indexPoint = itP.currentGlobalIndex();

                // calcul de l'index de la couche
                float indexZ = (((int)((point(2)*iThickness)/thicknessDivider))+1) * _thicknessInMeters;

                CT_StandardItemGroup *layer = mapMultithread.value(indexZ, NULL);

                CT_StandardContext *context;
                if(layer == NULL)
                {
                    layer = new CT_StandardItemGroup(DEF_SearchOutLayerGroup, _outResult);
                    context = new CT_StandardContext();
                    context->setStep(this);
                    layer->setContext(context);
                    mapMultithread.insert(indexZ, layer);
                } else {
                    context = (CT_StandardContext*) layer->getContext();
                }

                MyPoint* pt = new MyPoint(point, indexPoint);
                context->add("p", pt);

                // progres de 0 a 25
                setProgress((i*25)/indexSize);

                ++i;
            }
        }
    }


    _sizeMap = mapMultithread.size();

    if (_sizeMap > 0)
    {
        _nLayerFinished = 0;

        QtConcurrent::blockingMap(mapMultithread, &ARFR_StepHorizontalClustering04::staticComputeLayer);

        QMapIterator<float, CT_StandardItemGroup*> it(mapMultithread);

        while(it.hasNext())
        {
            it.next();
            CT_StandardContext* context = ((CT_StandardContext*) it.value()->getContext());
            QList<void*> mypoints = context->get("p");
            int size = mypoints.size();
            for (int i = 0 ; i < size ; i++)
            {
                delete ((MyPoint*) mypoints.at(i));
            }

            delete context;
            it.value()->setContext(NULL);
        }

        it.toFront();

        if(!isStopped())
        {
            while(it.hasNext())
            {
                it.next();
                _outResult->addGroup(it.value());
            }
        }
        else
        {
            qDeleteAll(mapMultithread);
        }
        mapMultithread.clear();

        PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("L'étape a généré %1 couches horizontales.")).arg(_sizeMap));
    }

}

void ARFR_StepHorizontalClustering04::addLayerFinished()
{
    int progress;

    _mutexUpdateProgress.lock();

    ++_nLayerFinished;
    progress = 25 + ((_nLayerFinished*75)/_sizeMap);

    _mutexUpdateProgress.unlock();

    setProgress(progress);
}

void ARFR_StepHorizontalClustering04::staticComputeLayer(CT_StandardItemGroup *layer)
{
    CT_StandardContext* context = (CT_StandardContext*) layer->getContext();
    ARFR_StepHorizontalClustering04* step = (ARFR_StepHorizontalClustering04*) context->step();
    QList<void*> mypoints = context->get("p");

    QList<CT_PointCluster*> listCluster;

    while(!step->isStopped()
          && !mypoints.isEmpty())
    {
        MyPoint *p = (MyPoint*) mypoints.takeFirst();

        bool continueLoop = true;
        QListIterator<CT_PointCluster*> it(listCluster);

        while(continueLoop
              && it.hasNext())
        {
            continueLoop = staticAddPointToPointCluster(*(p->_point), p->_index, *(it.next()), step);
        }

        if(continueLoop)
        {
            CT_PointCluster *pCluster = new CT_PointCluster(DEF_SearchOutCluster, step->_outResult);
            pCluster->addPoint(p->_index);

            listCluster.append(pCluster);

        }
    }

    while(!listCluster.isEmpty()
          && !step->isStopped())
    {
        CT_PointCluster *pClusterToOptimize = listCluster.takeFirst();

        int size = listCluster.size();
        bool continueLoop = true;

        do
        {
            continueLoop = true;
            int j = 0;

            while(continueLoop
                  && (j<size)
                  && !step->isStopped())
            {
                CT_PointCluster *pClusterToVerify = (CT_PointCluster*)listCluster.at(j);

                CT_PointCluster *pClusterMerged = staticCombinePointCluster(pClusterToOptimize->id(), *(step->_outResult), *pClusterToOptimize, *pClusterToVerify, step);

                if(pClusterMerged != NULL)
                {
                    delete pClusterToOptimize;
                    pClusterToOptimize = pClusterMerged;

                    listCluster.removeAt(j);
                    delete pClusterToVerify;

                    --size;
                    continueLoop = false;
                }
                else
                {
                    ++j;
                }
            }

        } while(!continueLoop);

        CT_StandardItemGroup* grp = new CT_StandardItemGroup(DEF_SearchOutClusterGroup, step->_outResult);
        grp->addItemDrawable(pClusterToOptimize);
        layer->addGroup(grp);
    }

    //layer->updateCenter();


    step->addLayerFinished();
}

bool ARFR_StepHorizontalClustering04::staticAddPointToPointCluster(const CT_Point &point,
                                                                 const size_t &pointIndex,
                                                                 CT_PointCluster &pCluster,
                                                                 ARFR_StepHorizontalClustering04 *ptrClass)
{
    Eigen::Vector3d min, max;
    pCluster.getBufferedBoundingBox(ptrClass->_maxSearchRadiusInMeters, ptrClass->_maxSearchRadiusInMeters, 1, min, max);

    if(CT_MathBoundingShape::containsPointIn2D(min, max, point))
    {
        const CT_AbstractPointCloudIndex *pointCloudIndex = ((const CT_PointCluster&)pCluster).getPointCloudIndex();

        if (pointCloudIndex->size() == 0) {return true;}

        CT_PointIterator itP(pointCloudIndex);

        itP.toBack();
        while (itP.hasPrevious())
        {
            const CT_Point &otherPoint = itP.previous().currentPoint();

            if(CT_MathPoint::distance2D(otherPoint, point) < ptrClass->_maxSearchRadiusInMeters)
            {
                pCluster.addPoint(pointIndex);
                return false;
            }
        }
    }
    return true;
}

CT_PointCluster* ARFR_StepHorizontalClustering04::staticCombinePointCluster(int id,
                                                                          CT_AbstractResult &outResult,
                                                                          CT_PointCluster &pClusterToOptimize,
                                                                          CT_PointCluster &pClusterToCombine,
                                                                          ARFR_StepHorizontalClustering04 *ptrClass)
{
    Eigen::Vector3d min, max;
    pClusterToOptimize.getBufferedBoundingBox(ptrClass->_maxSearchRadiusInMeters, ptrClass->_maxSearchRadiusInMeters, 1, min, max);

    const CT_AbstractPointCloudIndex *pointCloudIndexGpToOptimize = pClusterToOptimize.getPointCloudIndex();
    const CT_AbstractPointCloudIndex *pointCloudIndexGpToCombine = pClusterToCombine.getPointCloudIndex();

    if (pointCloudIndexGpToOptimize->size() == 0) {return NULL;}
    if (pointCloudIndexGpToCombine->size() == 0) {return NULL;}


    CT_PointIterator itP(pointCloudIndexGpToCombine);
    itP.toBack();

    while (itP.hasPrevious())
    {
        const CT_Point &point = itP.previous().currentPoint();

        if(CT_MathBoundingShape::containsPointIn2D(min, max, point))
        {
            CT_PointIterator itP2(pointCloudIndexGpToOptimize);
            itP2.toBack();

            while (itP2.hasPrevious())
            {
                const CT_Point &other_point = itP2.previous().currentPoint();

                if(CT_MathPoint::distance2D(other_point, point) < ptrClass->_maxSearchRadiusInMeters)
                {
                    return CT_PointCluster::merge(pClusterToOptimize, pClusterToCombine, DEF_SearchOutCluster, id, outResult, false);
                }
            }
        }

    }

    return NULL;
}