#include "onf_stepfilterwatershedbyradius.h"
#include "ct_itemdrawable/tools/iterator/ct_groupiterator.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_result/ct_resultgroup.h"
#include "ct_result/model/inModel/ct_inresultmodelgrouptocopy.h"
#include "ct_result/model/outModel/tools/ct_outresultmodelgrouptocopypossibilities.h"
#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_itemdrawable/abstract/ct_abstractitemdrawablewithpointcloud.h"
#include "ct_pointcloudindex/ct_pointcloudindexvector.h"
#include "ct_itemdrawable/ct_scene.h"

#include <QDebug>

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

// Alias for indexing models
#define DEFin_res "res"
#define DEFin_grp "grp"
#define DEFin_clusters "clusters"
#define DEFin_raster "raster"

// Constructor : initialization of parameters
ONF_StepFilterWatershedByRadius::ONF_StepFilterWatershedByRadius(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _radius = 10.0;
}

// Step description (tooltip of contextual menu)
QString ONF_StepFilterWatershedByRadius::getStepDescription() const
{
    return tr("Filter un watershed par rayon (d'apex)");
}

// Step detailled description
QString ONF_StepFilterWatershedByRadius::getStepDetailledDescription() const
{
    return tr("No detailled description for this step");
}

// Step URL
QString ONF_StepFilterWatershedByRadius::getStepURL() const
{
    //return tr("STEP URL HERE");
    return CT_AbstractStep::getStepURL(); //by default URL of the plugin
}

// Step copy method
CT_VirtualAbstractStep* ONF_StepFilterWatershedByRadius::createNewInstance(CT_StepInitializeData &dataInit)
{
    return new ONF_StepFilterWatershedByRadius(dataInit);
}

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

// Creation and affiliation of IN models
void ONF_StepFilterWatershedByRadius::createInResultModelListProtected()
{
    CT_InResultModelGroupToCopy *resIn_res = createNewInResultModelForCopy(DEFin_res, tr("Image 2D"));
    resIn_res->setZeroOrMoreRootGroup();
    resIn_res->addGroupModel("", DEFin_grp, CT_AbstractItemGroup::staticGetType(), tr("Groupe"));
    resIn_res->addItemModel(DEFin_grp, DEFin_clusters, CT_Image2D<qint32>::staticGetType(), tr("Clusters"));
    resIn_res->addItemModel(DEFin_grp, DEFin_raster, CT_Image2D<float>::staticGetType(), tr("Hauteurs"));
}

// Creation and affiliation of OUT models
void ONF_StepFilterWatershedByRadius::createOutResultModelListProtected()
{
    CT_OutResultModelGroupToCopyPossibilities *resCpy_res = createNewOutResultModelToCopy(DEFin_res);
    if (resCpy_res != NULL)
    {
        resCpy_res->addItemModel(DEFin_grp, _outClusters_ModelName, new CT_Image2D<qint32>(), tr("Clusters filtrés"));
    }
}

// Semi-automatic creation of step parameters DialogBox
void ONF_StepFilterWatershedByRadius::createPostConfigurationDialog()
{
    CT_StepConfigurableDialog *configDialog = newStandardPostConfigurationDialog();

    configDialog->addDouble(tr("Rayon de filtrage : "), "m", 0, std::numeric_limits<double>::max(), 4, _radius);
}

void ONF_StepFilterWatershedByRadius::compute()
{
    QList<CT_ResultGroup*> outResultList = getOutResultList();
    CT_ResultGroup* resOut = outResultList.at(0);

    // COPIED results browsing
    CT_ResultGroupIterator itCpy_grp(resOut, this, DEFin_grp);
    while (itCpy_grp.hasNext() && !isStopped())
    {
        CT_StandardItemGroup* grp = (CT_StandardItemGroup*) itCpy_grp.next();
        
        CT_Image2D<qint32>* inClusters = (CT_Image2D<qint32>*)grp->firstItemByINModelName(this, DEFin_clusters);
        CT_Image2D<float>* raster = (CT_Image2D<float>*)grp->firstItemByINModelName(this, DEFin_raster);

        if (inClusters != NULL && raster != NULL)
        {
            CT_Image2D<qint32>* outClusters = new CT_Image2D<qint32>(_outClusters_ModelName.completeName(), resOut, inClusters->minX(), inClusters->minY(), inClusters->colDim(), inClusters->linDim(), inClusters->resolution(), inClusters->level(), inClusters->NA(), inClusters->NA());
            //CT_Image2D<qint32>* outClusters = new CT_Image2D<qint32>(_outClusters_ModelName.completeName(), resOut, inClusters->minX(), inClusters->minY(), inClusters->colDim(), inClusters->linDim(), inClusters->resolution(), inClusters->level(), -1, -1);

            QVector<double> zmax(inClusters->dataMax()+1);
            zmax.fill(-std::numeric_limits<double>::max());

            QVector<Eigen::Vector3d> centers(inClusters->dataMax()+1);

            for (int col = 0 ; col < inClusters->colDim() ; col++)
            {
                for (int lin = 0 ; lin < inClusters->linDim() ; lin++)
                {
                    qint32 cluster = inClusters->value(col, lin);
                    float val = raster->value(col, lin);

                    if (cluster != inClusters->NA() && val != raster->NA())
                    {
                        if (val > zmax[cluster])
                        {
                            zmax[cluster] = val;
                            inClusters->getCellCenterCoordinates(col, lin, centers[cluster]);
                        }
                    }
                }
            }

            for (int col = 0 ; col < inClusters->colDim() ; col++)
            {
                for (int lin = 0 ; lin < inClusters->linDim() ; lin++)
                {
                    qint32 cluster = inClusters->value(col, lin);

                    if (cluster != inClusters->NA() && zmax[cluster] > -std::numeric_limits<double>::max())
                    {
                        Eigen::Vector3d cellCenter;
                        inClusters->getCellCenterCoordinates(col, lin, cellCenter);

                        double distance = std::sqrt(pow(cellCenter(0) - centers[cluster](0), 2) + pow(cellCenter(1) - centers[cluster](1), 2));

                        if (distance <= _radius)
                        {
                            outClusters->setValue(col, lin, cluster);
                        }
                    }
                }
            }

            outClusters->computeMinMax();
            grp->addItemDrawable(outClusters);
        }
    }
}