#include "tk_stepcentercloud.h"

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

#include "ct_math/ct_mathpoint.h"

#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_result/model/inModel/ct_inresultmodelgroup.h"
#include "ct_result/model/outModel/ct_outresultmodelgroup.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_transformationmatrix.h"
#include "ct_iterator/ct_pointiterator.h"
#include "ct_iterator/ct_mutablepointiterator.h"

// Alias for indexing in models
#define DEF_resultIn_inputResult "inputResult"
#define DEF_groupIn_inputScene "inputGroup"
#define DEF_itemIn_scene "inputScene"

// Alias for indexing out models
#define DEF_resultOut_translated "translatedResult"
#define DEF_groupOut_pointCloud "translatedGroup"
#define DEF_itemOut_scene "translatedScene"
#define DEFout_trMat "transMat"

#include <limits>

#define BBOX_CENTER 0
#define BBOX_CENTER_MIN_HEIGHT 1
#define CENTROID 2
#define CENTROID_MIN_HEIGHT 3
#define KEEP 4

// Constructor : initialization of parameters
TK_StepCenterCloud::TK_StepCenterCloud(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    _centerChoice = CENTROID_MIN_HEIGHT;
}

// Step description (tooltip of contextual menu)
QString TK_StepCenterCloud::getStepDescription() const
{
    return tr("Recentrer nuage de points");
}

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

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

// Creation and affiliation of IN models
void TK_StepCenterCloud::createInResultModelListProtected()
{    
    CT_InResultModelGroup *resultModel = createNewInResultModel(DEF_resultIn_inputResult, tr("Scene(s)"));

    resultModel->setZeroOrMoreRootGroup();
    resultModel->addGroupModel("", DEF_groupIn_inputScene, CT_AbstractItemGroup::staticGetType(), tr("Group"));
    resultModel->addItemModel(DEF_groupIn_inputScene, DEF_itemIn_scene, CT_Scene::staticGetType(), tr("Scene(s)"));
}

// Creation and affiliation of OUT models
void TK_StepCenterCloud::createOutResultModelListProtected()
{
    CT_OutResultModelGroup *resultModel = createNewOutResultModel(DEF_resultOut_translated, tr("Centered Point Cloud"));

    resultModel->setRootGroup(DEF_groupOut_pointCloud);
    resultModel->addItemModel(DEF_groupOut_pointCloud, DEF_itemOut_scene, new CT_Scene(), tr("Centered Scene"));
    resultModel->addItemModel(DEF_groupOut_pointCloud, DEFout_trMat, new CT_TransformationMatrix(), tr("Reverse transformation matrix"));
}

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


    CT_ButtonGroup &bg_center = configDialog->addButtonGroup( _centerChoice );

    configDialog->addExcludeValue("", "", tr("Center of bounding box"), bg_center, BBOX_CENTER);
    configDialog->addExcludeValue("", "", tr("Center of bounding box with minimum height"), bg_center, BBOX_CENTER_MIN_HEIGHT);
    configDialog->addExcludeValue("", "", tr("Centroid"), bg_center, CENTROID);
    configDialog->addExcludeValue("", "", tr("Centroid with minimum height"), bg_center, CENTROID_MIN_HEIGHT);
    configDialog->addExcludeValue("", "", tr("Keep as it is"), bg_center, KEEP);
}

void TK_StepCenterCloud::compute()
{
    CT_ResultGroup* resultIn_inputResult = getInputResults().first();
    CT_ResultGroup* resultOut_translated = getOutResultList().first();


    CT_ResultItemIterator it(resultIn_inputResult, this, DEF_itemIn_scene);
    while (it.hasNext())
    {
        const CT_Scene* itemIn_scene = (const CT_Scene*)it.next();
        const CT_AbstractPointCloudIndex *cloudIndex = itemIn_scene->getPointCloudIndex();
        size_t nbPoints = cloudIndex->size();
        CT_PointIterator itP(cloudIndex);

        // On Cree un nouveau nuage qui sera le translate
        CT_NMPCIR translatedCloud = PS_REPOSITORY->createNewPointCloud(cloudIndex->size());
        CT_MutablePointIterator itPM(translatedCloud);


        // On cree le centre a recentrer
        Eigen::Vector3d center;

        switch ( _centerChoice )
        {
        case BBOX_CENTER :
        {
            center(0) =  itemIn_scene->getCenterX();
            center(1) =  itemIn_scene->getCenterY();
            center(2) =  itemIn_scene->getCenterZ();
            break;
        }

        case BBOX_CENTER_MIN_HEIGHT :
        {
            center(0) =  itemIn_scene->getCenterX();
            center(1) =  itemIn_scene->getCenterY();
            center(2) =  itemIn_scene->minZ();
            break;
        }

        case CENTROID :
        {
            while (itP.hasNext()&& !isStopped())
            {
                const CT_Point &currentPoint = itP.next().currentPoint();
                center += currentPoint;
            }

            center /= nbPoints;

            break;
        }

        case CENTROID_MIN_HEIGHT :
        {
            while (itP.hasNext()&& !isStopped())
            {
                const CT_Point &currentPoint = itP.next().currentPoint();
                center(0) = center(0) + currentPoint(0);
                center(1) =  center(1) + currentPoint(1);
            }

            center(0) = center(0) / (double)nbPoints;
            center(1) = center(1) / (double)nbPoints;
            center(2) = itemIn_scene->minZ();

            break;
        }

        case KEEP :
        {
            center(0) = 0;
            center(1) = 0;
            center(2) = 0;

            break;
        }

        default : {break;}
        }


        size_t i = 0;
        itP.toFront();
        // On applique la translation a tous les points du nuage
        while (itP.hasNext() && itPM.hasNext() && !isStopped())
        {
            itP.next();
            itPM.next().replaceCurrentPoint(itP.currentPoint() - center);

            // Barre de progression
            setProgress( 100.0*i++ /nbPoints );

            // On regarde si on est en debug mode
            waitForAckIfInDebugMode();
        }

        CT_StandardItemGroup* groupOut_pointCloud = new CT_StandardItemGroup(DEF_groupOut_pointCloud, resultOut_translated);

        CT_Scene* itemOut_scene = new CT_Scene(DEF_itemOut_scene, resultOut_translated, translatedCloud);
        itemOut_scene->setBoundingBox( itemIn_scene->minX() - center(0), itemIn_scene->minY() - center(1), itemIn_scene->minZ() - center(2), itemIn_scene->maxX() - center(0), itemIn_scene->maxY() - center(1), itemIn_scene->maxZ() - center(2));

        groupOut_pointCloud->addItemDrawable(itemOut_scene);

        Eigen::Matrix4d transf3D = Eigen::Matrix4d::Identity(4,4);
        transf3D(0,3) = center(0);
        transf3D(1,3) = center(1);
        transf3D(2,3) = center(2);

        CT_TransformationMatrix *transfMatItem = new CT_TransformationMatrix(DEFout_trMat, resultOut_translated, transf3D);
        groupOut_pointCloud->addItemDrawable(transfMatItem);

        resultOut_translated->addGroup(groupOut_pointCloud);                
    }
}