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

#include "tools/onf_citations.h"
#include "ct_math/ct_mathpoint.h"

ONF_StepMergeEndToEndSections04::ONF_StepMergeEndToEndSections04() : SuperClass()
{
    _thickness = 0.10;
    _searchDistance = 1;
    _n = 10;
    _mult = 2;
    _zTolerance = 0.20;
}

QString ONF_StepMergeEndToEndSections04::description() const
{
    return tr("Fusionner les billons successifs");
}

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

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

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

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

QStringList ONF_StepMergeEndToEndSections04::getStepRISCitations() const
{
    return QStringList() << ONF_citations::citation()._citationOthmaniEtAl2001;
}

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

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

void ONF_StepMergeEndToEndSections04::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Billons"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroupSection, tr("Billons"));
    manager.addGroup(_inGroupSection, _inGroupCluster, tr("Clusters"));
    manager.addItem(_inGroupCluster, _inCluster, tr("Points"));
}

void ONF_StepMergeEndToEndSections04::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Epaisseur des groupes en Z  :"), "cm", 0, 1000, 2, _thickness, 100);
    postInputConfigDialog->addDouble(tr("Distance maximale entre extrémités de billons à fusionner :"), "m", 0, 100, 2, _searchDistance);
    postInputConfigDialog->addDouble(tr("Nombre de barycentres à considérer aux extrémités :"), "", 0, 50, 0, _n);
    postInputConfigDialog->addDouble(tr("Facteur multiplicatif de distance maximale :"), "", 0, 10000, 0, _mult);
    postInputConfigDialog->addDouble(tr("Chevauchement toléré en Z :"), "cm", 0, 1000, 0, _zTolerance, 100);
}

void ONF_StepMergeEndToEndSections04::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResult(_outResult, tr("Billons fusionnées"));
    manager.setRootGroup(_outResult, _outGroupSection, tr("Billons"));
    manager.addGroup(_outGroupSection, _outGroupCluster, tr("Clusters"));
    manager.addItem(_outGroupCluster, _outCluster, tr("Points"));
    manager.addItem(_outGroupCluster, _outRefPoint, tr("Barycentre"));
}

void ONF_StepMergeEndToEndSections04::compute()
{
    ////////////////////////////////////////////////////
    // CREATION DES SECTION INITIALES (CHANGETHICKNESS + AJOUT DES POINTS DE REFERENCE BARYCENTRES)
    ////////////////////////////////////////////////////

    QList<ClusterSection*> clusterSections;
    QList<ClusterSection*> finalClusterSections;

    // Parcours des sections in

    for (const CT_StandardItemGroup* section : _inGroupSection.iterateInputs(_inResult))
    {
        if (isStopped()) {return;}

        ClusterSection* clusterSection = new ClusterSection();

        for (const CT_StandardItemGroup* group : section->groups(_inGroupCluster))
        {
            const CT_PointCluster *cluster = group->singularItem(_inCluster);
            if (cluster!=nullptr) {clusterSection->addCluster(cluster);}
        }

        clusterSection->initRefPoints(_thickness);
        clusterSections.append(clusterSection);
    }

    std::sort(clusterSections.begin(), clusterSections.end(), ClusterSection::sortByLength);
    int size = clusterSections.size();


    ////////////////////////////////////////////////////
    // ALGORITHME DE FUSION CONDITIONNELLE DES SECTIONS
    ////////////////////////////////////////////////////
    while (!clusterSections.isEmpty() && (!isStopped()))
    {
        // récupération de la section la plus longue : BASE
        ClusterSection* baseSection = clusterSections.takeFirst();

        bool hasToLoopAgain = true;
        while (hasToLoopAgain)
        {
                hasToLoopAgain = false;

                // Creation de la QList des _n barycentres du haut de BASE
                double maxDist_base = 0;
                QList <Eigen::Vector3d> n_bary_base;
                QMap<double, Eigen::Vector4d> refPoints_base;

                int size_seg_base = baseSection->_refZvalues.size();
                for (int bs = 0 ; bs < size_seg_base ; bs++)
                {
                    const Eigen::Vector4d& refPoint = baseSection->_refPoints.at(bs);
                    refPoints_base.insert(refPoint(2), refPoint);
                }

                double x_base = -std::numeric_limits<double>::max();
                double y_base = -std::numeric_limits<double>::max();
                double z_base = -std::numeric_limits<double>::max();
                int i_base = 0;
                int size_base = std::min(int(_n), refPoints_base.size());

                QMapIterator<double, Eigen::Vector4d> it_base(refPoints_base);
                it_base.toBack();
                while (it_base.hasPrevious() && (i_base < size_base))
                {
                    it_base.previous();
                    const Eigen::Vector4d& refPt_base = it_base.value();
                    n_bary_base.append(refPt_base.head<3>());
                    if (maxDist_base < refPt_base(3)) {maxDist_base = refPt_base(3);}
                    if (i_base == 0)
                    {
                        x_base = refPt_base(0);
                        y_base = refPt_base(1);
                        z_base = refPt_base(2);
                    }
                    ++i_base;
                }

                // Ajustement de la droite passant par les _n barycentres du haut de BASE
                CT_LineData *lineb = CT_LineData::staticCreateLineDataFromPointCloud(n_bary_base);

                // Parcours des autres segments : TESTED (toujours dans l'ordre des Zmin)
                QListIterator<ClusterSection*> it(clusterSections);
                while (it.hasNext() && !hasToLoopAgain)
                {
                    ClusterSection* testedSection = it.next();

                    // Est-ce que les segments BASE et TESTED se chevauchent en Z ?
                    const Eigen::Vector4d& testedPt = testedSection->_refPoints.first();
                    const Eigen::Vector4d& basePt = baseSection->_refPoints.last();

                    double testedMinZ = testedPt(2);
                    double baseMaxZ = basePt(2);

                    double extremitiesDistance = sqrt(pow(testedPt(0) - basePt(0), 2) + pow(testedPt(1) - basePt(1), 2) + pow(testedPt(2) - basePt(2), 2));

                    // Est-ce que TESTED commence au dessus de BASE
                    if ((extremitiesDistance < _searchDistance) && (testedMinZ >= (baseMaxZ - _zTolerance)))
                    {
                        // a priori le TESTED doit etre fusionne avec le BASE sauf si...
                        bool hasToBeMerged = true;

                        // Creation de la QList des _n barycentres du bas de TESTED
                        double maxDist_tested = 0;
                        QList <Eigen::Vector3d> n_bary_tested;
                        QMap<double, Eigen::Vector4d> refPoints_tested;

                        int size_seg_tested = testedSection->_refZvalues.size();
                        for (int ts = 0 ; ts < size_seg_tested ; ts++)
                        {
                            const Eigen::Vector4d& refPoint = testedSection->_refPoints.at(ts);
                            refPoints_tested.insert(refPoint(2), refPoint);
                        }

                        double x_tested = -std::numeric_limits<double>::max();
                        double y_tested = -std::numeric_limits<double>::max();
                        double z_tested = -std::numeric_limits<double>::max();
                        int i_tested = 0;
                        int size_tested = std::min (int(_n), refPoints_tested.size());
                        QMapIterator<double, Eigen::Vector4d> it_tested(refPoints_tested);
                        while (it_tested.hasNext() && (i_tested < size_tested))
                        {
                            it_tested.next();
                            const Eigen::Vector4d& refPt_tested = it_tested.value();
                            n_bary_tested.append(refPt_tested.head<3>());
                            if (maxDist_tested < refPt_tested(3)) {maxDist_tested = refPt_tested(3);}
                            if (i_tested == 0)
                            {
                                x_tested = refPt_tested(0);
                                y_tested = refPt_tested(1);
                                z_tested = refPt_tested(2);
                            }
                            ++i_tested;
                        }

                        // Ajustement de la droite passant par les _n barycentres du bas de TESTED
                        CT_LineData *linet = CT_LineData::staticCreateLineDataFromPointCloud(n_bary_tested);

                        double dist_base   = std::numeric_limits<double>::max();
                        double dist_tested = std::numeric_limits<double>::max();

                        if (linet != nullptr)
                        {
                            dist_base   = distanceFromExtremityToLine(linet, x_base, y_base, z_base);
                            delete linet;
                        }

                        if (lineb != nullptr)
                        {
                            dist_tested = distanceFromExtremityToLine(lineb, x_tested, y_tested, z_tested);
                        }


                        if (dist_base   > _mult*maxDist_base) {hasToBeMerged = false;}
                        if (size_tested > 1 && dist_tested > _mult*maxDist_tested) {hasToBeMerged = false;}

                        // Si la fusion de TESTED et BASE n'a pas ete rejetee
                        if (hasToBeMerged)
                        {
                            // Creation du segment fusionne : MERGED
                            ClusterSection* mergedSection = ClusterSection::mergeSections(baseSection, testedSection, _thickness);

                            // Le segment MERGED est retenu, donc remplace BASE pour les tests suivants
                            // BASE et TESTED sont donc supprimés
                            delete baseSection;
                            baseSection = mergedSection;

                            clusterSections.removeOne(testedSection);
                            delete testedSection;

                            hasToLoopAgain = true;

                        }
                    }
                } // FIN du while des TESTED
                if (lineb != nullptr) {delete lineb;}
        }

        finalClusterSections.append(baseSection);

        waitForAckIfInDebugMode();
        setProgress(100*int(1.0 - double(clusterSections.size())/double(size)));
    }

    // Create and add output data
    for (CT_ResultGroup* outRes : _outResult.iterateOutputs())
    {
        for (int i = 0 ; i < finalClusterSections.size() ; i++)
        {
            ClusterSection* clusterSection = finalClusterSections.at(i);
            clusterSection->computeOutData(_thickness);

            CT_StandardItemGroup* section = new CT_StandardItemGroup();
            outRes->addRootGroup(_outGroupSection, section);

            for (int j = 0 ; j < clusterSection->_refZvalues.size() ; j++)
            {
                CT_StandardItemGroup* clusterGroup = new CT_StandardItemGroup();
                section->addGroup(_outGroupCluster, clusterGroup);
                clusterGroup->addSingularItem(_outCluster, clusterSection->_outClusters[j]);
                clusterGroup->addSingularItem(_outRefPoint, clusterSection->_outRefPoints[j]);
            }
        }
    }

    qDeleteAll(finalClusterSections);

    setProgress(100);
}

double ONF_StepMergeEndToEndSections04::distanceFromExtremityToLine(CT_LineData *lineL, double plan_x, double plan_y, double plan_z)
{

    double x_inter = 0;
    double y_inter = 0;
    double z_inter = 0;

    Eigen::Vector3d vert(0,0,1);

    bool ok = lineL->intersectionWithRect3D(plan_x, plan_y, plan_z, vert, &x_inter, &y_inter, &z_inter);

    if (!ok) {
        return 10000;
    }

    double dx = x_inter - plan_x;
    double dy = y_inter - plan_y;

    return  sqrt(dx*dx + dy*dy);
}