#include "tk_stepreducepointsdensity.h"

#include "ct_accessor/ct_pointaccessor.h"
#include "ct_log/ct_logmanager.h"

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

TK_StepReducePointsDensity::TK_StepReducePointsDensity() : SuperClass()
{
    _resolution = 0.005;
}

QString TK_StepReducePointsDensity::description() const
{
    return tr("Réduire la Densité de points");
}

QString TK_StepReducePointsDensity::detailledDescription() const
{
    return tr("Créée une grille régulière de la <b>résolution</b> choisie. Ne garde que le point le plus proche du centre dans chaque case. ");
}

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

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

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

// Création et affiliation des modèles OUT
void TK_StepReducePointsDensity::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outScene, tr("Scène extraite"));
}

void TK_StepReducePointsDensity::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{


    postInputConfigDialog->addDouble(tr("Résolution de la grille :"), "cm", 0, 10000, 3, _resolution, 100);
}

void TK_StepReducePointsDensity::compute()
{

    CT_PointAccessor pAccess;

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

            CT_PointIterator itP(inScene->pointCloudIndex());
            size_t n_points = itP.size();

            _minx = inScene->minX();
            _miny = inScene->minY();
            _minz = inScene->minZ();

            _dimx = size_t(ceil((inScene->maxX() - _minx)/_resolution));
            _dimy = size_t(ceil((inScene->maxY() - _miny)/_resolution));
            _dimz = size_t(ceil((inScene->maxZ() - _minz)/_resolution));

            _halfResolution = _resolution / 2;

            PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène d'entrée comporte %1 points.")).arg(n_points));

            QMap<size_t, size_t> indexMap;

            // Extraction des points de la placette
            size_t i = 0;

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

                size_t col, row, levz;
                size_t grdIndex = gridIndex(point(0), point(1), point(2), col, row, levz);

                size_t previousPointGlobalIndex = indexMap.value(grdIndex, std::numeric_limits<size_t>::max());

                if (previousPointGlobalIndex == std::numeric_limits<size_t>::max())
                {
                    indexMap.insert(grdIndex, pointIndex);
                } else {

                    double gridx, gridy, gridz;
                    cellCoordinates(col, row, levz, gridx, gridy, gridz);

                    double distance = pow(point(0) - gridx, 2) + pow(point(1) - gridy, 2) + pow(point(2) - gridz, 2);

                    const CT_Point &previousPoint = pAccess.constPointAt(previousPointGlobalIndex);

                    double previousDistance = pow(previousPoint(0) - gridx, 2) + pow(previousPoint(1) - gridy, 2) + pow(previousPoint(2) - gridz, 2);

                    if (distance < previousDistance)
                        indexMap.insert(grdIndex, pointIndex);
                }
                ++i;

                setProgress(float(50.0 * i / n_points));
            }

            QMapIterator<size_t, size_t> it(indexMap);

            size_t npts = size_t(indexMap.size());
            i = 0;

            CT_PointCloudIndexVector *resPointCloudIndex = new CT_PointCloudIndexVector(npts);
            resPointCloudIndex->setSortType(CT_AbstractCloudIndex::NotSorted);

            while (it.hasNext() && (!isStopped()))
            {
                it.next();

                resPointCloudIndex->replaceIndex(i, int(it.value()));

                setProgress(50 + 49 * i / npts);
                ++i;
            }

            resPointCloudIndex->setSortType(CT_AbstractCloudIndex::SortedInAscendingOrder);

            if (resPointCloudIndex->size() > 0)
            {
                CT_Scene *outScene = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(resPointCloudIndex));
                outScene->updateBoundingBox();

                group->addSingularItem(_outScene, outScene);

                PS_LOG->addMessage(LogInterface::info, LogInterface::step, QString(tr("La scène de densité réduite comporte %1 points.")).arg(outScene->pointCloudIndex()->size()));
            } else {

                PS_LOG->addMessage(LogInterface::info, LogInterface::step, tr("Aucun point conservé pour cette scène"));
            }

        }

        setProgress(99);
    }

}

size_t TK_StepReducePointsDensity::gridIndex(const double &x, const double &y, const double &z, size_t &colx, size_t &liny, size_t &levz) const
{
    colx = size_t(floor((x - _minx) / _resolution));
    liny = size_t(floor((y - _miny) / _resolution));
    levz = size_t(floor((z - _minz) / _resolution));

    return levz*_dimx*_dimy + liny*_dimx + colx;;
}

void TK_StepReducePointsDensity::cellCoordinates(const size_t &colx, const size_t &liny, const size_t &levz, double &x, double &y, double &z) const
{
    x = _minx + colx*_resolution + _halfResolution;
    y = _miny + liny*_resolution + _halfResolution;
    z = _minz + levz*_resolution + _halfResolution;
}