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

 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 adaptations by : Alexandre PIBOULE (ONF)

 This file is part of PluginONFLSIS library 2.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_stepfilterarcpolylines02.h"

#include "ct_itemdrawable/tools/ct_standardcontext.h"

#include "ct_pointcloudindex/abstract/ct_abstractpointcloudindex.h"
#include "ct_accessor/ct_pointaccessor.h"

#include "ct_itemdrawable/ct_pointcluster.h"
#include "ct_math/ct_mathfittedline2d.h"
#include "ct_itemdrawable/tools/pointclustertools/ct_polylinesalgorithms.h"

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

OL_StepFilterArcPolylines02::OL_StepFilterArcPolylines02() : CT_AbstractStep()
{
    _filterRMSE = true;
    _RMSEMax = 15;
    _filterErrorMax = false;
    _ErrorMax = 50;
    _filterR2 = false;
    _R2Min = 0.60;
    _filterByArcradius = true;
    _maxArcRadius = 2;
}

QString OL_StepFilterArcPolylines02::description() const
{
    return tr("Filtrage de polylines : garde les arcs de cercles");
}

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

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

void OL_StepFilterArcPolylines02::declareInputModels(CT_StepInModelStructureManager& manager)
{
    CT_InResultModelGroupToCopy * resultModel = createNewInResultModelForCopy(DEF_SearchInResult, tr("Polylignes"));
    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_SearchInGroup, CT_AbstractItemGroup::staticGetType(), tr("Groupe"));
    resultModel->addItemModel(DEF_SearchInGroup, DEF_SearchInPolyline, CT_PointCluster::staticGetType(), tr("Polyligne"));
}

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

    configDialog->addBool("Filter sur RMSE", "", "", _filterRMSE);
    configDialog->addDouble(tr("RMSE maximale acceptée "), "°", 0, 360, 2, _RMSEMax);
    configDialog->addBool("Filter sur l'erreur maximale", "", "", _filterErrorMax);
    configDialog->addDouble(tr("Erreur maximale acceptée"), "°", 0, 360, 2, _ErrorMax);
    configDialog->addBool("Filter sur le R²", "", "", _filterR2);
    configDialog->addDouble(tr("R² ajusté minimal accepté"), "", 0, 1, 2, _R2Min);
    configDialog->addBool("Filter sur le rayon de l'arc ajusté", "", "", _filterByArcradius);
    configDialog->addDouble(tr("Rayon d'arc maximal"), "cm", 0, 1000, 2, _maxArcRadius, 100);
}

void OL_StepFilterArcPolylines02::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    createNewOutResultModelToCopy(DEF_SearchInResult);
}

void OL_StepFilterArcPolylines02::compute()
{

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

    double RMSEMaxRadians = (_RMSEMax / 180)*M_PI;
    double errorMaxRadians = (_ErrorMax / 180)*M_PI;

    CT_PointAccessor accessor;

    CT_ResultGroupIterator it(outRes, this, DEF_SearchInGroup);
    while (it.hasNext())
    {
        CT_AbstractItemGroup *group = (CT_AbstractItemGroup*) it.next();

        // on récupére le groupe de point
        const CT_PointCluster *line = (const CT_PointCluster*)group->firstItemByINModelName(this, DEF_SearchInPolyline);

        // ________________________________________________________________________________________________
        // Passage en espace des tangentes
        QList<Eigen::Vector2d*> tangentSpace;

        const CT_AbstractPointCloudIndex* lineIndices = line->getPointCloudIndex();

        // On ajoute le premier point de coordonnee (0,0)
        tangentSpace.push_back( new Eigen::Vector2d(0,0) );

        // Puis les autres dependant des precedents
        for ( size_t i = 0 ; i < lineIndices->size()-2 ; i++ )
        {
            const CT_Point& point_i   = accessor.constPointAt(lineIndices->constIndexAt(i));
            const CT_Point& point_ip1 = accessor.constPointAt(lineIndices->constIndexAt(i+1));
            const CT_Point& point_ip2 = accessor.constPointAt(lineIndices->constIndexAt(i+2));

            Eigen::Vector2d vector1(point_ip1(0) - point_i(0), point_ip1(1) - point_i(1));
            Eigen::Vector2d vector2(point_ip2(0) - point_ip1(0), point_ip2(1) - point_ip1(1));

            // Calcule l'angle entre deux vecteurs successifs
            if (vector1.norm() > 0 && vector2.norm() > 0)
            {
                double angle;

                double norm1 = vector1.norm();
                double norm2 = vector2.norm();
                double scalarProduct = vector1.dot(vector2);
                double detDotProduct = vector1(0) * vector2(1) - vector1(1) * vector2(0);

                double arcCos = scalarProduct / ( norm1 * norm2);

                if ( detDotProduct > 0 ) {angle =  acos(arcCos);}
                else                     {angle = -acos(arcCos);}

                // Point 1 & 2 cf. "Segmentation en arcs discrets en temps lineaire" Thanh Phuong NGUYEN - Isabelle DEBLED-RENNESSON
                tangentSpace.push_back( new Eigen::Vector2d( (*tangentSpace.last())(0) + norm1, (*tangentSpace.last())(1) ) );
                tangentSpace.push_back( new Eigen::Vector2d( (*tangentSpace.last())(0), (*tangentSpace.last())(1) + angle) );
            }
        }

        const CT_Point& point_e1 = accessor.constPointAt(lineIndices->constIndexAt(lineIndices->size() - 1));
        const CT_Point& point_e2 = accessor.constPointAt(lineIndices->constIndexAt(lineIndices->size() - 2));

        // On ajoute le dernier point 1
        Eigen::Vector2d lastVector (point_e1(0) - point_e2(0), point_e1(1) - point_e2(1));

        double lastNorm = lastVector.norm();
        tangentSpace.push_back( new Eigen::Vector2d ( (*tangentSpace.last())(0) + lastNorm, (*tangentSpace.last())(1) ) );

        // ________________________________________________________________________________________________
        // Passage aux points médians dans l'espace des tangentes
        QList<Eigen::Vector2d*> middlePoints;
        for ( int i = 0 ; i < tangentSpace.size() ; i = i+2 )
        {
            middlePoints.push_back( new Eigen::Vector2d( ( (*tangentSpace[i]) + (*tangentSpace[i+1]))/2 ) );
        }

        while (!tangentSpace.isEmpty())
        {
            delete tangentSpace.takeLast();
        }

        // ________________________________________________________________________________________________
        // Test de "l'arcitude" de la polyline
        CT_MathFittedLine2D* fittedLine = new CT_MathFittedLine2D(middlePoints);

        double sagitta, chord, radius;
        CT_PolylinesAlgorithms::compute2DArcData(line, sagitta, chord, radius);

        // ________________________________________________________________________________________________
        // Si la polyligne courante n'est pas considerée comme un arc de cercle on la supprime
        if ((_filterRMSE && (fittedLine->getRMSE() > RMSEMaxRadians)) ||
            (_filterErrorMax && (sqrt(fittedLine->getMaxError()) > errorMaxRadians)) ||
            (_filterByArcradius && (radius > _maxArcRadius)) ||
            (_filterR2 && (fittedLine->getAdjustedR2() < _R2Min)))
        {
            CT_AbstractItemGroup *parentG = group->parentGroup();

            if(parentG != NULL)
            {
                parentG->removeGroup(group);

                if(parentG->isEmpty())
                    recursiveRemoveGroupLaterIfEmpty(parentG->parentGroup(), parentG);
            }
            else
            {
                ((CT_ResultGroup*)group->result())->removeGroupSomethingInStructure(group);
            }
        }

        delete fittedLine;
        qDeleteAll(middlePoints);

        waitForAckIfInDebugMode();
    }

    setProgress( 100 );
}

void OL_StepFilterArcPolylines02::recursiveRemoveGroupLaterIfEmpty(CT_AbstractItemGroup *parent, CT_AbstractItemGroup *group) const
{
    if(parent != NULL)
    {
        parent->removeGroup(group);

        if(parent->isEmpty())
            recursiveRemoveGroupLaterIfEmpty(parent->parentGroup(), parent);
    }
    else
    {
        ((CT_ResultGroup*)group->result())->removeGroupSomethingInStructure(group);
    }
}