/****************************************************************************
 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_stepgeneratecubecloud.h"

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

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

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

    setDebuggable(true);
}

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

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


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

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

void GEN_StepGenerateCubeCloud::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 Cube Cloud"));
}

void GEN_StepGenerateCubeCloud::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addText(tr("Bottom left point"), "", "");
    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("Top right point"), "", "");
    postInputConfigDialog->addDouble("X", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _topX);
    postInputConfigDialog->addDouble("Y", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _topY);
    postInputConfigDialog->addDouble("Z", "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _topZ);
    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("Resolution"), "", "");
    postInputConfigDialog->addDouble("X", "", 0.0001, std::numeric_limits<double>::max(), 4, _resX);
    postInputConfigDialog->addDouble("Y", "", 0.0001, std::numeric_limits<double>::max(), 4, _resY);
    postInputConfigDialog->addDouble("Z", "", 0.0001, std::numeric_limits<double>::max(), 4, _resZ);
    postInputConfigDialog->addText(tr("Add noise"), "", "");
    postInputConfigDialog->addDouble("X", "", 0, std::numeric_limits<double>::max(), 4, _noiseX);
    postInputConfigDialog->addDouble("Y", "", 0, std::numeric_limits<double>::max(), 4, _noiseY);
    postInputConfigDialog->addDouble("Z", "", 0, std::numeric_limits<double>::max(), 4, _noiseZ);
}

void GEN_StepGenerateCubeCloud::compute()
{
    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)));

        assert(_topX > _botX);
        assert(_topY > _botY);
        assert(_topZ > _botZ);
        if (_botX >= _topX || _botY >= _topY || _botZ >= _topZ) {qDebug() << "GEN_StepGenerateCubeCloud::compute" << ", " << "_botX >= _topX || _botY >= _topY || _botZ >= _topZ";}

        int nbPts = 0;
        int nbPointsX = int(ceil ((_topX - _botX) / _resX));
        int nbPointsY = int(ceil ((_topY - _botY) / _resY));
        int nbPointsZ = int(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 (int i = 0 ; i < nbPointsX && !isStopped() ; i++)
        {
            for (int 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(float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY));

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

        // On construit la face de face centre en 0,0,0
        for (int i = 0 ; i < nbPointsX && !isStopped() ; i++)
        {
            for (int 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((float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY)) + (50.0f/6.0f));

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

        // On construit la face de derriere centre en 0,0,0
        for (int i = 0 ; i < nbPointsX && !isStopped() ; i++)
        {
            for (int 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((float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY)) + (100.0f/6.0f));

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

        // On construit la face de gauche centre en 0,0,0
        for (int i = 0 ; i < nbPointsY && !isStopped() ; i++)
        {
            for (int 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((float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY)) + (150.0f/6.0f));

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

        // On construit la face de droite centre en 0,0,0
        for (int i = 0 ; i < nbPointsY && !isStopped() ; i++)
        {
            for (int 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((float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY)) + (200.0f/6.0f));

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

        // On construit la face du haut centre en 0,0,0
        for (int i = 0 ; i < nbPointsX && !isStopped() ; i++)
        {
            for (int 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((float(j + (nbPointsX*i)) * (50.0f/6.0f) / float(nbPointsX*nbPointsY)) + (250.0f/6.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);

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

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

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

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