#include "qdebug.h"
#include "lvox3_stepcomputelvoxgrids.h"
//In/Out
#include "ct_result/model/inModel/ct_inresultmodelgrouptocopy.h"
#include "ct_result/model/outModel/tools/ct_outresultmodelgrouptocopypossibilities.h"

#include "ct_result/ct_resultgroup.h"

//Items
#include "ct_itemdrawable/ct_scene.h"
#include "ct_itemdrawable/ct_scanner.h"
#include "ct_itemdrawable/ct_shootingpatternd.h"
#include "ct_itemdrawable/ct_grid3d.h"
#include "ct_itemdrawable/abstract/ct_abstractimage2d.h"
#include "ct_itemdrawable/ct_pointcluster.h"

#include "ct_view/ct_stepconfigurabledialog.h"
#include "ct_view/ct_buttongroup.h"

//Workers
#include "tools/worker/lvox3_filtervoxelsbyzvaluesofraster.h"
#include "tools/worker/lvox3_computehits.h"
#include "tools/worker/lvox3_computebefore.h"
#include "tools/worker/lvox3_computetheoriticals.h"
#include "tools/worker/lvox3_computeall.h"

//Tools
#include "tools/lvox3_computelvoxgridspreparator.h"
#include "tools/lvox3_gridtype.h"
#include "tools/lvox3_errorcode.h"

#define DEF_SearchInResult          "r"
#define DEF_SearchInGroup           "gr"
#define DEF_SearchInScene           "sc"
#define DEF_SearchInScan            "sca"
#define DEF_SearchInShotPattern     "pat"
#define DEF_SearchInMNT             "mnt"
#define DEF_SearchInSky             "sky"

#define DEF_SearchInRefGridResult   "rgr"
#define DEF_SearchInRefGridGroup    "sr"
#define DEF_SearchInRefGrid         "ref"

#include <math.h>
//# define M_PI           3.14159265358979323846  /* pi */

/**
 * @brief Compute the LVOX Ni, Nb, Nt grids
 */
LVOX3_StepComputeLvoxGrids::LVOX3_StepComputeLvoxGrids(CT_StepInitializeData &dataInit) : CT_AbstractStep(dataInit)
{
    m_resolutionx = 0.5;
    m_resolutiony = 0.5;
    m_resolutionz = 0.5;
    m_computeDistances = false;
    //_onlyHits = false;
    m_gridMode = lvox::BoundingBoxOfTheScene;
    m_coordinates.x() = 0.0;
    m_coordinates.y() = 0.0;
    m_coordinates.z() = 0.0;

    m_dimensions.x() = 80;
    m_dimensions.y() = 80;
    m_dimensions.z() = 80;

    _LengthOrExtension1 = 6.0;
    _DiameterOrExtension2 = 2.0;
    _FlatProjectedArea = 0.1;
    _elementProjectedAreaMethodCollection.insert(tr("Needle (from Length and Diameter)"),NeedleFromDimension);
    _elementProjectedAreaMethodCollection.insert(tr("BroadLeaf (from Extension1 and Extension2)"),BroadLeafFromDimension);
    _elementProjectedAreaMethodCollection.insert(tr("Needle Area (from flat scan)"),NeedleFromFlatArea);
    _elementProjectedAreaMethodCollection.insert(tr("BroadLead Area (from flat scan)"),BroadLeafFromFlatArea);

    _confidenceLevelCollection.insert(tr("0,5"),0.5);
    _confidenceLevelCollection.insert(tr("0,9"),0.9);
    _confidenceLevelCollection.insert(tr("0,95"),0.95);
}

/**
 * @brief Return a short description of what this class does
 */
QString LVOX3_StepComputeLvoxGrids::getStepDescription() const
{
    return tr("2 - Calculer les grilles LVOX (LVOX 3)");
}

/*! \brief Step detailed description
 *
 * Return a detailed description of the step function
 */
QString LVOX3_StepComputeLvoxGrids::getStepDetailledDescription() const
{
    return tr("Cette étape permet de créer une grille 3D pour les bons tirs, les tirs théoriques et les tirs interceptés à partir d'un taille de voxel défini.");
}

/**
 * @brief Return a new empty instance of this class
 */
CT_VirtualAbstractStep* LVOX3_StepComputeLvoxGrids::createNewInstance(CT_StepInitializeData &dataInit)
{
    // Creates an instance of this step
    return new LVOX3_StepComputeLvoxGrids(dataInit);
}

/**
 * @brief This method defines what kind of input the step can accept
 */
void LVOX3_StepComputeLvoxGrids::createInResultModelListProtected()
{
    // We must have
    // - a scene
    // - a scanner OR a shooting pattern to simulate shots (at least one is obligatory)
    // - an MNT (optionnal)

    CT_InResultModelGroupToCopy *resultScan = createNewInResultModelForCopy(DEF_SearchInResult, tr("Scène"), "", true);
    resultScan->setZeroOrMoreRootGroup();
    resultScan->addGroupModel("", DEF_SearchInGroup, CT_AbstractItemGroup::staticGetType(), tr("Scan"));
    resultScan->addItemModel(DEF_SearchInGroup, DEF_SearchInScene, CT_Scene::staticGetType(), tr("Scène"));

    // TODO : in ComputreeV5 set the scanner and the pattern in an exclusive group
    resultScan->addItemModel(DEF_SearchInGroup, DEF_SearchInScan, CT_Scanner::staticGetType(), tr("Scanner"), tr("The scanner is used to simulate the shooting pattern. If it was not present choose at least a shooting pattern !"), CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);
    resultScan->addItemModel(DEF_SearchInGroup, DEF_SearchInShotPattern, CT_ShootingPatternD::staticGetType(), tr("Shooting pattern"), tr("The shooting pattern is used to simulate it. If it was not present choose at least a scanner !"), CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);

    resultScan->addItemModel(DEF_SearchInGroup, DEF_SearchInMNT, CT_AbstractImage2D::staticGetType(), tr("MNT"), "", CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);
    resultScan->addItemModel(DEF_SearchInGroup, DEF_SearchInSky, CT_AbstractImage2D::staticGetType(), tr("MNC"), "", CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);

    CT_InResultModelGroupToCopy *resultRefGrid = createNewInResultModelForCopy(DEF_SearchInRefGridResult, tr("Grid de référence"), "", true);

    resultRefGrid->setZeroOrMoreRootGroup();
    resultRefGrid->addGroupModel("", DEF_SearchInRefGridGroup, CT_AbstractItemGroup::staticGetType(), tr("Groupe de référence"), "", CT_InAbstractGroupModel::CG_ChooseOneIfMultiple);
    resultRefGrid->addItemModel(DEF_SearchInRefGridGroup, DEF_SearchInRefGrid, lvox::Grid3Di::staticGetType(), tr("Grille de référence"), "",
                             CT_InAbstractModel::C_ChooseOneIfMultiple, CT_InAbstractModel::F_IsOptional);

}

/**
 * @brief This method creates a window for the user to set the different parameters of the step.
 */
void LVOX3_StepComputeLvoxGrids::createPreConfigurationDialog()
{
    /*CT_StepConfigurableDialog *configDialog = newStandardPreConfigurationDialog();*/

    /*********************************************/
    /*              Attributes of LVox           */
    /*********************************************/
    /*configDialog->addDouble(tr("Resolution of the grids x axis"),tr("meters"),0.0001,10000,2, m_resolutionx );
    configDialog->addDouble(tr("Resolution of the grids y axis"),tr("meters"),0.0001,10000,2, m_resolutiony );
    configDialog->addDouble(tr("Resolution of the grids z axis"),tr("meters"),0.0001,10000,2, m_resolutionz );
    configDialog->addBool("", "", tr("Compute Distances"), m_computeDistances,"Cette option permet de calculer la moyenne de distance dans chaque voxel pour le calcul du PAD.(Pas nécessaire pour le RDI)");
    //Defines the grid params
    configDialog->addText(tr("Reference for (minX, minY, minZ) corner of the grid :"),"", "");

    CT_ButtonGroup &bg_gridMode = configDialog->addButtonGroup(m_gridMode);
    configDialog->addExcludeValue("", "", tr("Default mode : Bounding box of the scene"), bg_gridMode, 0);
    configDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates:"), bg_gridMode, 1);
    configDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates + custom dimensions:"), bg_gridMode, 2);
    configDialog->addExcludeValue("", "", tr("Custom mode : centered on folowing coordinates + custom dimensions:"), bg_gridMode, 3,"Cette option est différente des trois autres: \nles coordonnées X,Y,Z sont le point centre de la grille\nla dimension est la dimension est la dimension de chaque côté du point centre.");

#ifdef CT_LARCHIHEADER_H
    configDialog->addExcludeValue("", "", tr("Automatic mode : use grid paramters from grid file"), bg_gridMode, 4);
    configDialog->addFileChoice(tr("Choisir le fichier .grid"), CT_FileChoiceButton::OneExistingFile, "Fichier L-Architect (*.grid)", m_gridFilePath);
#endif

    float maxFloat = std::numeric_limits<float>::max();
    float minFloat = -maxFloat;

    configDialog->addText(tr("Parameters for custom modes:"),"", "");
    configDialog->addDouble(tr("X coordinate:"), "", minFloat, maxFloat, 4, m_coordinates.x());
    configDialog->addDouble(tr("Y coordinate:"), "", minFloat, maxFloat, 4, m_coordinates.y());
    configDialog->addDouble(tr("Z coordinate:"), "", minFloat, maxFloat, 4, m_coordinates.z());

    configDialog->addInt(tr("X dimension:"), "", 1, 1000, m_dimensions.x());
    configDialog->addInt(tr("Y dimension:"), "", 1, 1000, m_dimensions.y());
    configDialog->addInt(tr("Z dimension:"), "", 1, 1000, m_dimensions.z());
    configDialog->addEmpty();
    configDialog->addText(tr("<b>Attention: une résolution x,y,z de 1 cm pour une zone de 10m^3 prend au moins 16GB de mémoire. Un manque d'espace causera un arrêt forcé de CompuTree.</b>"),"", "");
*/}

void LVOX3_StepComputeLvoxGrids::createPostConfigurationDialog()
{
    /*if (m_computeDistances)
    {*/
        CT_StepConfigurableDialog *configDialog = newStandardPostConfigurationDialog();

        //********************************************//
        //              Attributes of LVox            //
        //********************************************//
        configDialog->addDouble(tr("Resolution of the grids x axis"),tr("meters"),0.0001,10000,2, m_resolutionx );
        configDialog->addDouble(tr("Resolution of the grids y axis"),tr("meters"),0.0001,10000,2, m_resolutiony );
        configDialog->addDouble(tr("Resolution of the grids z axis"),tr("meters"),0.0001,10000,2, m_resolutionz );
        configDialog->addBool("", "", tr("Compute Distances"), m_computeDistances,"Cette option permet de calculer la moyenne de distance dans chaque voxel pour le calcul du PAD.(Pas nécessaire pour le RDI)");
        //Defines the grid params
        configDialog->addText(tr("Reference for (minX, minY, minZ) corner of the grid :"),"", "");

        CT_ButtonGroup &bg_gridMode = configDialog->addButtonGroup(m_gridMode);
        configDialog->addExcludeValue("", "", tr("Default mode : Bounding box of the scene"), bg_gridMode, lvox::BoundingBoxOfTheScene);
        configDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates:"), bg_gridMode, lvox::RelativeToCoordinates);
        configDialog->addExcludeValue("", "", tr("Custom mode : Relative to folowing coordinates + custom dimensions:"), bg_gridMode, lvox::RelativeToCoordinatesAndCustomDimensions);
        configDialog->addExcludeValue("", "", tr("Custom mode : centered on folowing coordinates + custom dimensions:"), bg_gridMode, lvox::CenteredOnCoordinatesAndCustomDimensions,"Cette option est différente des trois autres: \nles coordonnées X,Y,Z sont le point centre de la grille\nla dimension est la dimension est la dimension de chaque côté du point centre.");
        configDialog->addExcludeValue("", "", tr("From other grid:"), bg_gridMode, lvox::FromOtherGrid,"Get dimesions from a gid (hit) from a previous step");

    #ifdef CT_LARCHIHEADER_H
        configDialog->addExcludeValue("", "", tr("Automatic mode : use grid paramters from grid file"), bg_gridMode, 4);
        configDialog->addFileChoice(tr("Choisir le fichier .grid"), CT_FileChoiceButton::OneExistingFile, "Fichier L-Architect (*.grid)", m_gridFilePath);
    #endif

        float maxFloat = std::numeric_limits<float>::max();
        float minFloat = -maxFloat;

        configDialog->addText(tr("Parameters for custom modes:"),"", "");
        configDialog->addDouble(tr("X coordinate:"), "", minFloat, maxFloat, 5, m_coordinates.x());
        configDialog->addDouble(tr("Y coordinate:"), "", minFloat, maxFloat, 5, m_coordinates.y());
        configDialog->addDouble(tr("Z coordinate:"), "", minFloat, maxFloat, 5, m_coordinates.z());

        configDialog->addDouble(tr("X dimension:"), "", 1, 1000, 5, m_dimensions.x());
        configDialog->addDouble(tr("Y dimension:"), "", 1, 1000, 5, m_dimensions.y());
        configDialog->addDouble(tr("Z dimension:"), "", 1, 1000, 5, m_dimensions.z());
        //TODO
        //configDialog->addDouble(tr("X dimension:"), "", minFloat, maxFloat, 4, m_dimensions.x());
        //configDialog->addDouble(tr("Y dimension:"), "", minFloat, maxFloat, 4, m_dimensions.y());
        //configDialog->addDouble(tr("Z dimension:"), "", minFloat, maxFloat, 4, m_dimensions.z());

        configDialog->addEmpty();
        configDialog->addText(tr("<b>Attention: une résolution x,y,z de 1 cm pour une zone de 10m^3 prend au moins 16GB de mémoire. Un manque d'espace causera un arrêt forcé de CompuTree.</b>"),"", "");


        configDialog->addText(tr("Estimation of vegetation element area (for PAD computation)"),"", "");
        configDialog->addStringChoice(tr("Method"), "", _elementProjectedAreaMethodCollection.keys(), _elementProjectedAreaMethods);
        configDialog->addDouble(tr("Length/Extension1 (cm)"), "", 0, 100, 4, _LengthOrExtension1);
        configDialog->addDouble(tr("Diameter/Extension2 (cm)"), "", 0, 100, 4, _DiameterOrExtension2);
        configDialog->addDouble(tr("Projected Area (from flat scan, cm^2)"), "", 0, 100, 4, _FlatProjectedArea);

        //configDialog->addBool(tr("Compute Only the Hits grid"),"","",_onlyHits);

    //}
}

/**
 * @brief This method defines what kind of output the step produces
 */
void LVOX3_StepComputeLvoxGrids::createOutResultModelListProtected()
{
    // create a new OUT result that is a copy of the IN result selected by the user
    CT_OutResultModelGroupToCopyPossibilities *res = createNewOutResultModelToCopy(DEF_SearchInResult);
    // Used for compute reference grid
    //CT_OutResultModelGroupToCopyPossibilities *refGridResult = createNewOutResultModelToCopy(DEF_SearchInRefGridResult);

    if (res != NULL)
    {
        // Add all grids : Ni, Nt,Nb
        res->addItemModel(DEF_SearchInGroup, _hits_ModelName, new lvox::Grid3Di(), tr("Hits"));
        res->addItemAttributeModel(_hits_ModelName, _NiFlag_ModelName, new CT_StdItemAttributeT<bool>("LVOX_GRD_NI"), tr("isNi"));

        //if(!_onlyHits)
        //{
            res->addItemModel(DEF_SearchInGroup, _theo_ModelName, new lvox::Grid3Di(), tr("Theoretical"));
            res->addItemAttributeModel(_theo_ModelName, _NtFlag_ModelName, new CT_StdItemAttributeT<bool>("LVOX_GRD_NT"), tr("isNt"));

            res->addItemModel(DEF_SearchInGroup, _bef_ModelName, new lvox::Grid3Di(), tr("Before"));
            res->addItemAttributeModel(_bef_ModelName, _NbFlag_ModelName, new CT_StdItemAttributeT<bool>("LVOX_GRD_NB"), tr("isNb"));

            //res->addItemModel(DEF_SearchInGroup, _MinMaxPoint_ModelName,  new CT_PointCluster(), tr("Min /max Points"));
        //}

        // and if must compute distance, grids with distance
        /*if (m_computeDistances)
        {
            res->addItemModel(DEF_SearchInGroup, _hitStats_ModelName, new lvox::Grid3Di(), tr("StatsHits"));
            res->addItemModel(DEF_SearchInGroup, _theoStats_ModelName, new lvox::Grid3Di(), tr("StatsTheoretical"));
            res->addItemModel(DEF_SearchInGroup, _befStats_ModelName, new lvox::Grid3Di(), tr("StatsBefore"));
        }*/
    }
}

/**
 * @brief This method does the job
 * It creates three 3D grids from point cloud information and user defined voxel size (Hits, Theoritical Rays, Before Rays)
 */
void LVOX3_StepComputeLvoxGrids::compute()
{
    CT_ResultGroup* outResult = getOutResultList().first();    

    CT_ResultGroupIterator itGrp(outResult, this, DEF_SearchInGroup);

    LVOX3_ComputeLVOXGridsPreparator::Coordinates coo;
    //Coordinates and dimensions for custom mode grid creation
    coo.coordinate = m_coordinates;
    coo.dimension = m_dimensions;    


    bool isExtend = true;
    if(m_gridMode == lvox::FromOtherGrid){
        CT_ResultGroup* refResult = getInputResults().at(1);        
        if (refResult != NULL){            
            CT_ResultGroupIterator itRefGrp(refResult, this, DEF_SearchInRefGridGroup);
            while (itRefGrp.hasNext())
            {                
                CT_AbstractItemGroup *refGroup = (CT_AbstractItemGroup*) itRefGrp.next();
                if(refGroup != NULL){                    
                    const lvox::Grid3Di* refGrid =  (const lvox::Grid3Di*) refGroup->firstItemByINModelName(this, DEF_SearchInRefGrid);
                    if(refGrid != NULL){
                        Eigen::Vector3d minCoords;
                        Eigen::Vector3d maxCoords;                        
                        refGrid->getBoundingBox(minCoords, maxCoords);
                        //refGrid->getMinCoordinates(minCoords);
                        //refGrid->getMaxCoordinates(maxCoords);
                        coo.coordinate = minCoords;
                        coo.dimension = maxCoords - minCoords;
                        //don't extend the grid dimension by one unit when use ref group
                        isExtend = false;
                        qDebug()<<"min max from ref" << minCoords.x() << minCoords.y() << minCoords.z() << maxCoords.x() << maxCoords.y() << maxCoords.z();
                        qDebug()<<"Set coords from ref grid finished.@@@";
                        break;
                    }
                }
            }
        }
    }

    qDebug()<<"Input coord" << coo.coordinate.x() << coo.coordinate.y() << coo.coordinate.z() << coo.dimension.x() << coo.dimension.y() << coo.dimension.z();
    LVOX3_ComputeLVOXGridsPreparator p;
    LVOX3_ComputeLVOXGridsPreparator::Result pRes = p.prepare(this,
                                                              itGrp,
                                                              DEF_SearchInScene,
                                                              DEF_SearchInScan,
                                                              DEF_SearchInShotPattern,
                                                              DEF_SearchInMNT,
                                                              DEF_SearchInSky,
                                                              //DEF_SearchInRefGrid,
                                                              m_resolutionx,
                                                              m_resolutiony,
                                                              m_resolutionz,
                                                              (lvox::GridMode)m_gridMode,
                                                              coo,
                                                              m_gridFilePath.isEmpty() ? "" : m_gridFilePath.first());

    //If the grid had it's dimension fitted according to config
    if(pRes.valid) {
        LVOX3_ComputeAll workersManager; //Worker manager (Thread and progress manager)
        LVOX3_ComputeLVOXGridsPreparator::Result::ToComputeCollectionIterator it(pRes.elementsToCompute);

        while (it.hasNext()
               && !isStopped())
        {
            it.next();
            CT_AbstractItemGroup* group = it.key();
            const LVOX3_ComputeLVOXGridsPreparator::ToCompute& tc = it.value();

            lvox::Grid3Di*   hitGrid = NULL;
            lvox::Grid3Di*   theoriticalGrid = NULL;
            lvox::Grid3Di*   beforeGrid = NULL;


            //Used to compute distances (when option is checked)
            /*lvox::Grid3Di*   StatsInGrid = NULL;
            lvox::Grid3Di*   StatsTheoritical = NULL;
            lvox::Grid3Di*   StatsBefore = NULL;*/

            LVOX3_ComputeTheoriticals* theoriticalWorker = NULL;
            LVOX3_ComputeHits* hitsWorker = NULL;
            LVOX3_ComputeBefore* beforeWorker = NULL;

            //Used to pass to voxel filtering step more easily
            QList<LVOX3_AbstractGrid3D*> allGrids;             
            // Declaring the output grids
            qDebug() << "Box"<< pRes.minBBox.x() << pRes.minBBox.y() << pRes.minBBox.z() << pRes.maxBBox.x() << pRes.maxBBox.y() << pRes.maxBBox.z();
            qDebug() << "coords + dimensions: "<< pRes.minBBox.x() << pRes.minBBox.y() << pRes.minBBox.z() << pRes.maxBBox.x() - pRes.minBBox.x()<< pRes.maxBBox.y() - pRes.minBBox.y() << pRes.maxBBox.z() - pRes.minBBox.z();
            hitGrid = lvox::Grid3Di::createGrid3DFromXYZCoords(_hits_ModelName.completeName(), outResult, pRes.minBBox.x(), pRes.minBBox.y(), pRes.minBBox.z(), pRes.maxBBox.x(), pRes.maxBBox.y(), pRes.maxBBox.z(), m_resolutionx,m_resolutiony,m_resolutionz, lvox::Max_Error_Code, 0, isExtend);
            theoriticalGrid = new lvox::Grid3Di(_theo_ModelName.completeName(), outResult, hitGrid->minX(), hitGrid->minY(), hitGrid->minZ(), hitGrid->xdim(), hitGrid->ydim(), hitGrid->zdim(), m_resolutionx,m_resolutiony,m_resolutionz, lvox::Max_Error_Code, 0);
            beforeGrid = new lvox::Grid3Di(_bef_ModelName.completeName(), outResult, hitGrid->minX(), hitGrid->minY(), hitGrid->minZ(), hitGrid->xdim(), hitGrid->ydim(), hitGrid->zdim(), m_resolutionx,m_resolutiony,m_resolutionz, lvox::Max_Error_Code, 0);

            allGrids.append(hitGrid);
            allGrids.append(theoriticalGrid);
            allGrids.append(beforeGrid);

            //Adding item attribute to identify each grid in subsequent steps
            hitGrid->addItemAttribute(new CT_StdItemAttributeT<bool>(_NiFlag_ModelName.completeName(), "LVOX_GRD_NI", outResult, true));
            theoriticalGrid->addItemAttribute(new CT_StdItemAttributeT<bool>(_NtFlag_ModelName.completeName(), "LVOX_GRD_NT", outResult, true));
            beforeGrid->addItemAttribute(new CT_StdItemAttributeT<bool>(_NbFlag_ModelName.completeName(), "LVOX_GRD_NB", outResult, true));


            // and if must compute distance, grids with distance
            if (m_computeDistances)
            {
                //Creating the grids for mean distances
                /*StatsInGrid = new lvox::Grid3Di(_hitStats_ModelName.completeName(), outResult, hitGrid->minX(), hitGrid->minY(), hitGrid->minZ(), hitGrid->xdim(), hitGrid->ydim(), hitGrid->zdim(), m_resolutionx,m_resolutiony,m_resolutionz, -1, 0);
                StatsTheoritical = new lvox::Grid3Di(_theoStats_ModelName.completeName(), outResult, hitGrid->minX(), hitGrid->minY(), hitGrid->minZ(), hitGrid->xdim(), hitGrid->ydim(), hitGrid->zdim(), m_resolutionx,m_resolutiony,m_resolutionz, -1, 0);
                StatsBefore = new lvox::Grid3Di(_befStats_ModelName.completeName(), outResult, hitGrid->minX(), hitGrid->minY(), hitGrid->minZ(), hitGrid->xdim(), hitGrid->ydim(), hitGrid->zdim(), m_resolutionx,m_resolutiony,m_resolutionz, -1, 0);

                //Added to the group for UI drawing purposes
                group->addItemDrawable(StatsInGrid);
                group->addItemDrawable(StatsTheoritical);
                group->addItemDrawable(StatsBefore);*/
                double elementProjectedArea = computeElementProjectedArea();
                qDebug()<< "##################" << elementProjectedArea;
                hitGrid->setLambda1(elementProjectedArea);
                theoriticalGrid->setLambda1(elementProjectedArea);
                beforeGrid->setLambda1(elementProjectedArea);
            }

            //Added to the group for UI drawing purposes
            group->addItemDrawable(hitGrid);
            group->addItemDrawable(theoriticalGrid);
            group->addItemDrawable(beforeGrid);

            //Workers in tools to compute hits, theoretical and before voxels from shooting pattern and point cloud
            /*
            hitsWorker = new LVOX3_ComputeHits(tc.pattern, tc.scene->getPointCloudIndex(), hitGrid, StatsInGrid);
            theoriticalWorker = new LVOX3_ComputeTheoriticals(tc.pattern, theoriticalGrid, StatsTheoritical);
            beforeWorker = new LVOX3_ComputeBefore(tc.pattern, tc.scene->getPointCloudIndex(), beforeGrid, StatsBefore);
            */

            hitsWorker = new LVOX3_ComputeHits(tc.pattern, tc.scene->getPointCloudIndex(), hitGrid, m_computeDistances);
            theoriticalWorker = new LVOX3_ComputeTheoriticals(tc.pattern, theoriticalGrid, m_computeDistances);
            beforeWorker = new LVOX3_ComputeBefore(tc.pattern, tc.scene->getPointCloudIndex(), beforeGrid, m_computeDistances);

            //Added to manager for progress bar and mutex multithreading
            workersManager.addWorker(1, beforeWorker);
            workersManager.addWorker(1, hitsWorker);
            workersManager.addWorker(1, theoriticalWorker);

            //Legacy code for filtering MNT and Sky, is now a separate step in filtering

            LVOX3_FilterVoxelsByZValuesOfRaster* filterVoxelsBelowMNTWorker = NULL;
            LVOX3_FilterVoxelsByZValuesOfRaster* filterVoxelsInSkyWorker = NULL;

            if(tc.mnt != NULL)
                filterVoxelsBelowMNTWorker = new LVOX3_FilterVoxelsByZValuesOfRaster(allGrids, tc.mnt, LVOX3_FilterVoxelsByZValuesOfRaster::Below, lvox::MNT);

            if(tc.sky != NULL)
                filterVoxelsInSkyWorker = new LVOX3_FilterVoxelsByZValuesOfRaster(allGrids, tc.sky, LVOX3_FilterVoxelsByZValuesOfRaster::Above, lvox::Sky);

            if(filterVoxelsBelowMNTWorker != NULL)
                workersManager.addWorker(0, filterVoxelsBelowMNTWorker);

            if(filterVoxelsInSkyWorker != NULL)
                workersManager.addWorker(0, filterVoxelsInSkyWorker);
            //Legacy code for filtering MNT and Sky, is now a separate step in filtering
        }


        //Connected to show progress of the workers
        connect(&workersManager, SIGNAL(progressChanged(int)), this, SLOT(progressChanged(int)), Qt::DirectConnection);
        connect(this, SIGNAL(stopped()), &workersManager, SLOT(cancel()), Qt::DirectConnection);
        workersManager.compute();        
    }
}

/**
 * @brief Called from worker manager when progress changed
 */
void LVOX3_StepComputeLvoxGrids::progressChanged(int p)
{
    setProgress(p);
}
/**
 * @brief LVOX3_StepComputeLvoxGrids::computeElementProjectedArea
 * @return this function computes the random projected area of vegetation elements.
 * which is equal to lambda1*resx*resy*resz
 * This can be computed according to four different methods
 *
 */
float LVOX3_StepComputeLvoxGrids::computeElementProjectedArea()
{
    if(_elementProjectedAreaMethodCollection.value(_elementProjectedAreaMethods) == NeedleFromDimension)
    {        
        return (2 * M_PI * (_LengthOrExtension1/100) * (_DiameterOrExtension2/100)/2) / 4;//= (2pi *  lenght * radius) / 4
    }
    else if(_elementProjectedAreaMethodCollection.value(_elementProjectedAreaMethods) == BroadLeafFromDimension)
    {
        return (2 * M_PI * pow((_LengthOrExtension1/100),2)) / 4;//= (2pi * radius^2) / 4
    }
    else if(_elementProjectedAreaMethodCollection.value(_elementProjectedAreaMethods) == NeedleFromFlatArea)
    {
        return (M_PI *(_FlatProjectedArea/10000)) / 4;//= (pi*flatarea) / 4
    }
    else if(_elementProjectedAreaMethodCollection.value(_elementProjectedAreaMethods) == BroadLeafFromFlatArea)
    {
        return (2 *(_FlatProjectedArea/10000)) / 4;//= (2halfarea) / 4
    }
}