#include "gen_stepgeneratespherecloud.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_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"

#define RAD_TO_DEG 57.2957795131
#define DEG_TO_RAD 0.01745329251

// Constructor : initialization of parameters
GEN_StepGenerateSphereCloud::GEN_StepGenerateSphereCloud(CT_StepInitializeData &dataInit) : CT_AbstractStepCanBeAddedFirst(dataInit)
{
    _botX = 0;
    _botY = 0;
    _botZ = 0;
    _radius = 1;
    _thetaMin = 0;
    _thetaMax = 360;
    _phiMin = 0;
    _phiMax = 360;
    _resTheta = 5;
    _resPhi = 5;
    _noiseTheta = 0;
    _noisePhi = 0;
    _noiseRadius = 0;
}

// Step description (tooltip of contextual menu)
QString GEN_StepGenerateSphereCloud::getStepDescription() const
{
    return tr("Créer une Sphère de points");
}

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

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

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

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

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

    configDialog->addDouble(tr("Radius"), "", -1e+10, 1e+10, 4, _radius, 0);
    configDialog->addText(tr("Sphere 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("Limits"), "", "");
    configDialog->addDouble(tr("Minimum theta"), "", 0, 359.9999, 4, _thetaMin, 0);
    configDialog->addDouble(tr("Maximum theta"), "", 0, 360, 4, _thetaMax, 0);
    configDialog->addDouble(tr("Minimum phi"), "", 0, 179.9999, 4, _phiMin, 0);
    configDialog->addDouble(tr("Maximum phi"), "", 0, 180, 4, _phiMax, 0);
    configDialog->addText(tr("Resolution"), "", "");
    configDialog->addDouble(tr("Theta"), "", 0.0001, 1e+10, 4, _resTheta, 0);
    configDialog->addDouble(tr("Phi"), "", 0.0001, 1e+10, 4, _resPhi, 0);
    configDialog->addText(tr("Add noise"), "", "");
    configDialog->addDouble(tr("Theta"), "", 0, 1e+10, 4, _noiseTheta, 0);
    configDialog->addDouble(tr("Phi"), "", 0, 1e+10, 4, _noisePhi, 0);
    configDialog->addDouble(tr("Radius"), "", 0, 1e+10, 4, _noiseRadius, 0);
}

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

    /******************************************************************************
     *      User's Compute
     ******************************************************************************/
    assert( _thetaMin < _thetaMax );
    assert( _phiMin < _phiMax );

    // Conversion des angles en radians
    _thetaMin *= DEG_TO_RAD;
    _thetaMax *= DEG_TO_RAD;
    _phiMin *= DEG_TO_RAD;
    _phiMax *= DEG_TO_RAD;
    _resTheta *= DEG_TO_RAD;
    _resPhi *= DEG_TO_RAD;

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

    size_t nbPts = 0;
    size_t nbPtsTheta = ceil( (_thetaMax - _thetaMin) / _resTheta );
    size_t nbPtsPhi = ceil( (_phiMax - _phiMin) / _resPhi );
    size_t nbPtsTotal = nbPtsPhi * nbPtsTheta;

    CT_AbstractUndefinedSizePointCloud *undepositPointCloud = PS_REPOSITORY->createNewUndefinedSizePointCloud();

    // Construction du cote du cylindre
    float valTheta, valPhi, valRadius;
    for ( float i = _thetaMin ; (i < _thetaMax) && !isStopped() ; i += _resTheta )
    {
        for ( float j = _phiMin ; (j <= _phiMax) && !isStopped() ; j += _resPhi )
        {
            // On ajoute un point en tenant compte de la variabilité en epsilone
            valTheta = i - _noiseTheta + ( ((double)rand()/RAND_MAX) * 2 * _noiseTheta );
            valPhi = j - _noisePhi + ( ((double)rand()/RAND_MAX) * 2 * _noisePhi );
            valRadius = _radius - _noiseRadius + ( ((double)rand()/RAND_MAX) * 2 * _noiseRadius );

            undepositPointCloud->addPoint( Eigen::Vector3d(_botX + ( valRadius * sin(valPhi) * cos( valTheta ) ), _botY + ( valRadius * sin(valPhi) * sin( valTheta ) ), _botZ + ( valRadius * cos(valPhi) ) ));

            nbPts++;

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

            // 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()) {
        // ------------------------------
        // Create OUT groups and items

        // ----------------------------------------------------------------------------
        // Works on the result corresponding to DEF_resultOut_resultScene
        CT_StandardItemGroup* groupOut_groupScene = new CT_StandardItemGroup(DEF_groupOut_groupScene, resultOut_resultScene);

        // UNCOMMENT Following lines and complete parameters of the item's contructor
        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
    _thetaMax *= RAD_TO_DEG;
    _thetaMin *= RAD_TO_DEG;
    _phiMax *= RAD_TO_DEG;
    _phiMin *= RAD_TO_DEG;
    _resTheta *= RAD_TO_DEG;
    _resPhi *= RAD_TO_DEG;
}