/****************************************************************************
 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 "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"

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

// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"
#include "ct_itemdrawable/ct_grid3d_sparse.h"
#include "ct_itemdrawable/ct_grid3d.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_pointcloudindex/ct_pointcloudindexvector.h"

#include "ct_view/ct_stepconfigurabledialog.h"

#include <QFileInfo>
#include <QDebug>
#include <limits>
#include <Eigen/Core>

#define DEF_SearchInResult "r"
#define DEF_SearchInScene   "sc"
#define DEF_SearchInGroup   "gr"

#define DEF_itemOut_grxy "grxy"
#define DEF_itemOut_grxz "grxz"
#define DEF_itemOut_gryz "gryz"

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

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

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

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

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

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

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

    if(res != NULL)
    {
        //res->addItemModel(DEF_SearchInGroup, _outSceneModelName, new CT_Scene(), tr("Points"));
        res->addItemModel(DEF_SearchInGroup, _hits_ModelName, new CT_Grid3D_Sparse<int>(), tr("Hits"));
        res->addItemModel(DEF_SearchInGroup, _empty_ModelName, new CT_Grid3D<bool>(), tr("Empty"));
        res->addItemModel(DEF_SearchInGroup, _radius_ModelName, new CT_Grid3D<float>(), tr("Radius"));
        res->addItemModel(DEF_SearchInGroup, _distNest_ModelName, new CT_Grid3D<float>(), tr("distNest"));
        res->addItemModel(DEF_SearchInGroup, _toto_ModelName, new CT_Grid3D<int>(), tr("toto"));
    }
}

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

    configDialog->addDouble(tr("Résolution de la grille"),tr("m"),0.0001,10000,3, _res);
    configDialog->addDouble(tr("Rayon de recherche"),tr("m"),0.0001,10000,2, _radius);
    configDialog->addInt(tr("Seuil en nombre de points"),tr("m"),0,10000, _nbPtsMin);

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

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

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

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

    CT_ResultGroupIterator it(outResult, this, DEF_SearchInGroup);
    // iterate over all groups
    while(!isStopped() && it.hasNext())
    {
        CT_AbstractItemGroup *group = (CT_AbstractItemGroup*)it.next();
        const CT_Scene* scene = (CT_Scene*)group->firstItemByINModelName(this, DEF_SearchInScene);

        if (scene!=NULL)
        {
            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(_hits_ModelName.completeName(), outResult,
                                                                                                minX, minY, minZ,
                                                                                                maxX, maxY, maxZ,
                                                                                                _res, -1, 0);
            CT_Grid3D<bool>* emptyGrid = CT_Grid3D<bool>::createGrid3DFromXYZCoords(_empty_ModelName.completeName(), outResult,
                                                                                                minX, minY, minZ,
                                                                                                maxX, maxY, maxZ,
                                                                                                _res, false, false);

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

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




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

            CT_PointIterator itP(scene->getPointCloudIndex()) ;
            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 (size_t xx = 0 ; xx < hitGrid->xdim() ; xx++)
            {
                for (size_t yy = 0 ; yy < hitGrid->ydim() ; yy++)
                {
                    for (size_t 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 (size_t xx = 0 ; xx < emptyGrid->xdim() ; xx++)
            {
                for (size_t yy = 0 ; yy < emptyGrid->ydim() ; yy++)
                {
                    for (size_t 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, _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 < radiusGrid->value(xx, yy, zz))
                                    {
                                        radiusGrid->setValue(xx, yy, zz, dist);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            setProgress(60);

            hitGrid->computeMinMax();
            group->addItemDrawable(hitGrid);

            emptyGrid->computeMinMax();
            group->addItemDrawable(emptyGrid);

            radiusGrid->computeMinMax();
            group->addItemDrawable(radiusGrid);

            setProgress(65);


            double _distThreshold = 1.4;
            double _initRadius = _distThreshold*2.0;
            int ncell = (_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);

                size_t colx, liny, levz;
                distToNest->indexToGrid(gridIndex, colx, liny, levz);

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

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


                for (size_t xx = minXX ; xx <= maxXX ; xx++)
                {
                    for (size_t yy = minYY ; yy <= maxYY ; yy++)
                    {
                        for (size_t zz = levz ; 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, dist);

                                    size_t grdI;
                                    distToNest->index(xx, yy, zz, grdI);
                                    distMap.insert(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);

                        size_t colx, liny, levz;
                        distToNest->indexToGrid(indexBase, colx, liny, levz);

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


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

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

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

                                    if (radiusGrid->value(xx, yy, zz) >= _distThreshold && 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)) + distBase;

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

                                        size_t grdI;
                                        distToNest->index(xx, yy, zz, grdI);
                                        distMapTMP.insert(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->addItemDrawable(distToNest);

            toto->computeMinMax();
            group->addItemDrawable(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->addItemDrawable(outScene);

        }
    }


    setProgress(99);
}