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

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

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

ONF_StepComputeEmptinessGrid::ONF_StepComputeEmptinessGrid() : SuperClass()
{
    _resolution   = 0.20;
}

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

QString ONF_StepComputeEmptinessGrid::detailledDescription() const
{
    return tr("A cell of the emptinexx grid is = true, if it's behind a point, in the direction of the ray."
              "Cells with points set to false."
              "Optionnally, a second result can be set, to restrict computation bounding box to existing item(s) volume.");
}

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

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

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

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

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

void ONF_StepComputeEmptinessGrid::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scene(s)"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scene(s)"));
    manager.addPointAttribute(_inGroup, _inNormals, tr("Ray directions"));

    manager.addResult(_inResultBB, tr("Bounding Box"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultBB, _inZeroOrMoreRootGroupBB);
    manager.addGroup(_inZeroOrMoreRootGroupBB, _inGroupBB);
    manager.addItem(_inGroupBB, _inBB, tr("Bounding Box"));

}

// Création et affiliation des modèles OUT
void ONF_StepComputeEmptinessGrid::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outEmptinessGrid, tr("Emptiness grid"));
}

void ONF_StepComputeEmptinessGrid::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Resolution :"), "m", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _resolution);
}

void ONF_StepComputeEmptinessGrid::compute()
{
    // récupération du résultats IN et OUT
    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;

    for (const CT_AbstractSingularItemDrawable* item : _inBB.iterateInputs(_inResultBB))
    {
        bb = true;

        Eigen::Vector3d min, max;
        item->boundingBox(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);}
    }


    for (CT_StandardItemGroup* group : _inGroup.iterateOutputs(_inResult))
    {
        for (const CT_AbstractItemDrawableWithPointCloud* inScene : group->singularItems(_inScene))
        {
            if (isStopped()) {return;}

            const CT_PointsAttributesNormal *scanDir = group->singularItem(_inNormals);

            if (scanDir != nullptr)
            {
                const CT_AbstractPointCloudIndex *pointCloudIndex = inScene->pointCloudIndex();
                size_t n_points = pointCloudIndex->size();

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

                CT_Grid3D_Sparse<bool> *emptinessGrid = new CT_Grid3D_Sparse<bool>(bot(0), bot(1), bot(2), top(0), top(1), top(2), _resolution, false, false);
                group->addSingularItem(_outEmptinessGrid, emptinessGrid);

                QList<CT_AbstractGrid3DBeamVisitor*> list;

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

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

                CT_Beam beam;

                size_t i = 0;
                CT_PointIterator itP(pointCloudIndex);
                bool isSet;

                while(itP.hasNext() && (!isStopped()))
                {
                    const CT_Point &point = itP.next().currentPoint();
                    const size_t index = itP.currentGlobalIndex();

                    const CT_Normal& ptScanDir = scanDir->constNormalAt(index, &isSet);

                    if(isSet)
                    {
                        Eigen::Vector3d dir(double(ptScanDir(0)), double(ptScanDir(1)), double(ptScanDir(2)));

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

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

                    // progres de 0 à 100
                    setProgress(float(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);
                }
            }
        }
    }

}