#include "onf_stepfilterwires.h"

#include "ct_itemdrawable/abstract/ct_abstractitemdrawablewithpointcloud.h"
#include "ct_itemdrawable/tools/iterator/ct_groupiterator.h"
#include "ct_itemdrawable/ct_pointsattributesscalartemplated.h"
#include "ct_itemdrawable/ct_grid3d_points.h"
#include "ct_itemdrawable/ct_scene.h"
#include "ct_result/ct_resultgroup.h"
#include "ct_result/model/inModel/ct_inresultmodelgroup.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_pointcloudindex/ct_pointcloudindexvector.h"

#include "ct_shapedata/ct_linedata.h"

#include "ct_accessor/ct_pointaccessor.h"
#include "ct_iterator/ct_pointiterator.h"

// Alias for indexing models
#define DEFin_r "r"
#define DEFin_g "g"
#define DEFin_scene "scene"


// Constructor : initialization of parameters
ONF_StepFilterWires::ONF_StepFilterWires(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _nbNeigbours = 10;
    _distMax = 1.0;
    _resGrid = 0.1;

    _slopeThreshold = 0.05;
    _rmseThreshold  = 0.02;
    _slopeMax = false;
    _rmseMax = false;

}

// Step description (tooltip of contextual menu)
QString ONF_StepFilterWires::getStepDescription() const
{
    return tr("Filtrage des fils / cables");
}

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

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

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

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

// Creation and affiliation of IN models
void ONF_StepFilterWires::createInResultModelListProtected()
{
    CT_InResultModelGroupToCopy *resIn_r = createNewInResultModelForCopy(DEFin_r);
    resIn_r->setZeroOrMoreRootGroup();
    resIn_r->addGroupModel("", DEFin_g);
    resIn_r->addItemModel(DEFin_g, DEFin_scene, CT_AbstractItemDrawableWithPointCloud::staticGetType(), tr("Point cloud"));

}

// Creation and affiliation of OUT models
void ONF_StepFilterWires::createOutResultModelListProtected()
{
    CT_OutResultModelGroupToCopyPossibilities *resOut_r = createNewOutResultModelToCopy(DEFin_r);

    if(resOut_r != NULL)
    {
        resOut_r->addItemModel(DEFin_g, m_slope_autoRenameModel, new CT_PointsAttributesScalarTemplated<float>(), tr("Slope"));
        resOut_r->addItemModel(DEFin_g, m_rmse_autoRenameModel, new CT_PointsAttributesScalarTemplated<float>(), tr("RMSE"));
        resOut_r->addItemModel(DEFin_g, _outSceneKeptModelName, new CT_Scene(), tr("Scène conservée"));
        resOut_r->addItemModel(DEFin_g, _outSceneWiresModelName, new CT_Scene(), tr("fils / cables"));
    }
}

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

    configDialog->addInt("Nombre de voisins à rechercher", "", 1, 9999, _nbNeigbours);
    configDialog->addDouble("Distance maximum pour les voisins à rechercher", "", 0, 9999 ,2 , _distMax);
    configDialog->addDouble(tr("Resolution d'optimisation"), "", 0.1, 99999, 2, _resGrid);

    configDialog->addEmpty();
    configDialog->addDouble(tr("Seuil de pente"), "", 0, 1, 2, _slopeThreshold);
    configDialog->addBool(tr("Seuil Maxi pour la pente ?"), "", "", _slopeMax);

    configDialog->addEmpty();
    configDialog->addDouble(tr("Seuil de RMSE"), "", 0, 99999, 10, _rmseThreshold);
    configDialog->addBool(tr("Seuil Maxi pour la RMSE ?"), "", "", _rmseMax);


}

void ONF_StepFilterWires::compute()
{
    CT_PointAccessor accessor;

    QList<CT_ResultGroup*> outResultList = getOutResultList();
    CT_ResultGroup* _res = outResultList.at(0);

    // IN results browsing
    CT_ResultGroupIterator itIn_g(_res, this, DEFin_g);
    while (itIn_g.hasNext() && !isStopped())
    {
        CT_AbstractItemGroup* grpIn_g = (CT_AbstractItemGroup*) itIn_g.next();
        const CT_AbstractItemDrawableWithPointCloud* scene = (CT_AbstractItemDrawableWithPointCloud*)grpIn_g->firstItemByINModelName(this, DEFin_scene);

        if (scene != NULL)
        {
            const CT_AbstractPointCloudIndex *pointCloudIndex = scene->getPointCloudIndex();
            size_t n_points = pointCloudIndex->size();


            CT_Grid3D_Points* grid = CT_Grid3D_Points::createGrid3DFromXYZCoords(NULL, NULL,
                                                                scene->minX() - 1.0, scene->minY() - 1.0, scene->minZ() - 1.0,
                                                                scene->maxX() + 1.0, scene->maxY() + 1.0, scene->maxZ() + 1.0,
                                                                _resGrid, false);

            CT_PointIterator it(pointCloudIndex);
            while (it.hasNext())
            {
                it.next();
                const CT_Point& pt = it.currentPoint();
                size_t index = it.currentGlobalIndex();
                grid->addPoint(index, pt(0), pt(1), pt(2));
            }

            CT_StandardCloudStdVectorT<float> *slopeCloud = new CT_StandardCloudStdVectorT<float>(n_points);
            CT_StandardCloudStdVectorT<float> *rmseCloud = new CT_StandardCloudStdVectorT<float>(n_points);

            CT_PointCloudIndexVector *cloudKept = new CT_PointCloudIndexVector();
            cloudKept->setSortType(CT_PointCloudIndexVector::NotSorted);

            CT_PointCloudIndexVector *cloudWires = new CT_PointCloudIndexVector();
            cloudWires->setSortType(CT_PointCloudIndexVector::NotSorted);

            float minSlope = std::numeric_limits<float>::max();
            float maxSlope = -std::numeric_limits<float>::max();

            float minRMSE = std::numeric_limits<float>::max();
            float maxRMSE = -std::numeric_limits<float>::max();

            size_t cptPt = 0;
            CT_PointIterator it2(pointCloudIndex);
            while (it2.hasNext())
            {
                it2.next();
                const CT_Point& pt = it2.currentPoint();
                size_t index = it2.currentGlobalIndex();

                QList<size_t> indexList;
                grid->getPointIndicesIncludingKNearestNeighbours(pt, _nbNeigbours, _distMax, indexList);

                QMultiMap<double, size_t> sortedIndices;
                for (int i = 0 ; i < indexList.size() ; i++)
                {
                    size_t neighbIndex = indexList.at(i);

                    if (neighbIndex != index)
                    {
                        const CT_Point& ptNeighb = accessor.constPointAt(neighbIndex);
                        double dist = sqrt(pow(pt(0) - ptNeighb(0), 2) + pow(pt(1) - ptNeighb(1), 2) + pow(pt(2) - ptNeighb(2), 2));
                        if (dist < _distMax)
                        {
                            sortedIndices.insert(dist, neighbIndex);
                        }
                    }
                }

                QList<Eigen::Vector3d> pts;
                QMapIterator<double, size_t> itM(sortedIndices);
                int cpt = 0;
                while (cpt < _nbNeigbours && itM.hasNext())
                {
                    itM.next();
                    size_t index = itM.value();
                    const CT_Point& pt = accessor.constPointAt(index);
                    pts.append(pt);
                    cpt++;
                }

                float slope = 0;
                float rmse = 0;

                if (pts.size() > 2)
                {

                    CT_LineData* line = CT_LineData::staticCreateLineDataFromPointCloud(pts);

                    Eigen::Vector3d direction = line->getDirection();
                    direction.normalize();

                    slope = abs(direction(2));
                    rmse = line->getRMSE();

                    bool keepSlope = (slope > _slopeThreshold);
                    if (_slopeMax) {keepSlope = (slope < _slopeThreshold);}

                    bool keepRMSE = (rmse > _rmseThreshold);
                    if (_rmseMax) {keepRMSE = (rmse < _rmseThreshold);}


                    if (keepSlope || keepRMSE)
                    {
                        cloudKept->addIndex(index);
                    } else {
                        cloudWires->addIndex(index);
                    }

                    if (slope < minSlope) {minSlope = slope;}
                    if (slope > maxSlope) {maxSlope = slope;}
                    if (rmse < minRMSE) {minRMSE = rmse;}
                    if (rmse > maxRMSE) {maxRMSE = rmse;}

                    slopeCloud->replaceT(cptPt, slope);
                    rmseCloud->replaceT(cptPt, rmse);
                } else {
                    cloudWires->addIndex(index);
                }
                cptPt++;
            }

            if (cloudKept->size() > 0)
            {
                cloudKept->setSortType(CT_PointCloudIndexVector::SortedInAscendingOrder);
                CT_Scene *outSceneKept = new CT_Scene(_outSceneKeptModelName.completeName(), _res, PS_REPOSITORY->registerPointCloudIndex(cloudKept));
                outSceneKept->updateBoundingBox();
                grpIn_g->addItemDrawable(outSceneKept);
            } else {
                delete cloudKept;
            }

            if (cloudWires->size() > 0)
            {
                cloudWires->setSortType(CT_PointCloudIndexVector::SortedInAscendingOrder);
                CT_Scene *outSceneWires = new CT_Scene(_outSceneWiresModelName.completeName(), _res, PS_REPOSITORY->registerPointCloudIndex(cloudWires));
                outSceneWires->updateBoundingBox();
                grpIn_g->addItemDrawable(outSceneWires);
            } else {
                delete cloudWires;
            }


            CT_PointsAttributesScalarTemplated<float>*  slopeAtt  = new CT_PointsAttributesScalarTemplated<float>(m_slope_autoRenameModel.completeName(),
                                                                                                                  _res,
                                                                                                                  scene->getPointCloudIndexRegistered(),
                                                                                                                  slopeCloud,
                                                                                                                  minSlope,
                                                                                                                  maxSlope);
            grpIn_g->addItemDrawable(slopeAtt);

            CT_PointsAttributesScalarTemplated<float>* rmseAtt  = new CT_PointsAttributesScalarTemplated<float>(m_rmse_autoRenameModel.completeName(),
                                                                                                                  _res,
                                                                                                                  scene->getPointCloudIndexRegistered(),
                                                                                                                  rmseCloud,
                                                                                                                  minRMSE,
                                                                                                                  maxRMSE);
            grpIn_g->addItemDrawable(rmseAtt);

            delete grid;
        }

    }
}