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


#include "ct_result/model/inModel/ct_inresultmodelgroup.h"
#include "ct_result/model/inModel/ct_inresultmodelgrouptocopy.h"
#include "ct_result/model/outModel/tools/ct_outresultmodelgrouptocopypossibilities.h"

// Inclusion of actions methods
#include "ct_tools/model/ct_outmodelcopyactionaddmodelitemingroup.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_iterator/ct_resultgroupiterator.h"
#include "ct_iterator/ct_resultitemiterator.h"

// Inclusion of standard result class
#include "ct_result/ct_resultgroup.h"

// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"
#include "tools/onf_computehitsthread.h"

#include "ct_itemdrawable/ct_image2d.h"

#include "ct_view/ct_stepconfigurabledialog.h"

#include <QFileInfo>
#include <QDebug>
#include <limits>

#define DEF_SearchInResultSc "rsc"
#define DEF_SearchInGroupSc   "grsc"
#define DEF_SearchInScene   "sc"

#define DEF_SearchInResult "r"
#define DEF_SearchInGroup   "gr"
#define DEF_SearchInGrid   "grid"

ONF_StepCreateSeedGridFromLinesOfScan::ONF_StepCreateSeedGridFromLinesOfScan(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
}

QString ONF_StepCreateSeedGridFromLinesOfScan::getStepDescription() const
{
    // Gives the descrption to print in the GUI
    return tr("Create seed voxel grid from Lines Of Scan");
}

// Step description (tooltip of contextual menu)
QString ONF_StepCreateSeedGridFromLinesOfScan::getStepDetailledDescription() const
{
    return tr("");
}

CT_VirtualAbstractStep* ONF_StepCreateSeedGridFromLinesOfScan::createNewInstance(CT_StepInitializeData &dataInit)
{
    // Creates an instance of this step
    return new ONF_StepCreateSeedGridFromLinesOfScan(dataInit);
}

void ONF_StepCreateSeedGridFromLinesOfScan::createInResultModelListProtected()
{

    CT_InResultModelGroup* resultModelScene = createNewInResultModel(DEF_SearchInResultSc, tr("Scène(s)"), "", true);
    resultModelScene->setZeroOrMoreRootGroup();
    resultModelScene->addGroupModel("", DEF_SearchInGroupSc);
    resultModelScene->addItemModel(DEF_SearchInGroupSc, DEF_SearchInScene, CT_AbstractItemDrawableWithPointCloud::staticGetType(), tr("Scène"));

    CT_InResultModelGroupToCopy *resultModel = createNewInResultModelForCopy(DEF_SearchInResult, tr("Grille"), "", true);

    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_SearchInGroup);
    resultModel->addItemModel(DEF_SearchInGroup, DEF_SearchInGrid, CT_AbstractGrid3D::staticGetType(), tr("Grille"), "", CT_InAbstractModel::C_ChooseMultipleIfMultiple);
}

void ONF_StepCreateSeedGridFromLinesOfScan::createOutResultModelListProtected()
{    
    CT_OutResultModelGroupToCopyPossibilities *res = createNewOutResultModelToCopy(DEF_SearchInResult);

    if(res != NULL)
    {
        res->addItemModel(DEF_SearchInGroup, _outSeedGrid_ModelName, new CT_Grid3D_Sparse<int>(), tr("Grille de graines"));
    }
}


void ONF_StepCreateSeedGridFromLinesOfScan::compute()
{
    // Gets the out result
    CT_ResultGroup* inResult = getInputResults().first();
    CT_ResultGroup* outResult = getOutResultList().first();


    CT_ResultGroupIterator itOut(outResult, this, DEF_SearchInGroup);
    // iterate over all groups
    while(itOut.hasNext())
    {
        CT_AbstractItemGroup *group = (CT_AbstractItemGroup*)itOut.next();
        const CT_AbstractGrid3D* gridIn = (CT_AbstractGrid3D*)group->firstItemByINModelName(this, DEF_SearchInGrid);

        if (gridIn != NULL)
        {
            // Declaring the output grids
            CT_Grid3D_Sparse<int>* outGrid = new CT_Grid3D_Sparse<int>(_outSeedGrid_ModelName.completeName(), outResult, gridIn->minX(), gridIn->minY(), gridIn->minZ(), gridIn->xdim(), gridIn->ydim(), gridIn->zdim(), gridIn->resolution(), -1, -1);


            QList<int> oldVals;
            QList<int> newVals;

            int oldVal;
            int cpt = 1;
            CT_ResultItemIterator it(inResult, this, DEF_SearchInScene);
            // iterate over all groups
            while(it.hasNext())
            {
                CT_Scene *scene = (CT_Scene*)it.next();

                const CT_AbstractPointCloudIndex *pointCloudIndex = scene->getPointCloudIndex();

                Eigen::Vector3d previousPoint;
                bool first = true;
                CT_PointIterator itP(pointCloudIndex);
                while(itP.hasNext())
                {
                    const CT_Point &pt = itP.next().currentPoint();
                    replaceCLusterId(outGrid, pt, cpt);

                    if (first) {first = false;}
                    else {
                        Eigen::Vector3d dir = pt - previousPoint;
                        double length = dir.norm();

                        for (double l = 0 ; l < length ; l += 0.01)
                        {
                            Eigen::Vector3d ptInt = previousPoint + dir * l / length;
                            replaceCLusterId(outGrid, ptInt, cpt);
                        }
                    }

                    previousPoint = pt;
                }
                ++cpt;
            }

            QList<size_t> indices;
            outGrid->getIndicesWithData(indices);
            for (int i = 0 ; i < indices.size() ; i++)
            {
                size_t index = indices.at(i);
                oldVal = outGrid->valueAtIndex(index);

                for (int j = 0 ; j < oldVals.size() ; j++)
                {
                    if (oldVal == oldVals.at(j))
                    {
                        outGrid->setValueAtIndex(index, newVals.at(j));
                    }
                }
            }

            outGrid->computeMinMax();
            group->addItemDrawable(outGrid);
        }
    }

    setProgress(99);
}

void ONF_StepCreateSeedGridFromLinesOfScan::replaceCLusterId(CT_Grid3D_Sparse<int>* outGrid, const Eigen::Vector3d &pt, int newId)
{
    int oldVal = outGrid->valueAtXYZ(pt(0), pt(1), pt(2));

    if (oldVal != newId)
    {
        if (oldVal > 0)
        {
            QList<size_t> indices;
            outGrid->getIndicesWithData(indices);
            for (int i = 0 ; i < indices.size() ; i++)
            {
                size_t index = indices.at(i);

                if (outGrid->valueAtIndex(index) == oldVal)
                {
                    outGrid->setValueAtIndex(index, newId);
                }
            }
        }
        outGrid->setValueAtXYZ(pt(0), pt(1), pt(2), newId);
    }
}