#include "onf_stepcomputescandirection.h"

// Utilise le depot
#include "ct_global/ct_context.h"

// Utilise les attributs optionnels
#include "ct_itemdrawable/ct_pointsattributesscalartemplated.h"

#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_result/model/inModel/ct_inresultmodelgroup.h"
#include "ct_result/model/inModel/ct_inresultmodelgrouptocopy.h"
#include "ct_result/model/outModel/ct_outresultmodelgroupcopy.h"
#include "ct_result/model/outModel/tools/ct_outresultmodelgrouptocopypossibilities.h"
#include "ct_view/ct_stepconfigurabledialog.h"

// Inclusion of standard result class
#include "ct_result/ct_resultgroup.h"

// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"
#include "ct_itemdrawable/ct_scanpath.h"
#include "ct_normalcloud/ct_normalcloudstdvector.h"
#include "ct_itemdrawable/ct_pointsattributesnormal.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_iterator/ct_groupiterator.h"
#include "ct_iterator/ct_mutablepointiterator.h"
#include "ctliblas/itemdrawable/las/ct_stdlaspointsattributescontainer.h"

#include <QDebug>

// Alias for indexing in models
#define DEFin_resSc "resSc"
#define DEFin_scGrp "scGrp"
#define DEFin_sc "sc"
#define DEFin_attLAS "attLAS"

#define DEFin_resTraj "resTraj"
#define DEFin_TrajGrp "TrajGrp"
#define DEFin_Traj "Traj"


// Constructor : initialization of parameters
ONF_StepComputeScanDirection::ONF_StepComputeScanDirection(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
}

// Step description (tooltip of contextual menu)
QString ONF_StepComputeScanDirection::getStepDescription() const
{
    return tr("Calcule les directions de scan (ALS)");
}

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

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

// Creation and affiliation of IN models
void ONF_StepComputeScanDirection::createInResultModelListProtected()
{
    CT_InResultModelGroupToCopy * resultSc = createNewInResultModelForCopy(DEFin_resSc, tr("Scene(s)"));
    resultSc->setZeroOrMoreRootGroup();
    resultSc->addGroupModel("", DEFin_scGrp, CT_AbstractItemGroup::staticGetType(), tr("Group"));
    resultSc->addItemModel(DEFin_scGrp, DEFin_sc, CT_AbstractItemDrawableWithPointCloud::staticGetType(), tr("Scene(s)"));
    resultSc->addItemModel(DEFin_scGrp, DEFin_attLAS, CT_StdLASPointsAttributesContainer::staticGetType(), tr("Attributs LAS"), tr("Attribut LAS"));

    CT_InResultModelGroup *resultDTM = createNewInResultModel(DEFin_resTraj, tr("Trajectory"), "", true);
    resultDTM->setZeroOrMoreRootGroup();
    resultDTM->addGroupModel("", DEFin_TrajGrp, CT_AbstractItemGroup::staticGetType(), tr("Group"));
    resultDTM->addItemModel(DEFin_TrajGrp, DEFin_Traj, CT_ScanPath::staticGetType(), tr("Trajectory"));
}

// Creation and affiliation of OUT models
void ONF_StepComputeScanDirection::createOutResultModelListProtected()
{
    CT_OutResultModelGroupToCopyPossibilities *res = createNewOutResultModelToCopy(DEFin_resSc);

    if(res != NULL)
    {
        res->addItemModel(DEFin_scGrp, _outScanDirectionAttributeModelName, new CT_PointsAttributesNormal(), tr("Scan direction"));
    }

}

// Semi-automatic creation of step parameters DialogBox
void ONF_StepComputeScanDirection::createPreConfigurationDialog()
{
    //CT_StepConfigurableDialog *configDialog = newStandardPreConfigurationDialog();
}

void ONF_StepComputeScanDirection::compute()
{
    CT_ResultGroup* resin_Traj = getInputResults().at(1);

    QList<CT_ScanPath*> trajectories;
    CT_ResultItemIterator it(resin_Traj, this, DEFin_Traj);
    while (it.hasNext())
    {
        CT_ScanPath* scanPath = (CT_ScanPath*) it.next();
        trajectories.append(scanPath);
    }

    if (trajectories.size() > 0)
    {
        CT_ResultGroup* resOut = getOutResultList().first();

        CT_ResultGroupIterator itSc(resOut, this, DEFin_scGrp);
        while (itSc.hasNext())
        {
            CT_StandardItemGroup* grp = (CT_StandardItemGroup*) itSc.next();

            if (grp != NULL)
            {
                CT_AbstractItemDrawableWithPointCloud* scene = (CT_AbstractItemDrawableWithPointCloud*) grp->firstItemByINModelName(this, DEFin_sc);
                const CT_StdLASPointsAttributesContainer* attributeLAS = (CT_StdLASPointsAttributesContainer*)grp->firstItemByINModelName(this, DEFin_attLAS);

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

                    // Retrieve attributes
                    QHashIterator<CT_LasDefine::LASPointAttributesType, CT_AbstractPointAttributesScalar *> itLAS(attributeLAS->lasPointsAttributes());
                    if (!itLAS.hasNext()) {return;}

                    CT_AbstractPointAttributesScalar *firstAttribute = itLAS.next().value();
                    if (firstAttribute == NULL) {return;}

                    const CT_AbstractPointCloudIndex* pointCloudIndexLAS = firstAttribute->getPointCloudIndex();
                    if (pointCloudIndexLAS == NULL) {return;}

                    CT_AbstractPointAttributesScalar* attributeGPS          = (CT_AbstractPointAttributesScalar*)attributeLAS->pointsAttributesAt(CT_LasDefine::GPS_Time);
                    CT_AbstractPointAttributesScalar* attributeIntensity     = (CT_AbstractPointAttributesScalar*)attributeLAS->pointsAttributesAt(CT_LasDefine::Intensity);

                    if (attributeIntensity == NULL || attributeGPS == NULL) {return;}


                    CT_PointIterator itP(pointCloudIndex);

                    CT_NormalCloudStdVector *normalCloud = new CT_NormalCloudStdVector( n_points );

                    size_t i = 0;
                    size_t errorCount = 0;
                    while (itP.hasNext() && !isStopped())
                    {
                        size_t globalIndex = itP.next().currentGlobalIndex();
                        size_t localIndex = pointCloudIndexLAS->indexOf(globalIndex);
                        CT_Point point = itP.currentPoint();

                        double gpsTime = 0;   // Récupération du temps GPS pour le point
                        double intensity = 0; // Récupération de l'intensité pour le point
                        if (localIndex < pointCloudIndexLAS->size())
                        {
                            gpsTime = attributeGPS->dValueAt(localIndex);
                            intensity = attributeIntensity->dValueAt(localIndex);
                        }                                               

                        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 = normalCloud->normalAt(i);
                        ctNormal.x() = dir(0);
                        ctNormal.y() = dir(1);
                        ctNormal.z() = dir(2);
                        ctNormal.w() = length;

                        setProgress( 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)
                    {
                        CT_PointsAttributesNormal* normalAttribute = new CT_PointsAttributesNormal(_outScanDirectionAttributeModelName.completeName(), resOut, scene->getPointCloudIndexRegistered(), normalCloud);
                        grp->addItemDrawable(normalAttribute);

                        CT_Point bboxBot, bboxTop;
                        scene->getBoundingBox( bboxBot, bboxTop );
                        normalAttribute->setBoundingBox( bboxBot.x(), bboxBot.y(), bboxBot.z(), bboxTop.x(), bboxTop.y(), bboxTop.z() );
                    }
                }
            }
        }
    }
}