#include "onf_stepcomputescandirection.h"

#include "ct_log/ct_logmanager.h"

ONF_StepComputeScanDirection::ONF_StepComputeScanDirection() : SuperClass()
{
}

QString ONF_StepComputeScanDirection::description() const
{
    return tr("Calcule les directions de scan (ALS)");
}

QString ONF_StepComputeScanDirection::detailledDescription() const
{
    return tr("Utilise la trajectographie du vol Lidar pour recalculer une direction de tir pour chaque point du nuage. ");
}

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

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

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


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

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

void ONF_StepComputeScanDirection::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scene(s)"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scène"));
    manager.addItem(_inGroup, _inLAS, 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_StepComputeScanDirection::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addPointAttribute(_inGroup, _outDir, tr("Scan direction"));
}

void ONF_StepComputeScanDirection::compute()
{
    QList<CT_ScanPath*> trajectories;
    for (const CT_ScanPath* traj : _inTraj.iterateInputs(_inResultTraj))
    {
        trajectories.append(const_cast<CT_ScanPath*>(traj));
    }

    if (trajectories.isEmpty())
        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();

        CT_AbstractPointAttributesScalar* attributeGPS = nullptr;

        for(const CT_StdLASPointsAttributesContainer* attributeLAS : grp->singularItems(_inLAS))
        {
            attributeGPS          = dynamic_cast<CT_AbstractPointAttributesScalar*>(attributeLAS->pointsAttributesAt(CT_LasDefine::GPS_Time));

            if (attributeGPS != nullptr)
                break;
        }

        if (isStopped())
            return;

        if (attributeGPS != nullptr)
        {
            CT_PointIterator itP(scene->pointCloudIndexRegistered());

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

            size_t errorCount = 0;
            size_t i = 0;
            const size_t n_points = itP.size();

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

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

                if(attributeGPS->hasBeenSet(gi))
                {
                    const double gpsTime = attributeGPS->scalarAsDoubleAt(gi);

                    bool found = false;
                    Eigen::Vector3d trajCoord;

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

                    double length = -1;
                    Eigen::Vector3d dir(0,0,0);

                    if (found)
                    {
                        length = sqrt(pow(point(0) - trajCoord(0), 2) + pow(point(1) - trajCoord(1), 2) + pow(point(2) - trajCoord(2), 2));
                        dir = trajCoord - point;
                        dir.normalize();

                    } 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)));
                        }
                    }

                    CT_Normal ctNormal;
                    ctNormal.x() = float(dir(0));
                    ctNormal.y() = float(dir(1));
                    ctNormal.z() = float(dir(2));
                    ctNormal.w() = float(length);

                    setter.setValueWithGlobalIndex(gi, ctNormal);

                    setProgress(float(95.0*i++ / 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(i));}

            if (i > 0)
            {
                grp->addSingularItem(_outDir, _outDir.createAttributeInstance(scene->pointCloudIndexRegistered()));
            }
        }
        ++it;
    }
}