#include "tk_stepnormalestimator.h"

#include "ct_itemdrawable/abstract/ct_abstractitemdrawablewithpointcloud.h"
#include "ct_itemdrawable/tools/iterator/ct_groupiterator.h"
#include "ct_itemdrawable/ct_pointsattributesnormal.h"
#include "ct_itemdrawable/ct_pointsattributesscalartemplated.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_normalcloud/ct_normalcloudstdvector.h"

#ifdef USE_PCL
#include "ct_normalcloud/tools/normalsestimator.h"
#include "ctlibpcl/tools/ct_pcltools.h"
#endif

#include <QDebug>


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


// Constructor : initialization of parameters
TK_StepNormalEstimator::TK_StepNormalEstimator(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _nbNeigbours = 8;
    _precisionThreshold = 0.001;
}

// Step description (tooltip of contextual menu)
QString TK_StepNormalEstimator::getStepDescription() const
{
    return tr("Estimation de normales dans un nuage de point");
}

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

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

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

void TK_StepNormalEstimator::setEstimationProgress(int p)
{
    setProgress(p);
}

bool TK_StepNormalEstimator::mustStopEstimation() const
{
    return isStopped();
}
//////////////////// PROTECTED METHODS //////////////////

// Creation and affiliation of IN models
void TK_StepNormalEstimator::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 TK_StepNormalEstimator::createOutResultModelListProtected()
{
    CT_OutResultModelGroupToCopyPossibilities *resOut_r = createNewOutResultModelToCopy(DEFin_r);

    if(resOut_r != NULL)
    {
        resOut_r->addItemModel(DEFin_g, m_autoRenameNormalsModel, new CT_PointsAttributesNormal());
        resOut_r->addItemModel(DEFin_g, m_autoRenameCurvatureModel, new CT_PointsAttributesScalarTemplated<float>(), tr("curvature"));
    }
}

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

    configDialog->addInt("Nombre de voisins à rechercher", "", 1, 100, _nbNeigbours);
    configDialog->addDouble("Seuil de précision sur les coordonnées", "m", 0.000001, 1, 6, _precisionThreshold);
}

void TK_StepNormalEstimator::compute()
{
    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* itemIn_scene = (CT_AbstractItemDrawableWithPointCloud*)grpIn_g->firstItemByINModelName(this, DEFin_scene);
        if (itemIn_scene != NULL)
        {
            #ifdef USE_PCL
            boost::shared_ptr<CT_PCLCloud> pclCloud;


            Eigen::Vector3d min, max;
            itemIn_scene->getBoundingBox(min, max);

            double maxCoordAbsolute = std::fabs(min(0));
            if (std::fabs(min(1)) > maxCoordAbsolute) {maxCoordAbsolute = std::fabs(min(1));}
            if (std::fabs(min(2)) > maxCoordAbsolute) {maxCoordAbsolute = std::fabs(min(2));}
            if (std::fabs(max(0)) > maxCoordAbsolute) {maxCoordAbsolute = std::fabs(max(0));}
            if (std::fabs(max(1)) > maxCoordAbsolute) {maxCoordAbsolute = std::fabs(max(1));}
            if (std::fabs(max(2)) > maxCoordAbsolute) {maxCoordAbsolute = std::fabs(max(2));}

            if (maxCoordAbsolute > (_precisionThreshold * 999999))
            {
                double offsetX = itemIn_scene->getCenterX();
                double offsetY = itemIn_scene->getCenterY();
                double offsetZ = itemIn_scene->getCenterZ();

                pclCloud = CT_PCLTools::staticConvertToPCLCloud(itemIn_scene->getPointCloudIndex(), offsetX, offsetY, offsetZ);
                qDebug() << "OFFSET Applied";
            } else {
                pclCloud = CT_PCLTools::staticConvertToPCLCloud(itemIn_scene->getPointCloudIndexRegistered());
                qDebug() << "OFFSET NOT Applied";
            }

            normalsEstimator estimator(*pclCloud.get(), this);
            pcl::PointCloud<pcl::PointNormal> pclNormals = estimator.getNormals(_nbNeigbours);

            if(!isStopped()) {
                size_t nbNormals = pclNormals.size();
                if(nbNormals == pclCloud->size()) {
                    CT_NormalCloudStdVector *normalCloud = new CT_NormalCloudStdVector( nbNormals );
                    CT_StandardCloudStdVectorT<float> *curvature = new CT_StandardCloudStdVectorT<float>( nbNormals );

                    pcl::PointCloud<pcl::PointNormal>::const_iterator it = pclNormals.begin();

                    float minCurv = std::numeric_limits<float>::max();
                    float maxCurv = -std::numeric_limits<float>::max();
                    for ( size_t i = 0 ; i < nbNormals && !isStopped() ; i++ )
                    {
                        const pcl::PointNormal &pclNormal = (*it);
                        CT_Normal &ctNormal = normalCloud->normalAt(i);
                        ctNormal.x() = pclNormal.normal_x;
                        ctNormal.y() = pclNormal.normal_y;
                        ctNormal.z() = pclNormal.normal_z;
                        ctNormal.w() = pclNormal.curvature;

                        float val = pclNormal.curvature;

                        curvature->replaceT(i, val);

                        if (val < minCurv) {minCurv = val;}
                        if (val > maxCurv) {maxCurv = val;}

                        setProgress( 100.0*i /nbNormals );
                        waitForAckIfInDebugMode();

                        ++it;
                    }

                    CT_PointsAttributesNormal* normalAttribute = new CT_PointsAttributesNormal(m_autoRenameNormalsModel.completeName(), _res, itemIn_scene->getPointCloudIndexRegistered(), normalCloud);
                    grpIn_g->addItemDrawable(normalAttribute);

                    CT_PointsAttributesScalarTemplated<float>*  curvatureAtt  = new CT_PointsAttributesScalarTemplated<float>(m_autoRenameCurvatureModel.completeName(),
                                                                                                                                   _res,
                                                                                                                                   itemIn_scene->getPointCloudIndexRegistered(),
                                                                                                                                   curvature,
                                                                                                                                   minCurv,
                                                                                                                                   maxCurv);
                    grpIn_g->addItemDrawable(curvatureAtt);


                    CT_Point bboxBot, bboxTop;
                    itemIn_scene->getBoundingBox( bboxBot, bboxTop );
                    normalAttribute->setBoundingBox( bboxBot.x(), bboxBot.y(), bboxBot.z(), bboxTop.x(), bboxTop.y(), bboxTop.z() );
                } else {
                    PS_LOG->addErrorMessage(this, tr("Problème le nombre de normales estimées ne correspond pas au nombre de points du nuage d'entrée"));
                }
            }
            #endif
        }

    }
}