/****************************************************************************

 Copyright (C) 2010-2016 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 Computree version 2.0.

 Computree is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Computree 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 General Public License
 along with Computree.  If not, see <http://www.gnu.org/licenses/>.

*****************************************************************************/

#include "onf_stepcomputeocclusionspace.h"

// Inclusion of used ItemDrawable classes
#include "tools/onf_countvisitor.h"
#include "tools/onf_nullifytraversedcellvisitor.h"

#include "ct_log/ct_logmanager.h"
#include <Eigen/Core>

#include <QFileInfo>

ONF_StepComputeOcclusionsSpace::ONF_StepComputeOcclusionsSpace() : SuperClass()
{
    _res = 0.03;
    _distThreshold = 0.3;
    _gridMode = 3;
    _xBase = 0.0;
    _yBase = 0.0;
    _zBase = 0.0;

    _xDim = int(10.0/_res);
    _yDim = int(10.0/_res);
    _zDim = int(20.0/_res);
}

QString ONF_StepComputeOcclusionsSpace::description() const
{
    // Gives the descrption to print in the GUI
    return tr("Compute occlusions space");
}

QString ONF_StepComputeOcclusionsSpace::detailledDescription() const
{
    return tr("Cette étape calcul les zones d'occlusion : une certain distance derrière les impacts (points). "
              "Si un autre rayon passe au même endroit (avant l'impact) l'occlusion est effacée. ");
}

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

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

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


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

void ONF_StepComputeOcclusionsSpace::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scène(s)"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inRootGroup);
    manager.addGroup(_inRootGroup, _inGroup, tr("Scan"));
    manager.addItem(_inGroup, _inScene, tr("Scène"));
    manager.addItem(_inGroup, _inScanner, tr("Scanner"));
}

void ONF_StepComputeOcclusionsSpace::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outGrid, tr("Occlusion space"));
}

void ONF_StepComputeOcclusionsSpace::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Resolution of the grids"),tr("meters"),0.0001,10000,2, _res );
    postInputConfigDialog->addDouble(tr("Distance Threshold"), "m", 0, std::numeric_limits<double>::max(), 4, _distThreshold);

    postInputConfigDialog->addEmpty();

    postInputConfigDialog->addText(tr("Reference for (minX, minY, minZ) corner of the grid :"),"", "");

    CT_ButtonGroup &bg_gridMode = postInputConfigDialog->addButtonGroup(_gridMode);
    postInputConfigDialog->addExcludeValue("", "", tr("Default mode : Bounding box of the scene"), bg_gridMode, 0);
    postInputConfigDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates:"), bg_gridMode, 1);
    postInputConfigDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates + custom dimensions:"), bg_gridMode, 2);
    postInputConfigDialog->addExcludeValue("", "", tr("Custom mode : centered on folowing coordinates + custom dimensions:"), bg_gridMode, 3);

    postInputConfigDialog->addDouble(tr("X coordinate:"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _xBase);
    postInputConfigDialog->addDouble(tr("Y coordinate:"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _yBase);
    postInputConfigDialog->addDouble(tr("Z coordinate:"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _zBase);

    postInputConfigDialog->addInt(tr("X dimension:"), "cells", 1, 1000000, _xDim);
    postInputConfigDialog->addInt(tr("Y dimension:"), "cells", 1, 1000000, _yDim);
    postInputConfigDialog->addInt(tr("Z dimension:"), "cells", 1, 1000000, _zDim);

}

void ONF_StepComputeOcclusionsSpace::compute()
{
    for (CT_StandardItemGroup* rootGroup : _inRootGroup.iterateOutputs(_inResult))
    {
        QMap<CT_StandardItemGroup*, QPair<const CT_AbstractItemDrawableWithPointCloud*, const CT_Scanner*> > pointsOfView;

        // Global limits of generated grids
        double xMin = std::numeric_limits<double>::max();
        double yMin = std::numeric_limits<double>::max();
        double zMin = std::numeric_limits<double>::max();
        double xMax = -std::numeric_limits<double>::max();
        double yMax = -std::numeric_limits<double>::max();
        double zMax = -std::numeric_limits<double>::max();

        double xMinScene = std::numeric_limits<double>::max();
        double yMinScene = std::numeric_limits<double>::max();
        double zMinScene = std::numeric_limits<double>::max();
        double xMaxScene = -std::numeric_limits<double>::max();
        double yMaxScene = -std::numeric_limits<double>::max();
        double zMaxScene = -std::numeric_limits<double>::max();

        double xMinScanner = std::numeric_limits<double>::max();
        double yMinScanner = std::numeric_limits<double>::max();
        double zMinScanner = std::numeric_limits<double>::max();
        double xMaxScanner = -std::numeric_limits<double>::max();
        double yMaxScanner = -std::numeric_limits<double>::max();
        double zMaxScanner = -std::numeric_limits<double>::max();

        int nscenes = 0;

        for (const CT_StandardItemGroup* group : rootGroup->groups(_inGroup))
        {
            for (const CT_AbstractItemDrawableWithPointCloud* scene : group->singularItems(_inScene))
            {
                const CT_Scanner* scanner = group->singularItem(_inScanner);

                if (scanner != nullptr)
                {
                    nscenes++;
                    pointsOfView.insert(const_cast<CT_StandardItemGroup*>(group), QPair<const CT_AbstractItemDrawableWithPointCloud*, const CT_Scanner*>(scene, scanner));

                    Eigen::Vector3d min, max;
                    scene->boundingBox(min, max);

                    if (min.x() < xMinScene) {xMinScene = min.x();}
                    if (min.y() < yMinScene) {yMinScene = min.y();}
                    if (min.z() < zMinScene) {zMinScene = min.z();}
                    if (max.x() > xMaxScene) {xMaxScene = max.x();}
                    if (max.y() > yMaxScene) {yMaxScene = max.y();}
                    if (max.z() > zMaxScene) {zMaxScene = max.z();}

                    if (scanner->centerX() < xMinScanner) {xMinScanner = scanner->centerX();}
                    if (scanner->centerY() < yMinScanner) {yMinScanner = scanner->centerY();}
                    if (scanner->centerZ() < zMinScanner) {zMinScanner = scanner->centerZ();}

                    if (scanner->centerX() > xMaxScanner) {xMaxScanner = scanner->centerX();}
                    if (scanner->centerY() > yMaxScanner) {yMaxScanner = scanner->centerY();}
                    if (scanner->centerZ() > zMaxScanner) {zMaxScanner = scanner->centerZ();}
                }
            }
        }

        if (_gridMode == 0) {

            xMin = std::min(xMinScene, xMinScanner);
            yMin = std::min(yMinScene, yMinScanner);
            zMin = std::min(zMinScene, zMinScanner);

            xMax = std::max(xMaxScene, xMaxScanner);
            yMax = std::max(yMaxScene, yMaxScanner);
            zMax = std::max(zMaxScene, zMaxScanner);

        } else if (_gridMode == 1) {

            double xMinAdjusted = std::min(xMinScene, xMinScanner);
            double yMinAdjusted = std::min(yMinScene, yMinScanner);
            double zMinAdjusted = std::min(zMinScene, zMinScanner);

            double xMaxAdjusted = std::max(xMaxScene, xMaxScanner);
            double yMaxAdjusted = std::max(yMaxScene, yMaxScanner);
            double zMaxAdjusted = std::max(zMaxScene, zMaxScanner);

            xMin = _xBase;
            yMin = _yBase;
            zMin = _zBase;

            while (xMin < xMinAdjusted) {xMin += _res;}
            while (yMin < yMinAdjusted) {yMin += _res;}
            while (zMin < zMinAdjusted) {zMin += _res;}

            while (xMin > xMinAdjusted) {xMin -= _res;}
            while (yMin > yMinAdjusted) {yMin -= _res;}
            while (zMin > zMinAdjusted) {zMin -= _res;}

            xMax = xMin + _res;
            yMax = yMin + _res;
            zMax = zMin + _res;

            while (xMax < xMaxAdjusted) {xMax += _res;}
            while (yMax < yMaxAdjusted) {yMax += _res;}
            while (zMax < zMaxAdjusted) {zMax += _res;}

        } else if (_gridMode == 2) {

            xMin = _xBase;
            yMin = _yBase;
            zMin = _zBase;

            xMax = xMin + _res*_xDim;
            yMax = yMin + _res*_yDim;
            zMax = zMin + _res*_zDim;

            bool enlarged = false;

            while (xMin > xMinScanner) {xMin -= _res; enlarged = true;}
            while (yMin > yMinScanner) {yMin -= _res; enlarged = true;}
            while (zMin > zMinScanner) {zMin -= _res; enlarged = true;}

            while (xMax < xMaxScanner) {xMax += _res; enlarged = true;}
            while (yMax < yMaxScanner) {yMax += _res; enlarged = true;}
            while (zMax < zMaxScanner) {zMax += _res; enlarged = true;}

            if (enlarged)
            {
                PS_LOG->addMessage(LogInterface::warning, LogInterface::step, tr("Dimensions spécifiées ne contenant pas les positions de scans : la grille a du être élargie !"));
            }
        } else {

            xMin = _xBase - _res*_xDim;
            yMin = _yBase - _res*_yDim;
            zMin = _zBase - _res*_zDim;

            xMax = _xBase + _res*_xDim;
            yMax = _yBase + _res*_yDim;
            zMax = _zBase + _res*_zDim;

            bool enlarged = false;

            while (xMin > xMinScanner) {xMin -= _res; enlarged = true;}
            while (yMin > yMinScanner) {yMin -= _res; enlarged = true;}
            while (zMin > zMinScanner) {zMin -= _res; enlarged = true;}

            while (xMax < xMaxScanner) {xMax += _res; enlarged = true;}
            while (yMax < yMaxScanner) {yMax += _res; enlarged = true;}
            while (zMax < zMaxScanner) {zMax += _res; enlarged = true;}

            if (enlarged)
            {
                PS_LOG->addMessage(LogInterface::warning, LogInterface::step, tr("Dimensions spécifiées ne contenant pas les positions de scans : la grille a du être élargie !"));
            }
        }

        xMin -= _res;
        yMin -= _res;
        zMin -= _res;
        xMax += _res;
        yMax += _res;
        zMax += _res;

        // Declaring the output grids
        CT_Grid3D_Sparse<int>*      occlGrid = CT_Grid3D_Sparse<int>::createGrid3DFromXYZCoords(xMin, yMin, zMin, xMax, yMax, zMax, _res, -1, 0, true);
        //group->addSingularItem(occlGrid);

        setProgress(10);
        int cpt  = 0;
        int cpt2 = 0;

        double progressPart = 40.0 / double(nscenes);

        // 1) Count the number of occluded beams traversing the cell within _threshold
        QMapIterator<CT_StandardItemGroup*, QPair<const CT_AbstractItemDrawableWithPointCloud*, const CT_Scanner*> > it(pointsOfView);
        while (it.hasNext())
        {
            it.next();
            const CT_AbstractItemDrawableWithPointCloud* scene = it.value().first;
            const CT_Scanner* scanner =it.value().second;
            const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();
            size_t npts = pointCloudIndex->size();

            Eigen::Vector3d bot, top;
            occlGrid->getMinCoordinates(bot);
            occlGrid->getMaxCoordinates(top);

            QList<CT_AbstractGrid3DBeamVisitor*> list;

            ONF_CountVisitor countVisitor(occlGrid);
            list.append(&countVisitor);

            // Creates traversal algorithm
            CT_Grid3DWooTraversalAlgorithm algo(occlGrid, false, list);

            CT_Beam beam;

            CT_PointIterator itP(pointCloudIndex);
            while (itP.hasNext())
            {
                const CT_Point &point = itP.next().currentPoint();
                Eigen::Vector3d dir = point - scanner->position();
                dir.normalize();
                Eigen::Vector3d endPoint = point + dir * _distThreshold;

                // Get the next ray
                beam.setOrigin(point);
                beam.setDirection(dir);

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

                if (++cpt % 10000 == 0) {setProgress(float(progressPart*cpt / npts + progressPart*cpt2 + 10.0));}
            }
            ++cpt2;
            cpt = 0;
        }

        cpt  = 0;
        cpt2 = 0;

        // 1) Clean cells traversed by beams
        it.toFront();
        while (it.hasNext() && !isStopped())
        {
            it.next();
            const CT_AbstractItemDrawableWithPointCloud* scene = it.value().first;
            const CT_Scanner* scanner =it.value().second;
            const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();
            size_t npts = pointCloudIndex->size();

            Eigen::Vector3d bot, top;
            occlGrid->getMinCoordinates(bot);
            occlGrid->getMaxCoordinates(top);

            QList<CT_AbstractGrid3DBeamVisitor*> list;

            ONF_NullifyTraveserdCellVisitor nullptrifyVisitor(occlGrid);
            list.append(&nullptrifyVisitor);

            // Creates traversal algorithm
            CT_Grid3DWooTraversalAlgorithm algo(occlGrid, false, list);

            CT_Beam beam;

            CT_PointIterator itP(pointCloudIndex);
            while (itP.hasNext())
            {
                const CT_Point &point = itP.next().currentPoint();
                Eigen::Vector3d dir = scanner->position() - point;
                dir.normalize();

                // Get the next ray
                beam.setOrigin(point);
                beam.setDirection(dir);

                if (beam.intersect(bot, top))
                {
                    Eigen::Vector3d posi = scanner->position();
                    algo.compute(beam, &posi);
                }
                if (++cpt % 10000 == 0) {setProgress(float(progressPart*cpt / npts + progressPart*cpt2 + 50.0));}
            }

            ++cpt2;
            cpt = 0;
        }

        occlGrid->computeMinMax();
        setProgress(90.0f);
        rootGroup->addSingularItem(_outGrid, occlGrid);
    }

    setProgress(100.0f);
}