#include "tk_stepnormalestimator.h"

#include "ct_log/ct_logmanager.h"
#include "ctlibclouds/ct_global/ct_repository.h"

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

TK_StepNormalEstimator::TK_StepNormalEstimator() : SuperClass()
{
    _nbNeigbours = 8;
    _precisionThreshold = 0.001;
}


QString TK_StepNormalEstimator::description() const
{
    return tr("Estimation de normales dans un nuage de point");
}

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

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


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

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

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


void TK_StepNormalEstimator::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.addResult(_inResult, tr("Scène(s)"));
    manager.setZeroOrMoreRootGroup(_inResult, _inZeroOrMoreRootGroup);
    manager.addGroup(_inZeroOrMoreRootGroup, _inGroup);
    manager.addItem(_inGroup, _inScene, tr("Scène à translater"));
}


void TK_StepNormalEstimator::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outScene, tr("Scene"));
    manager.addItem(_inGroup, _outNormalsModel, tr("Normales"));
    manager.addItem(_inGroup, _outCurvatureModel, tr("Courbures"));
}


void TK_StepNormalEstimator::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addInt("Nombre de voisins à rechercher", "", 1, 100, _nbNeigbours);
    postInputConfigDialog->addDouble("Seuil de précision sur les coordonnées", "m", 0.000001, 1, 6, _precisionThreshold);
}

void TK_StepNormalEstimator::compute()
{
    for (CT_StandardItemGroup* group : _inGroup.iterateOutputs(_inResult))
    {
        for (const CT_AbstractItemDrawableWithPointCloud* inScene : group->singularItems(_inScene))
        {
            if (isStopped()) {return;}

            const CT_AbstractPointCloudIndex *pointCloudIndex = inScene->pointCloudIndex();

            if (pointCloudIndex != nullptr)
            {
#ifdef USE_PCL
                boost::shared_ptr<CT_PCLCloud> pclCloud;

                Eigen::Vector3d min, max;
                inScene->boundingBox(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 = inScene->centerX();
                    double offsetY = inScene->centerY();
                    double offsetZ = inScene->centerZ();

                    pclCloud = CT_PCLTools::staticConvertToPCLCloud(inScene->pointCloudIndex(), offsetX, offsetY, offsetZ);
                    PS_LOG->addInfoMessage(LogInterface::step, tr("OFFSET Applied"));
                } else {
                    pclCloud = CT_PCLTools::staticConvertToPCLCloud(inScene->pointCloudIndexRegistered());
                    PS_LOG->addInfoMessage(LogInterface::step, tr("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_AbstractUndefinedSizePointCloud *uspc = PS_REPOSITORY->createNewUndefinedSizePointCloud();

                        CT_DensePointScalarManager<float>::UPCSSetterPtr curvatureSetter = _outCurvatureModel.createUndefinedPointCloudSizeAttributesSetterPtr(uspc);
                        CT_DensePointNormalManager::UPCSSetterPtr normalSetter = _outNormalsModel.createUndefinedPointCloudSizeAttributesSetterPtr(uspc);


                        // CT_PointsAttributesScalarTemplated<float>* curvatureSetter = _outCurvatureModel.createAttributeInstance(inScene->pointCloudIndexRegistered());
                        // CT_PointsAttributesNormal normalSetter = _outNormalsModel.createAttributeInstance(inScene->pointCloudIndexRegistered());

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

                        float minCurv = std::numeric_limits<float>::max();
                        float maxCurv = -std::numeric_limits<float>::max();

                        CT_PointIterator itP(pointCloudIndex);
                        for ( size_t i = 0 ; i < nbNormals && itP.hasNext() && !isStopped() ; i++ )
                        {
                            itP.next();
                            const pcl::PointNormal &pclNormal = (*it);

                            float val = pclNormal.curvature;
                            CT_Normal ctNormal;
                            ctNormal.x() = pclNormal.normal_x;
                            ctNormal.y() = pclNormal.normal_y;
                            ctNormal.z() = pclNormal.normal_z;
                            ctNormal.w() = pclNormal.curvature;

                            uspc->addPoint(itP.currentPoint());
                            curvatureSetter->setLastValue(val);
                            normalSetter->setLastValue(ctNormal);

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

                            setProgress( 100.0*i /nbNormals );

                            ++it;
                        }

                        CT_PCIR pcir = PS_REPOSITORY->registerUndefinedSizePointCloud(uspc);
                        CT_Scene* scene = new CT_Scene(pcir);
                        scene->updateBoundingBox();

                        group->addSingularItem(_outScene, scene);

                        if(curvatureSetter != nullptr)
                            group->addSingularItem(_outCurvatureModel, _outCurvatureModel.createAttributeInstance(pcir, minCurv, maxCurv));

                        if(normalSetter != nullptr)
                            group->addSingularItem(_outNormalsModel, _outNormalsModel.createAttributeInstance(pcir));

                    } else {
                        PS_LOG->addErrorMessage(LogInterface::step, tr("Problème le nombre de normales estimées ne correspond pas au nombre de points du nuage d'entrée"));
                    }
                }
#endif
            }

        }
    }
}