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

#include "ct_global/ct_context.h"

#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_result/model/inModel/ct_inresultmodelgrouptocopy.h"
#include "ct_result/model/outModel/ct_outresultmodelgroupcopy.h"
#include "ct_result/model/outModel/tools/ct_outresultmodelgrouptocopypossibilities.h"

#include "ct_result/ct_resultgroup.h"
#include "ct_itemdrawable/ct_scene.h"

#include "ct_pointcloudindex/ct_pointcloudindexvector.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_iterator/ct_resultgroupiterator.h"
#include "ct_view/ct_buttongroup.h"
#include "ct_itemdrawable/ct_grid3d_sparse.h"
#include "ct_itemdrawable/ct_pointsattributesnormal.h"

#include "ct_itemdrawable/ct_beam.h"
#include "ct_itemdrawable/tools/gridtools/ct_grid3dwootraversalalgorithm.h"
#include "tools/onf_settruevisitor.h"


#include <math.h>
#include <iostream>
#include <QList>
#include <limits>

#define DEF_SearchInResult "rin"
#define DEF_SearchInGroup   "gin"
#define DEF_SearchInScene   "scin"
#define DEF_in_scanDir   "norm"

#define DEF_SearchInResultBB "rbb"
#define DEF_SearchInGroupBB "gbb"
#define DEF_SearchInItemBB "itembb"

ONF_StepComputeEmptinessGrid::ONF_StepComputeEmptinessGrid(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _resolution   = 0.20;
}

QString ONF_StepComputeEmptinessGrid::getStepDescription() const
{
    return tr("Compute emptiness voxel grid");
}

QString ONF_StepComputeEmptinessGrid::getStepDetailledDescription() const
{
    return tr("");
}

CT_VirtualAbstractStep* ONF_StepComputeEmptinessGrid::createNewInstance(CT_StepInitializeData &dataInit)
{
    // cree une copie de cette etape
    return new ONF_StepComputeEmptinessGrid(dataInit);
}

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

void ONF_StepComputeEmptinessGrid::createInResultModelListProtected()
{
    CT_InResultModelGroupToCopy *resultModel = createNewInResultModelForCopy(DEF_SearchInResult, tr("Scène(s)"), "", true);

    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_SearchInGroup);
    resultModel->addItemModel(DEF_SearchInGroup, DEF_SearchInScene, CT_Scene::staticGetType(), tr("Scène"));
    resultModel->addItemModel(DEF_SearchInGroup, DEF_in_scanDir, CT_PointsAttributesNormal::staticGetType(), tr("Normals"));

    CT_InResultModelGroup *resultModelBB = createNewInResultModel(DEF_SearchInResultBB, tr("Zone de calcul (optionnel)"), "", true);

    resultModelBB->setZeroOrMoreRootGroup();
    resultModelBB->addGroupModel("", DEF_SearchInGroupBB);
    resultModelBB->addItemModel(DEF_SearchInGroupBB, DEF_SearchInItemBB, CT_AbstractSingularItemDrawable::staticGetType(), tr("Item"), "", CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);

    resultModelBB->setMinimumNumberOfPossibilityThatMustBeSelectedForOneTurn(0);

}

// Création et affiliation des modèles OUT
void ONF_StepComputeEmptinessGrid::createOutResultModelListProtected()
{    
    CT_OutResultModelGroupToCopyPossibilities *resultModel = createNewOutResultModelToCopy(DEF_SearchInResult);

    if (resultModel != NULL)
    {
        resultModel->addItemModel(DEF_SearchInGroup, _outEmptinessGridModelName, new CT_Grid3D_Sparse<bool>(), tr("Emptiness grid"));
    }
}

void ONF_StepComputeEmptinessGrid::createPostConfigurationDialog()
{
    CT_StepConfigurableDialog *configDialog = newStandardPostConfigurationDialog();

    configDialog->addDouble(tr("Resolution :"), "m", -1e+10, 1e+10, 4, _resolution);
}

void ONF_StepComputeEmptinessGrid::compute()
{
    // récupération du résultats IN et OUT
    CT_ResultGroup *outResult = getOutResultList().first();

    Eigen::Vector3d bot, top;

    bot(0) = std::numeric_limits<double>::max();
    bot(1) = std::numeric_limits<double>::max();
    bot(2) = std::numeric_limits<double>::max();

    top(0) = -std::numeric_limits<double>::max();
    top(1) = -std::numeric_limits<double>::max();
    top(2) = -std::numeric_limits<double>::max();

    bool bb = false;

    QList<CT_ResultGroup*> inResults = getInputResults();
    if (inResults.size() > 1)
    {
        CT_ResultGroup* inResBB = inResults.at(1);
        CT_ResultItemIterator itBB(inResBB, this, DEF_SearchInItemBB);
        while(itBB.hasNext())
        {
            CT_AbstractSingularItemDrawable *item = (CT_AbstractSingularItemDrawable*) itBB.next();
            bb = true;

            Eigen::Vector3d min, max;
            item->getBoundingBox(min, max);

            if (min(0) < bot(0)) {bot(0) = min(0);}
            if (min(1) < bot(1)) {bot(1) = min(1);}
            if (min(2) < bot(2)) {bot(2) = min(2);}
            if (max(0) > top(0)) {top(0) = max(0);}
            if (max(1) > top(1)) {top(1) = max(1);}
            if (max(2) > top(2)) {top(2) = max(2);}
        }
    }


    CT_ResultGroupIterator it(outResult, this, DEF_SearchInGroup);
    while(!isStopped() && it.hasNext())
    {
        CT_StandardItemGroup *group = (CT_StandardItemGroup*) it.next();

        if (group != NULL)
        {
            const CT_Scene *in_scene = (CT_Scene*) group->firstItemByINModelName(this, DEF_SearchInScene);
            const CT_PointsAttributesNormal *scanDir = (CT_PointsAttributesNormal*) group->firstItemByINModelName(this, DEF_in_scanDir);

            if (in_scene != NULL && scanDir != NULL)
            {
                const CT_AbstractPointCloudIndex *pointCloudIndex = in_scene->getPointCloudIndex();
                size_t n_points = pointCloudIndex->size();

                const CT_AbstractPointCloudIndex *scanDirPointCloudIndex = scanDir->getPointCloudIndex();
                CT_AbstractNormalCloud* scanDirCloud = scanDir->getNormalCloud();

                if (!bb)
                {
                    bot(0) = in_scene->minX();
                    bot(1) = in_scene->minY();
                    bot(2) = in_scene->minZ();
                    top(0) = in_scene->maxX();
                    top(1) = in_scene->maxY();
                    top(2) = in_scene->maxZ();
                }

                CT_Grid3D_Sparse<bool> *emptinessGrid = new CT_Grid3D_Sparse<bool>(_outEmptinessGridModelName.completeName(), outResult, bot(0), bot(1), bot(2), top(0), top(1), top(2), _resolution, false, false);
                group->addItemDrawable(emptinessGrid);

                QList<CT_AbstractGrid3DBeamVisitor*> list;

                ONF_SetTrueVisitor trueVisitor(emptinessGrid);
                list.append(&trueVisitor);

                // Creates traversal algorithm
                CT_Grid3DWooTraversalAlgorithm algo(emptinessGrid, true, list);

                CT_Beam beam(NULL, NULL);

                size_t i = 0;
                CT_PointIterator itP(pointCloudIndex);
                while(itP.hasNext() && (!isStopped()))
                {
                    const CT_Point &point = itP.next().currentPoint();
                    size_t index = itP.currentGlobalIndex();
                    size_t localIndex = scanDirPointCloudIndex->indexOf(index);

                    CT_Normal& ptScanDir = scanDirCloud->normalAt(localIndex);
                    Eigen::Vector3d dir(ptScanDir(0), ptScanDir(1), ptScanDir(2));

                    beam.setOrigin(point);
                    beam.setDirection(dir);

                    if (beam.intersect(bot, top))
                    {
                        algo.compute(beam);
                    }

                    // progres de 0 à 100
                    setProgress(90.0*i/n_points);
                    ++i;
                }

                // Set false for all cells containing points
                itP.toFront();
                while(itP.hasNext() && (!isStopped()))
                {
                    const CT_Point &point = itP.next().currentPoint();
                    emptinessGrid->setValueAtXYZ(point(0), point(1), point(2), false);
                }
            }
        }
    }
}