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

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

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

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

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

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

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

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

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

void GEN_StepGeneratePlaneCloud::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 Plane Cloud"));
}

void GEN_StepGeneratePlaneCloud::fillPostInputConfigurationDialog(CT_StepConfigurableDialog* postInputConfigDialog)
{
    postInputConfigDialog->addDouble(tr("Plane height"), "", -std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 4, _height);
    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->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->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->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_StepGeneratePlaneCloud::compute()
{
    assert(_topX > _botX);
    assert(_topY > _botY);
    if (_botX >= _topX || _botY >= _topY) {qDebug() << "GEN_StepGeneratePlaneCloud::compute" << ", " << "_botX >= _topX || _botY >= _topY";}

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

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

        int nbPts = 0;
        int nbPointsX = int(ceil ((_topX - _botX) / _resX));
        int nbPointsY = int(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 (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) + 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(float(j + (nbPointsX*i)) * 50.0f / float(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);

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