#include "tk_steprotatecloud.h"

#define RAD_TO_DEG 57.2957795131
#define DEG_TO_RAD 0.01745329251

TK_StepRotateCloud::TK_StepRotateCloud() : SuperClass()
{
    _alpha = 0;
    _axisX = 0;
    _axisY = 0;
    _axisZ = 1;
    _x = 0;
    _y = 0;
    _z = 0;

    // Cette etape est CT_debugable
    setDebuggable( true );
}


QString TK_StepRotateCloud::description() const
{
    return tr("Rotation des points");
}


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

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


void TK_StepRotateCloud::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 à pivoter"));
}


void TK_StepRotateCloud::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResultCopy(_inResult);
    manager.addItem(_inGroup, _outScene, tr("Scène pivotée"));
}


void TK_StepRotateCloud::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Angle de rotation"), "", -360, 360, 4, _alpha);
    postInputConfigDialog->addText(tr("Axe de rotation (direction)"), "", "");
    postInputConfigDialog->addDouble("dX", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisX);
    postInputConfigDialog->addDouble("dY", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisY);
    postInputConfigDialog->addDouble("dZ", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisZ);
    postInputConfigDialog->addText(tr("Point sur l'axe"), "", "");
    postInputConfigDialog->addDouble("X", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _x);
    postInputConfigDialog->addDouble("Y", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _y);
    postInputConfigDialog->addDouble("Z", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _z);
}

void TK_StepRotateCloud::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();

            CT_PointIterator itP(cloudIndex);

            // Cloud bounding box
            Eigen::Vector3d bboxBot;
            Eigen::Vector3d bboxTop;

            for ( int i = 0 ; i < 3 ; i++ )
            {
                bboxBot[i] = std::numeric_limits<double>::max();
                bboxTop[i] = -std::numeric_limits<double>::max();
            }

            // On cree le vecteur d'axe de rotation et le vecteur du centre de rotation
            Eigen::Vector3d axisVector( _axisX, _axisY, _axisZ );
            Eigen::Vector3d center( _x, _y, _z );
            double alpha = _alpha * DEG_TO_RAD;

            Eigen::Affine3d transformation(Eigen::AngleAxisd(alpha, axisVector));

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

            size_t i = 0;
            // On applique la translation a tous les points du nuage
            while (itP.hasNext() && itPM.hasNext())
            {
                itP.next();
                CT_Point point = itP.currentPoint();

                // Translation au centre
                point = point - center;

                // Rotation
                point = transformation * point;

                // Translation inverse
                point += center;

                // Update BBox
                for ( int j = 0 ; j < 3 ; j++ )
                {
                    if ( point[j] < bboxBot[j] )
                    {
                        bboxBot[j] = point[j];
                    }

                    if ( point[j] > bboxTop[j] )
                    {
                        bboxTop[j] = point[j];
                    }
                }

                itPM.next().replaceCurrentPoint(point);

                // Barre de progression
                setProgress(float(100.0*i++ /cloudIndex->size()));

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

            CT_Scene* outScene = new CT_Scene(rotatedCloud);
            outScene->setBoundingBox( bboxBot(0), bboxBot(1), bboxBot(2), bboxTop(0), bboxTop(1), bboxTop(2) );

            group->addSingularItem(_outScene, outScene);
        }
    }
}