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

#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/Geometry"
#include <QFile>
#include <QTextStream>
#include <QFileInfo>

ONF_StepMatchItemsPositions::ONF_StepMatchItemsPositions() : SuperClass()
{
    _distThreshold = 3;
    _relativeSizeThreshold = 0.2;
    _minRelativeSize = 0;
    _maxTheta = 180;
    _possiblyInvertedDirection = false;
    _coef_nbRwc = 1;
    _coef_nbTwc = 1;
    _coef_nbSim = 1;
    _drawMode = 0;
    _relativeMode = 0;
    _minval = 0;
    _maxval = 5;
    _exportReport = true;
    _exportData = true;
    _reportFileName.append("matchingReport.txt");
    _transformedDataFileName.append("transformedData.txt");
    _transformationDataFileName.append("transformationData.txt");
}

QString ONF_StepMatchItemsPositions::description() const
{
    return tr("Co-registration de deux ensembles de positions 2D");
}

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

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

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

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

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

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

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

void ONF_StepMatchItemsPositions::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResultRef, tr("Positions de référence"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultRef, _inZeroOrMoreRootGroupRef);
    manager.addGroup(_inZeroOrMoreRootGroupRef, _inGroupRef);
    manager.addItem(_inGroupRef, _inPositionRef, tr("Item de référence"));
    manager.addItemAttribute(_inPositionRef, _inXRef, CT_AbstractCategory::DATA_X, tr("Coordonnée X"));
    manager.addItemAttribute(_inPositionRef, _inYRef, CT_AbstractCategory::DATA_Y, tr("Coordonnée Y"));
    manager.addItemAttribute(_inPositionRef, _inIDRef, CT_AbstractCategory::DATA_ID, tr("ID"));
    manager.addItemAttribute(_inPositionRef, _inIDPlotRef, CT_AbstractCategory::DATA_ID, tr("IDplot"));
    manager.addItemAttribute(_inPositionRef, _inValueRef, CT_AbstractCategory::DATA_VALUE, tr("Valeur"));

    manager.addResult(_inResultTrans, tr("Positions à transformer"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultTrans, _inZeroOrMoreRootGroupTrans);
    manager.addGroup(_inZeroOrMoreRootGroupTrans, _inGroupTrans);
    manager.addItem(_inGroupTrans, _inPositionTrans, tr("Item à transformer"));
    manager.addItemAttribute(_inPositionTrans, _inXTrans, CT_AbstractCategory::DATA_X, tr("Coordonnée X"));
    manager.addItemAttribute(_inPositionTrans, _inYTrans, CT_AbstractCategory::DATA_Y, tr("Coordonnée Y"));
    manager.addItemAttribute(_inPositionTrans, _inIDTrans, CT_AbstractCategory::DATA_ID, tr("ID"));
    manager.addItemAttribute(_inPositionTrans, _inValueTrans, CT_AbstractCategory::DATA_VALUE, tr("Valeur"));

    manager.addResult(_inResultCounter, tr("Résultat compteur"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultCounter, _inZeroOrMoreRootGroupCounter);
    manager.addItem(_inZeroOrMoreRootGroupCounter, _inCounter, tr("Compteur"));


}

void ONF_StepMatchItemsPositions::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResult(_outResultTrans, tr("Positions transformées"));
    manager.setRootGroup(_outResultTrans, _outRootGrpTrans, tr("Groupe racine"));

    manager.addItem(_outRootGrpTrans, _outTrMat, tr("Matrice de transformation"));
    manager.addItem(_outRootGrpTrans, _outAttributes, tr("Qualité de Matching"));
    manager.addItemAttribute(_outAttributes, _outAttRmseDist, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("RMSE Dist"));
    manager.addItemAttribute(_outAttributes, _outAttRmseVal, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("RMSE Val"));
    manager.addItemAttribute(_outAttributes, _outAttMaxDist, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("Max Dist"));
    manager.addItemAttribute(_outAttributes, _outAttMaxDistDiff, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("Max Val diff"));

    manager.addGroup(_outRootGrpTrans, _outGrpTrans, tr("Groupe"));
    manager.addItem(_outGrpTrans, _outTransRefPoint, tr("Position transformée"));
    manager.addItemAttribute(_outTransRefPoint, _outTransId, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_ID), tr("ID position transformée"));
    manager.addItemAttribute(_outTransRefPoint, _outRefCorrespId, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_ID), tr("ID position de référence"));
    manager.addItemAttribute(_outTransRefPoint, _outDeltaValue, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("Ecart ValTrans - ValRef"));
    manager.addItemAttribute(_outTransRefPoint, _outDistance, PS_CATEGORY_MANAGER->findByUniqueName(CT_AbstractCategory::DATA_VALUE), tr("Distance 2D Trans - Ref"));
    manager.addItem(_outGrpTrans, _outTransposCorresp, tr("Position de référence correspondante"));
    manager.addItem(_outGrpTrans, _outCorrespLine, tr("Ligne de correspondance"));


    manager.addResult(_outResultInter, tr("Positions intermédiaires"));
    manager.setRootGroup(_outResultInter, _outGrpInter, tr("Groupe"));
    manager.addItem(_outGrpInter, _outInter, tr("Positions intermédiaires"));
}

void ONF_StepMatchItemsPositions::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addTitle(tr("Critères d'affiliation des positions :"));

    postInputConfigDialog->addDouble(tr("Distance maximale entre points appariés :"), "m", 0, 100, 3, _distThreshold, 1);
    postInputConfigDialog->addDouble(tr("Seuil de taille relative minimum entre items appariés :"), "", 0, 1, 2, _relativeSizeThreshold, 1);
    postInputConfigDialog->addDouble(tr("Taille relative minimale :"), "", 0, 1, 2, _minRelativeSize, 1);
    postInputConfigDialog->addDouble(tr("Rotation maximale autorisée :"), "°", 0, 180, 2, _maxTheta);
    postInputConfigDialog->addBool(tr("Inversion de direction possible (+- 180°)"), "", "", _possiblyInvertedDirection);

    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addTitle(tr("Critères de qualité de matching :"));

    postInputConfigDialog->addDouble(tr("Poid du critère Nb. pos. de référence ayant une pos. transformée proche :"), "", 0, 1000, 2, _coef_nbRwc);
    postInputConfigDialog->addDouble(tr("Poid du critère Nb. pos. transformées ayant une pos. de référence proche :"), "", 0, 1000, 2, _coef_nbTwc);
    postInputConfigDialog->addDouble(tr("Poid du critère Nb. pos. transformées ayant une pos. de référence proche avec une taille similaire :"), "", 0, 1000, 2, _coef_nbSim);

    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addTitle(tr("Mode de représentation :"));

    postInputConfigDialog->addTitle(tr("Type de représentation :"));
    CT_ButtonGroup &bg_drawMode = postInputConfigDialog->addButtonGroup(_drawMode);

    postInputConfigDialog->addExcludeValue("", "", tr("Valeur Z"), bg_drawMode, 0);
    postInputConfigDialog->addExcludeValue("", "", tr("Cercle"), bg_drawMode, 1);

    postInputConfigDialog->addTitle(tr("Comment représenter en Z la variable de taille ?"));

    CT_ButtonGroup &bg_relativeMode = postInputConfigDialog->addButtonGroup(_relativeMode);

    postInputConfigDialog->addExcludeValue("", "", tr("Valeur absolue"), bg_relativeMode, 0);
    postInputConfigDialog->addExcludeValue("", "", tr("Valeur relative"), bg_relativeMode, 1);
    postInputConfigDialog->addText(tr("En cas de valeur relative :"), "", "");
    postInputConfigDialog->addDouble(tr("Valeur de Z/Rayon minimum"), "m", -1e+09, 1e+09, 2, _minval, 1);
    postInputConfigDialog->addDouble(tr("Valeur de Z/Rayon maximum"), "m", -1e+09, 1e+09, 2, _maxval, 1);

    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addTitle(tr("Export des données :"));

    postInputConfigDialog->addBool(tr("Exporter un rapport de Recalage"), "", "", _exportReport);
    postInputConfigDialog->addFileChoice(tr("Fichier d'export du rapport de Recalage"), CT_FileChoiceButton::OneNewFile, tr("Fichier texte (*.txt)"), _reportFileName);

    postInputConfigDialog->addBool(tr("Exporter les données dans un fichier multi-placettes"), "", "", _exportData);
    postInputConfigDialog->addFileChoice(tr("Fichier d'export (multi-placettes) de données transformées"), CT_FileChoiceButton::OneNewFile, tr("Fichier texte (*.txt)"), _transformedDataFileName);
    postInputConfigDialog->addFileChoice(tr("Fichier d'export (multi-placettes) des paramètres de transformation"), CT_FileChoiceButton::OneNewFile, tr("Fichier texte (*.txt)"), _transformationDataFileName);

}

void ONF_StepMatchItemsPositions::compute()
{

    double maxThetaRad = M_PI*_maxTheta / 180.0;

    // determine si il faut créer les entêtes dans les fichiers cumulés
    bool first = true;

    for (const CT_LoopCounter* counter : _inCounter.iterateInputs(_inResultCounter))
    {
        if (counter != nullptr)
        {
            if (counter->currentTurn() > 1)
            {
                first = false;
            }
        }
    }

    QList<QPair<Eigen::Vector2d, double> > refPositions;
    QList<QPair<Eigen::Vector2d, double> > transPositions;
    QList<QPair<Eigen::Vector2d, double> > transPositionsTmp;

    QMap<int, QString> refIds;
    QMap<int, QString> transIds;
    QMap<int, double> deltaDistMap;
    QMap<int, double> deltaValMap;

    double minRefValue = std::numeric_limits<double>::max();
    double maxRefValue = -std::numeric_limits<double>::max();
    double minTransValue = std::numeric_limits<double>::max();
    double maxTransValue = -std::numeric_limits<double>::max();

    // create refPositions List
    QString idRefPlot = "";

    int cptRef = 0;
    for (const CT_StandardItemGroup* groupRef : _inGroupRef.iterateInputs(_inResultRef))
    {
        for (const CT_AbstractSingularItemDrawable* itemRef : groupRef->singularItems(_inPositionRef))
        {
            if (isStopped()) {return;}

            if (itemRef != nullptr)
            {
                const CT_AbstractItemAttribute* attXRef = itemRef->itemAttribute(_inXRef);
                const CT_AbstractItemAttribute* attYRef = itemRef->itemAttribute(_inYRef);
                const CT_AbstractItemAttribute* attIDRef = itemRef->itemAttribute(_inIDRef);
                const CT_AbstractItemAttribute* attIDPlotRef = itemRef->itemAttribute(_inIDPlotRef);
                const CT_AbstractItemAttribute* attValueRef = itemRef->itemAttribute(_inValueRef);

                double xRef = (attXRef == nullptr ? 0 : attXRef->toDouble(itemRef, nullptr));
                double yRef = (attYRef == nullptr ? 0 : attYRef->toDouble(itemRef, nullptr));
                QString idRef = (attIDRef == nullptr ? "" : attIDRef->toString(itemRef, nullptr));
                double valRef = (attValueRef == nullptr ? 0 : attValueRef->toDouble(itemRef, nullptr));

                if (cptRef == 0) {idRefPlot = (attIDPlotRef == nullptr ? "" : attIDPlotRef->toString(itemRef, nullptr));}

                if (valRef < minRefValue) {minRefValue = valRef;}
                if (valRef > maxRefValue) {maxRefValue = valRef;}

                refPositions.append(QPair<Eigen::Vector2d, double>(Eigen::Vector2d(xRef, yRef), valRef));
                refIds.insert(cptRef, idRef);

                cptRef++;
            }
        }
    }



    // create transPositions List
    int cptTrans = 0;
    for (const CT_StandardItemGroup* groupTrans : _inGroupTrans.iterateInputs(_inResultTrans))
    {
        for (const CT_AbstractSingularItemDrawable* itemTrans : groupTrans->singularItems(_inPositionTrans))
        {
            if (isStopped()) {return;}

            if (itemTrans != nullptr)
            {
                const CT_AbstractItemAttribute* attXTrans = itemTrans->itemAttribute(_inXTrans);
                const CT_AbstractItemAttribute* attYTrans = itemTrans->itemAttribute(_inYTrans);
                const CT_AbstractItemAttribute* attIDTrans = itemTrans->itemAttribute(_inIDTrans);
                const CT_AbstractItemAttribute* attValueTrans = itemTrans->itemAttribute(_inValueTrans);

                double xTrans = (attXTrans == nullptr ? 0 : attXTrans->toDouble(itemTrans, nullptr));
                double yTrans = (attYTrans == nullptr ? 0 : attYTrans->toDouble(itemTrans, nullptr));
                QString idTrans = (attIDTrans == nullptr ? "" : attIDTrans->toString(itemTrans, nullptr));
                double valTrans = (attValueTrans == nullptr ? 0 : attValueTrans->toDouble(itemTrans, nullptr));

                if (valTrans < minTransValue) {minTransValue = valTrans;}
                if (valTrans > maxTransValue) {maxTransValue = valTrans;}

                transPositions.append(QPair<Eigen::Vector2d, double>(Eigen::Vector2d(xTrans, yTrans), valTrans));
                transPositionsTmp.append(QPair<Eigen::Vector2d, double>(Eigen::Vector2d(xTrans, yTrans), valTrans));
                transIds.insert(cptTrans, idTrans);

                cptTrans++;
            }
        }
    }

    Eigen::Vector2d minRef, maxRef;

    // Ref Bounding Box
    minRef(0) = std::numeric_limits<double>::max();
    minRef(1) = std::numeric_limits<double>::max();
    maxRef(0) = -std::numeric_limits<double>::max();
    maxRef(1) = -std::numeric_limits<double>::max();

    int refPositionSize = refPositions.size();
    int transPositionsSize = transPositions.size();

    // Convert values to relatives values
    double deltaRef = maxRefValue - minRefValue;
    for (int i = 0 ; i < refPositionSize ; i++)
    {
        refPositions[i].second = (refPositions.at(i).second - minRefValue) / deltaRef;

        if (refPositions[i].first(0) < minRef(0)) {minRef(0) = refPositions[i].first(0);}
        if (refPositions[i].first(1) < minRef(1)) {minRef(1) = refPositions[i].first(1);}
        if (refPositions[i].first(0) > maxRef(0)) {maxRef(0) = refPositions[i].first(0);}
        if (refPositions[i].first(1) > maxRef(1)) {maxRef(1) = refPositions[i].first(1);}
    }

    minRef(0) -= _distThreshold;
    minRef(1) -= _distThreshold;
    maxRef(0) += _distThreshold;
    maxRef(1) += _distThreshold;

    double deltaTrans = maxTransValue - minTransValue;
    for (int i = 0 ; i < transPositionsSize ; i++)
    {
        transPositions[i].second = (transPositions.at(i).second - minTransValue) / deltaTrans;
        transPositionsTmp[i].second = (transPositionsTmp.at(i).second - minTransValue) / deltaTrans;
    }

    double bestScore = 0;

    Eigen::Vector2d rotationCenter;
    Eigen::Vector2d translationVector;
    Eigen::Vector2d delta;
    Eigen::Vector2d vectRef;
    Eigen::Vector2d vectTrans;
    Eigen::Vector2d minTrans, maxTrans;

    double cosTheta, theta, orientation, vectTransNorm, dist, trVal, rfVal, sizeDiff, globalScore;
    int Nbref_with_Corresp, Nbtrans_with_Corresp, NbtransRef_Similarity;

    Eigen::Matrix2d rotationMatrix;
    Eigen::Matrix2d rotation;

    QVector<bool> refPosCount(refPositionSize);
    QVector<bool> transPosCount(transPositionsSize);
    QVector<bool> similarPosCount(transPositionsSize);

    const double M_PIx2 = 2.0 * M_PI;

    // Points Matching algorithm
    for (int refCounter = 0 ; refCounter < refPositionSize && !isStopped(); refCounter++)
    {
        const Eigen::Vector2d &refPos = refPositions.at(refCounter).first;

        for (int transCounter = 0 ; transCounter < transPositionsSize && !isStopped(); transCounter++)
        {
            const Eigen::Vector2d &transPos = transPositions.at(transCounter).first;
            double transVal = transPositions.at(transCounter).second;

            // Compute translation for trans points
            delta = refPos - transPos;

            if (transVal >= _minRelativeSize)
            {
                // Search for second trans point
                for (int i = 0 ; i < transPositionsSize && !isStopped() ; i++)
                {

                    if (i != transCounter)
                    {
                        const Eigen::Vector2d &transPos_i = transPositions.at(i).first;
                        vectTrans = transPos_i - transPos;
                        vectTransNorm = vectTrans.norm();

                        // Search for second ref point
                        for (int j = 0 ; j < refPositionSize && !isStopped() ; j++)
                        {

                            if (j != refCounter)
                            {
                                const Eigen::Vector2d &refPos_j = refPositions.at(j).first;

                                vectRef = refPos_j - refPos;

                                // If distances have the same value +- _distThreshold
                                if (fabs(vectRef.norm() - vectTransNorm) < _distThreshold)
                                {

                                    // Compute rotation for trans points
                                    cosTheta = vectTrans.dot(vectRef)/(vectTrans.norm()*vectRef.norm());
                                    theta = acos(cosTheta);

                                    orientation = vectRef.x()*vectTrans.y() - vectRef.y()*vectTrans.x();

                                    if (orientation < 0) {theta = M_PIx2 - theta;}

                                    bool rotationPossible = true;
                                    if (maxThetaRad < M_PI)
                                    {
                                        rotationPossible = false;
                                        if ((theta <= maxThetaRad) || (theta >= (M_PIx2 - maxThetaRad))) {rotationPossible = true;}
                                        if (_possiblyInvertedDirection && ((theta >= (M_PI - maxThetaRad)) && (theta <= (M_PI + maxThetaRad)))) {rotationPossible = true;}
                                    }

                                    if (rotationPossible)
                                    {
                                        rotation << cos(theta), -sin(theta),
                                                sin(theta),  cos(theta);

                                        // Trans Bounding Box
                                        minTrans(0) = std::numeric_limits<double>::max();
                                        minTrans(1) = std::numeric_limits<double>::max();
                                        maxTrans(0) = -std::numeric_limits<double>::max();
                                        maxTrans(1) = -std::numeric_limits<double>::max();

                                        // Apply translation and rotation to all trans points
                                        for (int k = 0 ; k < transPositionsTmp.size() ; k++)
                                        {
                                            transPositionsTmp[k].first = transPositions[k].first + delta;
                                            transPositionsTmp[k].first = rotation*(transPositionsTmp[k].first - refPos) + refPos;

                                            const QPair<Eigen::Vector2d, double> &pair = transPositionsTmp.at(k);

                                            if (pair.first(0) < minTrans(0)) {minTrans(0) = pair.first(0);}
                                            if (pair.first(1) < minTrans(1)) {minTrans(1) = pair.first(1);}
                                            if (pair.first(0) > maxTrans(0)) {maxTrans(0) = pair.first(0);}
                                            if (pair.first(1) > maxTrans(1)) {maxTrans(1) = pair.first(1);}
                                        }

                                        minTrans(0) -= _distThreshold;
                                        minTrans(1) -= _distThreshold;
                                        maxTrans(0) += _distThreshold;
                                        maxTrans(1) += _distThreshold;

                                        // Compute match scores

                                        // Number of ref points with a trans point within _distThreshold
                                        Nbref_with_Corresp = 0;
                                        // Number of trans points with a ref point within _distThreshold
                                        Nbtrans_with_Corresp = 0;
                                        // Number of trans points with a ref point within _distThreshold AND relatives sizes are similar (difference < _relativeSizeThreshold)
                                        NbtransRef_Similarity = 0;

                                        refPosCount.fill(false);
                                        transPosCount.fill(false);
                                        similarPosCount.fill(false);

                                        for (int tr = 0 ; tr < transPositionsSize; tr++)
                                        {
                                            const Eigen::Vector2d &trPos = transPositionsTmp.at(tr).first;
                                            trVal = transPositionsTmp.at(tr).second;

                                            if (trPos(0) > minRef(0) &&
                                                    trPos(1) > minRef(1) &&
                                                    trPos(0) < maxRef(0) &&
                                                    trPos(1) < maxRef(1))
                                            {

                                                for (int rf = 0 ; rf < refPositionSize; rf++)
                                                {
                                                    const Eigen::Vector2d &rfPos = refPositions.at(rf).first;

                                                    if (rfPos(0) > minTrans(0) &&
                                                            rfPos(1) > minTrans(1) &&
                                                            rfPos(0) < maxTrans(0) &&
                                                            rfPos(1) < maxTrans(1))
                                                    {
                                                        rfVal = refPositions.at(rf).second;
                                                        dist = (trPos - rfPos).norm();

                                                        if (dist < _distThreshold)
                                                        {
                                                            refPosCount[rf] = true;
                                                            transPosCount[tr] = true;

                                                            sizeDiff = fabs(rfVal - trVal);
                                                            if (sizeDiff < _relativeSizeThreshold)
                                                            {
                                                                similarPosCount[tr] = true;
                                                            }
                                                        }
                                                    }
                                                }
                                            }
                                        }

                                        for (int rf = 0 ; rf < refPositionSize; rf++)
                                        {
                                            if (refPosCount[rf]) {Nbref_with_Corresp++;}
                                        }

                                        for (int tr = 0 ; tr < transPositionsSize; tr++)
                                        {
                                            if (transPosCount[tr]) {Nbtrans_with_Corresp++;}
                                            if (similarPosCount[tr]) {NbtransRef_Similarity++;}
                                        }

                                        // Compute global score
                                        globalScore =   _coef_nbRwc * (double(Nbref_with_Corresp)    / double(refPositions.size()))   +
                                                _coef_nbTwc * (double(Nbtrans_with_Corresp)  / double(transPositionsTmp.size())) +
                                                _coef_nbSim * (double(NbtransRef_Similarity) / double(transPositionsTmp.size())) ;

                                        if (globalScore > bestScore || qFuzzyIsNull(bestScore))
                                        {
                                            bestScore = globalScore;
                                            translationVector = delta;
                                            rotationMatrix = rotation;
                                            rotationCenter = refPos;
                                        }

                                    }

                                }
                            }
                        }
                    }
                }
            }
        }
        setProgress(float(80.0 * double(refCounter) / double(refPositions.size())));
    }

    // Compute transformation matrix
    Eigen::Matrix3d transformationMatrix = computeTransfMatrix2D(rotationCenter, translationVector, rotationMatrix);

    // Apply selected transformation to trans data
    for (int i = 0 ; i < transPositions.size() ; i++)
    {
        Eigen::Vector3d tmp;
        tmp[0] = transPositions[i].first[0];
        tmp[1] = transPositions[i].first[1];
        tmp[2] = 1;
        tmp = transformationMatrix*tmp;

        transPositions[i].first[0] = tmp[0];
        transPositions[i].first[1] = tmp[1];
        //        transPositions[i].first = transPositions[i].first + translationVector;
        //        transPositions[i].first = rotationMatrix*(transPositions[i].first - rotationCenter) + rotationCenter;
    }



    // Export transformed coordinates

    for (CT_ResultGroup* res_trans : _outResultInter.iterateOutputs())
    {
        for (int i = 0 ; i < transPositions.size() ; i++)
        {
            // OUT results creation (move it to the appropried place in the code)
            CT_StandardItemGroup* grpI = new CT_StandardItemGroup();
            res_trans->addRootGroup(_outGrpInter, grpI);

            CT_ReferencePoint* item_transpos = new CT_ReferencePoint(transPositions[i].first[0], transPositions[i].first[1], 0, 0);
            grpI->addSingularItem(_outInter, item_transpos);
        }
    }

    for (CT_ResultGroup* res_trans2 : _outResultTrans.iterateOutputs())
    {
        CT_StandardItemGroup *rootGroup = new CT_StandardItemGroup();
        res_trans2->addRootGroup(_outRootGrpTrans, rootGroup);

        // Find matching points pairs
        // first: compute all ref/trans distances
        QMultiMap<double, QPair<int, int> > distances;
        for (int refCounter = 0 ; refCounter < refPositions.size() ; refCounter++)
        {
            const Eigen::Vector2d &refPos = refPositions.at(refCounter).first;

            for (int transCounter = 0 ; transCounter < transPositions.size() ; transCounter++)
            {
                const Eigen::Vector2d &transPos = transPositions.at(transCounter).first;

                Eigen::Vector2d delta = (transPos - refPos);

                if (delta.norm() < _distThreshold)
                {
                    distances.insert(delta.norm(), QPair<int, int>(refCounter, transCounter));
                }
            }
        }

        // second: keep only one correspondance for each ref/trans
        QList<QPair<int, int> > candidates = distances.values();
        QMap<int, int> correspondances;
        while (!candidates.isEmpty())
        {
            QPair<int, int> pair = candidates.takeFirst();
            correspondances.insert(pair.first, pair.second);

            for (int i = candidates.size() - 1 ; i >= 0 ; i--)
            {
                const QPair<int, int> &pair2 = candidates.at(i);
                if (pair.first == pair2.first || pair.second == pair2.second)
                {
                    candidates.removeAt(i);
                }
            }
        }

        int nbMatches = correspondances.size();

        Eigen::MatrixXd refMat(nbMatches, 3);
        Eigen::MatrixXd transMat(nbMatches, 3);

        // put coordinates in matrices
        QMapIterator<int, int> it(correspondances);
        int cpt = 0;
        while (it.hasNext())
        {
            it.next();

            const Eigen::Vector2d &refPos = refPositions.at(it.key()).first;
            const Eigen::Vector2d &transPos = transPositions.at(it.value()).first;

            refMat(cpt, 0) = refPos(0);
            refMat(cpt, 1) = refPos(1);
            refMat(cpt, 2) = 0;

            transMat(cpt, 0) = transPos(0);
            transMat(cpt, 1) = transPos(1);
            transMat(cpt, 2) = 0;
            ++cpt;
        }

        Eigen::Vector3d center3D;
        Eigen::Vector3d translation3D;

        Eigen::Matrix3d rotationMatrix3D = Kabsch(refMat, transMat, nbMatches, center3D, translation3D);
        Eigen::Matrix2d rotationMatrix2D = rotationMatrix3D.block<2,2>(0,0);

        Eigen::Vector2d rotationCenter2D;
        Eigen::Vector2d translationVector2D;
        rotationCenter2D[0] = center3D[0];
        rotationCenter2D[1] = center3D[1];
        translationVector2D[0] = translation3D[0];
        translationVector2D[1] = translation3D[1];

        Eigen::Matrix3d transformationMatrix2 = computeTransfMatrix2D(rotationCenter2D, translationVector2D, rotationMatrix2D);

        // Apply selected transformation to trans data
        for (int i = 0 ; i < transPositions.size() ; i++)
        {
            Eigen::Vector3d tmp;
            tmp[0] = transPositions[i].first[0];
            tmp[1] = transPositions[i].first[1];
            tmp[2] = 1;
            tmp = transformationMatrix2*tmp;

            transPositions[i].first[0] = tmp[0];
            transPositions[i].first[1] = tmp[1];
        }

        // Quality criteria
        double rmseDist = 0;
        double rmseVal = 0;
        double maxDist = 0;
        double maxVal = 0;


        // Export transformed coordinates
        for (int i = 0 ; i < transPositions.size() ; i++)
        {
            const Eigen::Vector2d &transPos = transPositions.at(i).first;
            double transVal = transPositions.at(i).second;
            QString transId = transIds.value(i, "");

            // OUT results creation (move it to the appropried place in the code)
            CT_StandardItemGroup* grp_grp2= new CT_StandardItemGroup();
            rootGroup->addGroup(_outGrpTrans, grp_grp2);

            CT_ReferencePoint* item_transpos2;
            double zValTrans = transVal;
            double transValAbs = transVal*deltaTrans + minTransValue;
            double delta = _maxval - _minval;

            if (_relativeMode == 0) // absolute value
            {
                zValTrans = transValAbs;
            } else {
                zValTrans = zValTrans*delta + _minval;
            }

            if (_drawMode == 0)
            {
                item_transpos2 = new CT_ReferencePoint(transPos[0], transPos[1], zValTrans, 0);
            } else {
                item_transpos2 = new CT_ReferencePoint(transPos[0], transPos[1], 0, zValTrans);
            }
            grp_grp2->addSingularItem(_outTransRefPoint, item_transpos2);

            item_transpos2->addItemAttribute(_outTransId, new CT_StdItemAttributeT<QString>(CT_AbstractCategory::DATA_ID, transId));

            int refIndice = correspondances.key(i, -1);
            if (refIndice >= 0)
            {
                const Eigen::Vector2d &refPos = refPositions.at(refIndice).first;
                double refVal = refPositions.at(refIndice).second;
                QString refId = refIds.value(refIndice);
                double distance = (refPos - transPos).norm();

                CT_ReferencePoint* item_refposCorresp;
                double zValRef = refVal;
                double refValAbs = refVal*deltaRef + minRefValue;

                double deltaVal = transValAbs - refValAbs;

                if (_relativeMode == 0) // absolute value
                {
                    zValRef = refValAbs;
                } else {
                    zValRef = zValRef*delta + _minval;
                }

                if (_drawMode == 0)
                {
                    item_refposCorresp = new CT_ReferencePoint(refPos[0], refPos[1], zValRef, 0);
                } else {
                    item_refposCorresp = new CT_ReferencePoint(refPos[0], refPos[1], 0, zValRef);
                }
                grp_grp2->addSingularItem(_outTransposCorresp, item_refposCorresp);

                item_transpos2->addItemAttribute(_outRefCorrespId, new CT_StdItemAttributeT<QString>(CT_AbstractCategory::DATA_ID, refId));
                item_transpos2->addItemAttribute(_outDeltaValue, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, deltaVal));
                item_transpos2->addItemAttribute(_outDistance, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, distance));

                deltaDistMap.insert(refIndice, distance);
                deltaValMap.insert(refIndice, deltaVal);

                rmseDist += distance*distance;
                rmseVal += deltaVal*deltaVal;

                if (distance > maxDist) {maxDist = distance;}
                if (fabs(deltaVal) > maxVal) {maxVal = fabs(deltaVal);}

                CT_Line *line;
                if (_drawMode == 0)
                {
                    line = new CT_Line(new CT_LineData(Eigen::Vector3d(transPos[0], transPos[1], zValTrans), Eigen::Vector3d(refPos[0], refPos[1], zValRef)));
                } else {
                    line = new CT_Line(new CT_LineData(Eigen::Vector3d(transPos[0], transPos[1], 0), Eigen::Vector3d(refPos[0], refPos[1], 0)));
                }
                grp_grp2->addSingularItem(_outCorrespLine, line);

            }
        }

        rmseDist = sqrt(rmseDist/double(nbMatches));
        rmseVal  = sqrt(rmseVal/double(nbMatches));

        CT_ItemAttributeList *attributes = new CT_ItemAttributeList();
        rootGroup->addSingularItem(_outAttributes, attributes);

        attributes->addItemAttribute(_outAttRmseDist, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, rmseDist));
        attributes->addItemAttribute(_outAttRmseVal, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, rmseVal));
        attributes->addItemAttribute(_outAttMaxDist, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, maxDist));
        attributes->addItemAttribute(_outAttMaxDistDiff, new CT_StdItemAttributeT<double>(CT_AbstractCategory::DATA_VALUE, maxVal));

        Eigen::Matrix3d resultingMatrix = transformationMatrix2*transformationMatrix;

        Eigen::Matrix4d transf3D = Eigen::Matrix4d::Identity(4,4);
        transf3D(0,0) = resultingMatrix(0,0);
        transf3D(0,1) = resultingMatrix(0,1);
        transf3D(1,0) = resultingMatrix(1,0);
        transf3D(1,1) = resultingMatrix(1,1);

        transf3D(0,3) = resultingMatrix(0,2);
        transf3D(1,3) = resultingMatrix(1,2);

        CT_TransformationMatrix *transfMatItem = new CT_TransformationMatrix(transf3D);
        rootGroup->addSingularItem(_outTrMat, transfMatItem);

        QString plotName;
        if (idRefPlot.size() > 0) {plotName = "_";}
        plotName.append(idRefPlot);

        QFileInfo fileInfo((_reportFileName.size()>0)?_reportFileName.first():"");
        QFile reportFile(QString("%1/%2%3.%4").arg(fileInfo.absolutePath(), fileInfo.baseName(), plotName, fileInfo.suffix()));

        QFile transformedDataFile((_transformedDataFileName.size()>0)?_transformedDataFileName.first():"");
        QFile transformationDataFile((_transformationDataFileName.size()>0)?_transformationDataFileName.first():"");

        QTextStream streamReport(&reportFile);
        QTextStream streamTransformed(&transformedDataFile);
        QTextStream streamTransformation(&transformationDataFile);

        bool exportReport = reportFile.open(QIODevice::WriteOnly | QIODevice::Text);
        bool exportTransformedData = false;
        bool exportTransformationData = false;

        if (first)
        {
            exportTransformedData = transformedDataFile.open(QIODevice::WriteOnly | QIODevice::Truncate | QIODevice::Text);
            exportTransformationData = transformationDataFile.open(QIODevice::WriteOnly | QIODevice::Truncate | QIODevice::Text);
        } else {
            exportTransformedData = transformedDataFile.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text);
            exportTransformationData = transformationDataFile.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text);
        }

        if (exportReport)
        {
            streamReport << "# Computree Matching report (generated by PluginBase/ONF_StepMatchItemsPositions)\n";
            streamReport << "\n";
            streamReport << "# 2D Transformation Matrix:\n";
            streamReport << QString::number(resultingMatrix(0,0), 'f', 10) << "\t" << QString::number(resultingMatrix(0,1), 'f', 10) << "\t" << QString::number(resultingMatrix(0,2), 'f', 10) << "\n";
            streamReport << QString::number(resultingMatrix(1,0), 'f', 10) << "\t" << QString::number(resultingMatrix(1,1), 'f', 10) << "\t" << QString::number(resultingMatrix(1,2), 'f', 10) << "\n";
            streamReport << QString::number(resultingMatrix(2,0), 'f', 10) << "\t" << QString::number(resultingMatrix(2,1), 'f', 10) << "\t" << QString::number(resultingMatrix(2,2), 'f', 10) << "\n";
            streamReport << "\n";
            streamReport << "# 3D Transformation Matrix:\n";
            streamReport << transfMatItem->getTransformationMatrixAsString();
            streamReport << "\n";
            streamReport << "# Matching quality criteria:\n";
            streamReport << "Number of reference positions                   :" << "\t" << refPositions.size() << "\n";
            streamReport << "Number of transformed positions                 :" << "\t" << transPositions.size() << "\n";
            streamReport << "Number of matching positions                    :" << "\t" << nbMatches << "\n";
            streamReport << "RMSE of matching distances (m)                  :" << "\t" << rmseDist << "\n";
            streamReport << "RMSE of differences between matching size values:" << "\t" << rmseVal << "\n";
            streamReport << "Maximum matching distance (m)                   :" << "\t" << maxDist << "\n";
            streamReport << "Maximum difference between matching size values :" << "\t" << maxVal << "\n";
            streamReport << "\n";
            streamReport << "# Matching algorithm parameters:\n";
            streamReport << "Maximum distance between matching positions (m)                           :" << "\t" << _distThreshold << "\n";
            streamReport << "Maximum allowed relative difference between size values to accept matching:" << "\t" << _relativeSizeThreshold << "\n";
            streamReport << "Minimum size value to consider transformed position in matching           :" << "\t" << _minRelativeSize << "\n";
            streamReport << "Score weight for reference positions positive matching                    :" << "\t" << _coef_nbRwc << "\n";
            streamReport << "Score weight for transformed positions positive matching                  :" << "\t" << _coef_nbTwc << "\n";
            streamReport << "Score weight for size similarity                                          :" << "\t" << _coef_nbSim << "\n";
            streamReport << "\n";
            streamReport << "\n";

            streamReport << "Algorithm from:\n";
            streamReport << "Marius Hauglin, Vegard Lien, Erik Naesset & Terje Gobakken (2014)\n";
            streamReport << "Geo-referencing forest field plots by co-registration of terrestrial and airborne laser scanning data,\n";
            streamReport << "International Journal of Remote Sensing, 35:9, 3135-3149, DOI: 10.1080/01431161.2014.903440\n";

            streamReport << "\n";
            streamReport << "\n";

            streamReport << "# Positions data:\n";
            streamReport << "IDref\tXref\tYref\tValref\tIDtrans\tXtrans\tYtrans\tValtrans\tDeltaDist\tDeltaVal\n";
        }

        if (exportTransformationData)
        {

            if (first)
            {
                streamTransformation << "IDplot" << "\t";
                streamTransformation << "Mat00" << "\t";
                streamTransformation << "Mat01" << "\t";
                streamTransformation << "Mat10" << "\t";
                streamTransformation << "Mat11" << "\t";
                streamTransformation << "Mat02" << "\t";
                streamTransformation << "Mat12" << "\t";
                streamTransformation << "NbRef" << "\t";
                streamTransformation << "NbTrans" << "\t";
                streamTransformation << "NbMatch" << "\t";
                streamTransformation << "RMSEdist" << "\t";
                streamTransformation << "RMSEdeltaVal" << "\t";
                streamTransformation << "MaxMatchDist" << "\t";
                streamTransformation << "MaxDeltaVal" << "\t";
                streamTransformation << "MaxdistParam" << "\t";
                streamTransformation << "MaxDeltaValParam" << "\t";
                streamTransformation << "MinSizeParam" << "\t";
                streamTransformation << "RefWeightParam" << "\t";
                streamTransformation << "TransWeightParam" << "\t";
                streamTransformation << "SimilirityWeigthParam" << "\n";
            }

            streamTransformation << idRefPlot << "\t";
            streamTransformation << QString::number(resultingMatrix(0,0), 'f', 10) << "\t";
            streamTransformation << QString::number(resultingMatrix(0,1), 'f', 10) << "\t";
            streamTransformation << QString::number(resultingMatrix(1,0), 'f', 10) << "\t";
            streamTransformation << QString::number(resultingMatrix(1,1), 'f', 10) << "\t";
            streamTransformation << QString::number(resultingMatrix(0,2), 'f', 10) << "\t";
            streamTransformation << QString::number(resultingMatrix(1,2), 'f', 10) << "\t";
            streamTransformation << refPositions.size() << "\t";
            streamTransformation << transPositions.size() << "\t";
            streamTransformation << nbMatches << "\t";
            streamTransformation << rmseDist << "\t";
            streamTransformation << rmseVal << "\t";
            streamTransformation << maxDist << "\t";
            streamTransformation << maxVal << "\t";
            streamTransformation << _distThreshold << "\t";
            streamTransformation << _relativeSizeThreshold << "\t";
            streamTransformation << _minRelativeSize << "\t";
            streamTransformation << _coef_nbRwc << "\t";
            streamTransformation << _coef_nbTwc << "\t";
            streamTransformation << _coef_nbSim << "\n";
        }

        if (first)
        {
            if (exportTransformedData)
            {
                streamTransformed << "IDplot\tIDref\tXref\tYref\tValref\tIDtrans\tXtrans\tYtrans\tValtrans\tDeltaDist\tDeltaVal\n";
            }
            first = false;
        }

        for (int refCounter = 0 ; refCounter < refPositions.size() ; refCounter++)
        {
            const Eigen::Vector2d &refPos = refPositions.at(refCounter).first;
            double refVal = refPositions.at(refCounter).second;
            double refValAbs = refVal*deltaRef + minRefValue;

            if (exportReport)
            {
                streamReport << refIds.value(refCounter, "");
                streamReport << "\t" << QString::number(refPos[0], 'f', 4);
                streamReport << "\t" << QString::number(refPos[1], 'f', 4);
                streamReport << "\t" << refValAbs;
            }

            if (exportTransformedData)
            {
                streamTransformed << idRefPlot;
                streamTransformed << "\t" << refIds.value(refCounter, "");
                streamTransformed << "\t" << QString::number(refPos[0], 'f', 4);
                streamTransformed << "\t" << QString::number(refPos[1], 'f', 4);
                streamTransformed << "\t" << refValAbs;
            }

            int transIndice = correspondances.value(refCounter, -1);
            if (transIndice >= 0)
            {
                const Eigen::Vector2d &transPos = transPositions.at(transIndice).first;
                double transVal = transPositions.at(transIndice).second;
                double transValAbs = transVal*deltaTrans + minTransValue;

                if (exportReport)
                {
                    streamReport << "\t" << transIds.value(transIndice, "");
                    streamReport << "\t" << QString::number(transPos[0], 'f', 4);
                    streamReport << "\t" << QString::number(transPos[1], 'f', 4);
                    streamReport << "\t" << transValAbs;
                    streamReport << "\t" << deltaDistMap.value(refCounter);
                    streamReport << "\t" << deltaValMap.value(refCounter);
                }

                if (exportTransformedData)
                {
                    streamTransformed << "\t" << transIds.value(transIndice, "");
                    streamTransformed << "\t" << QString::number(transPos[0], 'f', 4);
                    streamTransformed << "\t" << QString::number(transPos[1], 'f', 4);
                    streamTransformed << "\t" << transValAbs;
                    streamTransformed << "\t" << deltaDistMap.value(refCounter);
                    streamTransformed << "\t" << deltaValMap.value(refCounter);
                }

            } else {
                if (exportReport)
                {
                    streamReport << "\t\t\t\t\t\t";
                }

                if (exportTransformedData)
                {
                    streamTransformed << "\t\t\t\t\t\t";
                }
            }

            if (exportReport)
            {
                streamReport << "\n";
            }

            if (exportTransformedData)
            {
                streamTransformed << "\n";
            }
        }

        for (int transCounter = 0 ; transCounter < transPositions.size() ; transCounter++)
        {
            const Eigen::Vector2d &transPos = transPositions.at(transCounter).first;
            double transVal = transPositions.at(transCounter).second;
            double transValAbs = transVal*deltaTrans + minTransValue;

            if (correspondances.key(transCounter, -1) < 0)
            {
                if (exportReport)
                {
                    streamReport << "\t\t\t";
                    streamReport << "\t" << transIds.value(transCounter, "");
                    streamReport << "\t" << QString::number(transPos[0], 'f', 4);
                    streamReport << "\t" << QString::number(transPos[1], 'f', 4);
                    streamReport << "\t" << transValAbs;
                    streamReport << "\t\t\n";
                }

                if (exportTransformedData)
                {
                    streamTransformed << "\t\t\t";
                    streamTransformed << "\t" << transIds.value(transCounter, "");
                    streamTransformed << "\t" << QString::number(transPos[0], 'f', 4);
                    streamTransformed << "\t" << QString::number(transPos[1], 'f', 4);
                    streamTransformed << "\t" << transValAbs;
                    streamTransformed << "\t\t\n";
                }

            }
        }

        if (exportReport)
        {
            streamReport << "\n";
        }

        if (exportReport) {reportFile.close();}
        if (exportTransformedData) {transformedDataFile.close();}
        if (exportTransformationData) {transformationDataFile.close();}

    }

}

// Algorithm from https://github.com/MichalNowicki/Scientific/tree/master/TrackSLAM/Matching
Eigen::Matrix3d ONF_StepMatchItemsPositions::Kabsch(Eigen::MatrixXd &refPositions, Eigen::MatrixXd &transPositions, int pointsNumber, Eigen::Vector3d &center, Eigen::Vector3d &translation)
{
    double means[6];
    // Shift to center
    for(int j = 0 ; j < 6 ; j++) {means[j] = 0.0;}

    for(int j = 0 ; j < pointsNumber ; j++)
    {
        for(int h = 0 ; h < 3 ; h++) {means[h] += transPositions(j, h);}
        for(int h = 0 ; h < 3 ; h++) {means[h+3] += refPositions(j, h);}
    }
    for(int j = 0 ; j < 6 ; j++) {means[j] /= pointsNumber;}

    // After counting, we must subtract the arithmetic
    for(int j = 0 ; j < pointsNumber ; j++)
    {
        for(int h = 0 ; h < 3 ; h++) {transPositions(j, h) -= means[h];}
        for(int h = 0 ; h < 3 ; h++) {refPositions(j, h) -= means[h+3];}
    }

    // counting transpose P * Q
    Eigen::MatrixXd A = transPositions.transpose() * refPositions;

    // SVD
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd V = svd.matrixU();
    Eigen::MatrixXd W = svd.matrixV();

    // Do a right-handed
    Eigen::Matrix3d U = Eigen::Matrix3d::Identity(3,3);
    U(2,2) = sgn(A.determinant());

    // Rotation matrix is:
    U = W * U * V.transpose();

    for(int h = 0 ; h < 3 ; h++) {center[h] = means[h+3];}
    for(int h = 0 ; h < 3 ; h++) {translation[h] = means[h+3] - means[h];}

    return U;
}

Eigen::Matrix3d ONF_StepMatchItemsPositions::computeTransfMatrix2D(const Eigen::Vector2d &center, const Eigen::Vector2d &tranlation, const Eigen::Matrix2d &rotMat)
{
    Eigen::Vector2d T;
    T[0] = 0;
    T[1] = 0;

    T -= center;
    T += tranlation;
    T = rotMat*T;

    T += center;

    Eigen::Matrix3d transfMat =  Eigen::Matrix3d::Identity(3,3);

    transfMat(0,0) = rotMat(0,0);
    transfMat(0,1) = rotMat(0,1);
    transfMat(1,0) = rotMat(1,0);
    transfMat(1,1) = rotMat(1,1);
    transfMat(0,2) = T[0];
    transfMat(1,2) = T[1];

    return transfMat;
}