/****************************************************************************
 Copyright (C) 2010-2021 the Office National des Forêts (ONF), France
                         All rights reserved.

 Contact : alexandre.piboule@onf.fr

 Developers : Michael Krebbs (Independant)
              Alexandre PIBOULE (ONF)

 This file is part of PluginGenerate library.

 PluginGenerate is free library: you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 PluginGenerate is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with PluginGenerate.  If not, see <http://www.gnu.org/licenses/lgpl.html>.
*****************************************************************************/

#include "gen_stepgeneratecylindercloud.h"

#include <time.h>
#include <assert.h>

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

#define DEG_TO_RAD 0.01745329251

GEN_StepGenerateCylinder::GEN_StepGenerateCylinder() : SuperClass()
{
    _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;
}

QString GEN_StepGenerateCylinder::description() const
{
    return tr("Créer un Cylindre de points");
}

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

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

void GEN_StepGenerateCylinder::declareInputModels(CT_StepInModelStructureManager& manager)
{
    manager.setNotNeedInputResult();
}

void GEN_StepGenerateCylinder::declareOutputModels(CT_StepOutModelStructureManager& manager)
{
    manager.addResult(m_hOutResult, tr("Generated Point cloud"));
    manager.setRootGroup(m_hOutResult, m_hOutRootGroup);
    manager.addItem(m_hOutRootGroup, m_hOutScene, tr("Generated Cylinder cloud"));
}

void GEN_StepGenerateCylinder::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Height"), "", 0.0001, std::numeric_limits<double>::max(), 4, _height);
    postInputConfigDialog->addDouble(tr("Radius"), "", 0.0001, std::numeric_limits<double>::max(), 4, _radius);
    postInputConfigDialog->addDouble(tr("Minimum theta"), "°", 0, 359.9999, 4, _minAng, DEG_TO_RAD);
    postInputConfigDialog->addDouble(tr("Maximum theta"), "°", 0.0001, 360, 4, _maxAng, DEG_TO_RAD);
    postInputConfigDialog->addText(tr("Base center"), "", "");
    postInputConfigDialog->addDouble("X", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _botX);
    postInputConfigDialog->addDouble("Y", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _botY);
    postInputConfigDialog->addDouble("Z", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _botZ);
    postInputConfigDialog->addText(tr("Direction"), "", "");
    postInputConfigDialog->addDouble("X", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisX);
    postInputConfigDialog->addDouble("Y", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisY);
    postInputConfigDialog->addDouble("Z", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _axisZ);
    postInputConfigDialog->addText(tr("Resolutions"), "", "");
    postInputConfigDialog->addDouble("Z", "", 0.0001, std::numeric_limits<double>::max(), 4, _resHeight);
    postInputConfigDialog->addDouble(tr("Theta"), "°", 0.0001, std::numeric_limits<double>::max(), 4, _resTheta, DEG_TO_RAD);
    postInputConfigDialog->addDouble(tr("Radius"), "", 0.0001, std::numeric_limits<double>::max(), 4, _resRadius);
    postInputConfigDialog->addText(tr("Add noise"), "", "");
    postInputConfigDialog->addDouble(tr("Theta"), "°", 0, std::numeric_limits<double>::max(), 4, _noiseTheta, DEG_TO_RAD);
    postInputConfigDialog->addDouble(tr("Height"), "", 0, std::numeric_limits<double>::max(), 4, _noiseHeight);
    postInputConfigDialog->addDouble(tr("Radius"), "", 0, std::numeric_limits<double>::max(), 4, _noiseRadius);
    postInputConfigDialog->addBool(tr("Generate bot and top faces"),"","",_generateFaces);
}

void GEN_StepGenerateCylinder::compute()
{
    assert(_minAng < _maxAng);
    if (_minAng >= _maxAng) {qDebug() << "GEN_StepGenerateCylinder::compute" << ", " << "_minAng >= _maxAng";}

    for(CT_ResultGroup* result : m_hOutResult.iterateOutputs()) {

        CT_StandardItemGroup* rootGroup = m_hOutRootGroup.createInstance();
        result->addRootGroup(m_hOutRootGroup, rootGroup);

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

        int nbPts = 0;
        int nbPtsTheta = int(ceil((_maxAng - _minAng) / _resTheta));
        int nbPtsHeight = int(ceil(_height / _resHeight));
        int nbPtsRadius = int(ceil(_radius / _resRadius));
        int nbPtsCote = nbPtsTheta * nbPtsHeight;
        int 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(float(nbPts) * 40.0f / float(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(float(cpt++) * 5.0f / float(nbPtsFace) + 40.0f);

                    // 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(float(cpt++) * 5.0f / float(nbPtsFace) + 45.0f);

                    // 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(((float(i++) * 50.0f) / (float(nbPts))) + 50.0f);

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

        if(!isStopped()) {

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

            rootGroup->addSingularItem(m_hOutScene, itemOut_scene);
        }
    }
}