#include "onf_stepfiltermaximabyneighbourhood02.h"

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"

ONF_StepFilterMaximaByNeighbourhood02::ONF_StepFilterMaximaByNeighbourhood02() : SuperClass()
{
    _scoreThreshold = 0.5;
    _minRadius = 1.5;
    _maxRadius = 10.0;
}

QString ONF_StepFilterMaximaByNeighbourhood02::description() const
{
    return tr("Filtrer les maxima par voisinage");
}

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

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

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

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

QString ONF_StepFilterMaximaByNeighbourhood02::URL() const
{
    //return tr("STEP URL HERE");
    return SuperClass::URL(); //by default URL of the plugin
}

CT_VirtualAbstractStep* ONF_StepFilterMaximaByNeighbourhood02::createNewInstance() const
{
    return new ONF_StepFilterMaximaByNeighbourhood02();
}

//////////////////// PROTECTED METHODS //////////////////

void ONF_StepFilterMaximaByNeighbourhood02::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("MNS et Maxima"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inImage, tr("MNS"));
    manager.addItem(_inGroup, _inMaxima, tr("Maxima"));


    manager.addResult(_inResultDTM, tr("MNT"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultDTM, _inZeroOrMoreRootGroupDTM);
    manager.addGroup(_inZeroOrMoreRootGroupDTM, _inGroupDTM);
    manager.addItem(_inGroupDTM, _inDTM, tr("MNT"));
}

void ONF_StepFilterMaximaByNeighbourhood02::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _filteredMaxima, tr("Maxima filtrés"));
}

void ONF_StepFilterMaximaByNeighbourhood02::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{

    postInputConfigDialog->addDouble(tr("Seuil de profondeur pour regrouper deux houppiers"), "m", 0, 10, 2, _scoreThreshold);
    postInputConfigDialog->addDouble(tr("Rayon de houppier minimal"), "m", 0, 1000, 2, _minRadius);
    postInputConfigDialog->addDouble(tr("Rayon de houppier maximal"), "m", 0, 1000, 2, _maxRadius);
}

void ONF_StepFilterMaximaByNeighbourhood02::compute()
{

    const CT_Image2D<float>* mnt = nullptr;
    for (const CT_Image2D<float>* constMnt : _inDTM.iterateInputs(_inResultDTM))
    {
        mnt = constMnt;
    }

    for (CT_StandardItemGroup* grp : _inGroup.iterateOutputs(_inResult))
    {
        if (isStopped()) {return;}

        const CT_Image2D<float>* imageIn = grp->singularItem(_inImage);
        CT_Image2D<qint32>* maximaIn = const_cast<CT_Image2D<qint32>*>(grp->singularItem(_inMaxima));

        if (maximaIn != nullptr && imageIn != nullptr)
        {
            Eigen::Vector2d min;
            maximaIn->getMinCoordinates(min);
            CT_Image2D<qint32>* filteredMaxima = new CT_Image2D<qint32>(min(0), min(1), maximaIn->xdim(), maximaIn->ydim(), maximaIn->resolution(), maximaIn->level(), maximaIn->NA(), 0);
            grp->addSingularItem(_filteredMaxima, filteredMaxima);

            filteredMaxima->getMat().release();
            filteredMaxima->getMat() = maximaIn->getMat().clone();

            setProgress(20);

            // Get maxima coordinates list
            QMultiMap<qint32, Eigen::Vector3d*> maximaCoords;
            QMultiMap<double, qint32> maximaHeights;

            for (int xx = 0 ; xx < maximaIn->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < maximaIn->ydim() ; yy++)
                {
                    qint32 maximaID = maximaIn->value(xx, yy);

                    if (maximaID > 0 && maximaID != maximaIn->NA())
                    {
                        Eigen::Vector3d* coords = new Eigen::Vector3d();
                        if (maximaIn->getCellCenterCoordinates(xx, yy, *coords))
                        {
                            (*coords)(2) = double(imageIn->value(xx, yy));
                            double h = (*coords)(2);

                            if (mnt != nullptr)
                            {
                                float zmnt = mnt->valueAtCoords((*coords)(0), (*coords)(1));
                                if (!qFuzzyCompare(zmnt, mnt->NA()))
                                {
                                    h -= double(zmnt);
                                }
                            }

                            maximaCoords.insert(maximaID, coords);
                            maximaHeights.insert(h, maximaID);
                        }
                    }
                }
            }

            setProgress(25);

            // Compute ordered vector of maxima ids
            QList<qint32> validMaxima;

            QMapIterator<double, qint32> itH(maximaHeights);
            itH.toBack();
            while (itH.hasPrevious())
            {
                itH.previous();
                qint32 cl = itH.value();
                if (!validMaxima.contains(cl)) {validMaxima.append(cl);}
            }

            QVector<qint32> orderedMaxima = validMaxima.toVector();
            int mxSize = orderedMaxima.size();
            validMaxima.clear();

            CT_Grid3D_Points* optimGrid = new CT_Grid3D_Points(maximaIn->minX(), maximaIn->minY(), 0 - _maxRadius / 10.0, maximaIn->xdim(), maximaIn->ydim(), 1, _maxRadius/2.0);

            // Create maxima coords vector
            QVector<Eigen::Vector3d> coords(mxSize);
            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 id = orderedMaxima.at(i);

                QList<Eigen::Vector3d*> coordinates = maximaCoords.values(id);
                coords[i] = *(coordinates.at(0));

                // Compute position of the current maxima if more than one pixel
                int size = coordinates.size();
                if (size > 1)
                {
                    for (int j = 1 ; j < size ; j++)
                    {
                        Eigen::Vector3d* pos = coordinates.at(j);
                        coords[i](0) += (*pos)(0);
                        coords[i](1) += (*pos)(1);
                        if ((*pos)(2) > coords[i](2)) {coords[i](2) = (*pos)(2);}
                    }

                    coords[i](0) /= size;
                    coords[i](1) /= size;
                }

                size_t cellIndex = 0;
                optimGrid->indexAtXYZ(coords[i](0), coords[i](1), 0, cellIndex);
                optimGrid->addPointAtIndex(cellIndex, size_t(i));
            }

            setProgress(30);

            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 id = orderedMaxima.at(i);

                if (id > 0)
                {
                    size_t cellIndex = 0;
                    optimGrid->indexAtXYZ(coords[i](0), coords[i](1), 0, cellIndex);

                    QList<size_t> maximaToTriangulate;
                    optimGrid->getPointsInCellsIntersectingSphere(cellIndex, _maxRadius*2.0, &maximaToTriangulate);

                    if (maximaToTriangulate.size() > 0)
                    {
                        bool modified = true;

                        while (modified)
                        {
                            modified = false;
                            // Compute triangulation
                            CT_DelaunayTriangulation *delaunay = new CT_DelaunayTriangulation();
                            delaunay->init(maximaIn->minX(), maximaIn->minY(), maximaIn->maxX(), maximaIn->maxY());

                            CT_DelaunayVertex* baseVertex = delaunay->addVertex(new Eigen::Vector3d(coords[i](0), coords[i](1), i), true);

                            for (int j = 0 ; j < maximaToTriangulate.size() ; j++)
                            {
                                int idx = int(maximaToTriangulate.at(j));
                                qint32 id2 = orderedMaxima.at(idx);

                                if (idx != i && id2 > 0)
                                {
                                    delaunay->addVertex(new Eigen::Vector3d(coords[idx](0), coords[idx](1), idx), true);
                                }
                            }
                            delaunay->doInsertion();
                            delaunay->computeVerticesNeighbors();

                            QList<CT_DelaunayVertex*> &neighbours = baseVertex->getNeighbors();

                            for (int j = 0 ; j < neighbours.size() ; j++)
                            {
                                CT_DelaunayVertex* neighbour = neighbours.at(j);

                                double score = computeScore(imageIn, baseVertex, neighbour);

                                if (score < _scoreThreshold)
                                {
                                    orderedMaxima[qint32((*(neighbour->getData()))(2))] = 0;
                                    modified = true;
                                }
                            }
                            delete delaunay;
                        }
                    }
                }
            }

            setProgress(60);

            for (int i = 0 ; i < mxSize ; i++)
            {
                qint32 cl = orderedMaxima.at(i);

                if (cl > 0) {validMaxima.append(cl);}
            }

            setProgress(70);

            QMap<qint32, qint32> newIds;
            qint32 cpt = 1;
            // effectively delete toRemove maximum and numbers them in a continuous way
            for (int xx = 0 ; xx < filteredMaxima->xdim() ; xx++)
            {
                for (int yy = 0 ; yy < filteredMaxima->ydim() ; yy++)
                {
                    qint32 maximaID = filteredMaxima->value(xx, yy);

                    if (maximaID > 0)
                    {
                        if (validMaxima.contains(maximaID))
                        {
                            qint32 newId = newIds.value(maximaID, 0);
                            if (newId == 0)
                            {
                                newId = cpt++;
                                newIds.insert(maximaID, newId);
                            }
                            filteredMaxima->setValue(xx, yy, newId);
                        } else {
                            filteredMaxima->setValue(xx, yy, 0);
                        }
                    }
                }
            }
            newIds.clear();
            setProgress(90);

            filteredMaxima->computeMinMax();

            qDeleteAll(maximaCoords.values());
            setProgress(99);
        }
    }
    setProgress(100);
}

double ONF_StepFilterMaximaByNeighbourhood02::computeScore(const CT_Image2D<float>* heightImage, CT_DelaunayVertex* baseVertex, CT_DelaunayVertex* neighbourVertex)
{
    Eigen::Vector3d baseCoord3D = *(baseVertex->getData());
    Eigen::Vector3d neighbCoord3D = *(neighbourVertex->getData());

    baseCoord3D(2) = double(heightImage->valueAtCoords(baseCoord3D(0), baseCoord3D(1)));
    neighbCoord3D(2) = double(heightImage->valueAtCoords(neighbCoord3D(0), neighbCoord3D(1)));

    if (neighbCoord3D(2) > baseCoord3D(2)) {return std::numeric_limits<double>::max();}

    Eigen::Vector2d baseCoord(baseCoord3D(0), baseCoord3D(1));
    Eigen::Vector2d neighbCoord(neighbCoord3D(0), neighbCoord3D(1));

    double dist = sqrt(pow(baseCoord(0) - neighbCoord(0), 2) + pow(baseCoord(1) - neighbCoord(1), 2));

    if (dist <= _minRadius) {return 0;}
    if (dist >= _maxRadius) {return std::numeric_limits<double>::max();}

    double dist3D = sqrt(pow(baseCoord3D(0) - neighbCoord3D(0), 2) + pow(baseCoord3D(1) - neighbCoord3D(1), 2) + pow(baseCoord3D(2) - neighbCoord3D(2), 2));

    Eigen::Vector3d dir = neighbCoord3D - baseCoord3D;

    double offset =  1 / (dist3D / (heightImage->resolution() / 10.0));
    double deltaHMax = 0;

    for (double l = 0 ; l < 1 ; l += offset)
    {
        Eigen::Vector3d pos = baseCoord3D + dir*l;

        float h = heightImage->valueAtCoords(pos(0), pos(1));
        if (!qFuzzyCompare(h, heightImage->NA()))
        {
            double deltaH = pos(2) - double(h);
            if (deltaH > deltaHMax) {deltaHMax = deltaH;}
        }
    }

    return deltaHMax;
}