#include "gen_stepgeneratecubecloud.h"

#include <assert.h>
#include <limits>
#include <ctime>

// Using the point cloud deposit
#include "ct_global/ct_context.h"


// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"

#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_result/model/outModel/ct_outresultmodelgroup.h"

// Inclusion of standard result class
#include "ct_result/ct_resultgroup.h"
#include "ct_iterator/ct_mutablepointiterator.h"
#include "ct_coordinates/ct_defaultcoordinatesystem.h"

// Alias for indexing out models
#define DEF_resultOut_resultScene "resultScene"
#define DEF_groupOut_groupScene "groupScene"
#define DEF_itemOut_scene "scene"

#include "ct_math/ct_mathpoint.h"
#include "Eigen/Geometry"

// Constructor : initialization of parameters
GEN_StepGenerateCubeCloud::GEN_StepGenerateCubeCloud(CT_StepInitializeData &dataInit) : CT_AbstractStepCanBeAddedFirst(dataInit)
{
    _botX = -10;
    _botY = -10;
    _botZ = -10;
    _topX = 10;
    _topY = 10;
    _topZ = 10;
    _resX = 0.1;
    _resY = 0.1;
    _resZ = 0.1;
    _noiseX = 0;
    _noiseY = 0;
    _noiseZ = 0;
    _axisX = 0;
    _axisY = 0;
    _axisZ = 1;

    /*_m_doc = NULL;
    _resultPointCloud = new CT_PointCloudStdVector();
    _resultIndexCloud = new CT_PointCloudIndexVector();
    _resultBoundingBox = NULL;
    _resultScene = NULL*/;

    setDebuggable(true);
}

// Step description (tooltip of contextual menu)
QString GEN_StepGenerateCubeCloud::getStepDescription() const
{
    return tr("Créer un Cube de points");
}

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

//void GEN_StepGenerateCubeCloud::setDocuments(QList<DocumentInterface *> docList)
//{
//    if ( !docList.isEmpty() )
//    {
//        _m_doc = docList.first();
//    }

//    else
//    {
//        _m_doc = NULL;
//    }
//}

//void GEN_StepGenerateCubeCloud::preWaitForAckIfInDebugMode()
//{
//    if ( _m_doc != NULL )
//    {
//        _m_doc->addItemDrawable(*_resultScene);
//        _m_doc->redrawGraphics();
//    }
//}

//void GEN_StepGenerateCubeCloud::postWaitForAckIfInDebugMode()
//{
//    if ( _m_doc != NULL )
//    {
//        _m_doc->removeItemDrawable(*_resultScene);
//    }
//}


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

// Creation and affiliation of IN models
void GEN_StepGenerateCubeCloud::createInResultModelListProtected()
{
    // No in result is needed
    setNotNeedInputResult();
}

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

    resultModel->setRootGroup(DEF_groupOut_groupScene, new CT_StandardItemGroup(),tr("Group"));
    resultModel->addItemModel(DEF_groupOut_groupScene, DEF_itemOut_scene, new CT_Scene(), tr("Generated Cube"));
}

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

    configDialog->addText(tr("Bottom left point"), "", "");
    configDialog->addDouble("X", "", -1e+10, 1e+10, 4, _botX, 0);
    configDialog->addDouble("Y", "", -1e+10, 1e+10, 4, _botY, 0);
    configDialog->addDouble("Z", "", -1e+10, 1e+10, 4, _botZ, 0);
    configDialog->addText(tr("Top right point"), "", "");
    configDialog->addDouble("X", "", -1e+10, 1e+10, 4, _topX, 0);
    configDialog->addDouble("Y", "", -1e+10, 1e+10, 4, _topY, 0);
    configDialog->addDouble("Z", "", -1e+10, 1e+10, 4, _topZ, 0);
    configDialog->addText(tr("Direction"), "", "");
    configDialog->addDouble("X", "", -1e+10, 1e+10, 4, _axisX, 0);
    configDialog->addDouble("Y", "", -1e+10, 1e+10, 4, _axisY, 0);
    configDialog->addDouble("Z", "", -1e+10, 1e+10, 4, _axisZ, 0);
    configDialog->addText(tr("Resolution"), "", "");
    configDialog->addDouble("X", "", 0.0001, 1e+10, 4, _resX, 0);
    configDialog->addDouble("Y", "", 0.0001, 1e+10, 4, _resY, 0);
    configDialog->addDouble("Z", "", 0.0001, 1e+10, 4, _resZ, 0);
    configDialog->addText(tr("Add noise"), "", "");
    configDialog->addDouble("X", "", 0, 1e+10, 4, _noiseX, 0);
    configDialog->addDouble("Y", "", 0, 1e+10, 4, _noiseY, 0);
    configDialog->addDouble("Z", "", 0, 1e+10, 4, _noiseZ, 0);
}

void GEN_StepGenerateCubeCloud::compute()
{
    CT_ResultGroup* resultOut_resultScene = getOutResultList().first();

    /******************************************************************************
     *      User's Compute
     ******************************************************************************/
    // On initialise l'aleatoire pour le bruit par la suite
    srand( time(0) );

    assert( _topX > _botX );
    assert( _topY > _botY );
    assert( _topZ > _botZ );

    size_t nbPts = 0;
    size_t nbPointsX = ceil ( ( _topX - _botX ) / _resX );
    size_t nbPointsY = ceil ( ( _topY - _botY ) / _resY );
    size_t nbPointsZ = ceil ( ( _topZ - _botZ ) / _resZ );
    Eigen::Vector3d center( ( _topX + _botX ) / 2.0, ( _topY + _botY ) / 2.0, ( _topZ + _botZ ) / 2.0 );
    Eigen::Vector3d direction( _axisX, _axisY, _axisZ );
    Eigen::Vector3d sphericalDirection;
    CT_MathPoint::cartesianToSpherical( direction, sphericalDirection );

    CT_AbstractUndefinedSizePointCloud *undepositPointCloud = PS_REPOSITORY->createNewUndefinedSizePointCloud();

    // On construit la face du bas du cube centre en 0,0,0
    double valX, valY, valZ;
    for ( size_t i = 0 ; i < nbPointsX && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsY && !isStopped() ; j++ )
        {
            valX = (i*_resX) - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _botX;
            valY = (j*_resY) - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botY;
            valZ = - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  (j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)  );

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

    // On construit la face de face centre en 0,0,0
    for ( size_t i = 0 ; i < nbPointsX && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsZ && !isStopped() ; j++ )
        {
            valX = (i*_resX) - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _botX;
            valY = - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botY;
            valZ = (j*_resZ) - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  ((j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)) + (50.0/6) );

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

    // On construit la face de derriere centre en 0,0,0
    for ( size_t i = 0 ; i < nbPointsX && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsZ && !isStopped() ; j++ )
        {
            valX = (i*_resX) - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _botX;
            valY = - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _topY;
            valZ = (j*_resZ) - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  ((j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)) + (100.0/6) );

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

    // On construit la face de gauche centre en 0,0,0
    for ( size_t i = 0 ; i < nbPointsY && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsZ && !isStopped() ; j++ )
        {
            valX = - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _botX;
            valY = (i*_resY) - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botY;
            valZ = (j*_resZ) - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  ((j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)) + (150.0/6) );

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

    // On construit la face de droite centre en 0,0,0
    for ( size_t i = 0 ; i < nbPointsY && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsZ && !isStopped() ; j++ )
        {
            valX = - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _topX;
            valY = (i*_resY) - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botY;
            valZ = (j*_resZ) - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  ((j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)) + (200.0/6) );

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

    // On construit la face du haut centre en 0,0,0
    for ( size_t i = 0 ; i < nbPointsX && !isStopped() ; i++ )
    {
        for ( size_t j = 0 ; j < nbPointsY && !isStopped() ; j++ )
        {
            valX = (i*_resX) - _noiseX + ( ((double)rand()/RAND_MAX) * 2 * _noiseX ) + _botX;
            valY = (j*_resY) - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _botY;
            valZ = - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _topZ;

            undepositPointCloud->addPoint( createCtPoint( valX, valY, valZ ));
            nbPts++;

            setProgress(  ((j + (nbPointsX*i) ) * (50.0/6) / (nbPointsX*nbPointsY)) + (250.0/6) );

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

    // On enregistre le nuage de points cree dans le depot
    CT_NMPCIR depositPointCloud = PS_REPOSITORY->registerUndefinedSizePointCloud(undepositPointCloud);

    if(!isStopped()) {

        /* ***************************************************************************************************
         * PRISE EN COMPTE DE LA DIRECTION DONNEE
         * ***************************************************************************************************/
        // On applique une translation pour remettre le centre a l'origine
        // Puis les rotations a tous les points : d'abord autour de Z, puis de X (TO DO : passer aux rotations par quaternions)
        // Puis la translation inverse

        CT_MutablePointIterator itP(depositPointCloud);

        size_t i = 0;
        while (itP.hasNext())
        {
            CT_Point point = itP.next().currentPoint();

            // Translation vers le centre du cube
            point -= center;

            // Rotation autour de Y
            Eigen::Vector3d axisVectorY( 0, 1, 0 );
            Eigen::Affine3d transformationY(Eigen::AngleAxisd(sphericalDirection[2], axisVectorY));
            point = transformationY * point;

            // Rotation autour de Z
            Eigen::Vector3d axisVectorZ( 0, 0, 1 );
            Eigen::Affine3d transformationZ(Eigen::AngleAxisd(sphericalDirection[1], axisVectorZ));
            point = transformationZ * point;

            // Translation inverse
            point += center;

            itP.replaceCurrentPoint(point);

            if (i++ % 1000 == 0) {setProgress(((double)i * 50.0 )/((double)nbPts) + 50.0);}

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

        // ------------------------------
        // Create OUT groups and items
        CT_StandardItemGroup* groupOut_groupScene = new CT_StandardItemGroup(DEF_groupOut_groupScene, resultOut_resultScene);

        CT_Scene* itemOut_scene = new CT_Scene(DEF_itemOut_scene, resultOut_resultScene, depositPointCloud);
        itemOut_scene->updateBoundingBox();

        groupOut_groupScene->addItemDrawable(itemOut_scene);
        resultOut_resultScene->addGroup(groupOut_groupScene);
    }
}