#include "lvox3_computetheoriticals.h"

//Tools
#include "tools/traversal/woo/lvox3_grid3dwootraversalalgorithm.h"
#include "tools/traversal/woo/visitor/lvox3_countvisitor.h"
#include "tools/traversal/woo/visitor/lvox3_distancevisitor.h"
#include "tools/lvox3_errorcode.h"

LVOX3_ComputeTheoriticals::LVOX3_ComputeTheoriticals(CT_ShootingPattern* pattern,
                                                     lvox::Grid3Di* theoricals,
                                                     bool computedistance
                                                     /*lvox::Grid3Di *shotStatsDistance*/) : LVOX3_Worker()
{
    m_pattern = pattern;
    m_outputTheoriticalGrid = theoricals;
    //_outputStatsTheoriticalGrid = shotStatsDistance;
    _computedistance =computedistance;
}

LVOX3_ComputeTheoriticals::~LVOX3_ComputeTheoriticals()
{
}

void LVOX3_ComputeTheoriticals::doTheJob()
{
    // Creates visitors
    QVector<LVOX3_Grid3DVoxelWooVisitor*> list;

    LVOX3_CountVisitor<lvox::Grid3DiType> countVisitor(m_outputTheoriticalGrid);
    LVOX3_DistanceVisitor<lvox::Grid3DiType> distVisitor(m_outputTheoriticalGrid);

    if (!_computedistance)list.append(&countVisitor);
    else list.append(&distVisitor);

    // Creates traversal algorithm
    LVOX3_Grid3DWooTraversalAlgorithm<lvox::Grid3DiType> algo(m_outputTheoriticalGrid, true, list);

    const size_t nShot = m_pattern->getNumberOfShots();
    qDebug()<<"Nb shot Theo: "<<nShot;

    //setProgressRange(0, /*(_outputStatsTheoriticalGrid != NULL)*/_computedistance ? nShot+1 : nShot);
    setProgressRange(0, _computedistance ? nShot+1 : nShot);
    /*
    //debug, create a sphere of radius 1m and write to file
    QFile shotdebug(QString("shots.asc"));
    if (shotdebug.open(QIODevice::ReadWrite | QIODevice::Truncate | QIODevice::Text)) {
        QTextStream shotstream(&shotdebug);
        shotstream.setRealNumberNotation(QTextStream::FixedNotation);
        for(size_t i=0; (i<nShot) && !mustCancel(); ++i) {
            const CT_Shot shot = m_pattern->getShotAt(i);
            Eigen::Vector3d dir = shot.getDirection().normalized();
            Eigen::Vector3d aPoint = shot.getOrigin() + dir;
            //parallel pattern debug
            //Eigen::Vector3d org = shot.getOrigin();
            //shotstream << org(0) << " "<<org(1) <<" "<< org(2) << endl;
            shotstream << aPoint(0) << " "<<aPoint(1) <<" "<< aPoint(2) << endl;
        }
        shotdebug.close();
    }

    //end debug
    */

    for(size_t i=0; (i<nShot) && !mustCancel(); ++i) {
        const CT_Shot shot = m_pattern->getShotAt(i);

        // algo already check if the ray touch the grid or not so we don't have to do twice !
        algo.compute(shot.getOrigin(), shot.getDirection());

        setProgress(i);
    }

    //tempMat = list.at(1)->getDistances();

    //qDebug() << tempMat.rows() << tempMat.cols();

    // Don't forget to calculate min and max in order to visualize it as a colored map
    m_outputTheoriticalGrid->computeMinMax();

    if (/*(_outputStatsTheoriticalGrid != NULL)*/_computedistance
            && !mustCancel())
    {
        //qDebug() << m_outputDeltaTheoriticalGrid->getDistance().rows() << m_outputDeltaTheoriticalGrid->getDistance().cols();

        //const size_t size = _outputStatsTheoriticalGrid->nCells();

        // To get the mean distance we have to divide each voxel the sum of distances by the number of hits
        /*for (size_t i = 0 ; (i < size) && !mustCancel(); ++i )
        {
            const float nHits =  m_outputTheoriticalGrid->valueAtIndex(i);

            if (nHits <= 0)
                m_outputDeltaTheoriticalGrid->setValueAtIndex(i, nHits); // TODO : check if must set an error code here
            else
                m_outputDeltaTheoriticalGrid->setValueAtIndex(i, nHits);
        }*/

        //_outputStatsTheoriticalGrid->computeMinMax();

        setProgress(nShot+1);
    }

}