#include "tk_stepcentercloud.h"

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

TK_StepCenterCloud::TK_StepCenterCloud() : SuperClass()
{
    _centerChoice = CENTROID_MIN_HEIGHT;
}


QString TK_StepCenterCloud::description() const
{
    return tr("Recentrer des points");
}


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

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


void TK_StepCenterCloud::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 à centrer"));
}


void TK_StepCenterCloud::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outScene, tr("Scène centrée"));
    manager.addItem(_inGroup, _outTransfMatrix, tr("Matrice de transformation"));
}


void TK_StepCenterCloud::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    CT_ButtonGroup &bg_center = postInputConfigDialog->addButtonGroup( _centerChoice );

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

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

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

            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 = Eigen::Vector3d(0.0, 0.0, 0.0);

            switch ( _centerChoice )
            {
            case BBOX_CENTER :
            {
                center(0) =  inScene->centerX();
                center(1) =  inScene->centerY();
                center(2) =  inScene->centerZ();
                break;
            }

            case BBOX_CENTER_MIN_HEIGHT :
            {
                center(0) =  inScene->centerX();
                center(1) =  inScene->centerY();
                center(2) =  inScene->minZ();
                break;
            }

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

                center /= nbPoints;

                break;
            }

            case CENTROID_MIN_HEIGHT :
            {
                while (itP.hasNext())
                {
                    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) = inScene->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())
            {
                itP.next();
                itPM.next().replaceCurrentPoint(itP.currentPoint() - center);

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

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

            CT_Scene* outSscene = new CT_Scene(translatedCloud);
            outSscene->setBoundingBox(inScene->minX() - center(0), inScene->minY() - center(1), inScene->minZ() - center(2), inScene->maxX() - center(0), inScene->maxY() - center(1), inScene->maxZ() - center(2));

            group->addSingularItem(_outScene, outSscene);

            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(transf3D);
            group->addSingularItem(_outTransfMatrix, transfMatItem);
        }
    }
}