#include "onf_stepfilterwires.h"

#include "ct_shapedata/ct_linedata.h"

#include "ct_accessor/ct_pointaccessor.h"

ONF_StepFilterWires::ONF_StepFilterWires() : SuperClass()
{
    _nbNeigbours = 10;
    _distMax = 1.0;
    _resGrid = 0.1;

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

}

QString ONF_StepFilterWires::description() const
{
    return tr("Filtrage des fils / cables");
}

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

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

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

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

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

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

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

void ONF_StepFilterWires::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Point cloud"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Point cloud"));
}

void ONF_StepFilterWires::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addPointAttribute(_inGroup, _outSlope, tr("Slope"));
    manager.addPointAttribute(_inGroup, _outRMSE, tr("RMSE"));
    manager.addItem(_inGroup, _outSceneKept, tr("Scène conservée"));
    manager.addItem(_inGroup, _outSceneWires, tr("fils / cables"));
}

void ONF_StepFilterWires::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addInt("Nombre de voisins à rechercher", "", 1, 9999, _nbNeigbours);
    postInputConfigDialog->addDouble("Distance maximum pour les voisins à rechercher", "", 0, 9999 ,2 , _distMax);
    postInputConfigDialog->addDouble(tr("Resolution d'optimisation"), "", 0.1, 99999, 2, _resGrid);

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

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

}

void ONF_StepFilterWires::compute()
{
    CT_PointAccessor accessor;

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

        const CT_AbstractItemDrawableWithPointCloud* scene = grp_inG->singularItem(_inScene);

        if (scene != nullptr)
        {
            const CT_AbstractPointCloudIndex *pointCloudIndex = scene->pointCloudIndex();

            CT_Grid3D_Points* grid = CT_Grid3D_Points::createGrid3DFromXYZCoords(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));
            }

            auto slopeSetter = _outSlope.createAttributesSetter(scene->pointCloudIndexRegistered().dynamicCast<CT_NMPCIR::element_type>());
            auto rmseSetter = _outRMSE.createAttributesSetter(scene->pointCloudIndexRegistered().dynamicCast<CT_NMPCIR::element_type>());

            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 = float(abs(direction(2)));
                    rmse = float(line->getRMSE());

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

                    bool keepRMSE = (rmse > float(_rmseThreshold));
                    if (_rmseMax) {keepRMSE = (rmse < float(_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;}

                    slopeSetter.setValueWithGlobalIndex(index, slope);
                    rmseSetter.setValueWithGlobalIndex(index, rmse);
                } else {
                    cloudWires->addIndex(index);
                }
                cptPt++;
            }

            if (cloudKept->size() > 0)
            {
                cloudKept->setSortType(CT_PointCloudIndexVector::SortedInAscendingOrder);
                CT_Scene *outSceneKept = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(cloudKept));
                outSceneKept->updateBoundingBox();
                grp_inG->addSingularItem(_outSceneKept, outSceneKept);
            } else {
                delete cloudKept;
            }

            if (cloudWires->size() > 0)
            {
                cloudWires->setSortType(CT_PointCloudIndexVector::SortedInAscendingOrder);
                CT_Scene *outSceneWires = new CT_Scene(PS_REPOSITORY->registerPointCloudIndex(cloudWires));
                outSceneWires->updateBoundingBox();
                grp_inG->addSingularItem(_outSceneWires, outSceneWires);
            } else {
                delete cloudWires;
            }

            grp_inG->addSingularItem(_outSlope, _outSlope.createAttributeInstance(scene->pointCloudIndexRegistered(), minSlope, maxSlope));
            grp_inG->addSingularItem(_outRMSE, _outRMSE.createAttributeInstance(scene->pointCloudIndexRegistered(), minRMSE, maxRMSE));

            delete grid;
        }

    }
}