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

#include <QFileInfo>

#include <Eigen/Core>

ONF_StepComputeNestVolume::ONF_StepComputeNestVolume() : SuperClass()
{
    _radius = 5.0;
    _res = 0.25;
    _nbPtsMin = 2;
    _xBase = 0;
    _yBase = 0;
    _zBase = 0;
    _zoffset = 0.5;
    _gridMode = 0;
}

QString ONF_StepComputeNestVolume::description() const
{
    // Gives the descrption to print in the GUI
    return tr("Délimiter le volume accessible d'un nid");
}

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

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

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

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

CT_VirtualAbstractStep* ONF_StepComputeNestVolume::createNewInstance() const
{
    // Creates an instance of this step
    return new ONF_StepComputeNestVolume();
}

void ONF_StepComputeNestVolume::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)"));
}

void ONF_StepComputeNestVolume::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outhits, tr("Hits"));
    manager.addItem(_inGroup, _outempty, tr("Empty"));
    manager.addItem(_inGroup, _outradius, tr("Radius"));
    manager.addItem(_inGroup, _outdistNest, tr("distNest"));
    manager.addItem(_inGroup, _outtoto, tr("toto"));
}

void ONF_StepComputeNestVolume::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Résolution de la grille"),tr("m"),0.0001,10000,3, _res);
    postInputConfigDialog->addDouble(tr("Rayon de recherche"),tr("m"),0.0001,10000,2, _radius);
    postInputConfigDialog->addInt(tr("Seuil en nombre de points"),tr("m"),0,10000, _nbPtsMin);

    postInputConfigDialog->addDouble(tr("Centre X"), "m", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _xBase);
    postInputConfigDialog->addDouble(tr("Centre Y"), "m", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _yBase);
    postInputConfigDialog->addDouble(tr("Centre Z"), "m", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _zBase);
    postInputConfigDialog->addDouble(tr("Offset en Z"),tr("m"),0,10000,2, _zoffset);

    postInputConfigDialog->addEmpty();
    postInputConfigDialog->addText(tr("Calculer les donées sur "),"", "");

    CT_ButtonGroup &bg_gridMode = postInputConfigDialog->addButtonGroup(_gridMode);
    postInputConfigDialog->addExcludeValue("", "", tr("Une Shpère") , bg_gridMode, 0);
    postInputConfigDialog->addExcludeValue("", "", tr("Un Cylindre"), bg_gridMode, 1);
}

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

            double centerX = _xBase;
            double centerY = _yBase;
            double centerZ = _zBase + _zoffset;

            double resRadius = std::round(_radius / _res) * _res;

            double minX = centerX - resRadius;
            double minY = centerY - resRadius;
            double minZ = centerZ - resRadius;

            double maxX = centerX + resRadius;
            double maxY = centerY + resRadius;
            double maxZ = centerZ + resRadius;

            if (_gridMode == 1)
            {
                minZ = scene->minZ();
                maxZ = scene->maxZ();
            }

            // Points in sphere
            //            CT_PointCloudIndexVector *resPointCloudIndex = new CT_PointCloudIndexVector();
            //            resPointCloudIndex->setSortType(CT_PointCloudIndexVector::NotSorted);

            // Declaring the output grids
            CT_Grid3D_Sparse<int>* hitGrid = CT_Grid3D_Sparse<int>::createGrid3DFromXYZCoords(minX, minY, minZ, maxX, maxY, maxZ, _res, -1, 0);
            CT_Grid3D<bool>* emptyGrid = CT_Grid3D<bool>::createGrid3DFromXYZCoords(minX, minY, minZ, maxX, maxY, maxZ, _res, false, false);

            CT_Grid3D<float>* radiusGrid = CT_Grid3D<float>::createGrid3DFromXYZCoords(minX, minY, minZ, maxX, maxY, maxZ, _res, -1, -1);

            CT_Grid3D<float>* distToNest = CT_Grid3D<float>::createGrid3DFromXYZCoords(minX, minY, minZ, maxX, maxY, maxZ, _res, -1, -1);
            CT_Grid3D<int>* toto = CT_Grid3D<int>::createGrid3DFromXYZCoords(minX, minY, minZ, maxX, maxY, maxZ, _res, -1, -1);

            double radius2 = _radius*_radius;
            setProgress(10);

            CT_PointIterator itP(scene->pointCloudIndex()) ;
            while(itP.hasNext())
            {
                const CT_Point &point = itP.next().currentPoint();

                if (point(0) >= minX && point(1) >= minY && point(2) >= minZ &&
                        point(0) <= maxX && point(1) <= maxY && point(2) <= maxZ)
                {
                    double dist2 = 0;
                    if (_gridMode == 0)
                    {
                        dist2 = pow(point(0) - centerX, 2) + pow(point(1) - centerY, 2) + pow(point(2) - centerZ, 2);
                    } else {
                        dist2 = pow(point(0) - centerX, 2) + pow(point(1) - centerY, 2);
                    }

                    if (dist2 <= radius2)
                    {
                        //                        size_t index = itP.currentGlobalIndex();
                        //                        resPointCloudIndex->addIndex(index);

                        size_t gridIndex;
                        if (hitGrid->indexAtXYZ(point(0), point(1), point(2), gridIndex))
                        {
                            // Hits Computing
                            hitGrid->addValueAtIndex(gridIndex, 1);
                        } else {
                            qDebug() << "Le point n'est pas dans la grille";
                        }
                    }
                }
            }

            setProgress(20);

            QList<Eigen::Vector3d> refPts;

            for (int xx = 0 ; xx < hitGrid->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < hitGrid->ydim() ; yy++)
                {
                    for (int zz = 0 ; zz < hitGrid->zdim() ; zz++)
                    {
                        double cellCenterX = hitGrid->getCellCenterX(xx);
                        double cellCenterY = hitGrid->getCellCenterY(yy);
                        double cellCenterZ = hitGrid->getCellCenterZ(zz);

                        double dist2 = 0;
                        if (_gridMode == 0)
                        {
                            dist2 = pow(cellCenterX - centerX, 2) + pow(cellCenterY - centerY, 2) + pow(cellCenterZ - centerZ, 2);
                        } else {
                            dist2 = pow(cellCenterX - centerX, 2) + pow(cellCenterY - centerY, 2);
                        }

                        if (dist2 < radius2)
                        {
                            if (hitGrid->value(xx, yy, zz) >= _nbPtsMin)
                            {
                                refPts.append(Eigen::Vector3d(cellCenterX, cellCenterY, cellCenterZ));
                            } else {
                                emptyGrid->setValue(xx, yy, zz, true);
                            }
                        }
                    }
                }
            }

            setProgress(40);

            for (int xx = 0 ; xx < emptyGrid->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < emptyGrid->ydim() ; yy++)
                {
                    for (int zz = 0 ; zz < emptyGrid->zdim() ; zz++)
                    {
                        if (emptyGrid->value(xx, yy, zz))
                        {
                            double cellCenterX = emptyGrid->getCellCenterX(xx);
                            double cellCenterY = emptyGrid->getCellCenterY(yy);
                            double cellCenterZ = emptyGrid->getCellCenterZ(zz);

                            double dist2 = 0;
                            if (_gridMode == 0)
                            {
                                dist2 = pow(cellCenterX - centerX, 2) + pow(cellCenterY - centerY, 2) + pow(cellCenterZ - centerZ, 2);
                            } else {
                                dist2 = pow(cellCenterX - centerX, 2) + pow(cellCenterY - centerY, 2);
                            }

                            if (dist2 <= radius2)
                            {
                                radiusGrid->setValue(xx, yy, zz, float(_radius));

                                for (int i = 0 ; i < refPts.size() ; i++)
                                {
                                    const Eigen::Vector3d &refpt = refPts.at(i);
                                    double dist = std::sqrt(pow(cellCenterX - refpt(0), 2) + pow(cellCenterY - refpt(1), 2) + pow(cellCenterZ - refpt(2), 2));

                                    if (dist < double(radiusGrid->value(xx, yy, zz)))
                                    {
                                        radiusGrid->setValue(xx, yy, zz, float(dist));
                                    }
                                }
                            }
                        }
                    }
                }
            }

            setProgress(60);

            hitGrid->computeMinMax();
            group->addSingularItem(_outhits, hitGrid);

            emptyGrid->computeMinMax();
            group->addSingularItem(_outempty, emptyGrid);

            radiusGrid->computeMinMax();
            group->addSingularItem(_outradius, radiusGrid);

            setProgress(65);

            double _distThreshold = 1.4;
            double _initRadius = _distThreshold*2.0;
            int ncell = int(std::ceil(_initRadius / _res));

            int cpt = 0;

            // Trajectories computing
            QMultiMap<float, size_t> distMap;

            size_t gridIndex;
            if (distToNest->indexAtXYZ(centerX, centerY, centerZ, gridIndex))
            {
                distToNest->setValueAtIndex(gridIndex, 0);

                int colx = 0, liny = 0, levz = 0;
                distToNest->indexToGrid(gridIndex, colx, liny, levz);

                int minXX = colx - ncell;
                if (colx < 0) {minXX = 0;}
                int minYY = liny - ncell;
                if (liny < 0) {minYY = 0;}
                int minZZ = levz - ncell;
                if (levz < 0) {minZZ = 0;}

                int maxXX = colx + ncell;
                if (maxXX > distToNest->xdim()) {maxXX = distToNest->xdim();}
                int maxYY = liny + ncell;
                if (maxYY > distToNest->ydim()) {maxYY = distToNest->ydim();}
                int maxZZ = levz + ncell;
                if (maxZZ > distToNest->zdim()) {maxZZ = distToNest->zdim();}

                for (int xx = minXX ; xx <= maxXX ; xx++)
                {
                    for (int yy = minYY ; yy <= maxYY ; yy++)
                    {
                        for (int zz = minZZ ; zz <= maxZZ ; zz++)
                        {
                            if (xx != colx || yy != liny || zz != levz)
                            {
                                if (radiusGrid->value(xx, yy, zz) > 0)
                                {
                                    double cellCenterX = distToNest->getCellCenterX(xx);
                                    double cellCenterY = distToNest->getCellCenterY(yy);
                                    double cellCenterZ = distToNest->getCellCenterZ(zz);

                                    double dist = sqrt(pow(cellCenterX - centerX, 2) + pow(cellCenterY - centerY, 2) + pow(cellCenterZ - centerZ, 2));

                                    distToNest->setValue(xx, yy, zz, float(dist));

                                    size_t grdI = 0;
                                    distToNest->index(xx, yy, zz, grdI);
                                    distMap.insert(float(dist), grdI);
                                }
                            }
                        }
                    }
                }

                int size = distMap.size();
                while (size > 0)
                {
                    QMultiMap<float, size_t> distMapTMP;
                    QMapIterator<float, size_t> it(distMap);
                    while (it.hasNext())
                    {
                        it.next();
                        float distBase = it.key();
                        size_t indexBase = it.value();

                        toto->setValueAtIndex(indexBase, ++cpt);

                        int colx = 0, liny = 0, levz = 0;
                        distToNest->indexToGrid(indexBase, colx, liny, levz);

                        double baseCenterX = distToNest->getCellCenterX(colx);
                        double baseCenterY = distToNest->getCellCenterY(liny);
                        double baseCenterZ = distToNest->getCellCenterZ(levz);

                        int minXX = colx - 1;
                        if (colx < 0) {minXX = 0;}
                        int minYY = liny - 1;
                        if (liny < 0) {minYY = 0;}
                        int minZZ = levz - 1;
                        if (levz < 0) {minZZ = 0;}

                        int maxXX = colx + 1;
                        if (maxXX > distToNest->xdim()) {maxXX = distToNest->xdim();}
                        int maxYY = liny + 1;
                        if (maxYY > distToNest->ydim()) {maxYY = distToNest->ydim();}
                        int maxZZ = levz + 1;
                        if (maxZZ > distToNest->zdim()) {maxZZ = distToNest->zdim();}

                        for (int xx = minXX ; xx <= maxXX ; xx++)
                        {
                            for (int yy = minYY ; yy <= maxYY ; yy++)
                            {
                                for (int zz = minZZ ; zz <= maxZZ ; zz++)
                                {

                                    if (radiusGrid->value(xx, yy, zz) >= float(_distThreshold) && qFuzzyCompare(distToNest->value(xx, yy, zz), distToNest->NA()))
                                    {
                                        double cellCenterX = distToNest->getCellCenterX(xx);
                                        double cellCenterY = distToNest->getCellCenterY(yy);
                                        double cellCenterZ = distToNest->getCellCenterZ(zz);

                                        double dist = sqrt(pow(cellCenterX - baseCenterX, 2) + pow(cellCenterY - baseCenterY, 2) + pow(cellCenterZ - baseCenterZ, 2)) + double(distBase);

                                        distToNest->setValue(xx, yy, zz, float(dist));

                                        size_t grdI = 0;
                                        distToNest->index(xx, yy, zz, grdI);
                                        distMapTMP.insert(float(dist), grdI);
                                    }
                                }
                            }
                        }
                    }

                    distMap.clear();

                    QMapIterator<float, size_t> it2(distMapTMP);
                    while (it2.hasNext())
                    {
                        it2.next();
                        distMap.insert(it2.key(), it2.value());
                    }

                    size = distMap.size();
                }
            }

            distToNest->computeMinMax();
            group->addSingularItem(_outdistNest, distToNest);

            toto->computeMinMax();
            group->addSingularItem(_outtoto, toto);

            //            // creation et ajout de la scene
            //            resPointCloudIndex->setSortType(CT_PointCloudIndexVector::SortedInAscendingOrder);
            //            CT_Scene *outScene = new CT_Scene(_outSceneModelName.completeName(), outResult, PS_REPOSITORY->registerPointCloudIndex(resPointCloudIndex));
            //            outScene->updateBoundingBox();
            //            group->addSingularItem(outScene);

        }
    }

    setProgress(99);
}