#include "gen_stepgeneratecylindercloud.h"

#include <ctime>

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

// Inclusion of used ItemDrawable classes
#include "ct_itemdrawable/ct_scene.h"
#include "ct_point.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 <assert.h>
#include <limits>
#include "ct_math/ct_mathpoint.h"
#include "Eigen/Geometry"

#define RAD_TO_DEG 57.2957795131
#define DEG_TO_RAD 0.01745329251

// Constructor : initialization of parameters
GEN_StepGenerateCylinder::GEN_StepGenerateCylinder(CT_StepInitializeData &dataInit) : CT_AbstractStepCanBeAddedFirst(dataInit)
{
    _botX = 0;
    _botY = 0;
    _botZ = 0;
    _minAng = 0;
    _maxAng = 360;
    _height = 10;
    _radius = 1;
    _resTheta = 5;
    _resHeight = 0.1;
    _resRadius = 0.1;
    _noiseTheta = 0;
    _noiseHeight = 0;
    _noiseRadius = 0;
    _generateFaces = true;
    _axisX = 0;
    _axisY = 0;
    _axisZ = 1;
}

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

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

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

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

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

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

    configDialog->addDouble(tr("Height"), "", 0.0001, 1e+10, 4, _height, 0);
    configDialog->addDouble(tr("Radius"), "", 0.0001, 1e+10, 4, _radius, 0);
    configDialog->addDouble(tr("Minimum theta"), "", 0, 359.9999, 4, _minAng, 0);
    configDialog->addDouble(tr("Maximum theta"), "", 0.0001, 360, 4, _maxAng, 0);
    configDialog->addText(tr("Base center"), "", "");
    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("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("Resolutions"), "", "");
    configDialog->addDouble("Z", "", 0.0001, 1e+10, 4, _resHeight, 0);
    configDialog->addDouble(tr("Theta"), "", 0.0001, 1e+10, 4, _resTheta, 0);
    configDialog->addDouble(tr("Radius"), "", 0.0001, 1e+10, 4, _resRadius, 0);
    configDialog->addText(tr("Add noise"), "", "");
    configDialog->addDouble(tr("Theta"), "", 0, 1e+10, 4, _noiseTheta, 0);
    configDialog->addDouble(tr("Height"), "", 0, 1e+10, 4, _noiseHeight, 0);
    configDialog->addDouble(tr("Radius"), "", 0, 1e+10, 4, _noiseRadius, 0);
    configDialog->addBool(tr("Generate bot and top faces"),"","",_generateFaces);
}

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


    /******************************************************************************
     *      User's Compute
     ******************************************************************************/
    assert( _minAng < _maxAng );

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

    // Conversion des angles en radians
    _minAng *= DEG_TO_RAD;
    _maxAng *= DEG_TO_RAD;
    _noiseTheta *= DEG_TO_RAD;
    _resTheta *= DEG_TO_RAD;

    size_t nbPts = 0;
    size_t nbPtsTheta = ceil( (_maxAng - _minAng) / _resTheta );
    size_t nbPtsHeight = ceil( _height / _resHeight );
    size_t nbPtsRadius = ceil( _radius / _resRadius );
    size_t nbPtsCote = nbPtsTheta * nbPtsHeight;
    size_t nbPtsFace = nbPtsTheta * nbPtsRadius;
    Eigen::Vector3d centerBase( _botX, _botY, _botZ );
    Eigen::Vector3d direction( _axisX, _axisY, _axisZ );
    Eigen::Vector3d sphericalDirection;
    CT_MathPoint::cartesianToSpherical( direction, sphericalDirection );

    CT_AbstractUndefinedSizePointCloud *undepositPointCloud = PS_REPOSITORY->createNewUndefinedSizePointCloud();

    // Construction du cote du cylindre vertical qui a pour base 0,0,0
    double valTheta, valHeight, valRadius;
    for ( double i = _minAng ; (i <= _maxAng) && !isStopped() ; i += _resTheta )
    {
        for ( double j = 0 ; j <= _height ; j += _resHeight )
        {
            // On ajoute un point en tenant compte de la variabilité en epsilone
            valTheta = i - _noiseTheta + ( ((double)rand()/RAND_MAX) * 2 * _noiseTheta );
            valHeight = j - _noiseHeight + ( ((double)rand()/RAND_MAX) * 2 * _noiseHeight );
            valRadius = _radius - _noiseRadius + ( ((double)rand()/RAND_MAX) * 2 * _noiseRadius );

            undepositPointCloud->addPoint( Eigen::Vector3d( cos( valTheta ) * valRadius, sin( valTheta ) * valRadius, valHeight ));
            nbPts++;

            // Barre de progression (multiplie par 100/6 parce qu'on a huit face et qu'on est a la premiere
            setProgress( nbPts * 40 / (double)nbPtsCote);

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

    if ( _generateFaces )
    {
        size_t cpt = 0;
        // Construction de la face du bas et du haut du cylindre vertical de base 0,0,0
        for ( double i = _minAng ; (i <= _maxAng ) && !isStopped(); i += _resTheta )
        {
            for ( double j = 0 ; j < _radius ; j += _resRadius )
            {
                // On ajoute un point en tenant compte de la variabilité en epsilone
                valTheta = i - _noiseTheta + ( ((double)rand()/RAND_MAX) * 2 * _noiseTheta );
                valHeight = - _noiseHeight + ( ((double)rand()/RAND_MAX) * 2 * _noiseHeight );
                valRadius = j - _noiseRadius + ( ((double)rand()/RAND_MAX) * 2 * _noiseRadius );

                undepositPointCloud->addPoint( Eigen::Vector3d( cos( valTheta ) * valRadius, sin( valTheta ) * valRadius, valHeight ));
                nbPts++;

                // Barre de progression (multiplie par 100/6 parce qu'on a huit face et qu'on est a la premiere
               setProgress(  (cpt++ * 5 / (double)nbPtsFace) + 40  );

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

        cpt = 0;
        // Construction de la face du haut
        for ( double i = _minAng ; (i <= _maxAng ) && !isStopped(); i += _resTheta )
        {
            for ( double j = 0 ; j < _radius ; j += _resRadius )
            {
                // On ajoute un point en tenant compte de la variabilité en epsilone
                valTheta = i - _noiseTheta + ( ((double)rand()/RAND_MAX) * 2 * _noiseTheta );
                valHeight = - _noiseHeight + ( ((double)rand()/RAND_MAX) * 2 * _noiseHeight ) + _height;
                valRadius = j - _noiseRadius + ( ((double)rand()/RAND_MAX) * 2 * _noiseRadius );

                undepositPointCloud->addPoint( Eigen::Vector3d( cos( valTheta ) * valRadius, sin( valTheta ) * valRadius, valHeight ));
                nbPts++;

                // Barre de progression (multiplie par 100/6 parce qu'on a huit face et qu'on est a la premiere
                setProgress(  cpt++ * 5 / (double)nbPtsFace + 45 );

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

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

    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;

        // Translation inverse
        point += centerBase;

        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);
    }

    // Conversion des angles en degres
    _minAng *= RAD_TO_DEG;
    _maxAng *= RAD_TO_DEG;
    _noiseTheta *= RAD_TO_DEG;
    _resTheta *= RAD_TO_DEG;
}