#include "step/gen_stepgenerateplanecloud.h"

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

#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"

// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"
#include "ct_iterator/ct_mutablepointiterator.h"
#include "ct_coordinates/ct_defaultcoordinatesystem.h"

// Using the point cloud deposit
#include "ct_global/ct_context.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"

// Radians degree conversion
#define RAD_TO_DEG 57.2957795131
#define DEG_TO_RAD 0.01745329251

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

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

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

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

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

// Creation and affiliation of OUT models
void GEN_StepGeneratePlaneCloud::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 Plane"));
}

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

    configDialog->addDouble(tr("Plane height"), "", -1e+10, 1e+10, 4, _height, 0);
    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->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->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->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_StepGeneratePlaneCloud::compute()
{
    CT_ResultGroup* resultOut_resultScene = getOutResultList().first();

    /******************************************************************************
     *      User's Compute
     ******************************************************************************/
    assert( _topX > _botX );
    assert( _topY > _botY );

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

    // Creating a undefined size points cloud
    CT_AbstractUndefinedSizePointCloud *undepositPointCloud = PS_REPOSITORY->createNewUndefinedSizePointCloud();

    size_t     nbPts = 0;
    size_t     nbPointsX = ceil ( ( _topX - _botX ) / _resX );
    size_t     nbPointsY = ceil( ( _topY - _botY ) / _resY );
    double   centerX = ( ( (_topX + _botX ) / 2.0) );
    double   centerY = ( ( (_topY + _botY ) / 2.0) );
    Eigen::Vector3d direction( _axisX, _axisY, _axisZ );
    Eigen::Vector3d sphericalDirection;
    CT_MathPoint::cartesianToSpherical( direction, sphericalDirection );

    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 ) + centerX;
            valY = (j*_resY) - _noiseY + ( ((double)rand()/RAND_MAX) * 2 * _noiseY ) + centerY;
            valZ = - _noiseZ + ( ((double)rand()/RAND_MAX) * 2 * _noiseZ ) + _height;

            undepositPointCloud->addPoint( Eigen::Vector3d( valX, valY, valZ ));
            nbPts++;

            // Barre de progression
            setProgress(  (j + (nbPointsX*i) ) * 50 / (nbPointsX*nbPointsY)  );

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

    CT_NMPCIR depositPointCloud = PS_REPOSITORY->registerUndefinedSizePointCloud(undepositPointCloud);

    /* ***************************************************************************************************
     * PRISE EN COMPTE DE LA DIRECTION DONNEE
     * ***************************************************************************************************/
    // Applique les rotations a tous les points : d'abord autour de Z, puis de X (TO DO : passer aux rotations par quaternions)
    CT_MutablePointIterator itP(depositPointCloud);

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

        // 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;

        itP.replaceCurrentPoint(point);

        setProgress(  ( ( i++ * 50.0 ) / ((double)nbPts) ) + 50);

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

    if(!isStopped()) {
        // ------------------------------
        // 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);
    }
}