/****************************************************************************

 Copyright (C) 2010-2012 the Office National des Forêts (ONF), France
                     and the Laboratoire des Sciences de l'Information et des Systémes (LSIS), Marseille, France.
                     All rights reserved.

 Contact : alexandre.piboule@onf.fr
           alexandra.bac@esil.univmed.fr

 Developers : Joris Ravaglia (ONF/LSIS)
 With modifications by : Alexandre PIBOULE (ONF)

 This file is part of PluginONFLSIS library 1.0.

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

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

*****************************************************************************/

#include "ol_stepthrowparticules05.h"

#include "ct_itemdrawable/abstract/ct_abstractitemdrawablewithpointcloud.h"
#include "ct_itemdrawable/ct_circle.h"
#include "ct_point.h"
#include "ct_math/ct_mathpoint.h"
#include "ct_itemdrawable/tools/ct_standardcontext.h"
#include "ct_global/ct_context.h"
#include "ct_math/ct_math.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_accessor/ct_pointaccessor.h"

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

#include "qdebug.h"
#include "qvector2d.h"

#include <limits>

#define DEF_SearchInPointCluster  "p"
#define DEF_SearchInGroup       "g"
#define DEF_SearchInResult      "r"

OL_StepThrowParticules05::OL_StepThrowParticules05() : CT_AbstractStep()
{
    _nbPtsMin = 3;
    _distanceBetweenPoints = 0.03;
    _kNeighborsParticle = 1;
    _radiusRepulsion = 0.014;
    _kRepulsion = 1;
    _nbIterMax = 500;
    _convergeThresh = 0.01;
    _multiThread = true;
    _useOfOctree = false;

#ifdef USE_PCL
    _useOfOctree = true;
#endif
    _nbPtsForUsingOctree = 50;
    _octreeResolution = 0.05;
}

QString OL_StepThrowParticules05::description() const
{
    return tr("Simplification de clusters / lancé de particules");
}

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

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

void OL_StepThrowParticules05::declareInputModels(CT_StepInModelStructureManager& manager)
{
    CT_InResultModelGroupToCopy * resultModel = createNewInResultModelForCopy(DEF_SearchInResult, tr("Groupe de points"));
    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_SearchInGroup, CT_AbstractItemGroup::staticGetType(), tr("Groupe"));
    resultModel->addItemModel(DEF_SearchInGroup, DEF_SearchInPointCluster, CT_PointCluster::staticGetType(), tr("Groupe de points"));
}

void OL_StepThrowParticules05::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    CT_StepConfigurableDialog *configDialog = newStandardPostConfigurationDialog();

    configDialog->addInt       (tr("Nombre de points a partir duquel on opére une simplification sur le groupe"), "", 1, 1000, _nbPtsMin);
    configDialog->addDouble    (tr("Distance moyenne entre particules sur l'arc"), "cm", 0.01, 100, 2, _distanceBetweenPoints, 100);
    configDialog->addInt       (tr("Nombre de voisins a prendre en compte lors de la projection"), "", 1, 50, _kNeighborsParticle);
    configDialog->addDouble    (tr("Rayon de répulsion des particules"), "cm", 0.01, 1000, 2, _radiusRepulsion, 100);
    configDialog->addInt       (tr("Constante multiplicative lors du calcul de répulsion"), "", 1, 50, _kRepulsion);
    configDialog->addDouble    (tr("Seuil de convergence"), "cm", 0.01, 100, 2, _convergeThresh, 100);
    configDialog->addInt       (tr("Nombre d'itérations maximum sans convergence des particules"), "", 1, 10000, _nbIterMax);
    configDialog->addBool      (tr("Traitement Multithread"),"","",_multiThread);
    configDialog->addText      (tr("Si PCL est utilisé :"), "", "");
    configDialog->addBool      (tr("Utiliser un octree pour les gros groupes"),"","",_useOfOctree);
    configDialog->addInt       (tr("Nombre de points à partir duquel utiliser un octree"), "", 0, 1000, _nbPtsForUsingOctree);
    configDialog->addDouble    (tr("Résolution de l'octree"), "m", 0.01, 10.0, 2, _octreeResolution );

}

void OL_StepThrowParticules05::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    CT_OutResultModelGroupToCopyPossibilities *res = createNewOutResultModelToCopy(DEF_SearchInResult);

    if(res != NULL)
        res->addItemModel(DEF_SearchInGroup, _outParticulesModelName, new CT_PointCluster(), tr("Particules"));
}

void OL_StepThrowParticules05::compute()
{
#ifndef USE_PCL
    _useOfOctree = false;
#endif

    // on récupére le résultat copié
    _outRes = getOutResultList().first();

    /**************************************************/

    QList<CT_AbstractItemGroup*> multiThreadList;
    CT_StandardContext context;
    context.setStep(this);

    CT_ResultGroupIterator itGrp(_outRes, this, DEF_SearchInGroup);
    while (itGrp.hasNext() && (!isStopped()))
    {
        CT_AbstractItemGroup *group = (CT_AbstractItemGroup*) itGrp.next();

        group->setContext(&context);
        if (_multiThread)
        {
            multiThreadList.append(group);
        } else {
            OL_StepThrowParticules05::staticSimplifyPointCluster(group);
        }
    }

    setProgress(50);

    if (_multiThread)
    {
        QFuture<void> futur = QtConcurrent::map(multiThreadList, OL_StepThrowParticules05::staticSimplifyPointCluster);
        int progressMin = futur.progressMinimum();
        int progressTotal = futur.progressMaximum() - futur.progressMinimum();
        while (!futur.isFinished())
        {
            if (progressTotal > 0)
            {
                setProgress(50 + 49*(futur.progressValue() - progressMin)/progressTotal);
            }
        }
    }

    multiThreadList.clear();
    setProgress( 100 );
}

void OL_StepThrowParticules05::staticSimplifyPointCluster(CT_AbstractItemGroup *group)
{
    CT_StandardContext* context = (CT_StandardContext*) group->getContext();
    OL_StepThrowParticules05* step = (OL_StepThrowParticules05*) context->step();
    CT_PointCluster* currentCluster = (CT_PointCluster*) group->firstItemByINModelName(step, DEF_SearchInPointCluster);

    if (currentCluster==NULL) {return;}

    Eigen::Vector3d min, max;

    currentCluster->getBoundingBox(min, max);
    double length = fabs(max(0) - min(0)) + fabs(max(1) - min(1));
    CT_PointCluster* particules = step->simplifyPointCluster(currentCluster, length);

    if (particules != NULL) {
        group->addItemDrawable(particules);
    }
 }

CT_PointCluster* OL_StepThrowParticules05::simplifyPointCluster(const CT_PointCluster *inputGrp, double length)
{
    CT_PointCluster* particules = new CT_PointCluster(_outParticulesModelName.completeName(), _outRes);

    size_t grpSize = inputGrp->getPointCloudIndex()->size();

#ifdef USE_PCL
    bool use_octree = (_useOfOctree && (grpSize > _nbPtsForUsingOctree));
#endif

    int nb = qRound(length/_distanceBetweenPoints);
    if (nb <= _nbPtsMin) {nb = _nbPtsMin;}

    int kNeigborhood = _kNeighborsParticle;
    int demiSize = qRound((double)grpSize/2.0);
    if (demiSize < kNeigborhood) {kNeigborhood = demiSize;}

    // Si le groupe de points contient moins de points que le nombre de points minimum a partir duquel on opere une simplification
    // On fait une simple copie des indices
    if ((grpSize <= _nbPtsMin) || (grpSize <= nb))
    {
        const CT_AbstractPointCloudIndex* indices = inputGrp->getPointCloudIndex();

        for (size_t i = 0 ; i < indices->size() ; i++)
        {
            particules->addPoint((*indices)[i]);
        }
    } else
    {

#ifdef USE_PCL
        // Declaration de l'octree
        pcl::octree::OctreePointCloudSearch< CT_Point > *octree  = NULL;

        if (use_octree)
        {
            // Remplissage de l'octree
            octree  = new pcl::octree::OctreePointCloudSearch< CT_Point >(_octreeResolution);
            boost::shared_ptr< std::vector<size_t> > tabIndices;

            octree->setInputCloud(inputGrp->getPCLCloud());

            for (size_t i = 0 ; i < grpSize ; i++)
            {
                octree->addPointFromCloud( (*(inputGrp->getPointCloudIndex()))[i], tabIndices );
            }
        }
#endif

        QList<QPair<CT_Point, size_t> > resultParticles = createParticles(inputGrp, nb);

        // Premier positionnement des particules dans le nuage de points.
#ifdef USE_PCL
        projectParticles(inputGrp, resultParticles, kNeigborhood, octree);
#else
        projectParticles(inputGrp, resultParticles, kNeigborhood);
#endif
        bool converged = false;
        size_t cpt = 0;

        QList<QPair<CT_Point, size_t> > cpy;

        // Soit ca converge, soit on arete au bout d'un nombre maximum d'iterations
        while ( !converged && !isStopped() && cpt < _nbIterMax )
        {
            for (int i = 0 ; i < resultParticles.size() ; i++)
            {
                QPair<CT_Point, size_t> pair = resultParticles.at(i);
                cpy.append(pair);
            }

            relaxParticles(resultParticles, _radiusRepulsion, _kRepulsion);

#ifdef USE_PCL
            projectParticles(inputGrp, resultParticles, kNeigborhood, octree);
#else
            projectParticles(inputGrp, resultParticles, kNeigborhood);
#endif

            converged = hasConverged(cpy, resultParticles, _convergeThresh);
            cpy.clear();
            cpt++;
        }

        // Il faut maintenant reprendre la derniere etape pour sortir un tableau d'indice plutot qu'un tableau de points.
        // cpy contient l'etat des particules a l'avant derniere etape du traitement, il suffit de relacher puis projetter cpy en recuperant un tableau d'indices
        relaxParticles(cpy, _radiusRepulsion, _kRepulsion);
#ifdef USE_PCL
        projectParticles(inputGrp, resultParticles, kNeigborhood, octree);
#else
        projectParticles(inputGrp, resultParticles, kNeigborhood);
#endif

#ifdef USE_PCL
        if (octree != NULL) {delete octree;}
#endif

        for (int i = 0 ; i < resultParticles.size() ; i++)
        {
            particules->addPoint(resultParticles.at(i).second);
        }
    }

    return particules;
}

QList<QPair<CT_Point, size_t> > OL_StepThrowParticules05::createParticles(const CT_PointCluster *group, size_t n)
{
    QList<QPair<CT_Point, size_t> > resultParticles;

    // On recupere la BBox du groupe de points pour y jetter les particules
    Eigen::Vector3d min, max;
    group->getBoundingBox(min, max);

    // Ajout des particules a des positions aleatoires dans le bounding rectangle du groupe de points
    for (size_t i = 0 ; i < n ; i++)
    {
        QPair<CT_Point, size_t> pair;
        pair.first(0) = (((double)rand()/(double)RAND_MAX) * (max(0)-min(0)) ) + min(0);
        pair.first(1) = (((double)rand()/(double)RAND_MAX) * (max(1)-min(1)) ) + min(1);
        pair.first(2) = (((double)rand()/(double)RAND_MAX) * (max(2)-min(2)) ) + min(2);
        pair.second = -1;

        resultParticles.append(pair);
    }

    return resultParticles;
}

#ifdef USE_PCL
void OL_StepThrowParticules05::projectParticles(const CT_PointCluster *inCluster, QList<QPair<CT_Point, size_t> > &particles, size_t kNeighbors, pcl::octree::OctreePointCloudSearch<CT_Point> *octree)
#else
void OL_StepThrowParticules05::projectParticles(const CT_PointCluster *inCluster, QList<QPair<CT_Point, size_t> > &particles, size_t kNeighbors)
#endif
{
    const CT_AbstractPointCloudIndex* indexCloud = inCluster->getPointCloudIndex();

    if (indexCloud->size() > kNeighbors)
    {
        for (int i = 0 ; i < particles.size() ; i++)
        {
            QPair<CT_Point, size_t> &pair = (QPair<CT_Point, size_t>&) particles.at(i);
            CT_Point *position = &(pair.first);
            // Calcul de l'index du point le plus proche au centroide
            double distMin = std::numeric_limits<double>::max();
            size_t nearestPointGlobalIndex = std::numeric_limits<size_t>::max();

            // Contiendront les indices / distances des k-points de sortie de recherche
            std::vector<size_t> indices;
            std::vector<double> distances;

#ifdef USE_PCL
            // Calcul des k-voisins
            // Avec octree
            if (octree != NULL)
            {
                if (octree->nearestKSearch(*position, kNeighbors, indices, distances) <= 0)
                {
                    qDebug() << "Dans \"projectParticles\" :\n";
                    qDebug() << "Probleme lors de la recherche de voisinnage avec un octree : Nombre de voisins trouves = 0\n";
                    qDebug() << "Fin du programme.\n";
                    exit(0);
                }
            // Sans octree
            } else {
#endif
                // Initialisation des tableaux
                for ( size_t ii = 0 ; ii < kNeighbors ; ii++ )
                {
                    indices.push_back( std::numeric_limits<size_t>::max() );
                    distances.push_back( std::numeric_limits<double>::max() );
                }

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

                    double tmpDist = CT_MathPoint::distance2D(*position, point);

                    if ( tmpDist < distances[kNeighbors-1] )
                    {
                        bool stop = false;
                        for ( size_t j = 0 ; j < kNeighbors && !stop ; j++ )
                        {
                            if (distances[j] > tmpDist)
                            {
                                indices.insert(indices.begin() + j,index);
                                distances.insert(distances.begin() + j,tmpDist);

                                indices.pop_back();
                                distances.pop_back();

                                stop = true;
                            }
                        }
                    }
                }
#ifdef USE_PCL
            }
#endif

            // Calcul du barycentre des k-voisins resultats.
            Eigen::Vector3d centroid;

            centroid(0) = 0;
            centroid(1) = 0;

            CT_PointAccessor accessor;
            for ( size_t i = 0 ; i < indices.size() ; i++ )
            {
                const CT_Point& point = accessor.constPointAt(indexCloud->constIndexAt(i));
                centroid(0) += point(0);
                centroid(1) += point(1);
            }

            centroid(0) /= (double)indices.size();
            centroid(1) /= (double)indices.size();

            // On regarde quel est le point le plus proche du barycenter precedemment calcule
            for ( size_t ii = 0 ; ii < indices.size() ; ii++ )
            {
                const CT_Point& point = accessor.constPointAt(indexCloud->constIndexAt(ii));

                double dist = CT_MathPoint::distance2D(centroid, point);

                if ( dist < distMin )
                {
                    distMin = dist;
                    nearestPointGlobalIndex = indices[ii];
                }
            }

            const CT_Point &nearestPoint = accessor.constPointAt(nearestPointGlobalIndex);

            (*position)(0) = nearestPoint(0);
            (*position)(1) = nearestPoint(1);
            (*position)(2) = nearestPoint(2);

            pair.second = nearestPointGlobalIndex;
        }
    }
}

void OL_StepThrowParticules05::relaxParticles(QList<QPair<CT_Point, size_t> > &particles, const double repulsionRadius, const size_t kRepulsion)
{
    // Calcul des deplacements des particules
    QVector<Eigen::Vector3d> displacement(particles.size());

    for ( int i = 0 ; i < particles.size() ; i++ )
    {
        // calcul de la répulsion totale pour la particule
        Eigen::Vector3d pt(0, 0, 0);

        for ( int j = 0 ; j < particles.size() ; j++ )
        {
            if ( j != i )
            {
                CT_Point repulsionForce = computeParticleToParticleRepulsion(particles.at(i).first, particles.at(j).first, repulsionRadius, kRepulsion);

                pt(0) += repulsionForce(0);
                pt(1) += repulsionForce(1);
            }
        }

        displacement[i] = pt;
    }

    // Application des deplacements
    for (int i = 0 ; i < particles.size() ; i++)
    {
        QPair<CT_Point, size_t> &pair = (QPair<CT_Point, size_t>&) particles.at(i);

        pair.first(0) += displacement[i](0);
        pair.first(1) += displacement[i](1);
    }

}

CT_Point OL_StepThrowParticules05::computeParticleToParticleRepulsion(const CT_Point &query, const CT_Point &repulser, const double repulsionRadius, const size_t kRepulsion)
{
    CT_Point rslt;

    double dist = CT_MathPoint::distance2D(query, repulser);

    if (dist  < repulsionRadius)
    {
        if (query(0) == repulser(0) && query(1) == repulser(1))
        {
            // Si deux particules se retrouvent collees, on calcule un vecteur de repulsion au hassard dont les coordonnees sont dans l'intervale [-1,0[ U ]0,1] et ont une precision de 2 chiffres apres la virgule
            CT_Point vector;
            vector(0) = (double)( (double)( ( rand() % 200 ) + 1.0 ) / 100.0 ) - 1.0;
            vector(1) =(double)( (double)( ( rand() % 200 ) + 1.0 ) / 100.0 ) - 1.0;

            // On fait bien gaffe a ne pas tirer 0 au hasard
            while (vector(0) == 0)
            {
                vector(0) = (double)( (double)( ( rand() % 200 ) + 1.0 ) / 100.0 ) - 1.0;
            }

            while (vector(0) == 0)
            {
                vector(1) = (double)( (double)( ( rand() % 200 ) + 1.0 ) / 100.0 ) - 1.0;
            }

            // On le normalise
            double normVector = sqrt ( (vector(0) * vector(0)) + (vector(1) * vector(1)) );

            vector(0) /= normVector;
            vector(1) /= normVector;

            // Et on applique les forces de repulsion selon ce vecteur
            rslt(0) *= (kRepulsion * (repulsionRadius - dist ));
            rslt(1) *= (kRepulsion * (repulsionRadius - dist ));
            rslt(2) = 0;
        } else
        {
            // Je calcule normalise le vecteur directeur de la repulsion. La norme de ce vecteur est dist.
            CT_Point vector;
            vector(0) =  (query(0) - repulser(0)) / dist;
            vector(1) = (query(1) - repulser(1)) / dist;

            rslt(0) *= (kRepulsion * (repulsionRadius - dist ));
            rslt(1) *= (kRepulsion * (repulsionRadius - dist ));
            rslt(2) = 0;
        }

    // Si la distance entre les deux points est trop grande, aucune repulsion n'est appliquee.
    } else
    {
        rslt(0) = 0;
        rslt(1) = 0;
        rslt(2) = 0;

    }
    return rslt;
}

bool OL_StepThrowParticules05::hasConverged(const QList<QPair<CT_Point, size_t> > &before, const QList<QPair<CT_Point, size_t> > &after, const double convergeThresh)
{
    double sum = 0;

    if ( before.size() != after.size() )
    {
        return false;
    } else {
        for (int i = 0 ; i < before.size() ; i++)
        {
            sum += CT_MathPoint::distance2D(before.at(i).first, after.at(i).first);
        }
    }

    if (sum > convergeThresh) {return false;}
    return true;
}