#include "onf_stepstandardizeintensity.h"

#include "ct_log/ct_logmanager.h"

ONF_StepStandardizeIntensity::ONF_StepStandardizeIntensity() : SuperClass()
{
    _meanHeight = 500.0;
}

QString ONF_StepStandardizeIntensity::description() const
{
    return tr("Standardiser l'intensité (ALS)");
}

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

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

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

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

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

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

void ONF_StepStandardizeIntensity::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scene(s)"), "", true);
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scene(s)"));
    manager.addItem(_inGroup, _inAttLAS, tr("Attributs LAS"));

    manager.addResult(_inResultTraj, tr("Trajectory"), "", true);
    manager.setZeroOrMoreRootGroup(_inResultTraj, _inZeroOrMoreRootGroupTraj);
    manager.addGroup(_inZeroOrMoreRootGroupTraj, _inGroupTraj);
    manager.addItem(_inGroupTraj, _inTraj, tr("Trajectory"));
}

void ONF_StepStandardizeIntensity::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _lasAttributesOut, tr("Attributs LAS (Icorr)"));
    manager.addItem(_inGroup, _outIntensityAttribute, tr("Corrected intensity"));
}

void ONF_StepStandardizeIntensity::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{

    postInputConfigDialog->addDouble(tr("Altitude moyenne du vol"), "m", 1, std::numeric_limits<double>::max(), 2, _meanHeight);
}

void ONF_StepStandardizeIntensity::compute()
{
    QList<const CT_ScanPath*> trajectories;

    for (const CT_ScanPath* scanPath : _inTraj.iterateInputs(_inResultTraj))
    {
        trajectories.append(scanPath);
    }

    if (trajectories.size() == 0)
        return;

    auto iterator = _inScene.iterateOutputs(_inResult);

    auto it = iterator.begin();
    auto end = iterator.end();

    while(it != end)
    {
        const CT_AbstractItemDrawableWithPointCloud* scene = *it;

        CT_StandardItemGroup* grp = it.currentParent();

        const CT_StdLASPointsAttributesContainer* attributeLAS = nullptr;
        CT_AbstractPointAttributesScalar* attributeGPS = nullptr;
        CT_AbstractPointAttributesScalar* attributeIntensity = nullptr;

        for(const CT_StdLASPointsAttributesContainer* attLAS : grp->singularItems(_inAttLAS))
        {
            attributeGPS          = dynamic_cast<CT_AbstractPointAttributesScalar*>(attLAS->pointsAttributesAt(CT_LasDefine::GPS_Time));
            attributeIntensity    = dynamic_cast<CT_AbstractPointAttributesScalar*>(attLAS->pointsAttributesAt(CT_LasDefine::Intensity));

            if (attributeIntensity != nullptr && attributeGPS != nullptr)
            {
                attributeLAS = attLAS;
                break;
            }
        }

        if (isStopped())
            return;

        if (attributeIntensity != nullptr && attributeGPS != nullptr)
        {
            // Put old not modified data in new container
            // CT_LasDefine::Intensity : Modified after
            CT_StdLASPointsAttributesContainer* container = new CT_StdLASPointsAttributesContainer(*attributeLAS);

            auto intensityCorrectedSetter = _outIntensityAttribute.createAttributesSetter(scene->pointCloudIndexRegistered().dynamicCast<CT_NMPCIR::element_type>());

            double minAttribute = std::numeric_limits<double>::max();
            double maxAttribute = -std::numeric_limits<double>::max();

            size_t localIndex = 0;
            size_t errorCount = 0;
            const size_t n_points = scene->pointCloudIndexSize();

            CT_PointIterator itP(scene->pointCloudIndexRegistered());

            while(itP.hasNext())
            {
                if (isStopped())
                    return;

                CT_Point point = itP.next().currentPoint();
                const auto gi = itP.currentGlobalIndex();

                if(attributeGPS->hasBeenSet(gi) && attributeIntensity->hasBeenSet(gi))
                {
                    double gpsTime = attributeGPS->scalarAsDoubleAt(gi);
                    double intensity = attributeIntensity->scalarAsDoubleAt(gi);

                    bool found = false;
                    Eigen::Vector3d trajCoord;

                    for (int tra = 0 ; tra < trajectories.size() && !found ; tra++)
                    {
                        const CT_ScanPath* sp = trajectories.at(tra);
                        if (sp->isInScanPath(gpsTime))
                        {
                            trajCoord = const_cast<CT_ScanPath*>(sp)->getPathPointForGPSTime(gpsTime);
                            found = true;
                        }
                    }

                    double length = -1;
                    double corItensity = intensity;
                    if (found)
                    {
                        length = sqrt(pow(point(0) - trajCoord(0), 2) + pow(point(1) - trajCoord(1), 2) + pow(point(2) - trajCoord(2), 2));
                        double ratio = length/_meanHeight;
                        ratio = ratio*ratio;
                        corItensity = corItensity * ratio;

                        if (corItensity < minAttribute) {minAttribute = corItensity;}
                        if (corItensity > maxAttribute) {maxAttribute = corItensity;}
                    } else {

                        errorCount++;
                        if (errorCount < 5)
                        {
                            PS_LOG->addMessage(LogInterface::warning, LogInterface::step, QString(tr("Pas d'information de trajectoire pour le point (%1 ; %2 ; %3)")).arg(point(0)).arg(point(1)).arg(point(2)));
                        }
                    }

                    intensityCorrectedSetter.setValueWithGlobalIndex(gi, corItensity);
                }

                setProgress( 95.0f*float(localIndex++) / float(n_points));
            }

            if (errorCount > 0) {PS_LOG->addMessage(LogInterface::warning, LogInterface::step, QString(tr("Pas d'information de trajectoire pour au total %1 points sur %2")).arg(errorCount).arg(localIndex));}

            CT_PointsAttributesScalarTemplated<double>* attOut_Intensity = _outIntensityAttribute.createAttributeInstance(scene->pointCloudIndexRegistered(), minAttribute, maxAttribute);

            container->insertPointsAttributesAt(CT_LasDefine::Intensity, attOut_Intensity);

            grp->addSingularItem(_outIntensityAttribute, attOut_Intensity);
            grp->addSingularItem(_lasAttributesOut, container);

        }

        ++it;

    }

}