/*
 #
 #  File        : itkSTMS_BlurringSTMS.txx
 #                ( C++ header file - STMS )
 #
 #  Description : STMS lib that implements the STMS filter and clustering.
 #                This file is a part of the STMS Library project.
 #                ( https://www.creatis.insa-lyon.fr/site7/fr/realisations )
 #
 #  [1] S. Mure, Grenier, T., Meier, S., Guttmann, R. G., et Benoit-Cattin, H.,
 #       « Unsupervised spatio-temporal filtering of image sequences. A mean-shift specification »,
 #       Pattern Recognition Letters, vol. 68, Part 1, p. 48 - 55, 2015.
 #
 #  Copyright   : Thomas GRENIER - Simon MURE
 #                ( https://www.creatis.insa-lyon.fr/~grenier/ )
 #
 #  License     : CeCILL C
 #                ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.txt )
 #
 #  This software is governed by the CeCILL  license under French law and
 #  abiding by the rules of distribution of free software.  You can  use,
 #  modify and/ or redistribute the software under the terms of the CeCILL
 #  license as circulated by CEA, CNRS and INRIA at the following URL
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL license and that you accept its terms.
 #
*/
/* Please don't forget to cite our work :
  @article {MURE-15a,
    title = {Unsupervised spatio-temporal filtering of image sequences. A mean-shift specification},
    journal = {Pattern Recognition Letters},
    volume = {68, Part 1},
    year = {2015},
    pages = {48 - 55},
    issn = {0167-8655},
    doi = {http://dx.doi.org/10.1016/j.patrec.2015.07.021},
    url = {http://www.sciencedirect.com/science/article/pii/S0167865515002305},
    author = {S. Mure and T Grenier and Meier, S. and Guttmann, R.G. and H. Benoit-Cattin}
}
*/
#ifndef itkSTMS_BlurringSTMS_TXX
#define itkSTMS_BlurringSTMS_TXX

#include <limits>
#include <random>
#include <algorithm>
#include <iterator>
#include "itkSTMS_BlurringSTMS.h"
#include "itkSTMS_XMLFileParser.h"


double gettime()
{
    struct timespec timestamp;

    clock_gettime(CLOCK_REALTIME, &timestamp);
    return timestamp.tv_sec * 1000.0 + timestamp.tv_nsec * 1.0e-6;
}


namespace itkSTMS
{
template < class IndexType, class SpatialType, class PixelType, class ImageType >
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::itkSTMS_BlurringSTMS( IndexSampleSetType* idx, IndexSampleSetType* cla, IndexSampleSetType* wei,
                        SpatialSampleSetType* sp, RangeSampleSetType* ra, ParametersType* params, ParserOutputType* desc)
{
    this->indexSet   = idx;
    this->classSet   = cla;
    this->weightsSet = wei;
    this->spatialSet = sp;
    this->rangeSet   = ra;

    this->classSetMemory   = new IndexSampleSetType  (  *this->classSet  );
    this->spatialSetMemory = new SpatialSampleSetType( *this->spatialSet );

    this->stmsParams     = params;
    this->expDescription = desc;

    this->mergeFactor = 1000;
}


template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::GenerateData()
{
    double dtime;

    if( !stmsParams->merge )
    {
        std::cout<< std::endl<<"No merge Filtering!"<<std::endl<<"numSamples: "<< indexSet->size() <<std::endl<< std::endl;
        NoMergeSTMSFiltering();

        dtime=gettime();
        ClassificationNoMergeSTMSFiltering();
        dtime = gettime()-dtime;
        std::cout<< std::endl<<"Classif: " << dtime/1000 << " s" <<std::endl;
        FinalMerging();
    }
    else
    {
        std::cout<<std::endl<<"Merge Filtering!"<<std::endl<<"numSamples: "<< indexSet->size() <<std::endl<< std::endl;
        MergeSTMSFiltering();
        FinalMerging();
        //        ClassificationNoMergeSTMSFiltering();
    }
}

template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::ClassificationNoMergeSTMSFiltering()
{

    IndexSampleSetType*   newClassSet
            = new IndexSampleSetType();
    newClassSet->reserve  ( indexSet->size() );

    IndexSampleSetType*   newWeightsSet
            = new IndexSampleSetType();
    newWeightsSet->reserve( indexSet->size() );

    IndexSampleSetType*   newIndexSet
            = new IndexSampleSetType();
    newIndexSet->reserve  ( indexSet->size() );

    SpatialSampleSetType* newSpatialSet
            = new SpatialSampleSetType();

    newSpatialSet->reserve( indexSet->size() );

    RangeSampleSetType*   newRangeSet
            = new RangeSampleSetType();
    newRangeSet->reserve  ( indexSet->size() );


    newClassSet->push_back  ( 1 );
    newWeightsSet->push_back( 1 );
    newIndexSet->push_back  ( indexSet->at(0)   );
    newSpatialSet->push_back( spatialSet->at(0) );
    newRangeSet->push_back  ( rangeSet->at(0)   );
    classSetMemory->at( indexSet->at(0) ) = newClassSet->at(0);


    unsigned int k;
    bool newC;
    float spDist;
    bool  raNorm;

    for(unsigned int i=1 ; i<indexSet->size() ; ++i)
    {
        newC = true;
        k=0;

        while( (k<newClassSet->size()) && newC )
        {
            VectorDistance( spDist, newSpatialSet->at(k), spatialSet->at(i) );
            if( spDist<=1 )
            {
                InfiniteNorm( raNorm, newRangeSet->at(k), rangeSet->at(i) );
                if( raNorm )
                {
                    VectorWeightedMean( newSpatialSet->at(k), newWeightsSet->at(k), spatialSet->at(i), weightsSet->at(i) );
                    VectorWeightedMean( newRangeSet->at(k)  , newWeightsSet->at(k), rangeSet->at(i)  , weightsSet->at(i) );

                    classSetMemory->at( indexSet->at(i) ) = newClassSet->at(k);
                    newC = false;
                }
                else
                    ++k;
            }
            else
                ++k;
        }

        if( newC )
        {
            newClassSet->push_back( (IndexType)newClassSet->back()+1 );
            newWeightsSet->push_back( 1 );
            newIndexSet->push_back  ( indexSet->at(i)   );
            newSpatialSet->push_back( spatialSet->at(i) );
            newRangeSet->push_back  ( rangeSet->at(i)   );
            classSetMemory->at( indexSet->at(i) ) = newClassSet->back();
        }
    }

    classSet->swap  ( *newClassSet   );
    weightsSet->swap( *newWeightsSet );
    indexSet->swap  ( *newIndexSet   );
    spatialSet->swap( *newSpatialSet );
    rangeSet->swap  ( *newRangeSet   );

    delete newClassSet;
    delete newWeightsSet;
    delete newIndexSet;
    delete newSpatialSet;
    delete newRangeSet;
}


template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::NoMergeSTMSFiltering()
{

    SpatialSampleSetType* newSpatialSet
            = new SpatialSampleSetType( indexSet->size(), SpatialVectorType(stmsParams->dim, 0) );
    RangeSampleSetType*   newRangeSet
            = new RangeSampleSetType( indexSet->size(), RangeVectorType(stmsParams->numTimePoints, 0) );

    float globalEvolution = INFINITY;
    float epsilon = (stmsParams->epsilon)*(stmsParams->epsilon);
    float spDist, raDist;
    bool  raNorm;
    unsigned int iter = 0;

    double dtime;
    while( (globalEvolution>epsilon) & (iter<stmsParams->maxIt) )
    {
        dtime=gettime();
        globalEvolution = 0.0;

        #pragma omp parallel for reduction(+:globalEvolution) private(spDist, raDist, raNorm)
        for(unsigned int i=0 ; i<indexSet->size() ; ++i)
        {
            unsigned int count = 0;
            SpatialVectorType tmpSpatial( stmsParams->dim, 0 );
            RangeVectorType   tmpRange  ( stmsParams->numTimePoints, 0 );

            for(unsigned int j=0 ; j<indexSet->size() ; ++j)
            {
                VectorDistance( spDist, spatialSet->at(i), spatialSet->at(j));

                if( spDist<=1 )
                {
                    InfiniteNorm(raNorm, rangeSet->at(i), rangeSet->at(j));

                    if( raNorm )
                    {
                        VectorAcc( tmpSpatial, spatialSet->at(j) );
                        VectorAcc( tmpRange  , rangeSet->at(j)   );

                        ++count;
                    }
                }
            }

            if( count>1 )
            {
                VectorDiv( tmpSpatial, count );
                VectorDiv( tmpRange  , count );
            }

            VectorDistance(spDist, spatialSet->at(i), tmpSpatial);
            VectorDistance(raDist, rangeSet->at(i), tmpRange);

            globalEvolution += (spDist+raDist);

            #pragma omp critical
            {
                newSpatialSet->at(i) = tmpSpatial;
                newRangeSet->at(i)   = tmpRange;
            }
        }

        spatialSet->swap( *newSpatialSet );
        rangeSet->swap  (  *newRangeSet  );

        ++iter;
        dtime = gettime()-dtime;
        std::cout<< "Iter: " << iter <<"   "<< dtime/1000 << " s" <<"    GE: "<< globalEvolution <<std::endl;
    }

    delete newSpatialSet;
    delete newRangeSet;
}




template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::MergeSTMSFiltering()
{

    float globalEvolution = INFINITY;
    float epsilon = (stmsParams->epsilon)*(stmsParams->epsilon);
    float spDist, raDist;
    bool  raNorm, mergeNorm;
    unsigned int iter = 0;
    unsigned int merging;

    double dtime;
    while( (globalEvolution>epsilon) & (iter<stmsParams->maxIt) )
    {
        SpatialSampleSetType* newSpatialSet
                = new SpatialSampleSetType( *spatialSet );
        RangeSampleSetType*   newRangeSet
                = new RangeSampleSetType  (  *rangeSet  );

        dtime=gettime();
        globalEvolution = 0.0;
        merging = 0;

        #pragma omp parallel for reduction(+:globalEvolution, merging) private(spDist, raDist, raNorm, mergeNorm)
        for(unsigned int i=0 ; i<indexSet->size() ; ++i)
        {
            unsigned int weight = 0;
            bool merge = false;
            SpatialVectorType tmpSpatial( stmsParams->dim, 0 );
            RangeVectorType   tmpRange  ( stmsParams->numTimePoints, 0 );

            for(unsigned int j=0 ; j<indexSet->size() ; ++j)
            {
                VectorDistance( spDist, spatialSet->at(i), spatialSet->at(j));

                if( spDist<=1 )
                {
                    InfiniteNorm(raNorm, rangeSet->at(i), rangeSet->at(j));

                    if( raNorm )
                    {
                        VectorWeightedAcc(tmpSpatial, spatialSet->at(j), weightsSet->at(j));
                        VectorWeightedAcc(tmpRange  , rangeSet->at(j)  , weightsSet->at(j));

                        weight += weightsSet->at(j);

                        MergeInfiniteNorm(mergeNorm, rangeSet->at(i), rangeSet->at(j));
                        if( (spDist <= (1/mergeFactor)) && mergeNorm )
                            merge = true;
                    }
                }
            }

            if(weight > weightsSet->at(i))
            {
                if( merge )
                    merging += 1;
                else
                    merge = 0;
            }
            else
                merge = 0;


            VectorDiv( tmpSpatial, weight );
            VectorDiv( tmpRange  , weight );

            #pragma omp critical
            {
                newSpatialSet->at(i) = tmpSpatial;
                newRangeSet->at(i)   = tmpRange;
            }

            VectorDistance(spDist, spatialSet->at(i), tmpSpatial);
            VectorDistance(raDist, rangeSet->at(i), tmpRange);

            globalEvolution += (spDist+raDist);
        }

        spatialSet->swap( *newSpatialSet );
        rangeSet->swap  (  *newRangeSet  );

        delete newSpatialSet;
        delete newRangeSet;

        if(merging > 0)
            MergeSamples();

        ++iter;
        dtime = gettime()-dtime;
        std::cout<< "Iter: " << iter <<"   "<< dtime/1000 << " s" <<"    GE: "<< globalEvolution << "      numSamples: "<< classSet->size() <<std::endl;
    }
}


template< class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::MergeSamples()
{

    IndexSampleSetType*   newClassSet
            = new IndexSampleSetType();
    newClassSet->reserve  ( indexSet->size() );

    IndexSampleSetType*   newWeightsSet
            = new IndexSampleSetType();
    newWeightsSet->reserve( indexSet->size() );

    IndexSampleSetType*   newIndexSet
            = new IndexSampleSetType();
    newIndexSet->reserve  ( indexSet->size() );

    SpatialSampleSetType* newSpatialSet
            = new SpatialSampleSetType();

    newSpatialSet->reserve( indexSet->size() );

    RangeSampleSetType*   newRangeSet
            = new RangeSampleSetType();
    newRangeSet->reserve  ( indexSet->size() );

    IndexSampleSetType* newClassSetMemory
            = new IndexSampleSetType( *classSetMemory );

    IndexSampleSetType*   indexes
            = new IndexSampleSetType( *classSet );

    std::random_device rd;
    std::mt19937 g(rd());

    std::shuffle(indexes->begin(), indexes->end(), g);


    newClassSet->push_back  ( 1 );
    newWeightsSet->push_back( weightsSet->at( indexes->at(0)-1 ) );
    newIndexSet->push_back  ( indexSet->at  ( indexes->at(0)-1 ) );
    newSpatialSet->push_back( spatialSet->at( indexes->at(0)-1 ) );
    newRangeSet->push_back  ( rangeSet->at  ( indexes->at(0)-1 ) );

    for(unsigned int l=0 ; l< classSetMemory->size() ; ++l)
    {
        if(classSetMemory->at(l) == classSet->at(indexes->at(0)-1))
            newClassSetMemory->at(l) = 1;
    }

    unsigned int k;
    bool newC;
    float spDist;
    bool  raNorm;
    unsigned int i;

    for(unsigned int m=1 ; m<indexes->size() ; ++m)
    {
        i = indexes->at(m)-1;

        newC = true;
        k=0;

        while( (k<newClassSet->size()) && newC )
        {
            VectorDistance( spDist, newSpatialSet->at(k), spatialSet->at(i) );
            MergeInfiniteNorm( raNorm, newRangeSet->at(k), rangeSet->at(i) );
            if( (spDist<=1/mergeFactor)&&raNorm )
            {
                VectorWeightedAcc( newSpatialSet->at(k), newWeightsSet->at(k), spatialSet->at(i), weightsSet->at(i) );
                VectorWeightedAcc( newRangeSet->at(k)  , newWeightsSet->at(k), rangeSet->at(i)  , weightsSet->at(i) );

                newWeightsSet->at(k) += weightsSet->at(i);

                VectorDiv(newSpatialSet->at(k), newWeightsSet->at(k));
                VectorDiv(newRangeSet->at(k), newWeightsSet->at(k));

                for(unsigned int l=0 ; l<classSetMemory->size() ; ++l)
                {
                    if(classSetMemory->at(l) == classSet->at(i))
                        newClassSetMemory->at(l) = newClassSet->at(k);
                }

                newC = false;
            }
            else
                ++k;
        }

        if( newC )
        {
            newClassSet->push_back( newClassSet->back()+1 );
            newWeightsSet->push_back( weightsSet->at(i) );
            newIndexSet->push_back  ( indexSet->at(i)   );
            newSpatialSet->push_back( spatialSet->at(i) );
            newRangeSet->push_back  ( rangeSet->at(i)   );

            for(unsigned int l=0 ; l< classSetMemory->size() ; ++l)
            {
                if(classSetMemory->at(l) == classSet->at(i))
                    newClassSetMemory->at(l) = newClassSet->back();
            }
        }
    }

    classSet->swap  ( *newClassSet   );
    weightsSet->swap( *newWeightsSet );
    indexSet->swap  ( *newIndexSet   );
    spatialSet->swap( *newSpatialSet );
    rangeSet->swap  ( *newRangeSet   );
    classSetMemory->swap  ( *newClassSetMemory );

    delete newClassSet;
    delete newWeightsSet;
    delete newIndexSet;
    delete newSpatialSet;
    delete newRangeSet;
    delete newClassSetMemory;
    delete indexes;
}


template< class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::FinalMerging()
{
    unsigned int size = std::numeric_limits<unsigned int>::max() - 1; // INFINITY;
    std::string imagePath = expDescription->experimentPath+expDescription->inputFolder+expDescription->inputCommonRoot+STMS_NUMBERING_FORM_ONE+expDescription->imageExtension;

    while(size > classSet->size()){
        IndexSampleSetType*   newClassSet
                = new IndexSampleSetType();
        newClassSet->reserve  ( indexSet->size() );

        IndexSampleSetType*   newWeightsSet
                = new IndexSampleSetType();
        newWeightsSet->reserve( indexSet->size() );

        IndexSampleSetType*   newIndexSet
                = new IndexSampleSetType();
        newIndexSet->reserve  ( indexSet->size() );

        SpatialSampleSetType* newSpatialSet
                = new SpatialSampleSetType();

        newSpatialSet->reserve( indexSet->size() );

        RangeSampleSetType*   newRangeSet
                = new RangeSampleSetType();
        newRangeSet->reserve  ( indexSet->size() );

        IndexSampleSetType* newClassSetMemory
                = new IndexSampleSetType( *classSetMemory );

        newClassSet->push_back  ( 1 );
        newWeightsSet->push_back( 1 );
        newIndexSet->push_back  ( indexSet->at(0)   );
        newSpatialSet->push_back( spatialSet->at(0) );
        newRangeSet->push_back  ( rangeSet->at(0)   );

        unsigned int k;
        bool newC;
        bool  raNorm;

        for(unsigned int i=1 ; i<classSet->size() ; ++i)
        {
            newC = true;
            k=0;

            while( (k<newClassSet->size()) && newC )
            {
                InfiniteNorm( raNorm, newRangeSet->at(k), rangeSet->at(i) );
                if( raNorm ){

                    ReaderPointer reader = ReaderType::New();
                    ImagePointer image = ImageType::New();

                    reader->SetFileName( imagePath );
                    reader->Update();
                    image = reader->GetOutput();
                    image->FillBuffer( 0 );

                    ImageIndexSetType* refClass  = new ImageIndexSetType();
                    ImageIndexSetType* candClass = new ImageIndexSetType();
                    refClass->reserve( classSet->size()/2 );
                    candClass->reserve( classSet->size()/2 );


                    for(unsigned int m=0 ; m<newClassSetMemory->size() ; ++m)
                    {
                        ImageIndexType idx;
                        if(newClassSetMemory->at(m) == classSet->at(i)){

                            #pragma omp simd
                            for(unsigned int n=0 ; n<stmsParams->dim ; ++n)
                                idx[n] = spatialSetMemory->at(m)[n]*stmsParams->spScales[n];

                            candClass->push_back( idx );
                            image->SetPixel(candClass->back(), 1);
                        }
                        else
                        {
                            if(newClassSetMemory->at(m) == newClassSet->at(k)){

                                #pragma omp simd
                                for(unsigned int n=0 ; n<stmsParams->dim ; ++n)
                                    idx[n] = spatialSetMemory->at(m)[n]*stmsParams->spScales[n];

                                refClass->push_back( idx );
                                image->SetPixel(refClass->back(), 2);
                            }
                        }
                    }

                    bool connex = false;
                    if(stmsParams->dim == 2)
                    {
                        if(refClass->size() < candClass->size())
                        {
                            unsigned int m=0;
                            ImageIndexType idx;
                            while((m<refClass->size()) && !connex)
                            {
                                for(int x=-1 ; x<=1 ; ++x)
                                {
                                    for(int y=-1 ; y<=1 ; ++y)
                                    {
                                        idx[0] = refClass->at(m)[0]+x;
                                        idx[1] = refClass->at(m)[1]+y;

                                        if(   ( idx[0] < static_cast<long>(image->GetBufferedRegion().GetSize()[0]) )  && (idx[0]>0)
                                            && (idx[1] < static_cast<long>(image->GetBufferedRegion().GetSize()[1]) )  && (idx[1]>0) )
                                        {
                                            if(image->GetPixel(idx) == 2)
                                            {
                                                connex = true;
                                                x = 2;
                                                y = 2;
                                            }
                                        }
                                    }
                                }

                                ++m;
                            }
                        }
                        else
                        {
                            unsigned int m=0;
                            ImageIndexType idx;
                            while((m<candClass->size()) && !connex)
                            {
                                for(int x=-1 ; x<=1 ; ++x)
                                {
                                    for(int y=-1 ; y<=1 ; ++y)
                                    {
                                        idx[0] = candClass->at(m)[0]+x;
                                        idx[1] = candClass->at(m)[1]+y;

                                        if(   ( idx[0] < static_cast<long>(image->GetBufferedRegion().GetSize()[0]) ) && ( idx[0] > 0 )
                                           && ( idx[1] < static_cast<long>(image->GetBufferedRegion().GetSize()[1]) ) && ( idx[1] > 0 ) )
                                        {
                                            if(image->GetPixel(idx) == 1)
                                            {
                                                connex = true;
                                                x = 2;
                                                y = 2;
                                            }
                                        }
                                    }
                                }

                                ++m;
                            }
                        }

                    }
                    else
                    {
                        if(refClass->size() < candClass->size())
                        {
                            unsigned int m=0;
                            ImageIndexType idx;
                            while((m<refClass->size()) && !connex)
                            {
                                for(int x=-1 ; x<=1 ; ++x)
                                {
                                    for(int y=-1 ; y<=1 ; ++y)
                                    {
                                        for(int z=-1 ; z<=1 ; ++z)
                                        {
                                            idx[0] = refClass->at(m)[0]+x;
                                            idx[1] = refClass->at(m)[1]+y;
                                            idx[2] = refClass->at(m)[2]+z;

                                            if(  ( idx[0] < static_cast<long>(image->GetBufferedRegion().GetSize()[0]) ) && (idx[0] > 0) &&
                                                 ( idx[1] < static_cast<long>(image->GetBufferedRegion().GetSize()[1]) ) && (idx[1] > 0) &&
                                                 ( idx[2] < static_cast<long>(image->GetBufferedRegion().GetSize()[2]) ) && (idx[2] > 0)  )
                                            {
                                                if(image->GetPixel(idx) == 2)
                                                {
                                                    connex = true;
                                                    x = 2;
                                                    y = 2;
                                                    z = 2;
                                                }
                                            }
                                        }
                                    }
                                }

                                ++m;
                            }
                        }
                        else
                        {
                            unsigned int m=0;
                            ImageIndexType idx;
                            while((m<candClass->size()) && !connex)
                            {
                                for(int x=-1 ; x<=1 ; ++x)
                                {
                                    for(int y=-1 ; y<=1 ; ++y)
                                    {
                                        for(int z=-1 ; z<=1 ; ++z)
                                        {
                                            idx[0] = candClass->at(m)[0]+x;
                                            idx[1] = candClass->at(m)[1]+y;
                                            idx[2] = candClass->at(m)[2]+z;

                                            if(   ( idx[0] < static_cast<long>(image->GetBufferedRegion().GetSize()[0]) ) && ( idx[0] > 0 ) &&
                                                  ( idx[1] < static_cast<long>(image->GetBufferedRegion().GetSize()[1]) ) && ( idx[1] > 0 ) &&
                                                  ( idx[2] < static_cast<long>(image->GetBufferedRegion().GetSize()[2]) ) && ( idx[2] > 0 ) )
                                            {
                                                if(image->GetPixel(idx) == 1)
                                                {
                                                    connex = true;
                                                    x = 2;
                                                    y = 2;
                                                    z = 2;
                                                }
                                            }
                                        }
                                    }
                                }

                                ++m;
                            }
                        }
                    }

                    if( connex )
                    {

                        VectorWeightedAcc( newSpatialSet->at(k), newWeightsSet->at(k), spatialSet->at(i), weightsSet->at(i) );
                        VectorWeightedAcc( newRangeSet->at(k)  , newWeightsSet->at(k), rangeSet->at(i)  , weightsSet->at(i) );

                        newWeightsSet->at(k) += weightsSet->at(i);

                        VectorDiv(newSpatialSet->at(k), newWeightsSet->at(k));
                        VectorDiv(newRangeSet->at(k), newWeightsSet->at(k));

                        for(unsigned int l=0 ; l< classSetMemory->size() ; ++l)
                        {

                            if(classSetMemory->at(l) == classSet->at(i))
                                newClassSetMemory->at(l) = newClassSet->at(k);
                        }

                        newC = false;
                    }
                    else
                        ++k;

                    delete refClass;
                    delete candClass;
                }
                else
                    ++k;
            }

            if( newC )
            {
                newClassSet->push_back( newClassSet->back()+1 );
                newWeightsSet->push_back( weightsSet->at(i) );
                newIndexSet->push_back  ( indexSet->at(i)   );
                newSpatialSet->push_back( spatialSet->at(i) );
                newRangeSet->push_back  ( rangeSet->at(i)   );

                for(unsigned int l=0 ; l< classSetMemory->size() ; ++l)
                {
                    if(classSetMemory->at(l) == classSet->at(i))
                        newClassSetMemory->at(l) = newClassSet->back();
                }
            }
        }

        size = classSet->size();

        classSet->swap  ( *newClassSet   );
        weightsSet->swap( *newWeightsSet );
        indexSet->swap  ( *newIndexSet   );
        spatialSet->swap( *newSpatialSet );
        rangeSet->swap  ( *newRangeSet   );
        classSetMemory->swap  ( *newClassSetMemory );

        delete newClassSet;
        delete newWeightsSet;
        delete newIndexSet;
        delete newSpatialSet;
        delete newRangeSet;
        delete newClassSetMemory;
    }
}



template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorDistance(float &dist, std::vector<T> &a, std::vector<T> &b)
{
    dist = 0.0;

    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        dist += ( a[i]-b[i] )*( a[i]-b[i] );
}


template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::InfiniteNorm(bool &dist, RangeVectorType &a, RangeVectorType &b)
{
    dist = true;

    for(unsigned int i=0 ; i<a.size() ; ++i)
    {
        if(((a[i]-b[i])*(a[i]-b[i])) > 1)
        {
            dist = false;
            i = a.size();
        }
    }
}


template < class IndexType, class SpatialType, class PixelType, class ImageType >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::MergeInfiniteNorm(bool &dist, RangeVectorType &a, RangeVectorType &b)
{
    dist = true;

    for(unsigned int i=0 ; i<a.size() ; ++i)
    {
        if(((a[i]-b[i])*(a[i]-b[i])) > (1/mergeFactor))
        {
            dist = false;
            i = a.size();
        }
    }
}


template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorAcc(std::vector<T> &a, std::vector<T> &b)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        a[i] += b[i];
}

template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorWeightedAcc(std::vector<T> &a, std::vector<T> &b, unsigned int &b_w)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        a[i] += b_w*b[i];
}

template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorWeightedAcc(std::vector<T> &a, unsigned int &a_w, std::vector<T> &b, unsigned int &b_w)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i){
        a[i] *= a_w;
        a[i] += b_w*b[i];
    }
}

template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorWeightedMean(std::vector<T> &a, unsigned int &a_w, std::vector<T> &b, unsigned int &b_w)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        a[i] = ( (a[i]*a_w )+(b[i]*b_w) )/( a_w+b_w );

    a_w += b_w;
}


template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorMul(std::vector<T> &a, unsigned int &coeff)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        a[i] *= coeff;
}


template< class IndexType, class SpatialType, class PixelType, class ImageType >
template< class T >
void
itkSTMS_BlurringSTMS< IndexType, SpatialType, PixelType, ImageType >
::VectorDiv(std::vector<T> &a, unsigned int &coeff)
{
    #pragma omp simd
    for(unsigned int i=0 ; i<a.size() ; ++i)
        a[i] /= coeff;
}

} // end of namespace itkSTMS

#endif  // itkmsSTMS_BlurringSTMS_TXX