#ifdef NICE_USELIB_OPENMP
#include <omp.h>
#endif

#include <stdio.h>

#include "GMM.h"
#include <core/vector/Algorithms.h>

#include "vislearning/math/cluster/KMeans.h"

// #define DEBUGGMM

using namespace OBJREC;
using namespace NICE;
using namespace std;

///////////////////// ///////////////////// /////////////////////
//                   CONSTRUCTORS / DESTRUCTORS
///////////////////// ///////////////////// /////////////////////

GMM::GMM() : ClusterAlgorithm()
{
  this->i_numOfGaussians = 3;
  this->dim = -1;
  
  this->mu.clear();
  this->sparse_sigma.clear();
  this->priors.clear();  
  this->sparse_inv_sigma.clear();
  this->log_det_sigma.clear();

  
  this->mu2.clear();
  this->sparse_sigma2.clear();
  this->priors2.clear();  
  this->b_compareTo2ndGMM = false;
  
  this->maxiter = 200;
  this->featsperclass = 0;
  this->cdimval = -1; //TODO
  this->tau = 10.0;
  this->pyramid = false;

  srand ( time ( NULL ) );
}

GMM::GMM ( int _numOfGaussians ) : i_numOfGaussians ( _numOfGaussians )
{
  
  this->dim = -1;
  
  this->mu.clear();
  this->sparse_sigma.clear();
  this->priors.clear();  
  this->sparse_inv_sigma.clear();
  this->log_det_sigma.clear();  
  
  this->mu2.clear();
  this->sparse_sigma2.clear();
  this->priors2.clear();  
  this->b_compareTo2ndGMM = false;
  
  this->maxiter = 200;
  this->featsperclass = 0;
  this->tau = 0.0;
  this->pyramid = false;

  srand ( time ( NULL ) );
}

GMM::GMM ( const Config * _conf, int _numOfGaussians ) : i_numOfGaussians ( _numOfGaussians )
{
  this->initFromConfig( _conf );
}

GMM::GMM ( const Config * _conf, const std::string& _confSection )
{
  this->initFromConfig( _conf, _confSection );
}

GMM::~GMM()
{
}

void GMM::initFromConfig( const NICE::Config* _conf, const std::string& _confSection )
{
  if ( this->i_numOfGaussians < 2 )
    this->i_numOfGaussians = _conf->gI ( _confSection, "i_numOfGaussians", 2 );
  
  this->dim = -1;
  
  this->mu.clear();
  this->sparse_sigma.clear();
  this->priors.clear();  
  this->sparse_inv_sigma.clear();
  this->log_det_sigma.clear();  
  
  this->mu2.clear();
  this->sparse_sigma2.clear();
  this->priors2.clear();  
  
  this->b_compareTo2ndGMM = false;
  this->maxiter = _conf->gI ( _confSection, "maxiter", 200 );
  this->featsperclass = _conf->gI ( _confSection, "featsperclass", 0 );
  this->tau = _conf->gD ( _confSection, "tau", 100.0 );
  this->pyramid = _conf->gB ( _confSection, "pyramid", false );  

  srand ( time ( NULL ) );
}

///////////////////// ///////////////////// /////////////////////
//                      CLUSTERING STUFF
///////////////////// ///////////////////// ////////////////// 

void GMM::computeMixture ( Examples examples )
{
  int fsize = ( int ) examples.size();
  assert ( fsize >= i_numOfGaussians );

  dim = examples[0].second.vec->size();

  int samples = fsize;
  if ( featsperclass > 0 )
  {
    samples = featsperclass * i_numOfGaussians;
    samples = std::min ( samples, fsize );
  }

  // Copy data in Matrix
  VVector dataset;

  cout << "reduced training data for GMM from " << fsize << " features to " << samples << " features.";
  for ( int i = 0; i < samples; i++ )
  {
    int k = rand() % fsize;
    NICE::Vector *vec = examples[k].second.vec;
    dataset.push_back ( *vec );
  }

  computeMixture ( dataset );
}

void GMM::computeMixture ( const VVector &DataSet )
{
  // Learn the GMM model
  assert ( DataSet.size() >= ( uint ) i_numOfGaussians );
  //initEMkMeans(DataSet); // initialize the model
  srand ( time ( NULL ) );

  bool poweroftwo = false;
  int power = 1;
  while ( power <= i_numOfGaussians )
  {
    if ( i_numOfGaussians == power )
      poweroftwo = true;
    power *= 2;
  }

  if ( poweroftwo && pyramid )
  {
    initEM ( DataSet ); // initialize the model
    int g = 1;

    while ( g <= i_numOfGaussians )
    {
      cout << "g = " << g << endl;
      doEM ( DataSet, g );

      if ( 2*g <= i_numOfGaussians )
      {
        for ( int i = g; i < g*2; i++ )
        {
          mu[i] = mu[i-g];
          sparse_sigma[i] = sparse_sigma[i-g];
          sparse_inv_sigma[i] = sparse_inv_sigma[i-g];
          log_det_sigma[i] = log_det_sigma[i-g];
          priors[i] = priors[i-g];

          double interval = 1.0;
          for ( int k = 0; k < ( int ) mu[i].size(); k++ )
          {
            interval = mu[i][k];
            interval = std::max ( interval, 0.1 );
            double r = ( interval * ( ( double ) rand() / ( double ) RAND_MAX ) ) - interval / 2.0;
            mu[i][k] += r;
          }
        }
      }
      g *= 2;
    }
  }
  else
  {
    initEMkMeans ( DataSet ); // initialize the model
    doEM ( DataSet, i_numOfGaussians );
  }
  // performs EM
}

inline double diagDeterminant ( const NICE::Vector &sparse_mat )
{
  double det = 1.0;
  for ( int i = 0; i < ( int ) sparse_mat.size(); i++ )
  {
    det *= sparse_mat[i];
  }

  return det;
}

inline double logdiagDeterminant ( const NICE::Vector &sparse_mat )
{
  double det = 0.0;
  for ( int i = 0; i < ( int ) sparse_mat.size(); i++ )
  {
    det += log ( sparse_mat[i] );
  }

  return det;
}

inline NICE::Vector diagInverse ( const NICE::Vector &sparse_mat )
{
  NICE::Vector inv ( sparse_mat.size() );
  for ( int i = 0; i < ( int ) sparse_mat.size(); i++ )
  {
    inv[i] = 1.0 / sparse_mat[i];
  }
  return inv;
}

void GMM::initEMkMeans ( const VVector &DataSet )
{
  /*init GMM with k-Means*/
  OBJREC::KMeans k ( i_numOfGaussians );
  NICE::VVector means;
  std::vector<double> weights;
  std::vector<int> assignment;
  k.cluster ( DataSet, means, weights, assignment );

  int nData = DataSet.size();
  this->dim = DataSet[0].size();
  cdimval = dim * log ( 2 * 3.14159 );
  std::vector<int> pop ( i_numOfGaussians, 0 );
  priors.resize ( i_numOfGaussians );
  mu = VVector ( i_numOfGaussians, dim );
  log_det_sigma.clear();
  vector<int> allk;

  NICE::Vector globmean ( dim );
  globmean.set ( 0.0 );

  for ( int n = 0; n < ( int ) DataSet.size(); n++ ) /* getting the max value for time */
  {
    globmean = globmean + DataSet[n];
  }

  globmean *= ( 1.0 / nData );

  NICE::Vector sparse_globsigma;

  sparse_globsigma.resize ( dim );
  sparse_globsigma.set ( 0.0 );

  for ( int i = 0; i < ( int ) DataSet.size(); i++ ) // Covariances updates
  {
    // nur diagonal Elemente berechnen
    for ( int d = 0; d < dim; d++ )
    {
      double diff = ( DataSet[i][d] - globmean[d] );
      sparse_globsigma[d] += diff * diff;
    }
  }

  sparse_globsigma *= ( 1.0 / DataSet.size() );

  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    NICE::Vector _inv_sigma = diagInverse ( sparse_globsigma );

    sparse_sigma.push_back ( sparse_globsigma );
    sparse_inv_sigma.push_back ( _inv_sigma );
    log_det_sigma.push_back ( logdiagDeterminant ( sparse_globsigma ) );
    mu[i] = means[i];
    //priors[i]=1.0/(double)i_numOfGaussians; // set equi-probables states
    priors[i] = weights[i]; // set kMeans weights
  }
}

void GMM::initEM ( const VVector &DataSet )
{

  /* init the GaussianMixture by using randomized meanvectors */
  int nData = DataSet.size();
  this->dim = DataSet[0].size();
  cdimval = dim * log ( 2 * 3.14159 );
  vector<int> pop ( i_numOfGaussians, 0 );
  priors.resize ( i_numOfGaussians );
  mu = VVector ( i_numOfGaussians, dim );
  log_det_sigma.clear();
  vector<int> allk;

  NICE::Vector globmean ( dim );
  globmean.set ( 0.0 );

  for ( int n = 0; n < ( int ) DataSet.size(); n++ ) /* getting the max value for time */
  {
    globmean = globmean + DataSet[n];
  }

  globmean *= ( 1.0 / nData );

  NICE::Vector sparse_globsigma;

  sparse_globsigma.resize ( dim );
  sparse_globsigma.set ( 0.0 );

  for ( int i = 0; i < ( int ) DataSet.size(); i++ ) // Covariances updates
  {
    // nur diagonal Elemente berechnen
    for ( int d = 0; d < dim; d++ )
    {
      double diff = ( DataSet[i][d] - globmean[d] );
      sparse_globsigma[d] += diff * diff;
    }
  }

  sparse_globsigma *= ( 1.0 / DataSet.size() );

  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    priors[i] = 1.0 / ( double ) i_numOfGaussians; // set equi-probables states
    NICE::Vector _inv_sigma = diagInverse ( sparse_globsigma );

    sparse_sigma.push_back ( sparse_globsigma );
    sparse_inv_sigma.push_back ( _inv_sigma );
    log_det_sigma.push_back ( logdiagDeterminant ( sparse_globsigma ) );
    bool newv = false;
    int k;
    while ( !newv )
    {
      newv = true;
      k = rand() % nData;

      for ( int nk = 0; nk < ( int ) allk.size(); nk++ )
        if ( allk[nk] == k )
        {
          newv = false;
        }
      if ( newv )
        allk.push_back ( k );
    }
    mu[i] = DataSet[k];
  }
}

inline void sumRow ( NICE::Matrix mat, NICE::Vector &res )
{
  int row = mat.rows();
  int column = mat.cols();
  res = NICE::Vector ( column );
  res.set ( 1e-5f );
//#pragma omp parallel for
  for ( int i = 0; i < column; i++ ) {
    for ( int j = 0; j < row; j++ ) {
      res[i] += mat ( j, i );
    }
  }
}

double GMM::logpdfState ( const NICE::Vector &Vin, int state )
{
  /* get the probability density for a given state and a given vector */
  double p;
  NICE::Vector dif;

  dif = Vin;
  dif -= mu[state];

  p = 0.0;
  for ( int i = 0; i < ( int ) dif.size(); i++ )
  {
    p += dif[i] * dif[i] * sparse_inv_sigma[state][i];
  }

  p = -0.5 * ( p + cdimval + log_det_sigma[state] );
  return p;
}

int GMM::doEM ( const VVector &DataSet, int nbgaussians )
{
  /* perform Expectation/Maximization on the given Dataset :
  Matrix DataSet(nSamples,Dimensions).
  The GaussianMixture Object must be initialised before
  (see initEM or initEMkMeans methods ) */

#ifdef DEBUG
  cerr << "GMM::start EM" << endl;
#endif

  int nData = DataSet.size();
  int iter = 0;
  double log_lik;
  double log_lik_threshold = 1e-6f;
  double log_lik_old = -1e10f;

  NICE::Matrix unity ( dim, dim, 0.0 );
  for ( int k = 0; k < dim; k++ )
    unity ( k, k ) = 1.0;

  //EM loop
  while ( true )
  {
#ifdef DEBUGGMM
    cerr << "GMM::EM: iteration: " << iter << endl;
#endif

    vector<double> sum_p;
    sum_p.resize ( nData );

    for ( int i = 0; i < nData; i++ )
    {
      sum_p[i] = 0.0;
    }

    NICE::Matrix pxi ( nData, i_numOfGaussians );
    pxi.set ( 0.0 );
    NICE::Matrix pix ( nData, i_numOfGaussians );
    pix.set ( 0.0 );
    NICE::Vector E;

    iter++;
    if ( iter > maxiter ) {
      cerr << "EM stops here. Max number of iterations (" << maxiter << ") has been reached." << endl;
      return iter;
    }

    double sum_log = 0.0;

    // computing expectation
    double maxp = -numeric_limits<double>::max();
    vector<double> maxptmp ( nData, -numeric_limits<double>::max() );
#pragma omp parallel for
    for ( int i = 0; i < nData; i++ )
    {
      for ( int j = 0; j < nbgaussians; j++ )
      {
        double p = logpdfState ( DataSet[i], j ); // log(P(x|i))
        maxptmp[i] = std::max ( maxptmp[i], p );
        pxi ( i, j ) = p;
      }
    }

    for ( int i = 0; i < nData; i++ )
    {
      maxp = std::max ( maxp, maxptmp[i] );
    }

#pragma omp parallel for
    for ( int i = 0; i < nData; i++ )
    {
      sum_p[i] = 0.0;
      for ( int j = 0; j < nbgaussians; j++ )
      {
        double p = pxi ( i, j ) - maxp; // log(P(x|i))
        p = exp ( p ); // P(x|i)

        if ( p < 1e-40 )
          p = 1e-40;

        pxi ( i, j ) = p;
        sum_p[i] += p * priors[j];
      }
    }

    for ( int i = 0; i < nData; i++ )
    {
      sum_log += log ( sum_p[i] );
    }

#pragma omp parallel for
    for ( int j = 0; j < nbgaussians; j++ )
    {
      for ( int i = 0; i < nData; i++ )
      {
        pix ( i, j ) = ( pxi ( i, j ) * priors[j] ) / sum_p[i]; // then P(i|x)
      }
    }

    // here we compute the log likehood
    log_lik = sum_log / nData;
#ifdef DEBUGGMM
    cout << "diff: " << fabs ( ( log_lik / log_lik_old ) - 1 ) << " thresh: " << log_lik_threshold << " sum: " << sum_log <<  endl;
    //cout << "logold: " << log_lik_old << " lognew: " << log_lik << endl;
#endif

    if ( fabs ( ( log_lik / log_lik_old ) - 1 ) < log_lik_threshold )
    {
      //if log likehood hasn't move enough, the algorithm has converged, exiting the loop
      return iter;
    }

    log_lik_old = log_lik;

    // Update Step
    sumRow ( pix, E );

#pragma omp parallel for
    for ( int j = 0; j < nbgaussians; j++ )
    {
      priors[j] = ( E[j] + tau ) / ( nData + tau * nbgaussians ); // new priors

      NICE::Vector tmu ( dim );
      tmu.set ( 0.0 );

      NICE::Vector sparse_tmsigma ( dim );
      sparse_tmsigma.set ( 0.0 );

      for ( int i = 0; i < nData; i++ ) // Means update loop
      {
        tmu = tmu + ( DataSet[i] * pix ( i, j ) );
      }

      NICE::Vector tmu2 = mu[j];
      mu[j] = tmu + tau * tmu2;
      mu[j] = mu[j] * ( 1.0 / ( E[j] + tau ) );

      for ( int i = 0; i < nData; i++ ) // Covariances updates
      {
        // nur diagonal Elemente berechnen
        for ( int d = 0; d < dim; d++ )
        {
          sparse_tmsigma[d] += DataSet[i][d] * DataSet[i][d] * pix ( i, j );
        }
      }

      NICE::Vector sparse_tmsigma2 ( dim );
      for ( int d = 0; d < dim; d++ )
      {
        sparse_tmsigma[d] += 1e-5f;
        sparse_tmsigma2[d] = sparse_sigma[j][d] + tmu2[d] * tmu2[d];
        sparse_sigma[j][d] = ( sparse_tmsigma[d] + tau * sparse_tmsigma2[d] ) / ( E[j] + tau ) - ( mu[j][d] * mu[j][d] );
      }
      sparse_inv_sigma[j] = diagInverse ( sparse_sigma[j] );

      log_det_sigma[j] = logdiagDeterminant ( sparse_sigma[j] );
    }
    if ( b_compareTo2ndGMM )
    {
      double dist = this->compareTo2ndGMM();
      std::cout << "dist for iteration " << iter << std::endl;
      std::cout << "d: " << dist << std::endl;
    }
  }
#ifdef DEBUG
  cerr << "GMM::finished EM after " << iter << " iterations" << endl;
#endif
  return iter;
}

int GMM::getBestClass ( const NICE::Vector &v, double *bprob )
{
  int bestclass = 0;
  double maxprob = logpdfState ( v, 0 ) + log ( priors[0] );  // log(P(x|i))

  for ( int i = 1; i < i_numOfGaussians; i++ )
  {
    double prob = logpdfState ( v, i ) + log ( priors[i] );  // log(P(x|i))

    if ( prob > maxprob )
    {
      maxprob = prob;
      bestclass = i;
    }
  }

  if ( bprob != NULL )
    *bprob = maxprob;

  return bestclass;
}

void GMM::getProbs ( const NICE::Vector &vin, SparseVector &outprobs )
{
  outprobs.clear();
  outprobs.setDim ( i_numOfGaussians );
  Vector probs;
  getProbs ( vin, probs );
  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    if ( probs[i] > 1e-7f )
      outprobs[i] = probs[i];
  }
}

void GMM::getProbs ( const NICE::Vector &vin, Vector &outprobs )
{
  Vector probs;
  probs.resize ( i_numOfGaussians );
  probs.set ( 0.0 );

  double probsum = 0.0;
  double maxp = -numeric_limits<double>::max();
  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    probs[i] = logpdfState ( vin, i ) + log ( priors[i] );  // log(P(x|i))
    maxp = std::max ( maxp, probs[i] );
  }

  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    probs[i] -= maxp;
    probs[i] = exp ( probs[i] );
    probsum += probs[i];
  }

  // normalize probs
#pragma omp parallel for
  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    probs[i] /= probsum;
  }
  outprobs = probs;
}

void GMM::getFisher ( const NICE::Vector &vin, SparseVector &outprobs )
{
  int size = i_numOfGaussians * 2 * dim;
  outprobs.clear();
  outprobs.setDim ( size );

  int counter = 0;

  NICE::Vector classprobs;
  classprobs.resize ( i_numOfGaussians );
  classprobs.set ( 0.0 );

  double maxp = -numeric_limits<double>::max();
  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    classprobs[i] = logpdfState ( vin, i ) + log ( priors[i] );  // log(P(x|i))

    maxp = std::max ( maxp, classprobs[i] );
  }

  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    double p = classprobs[i] - maxp;

    p = exp ( p );

    for ( int d = 0; d < dim; d++ )
    {
      double diff = vin[d] - mu[i][d];

      double sigma2 = sparse_sigma[i][d] * sparse_sigma[i][d];
      double sigma3 = sigma2 * sparse_sigma[i][d];
      double normmu = sqrt ( priors[i] / sigma2 );
      double normsig = sqrt ( ( 2.0 * priors[i] ) / sigma2 );

      double gradmu = ( p * ( diff / sigma2 ) ) / normmu;
      double gradsig = ( p * ( ( diff * diff ) / sigma3 - 1.0 / sparse_sigma[i][d] ) ) / normsig;
      if ( fabs ( gradmu ) > 1e-7f )
        outprobs[counter] = gradmu;
      counter++;

      if ( fabs ( gradsig ) > 1e-7f )
        outprobs[counter] = gradsig;
      counter++;
    }
  }
}

void GMM::cluster ( const VVector & features, VVector & prototypes, vector<double> & weights, vector<int> & assignment )
{
  computeMixture ( features );

  prototypes.clear();
  weights.clear();
  assignment.clear();

  for ( int i = 0; i < ( int ) features.size(); i++ )
  {
    int c = getBestClass ( features[i] );
    assignment.push_back ( c );

    weights.push_back ( priors[c] );
  }
  for ( int c = 0; c < i_numOfGaussians; c++ )
    prototypes.push_back ( mu[c] );

  cout << "tau: " << tau << endl;
}

void GMM::saveData ( const std::string filename )
{
  ofstream fout ( filename.c_str() );

  fout << i_numOfGaussians << " " << dim << endl;

  mu >> fout;
  fout << endl;

  for ( int n = 0; n < i_numOfGaussians; n++ )
  {
    fout << sparse_sigma[n] << endl;
  }

  for ( int n = 0; n < i_numOfGaussians; n++ )
  {
    fout << priors[n] << " ";
  }

  fout.close();
}

bool GMM::loadData ( const std::string filename )
{
  cerr << "read GMM Data" << endl;
  ifstream fin ( filename.c_str() );

  if ( fin.fail() )
  {
    cerr << "GMM: Datei " << filename << " nicht gefunden!" << endl;
    return false;
  }

  fin >> i_numOfGaussians;
  fin >> dim;
  cdimval = log ( pow ( 2 * 3.14159, dim ) );
  mu.clear();

  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    NICE::Vector tmp;
    fin >> tmp;
    mu.push_back ( tmp );
  }

  log_det_sigma.clear();

  for ( int n = 0; n < i_numOfGaussians; n++ )
  {
    NICE::Matrix _sigma;
    NICE::Vector _sparse_sigma;

    _sparse_sigma = NICE::Vector ( dim );
    fin >> _sparse_sigma;
    sparse_sigma.push_back ( _sparse_sigma );

    sparse_inv_sigma.push_back ( diagInverse ( sparse_sigma[n] ) );
    log_det_sigma.push_back ( logdiagDeterminant ( sparse_sigma[n] ) );
  }

  for ( int n = 0; n < i_numOfGaussians; n++ )
  {
    double tmpd;
    fin >> tmpd;
    priors.push_back ( tmpd );
  }
  fin.close();

  cerr << "reading GMM Data finished" << endl;
  return true;
}

void GMM::getParams ( VVector &mean, VVector &sSigma, vector<double> &p ) const
{
  mean = this->mu;
  sSigma.resize ( this->i_numOfGaussians );
  p.clear();
  for ( int i = 0; i < this->i_numOfGaussians; i++ )
  {
    sSigma[i] = this->sparse_sigma[i];
    p.push_back ( this->priors[i] );
  }
}

void GMM::setGMMtoCompareWith ( NICE::VVector mean, NICE::VVector sSigma, std::vector<double> p )
{
  this->mu2           = mean;
  this->sparse_sigma2 = sSigma;
  this->priors2       = std::vector<double> ( p );
}

double GMM::kPPK ( NICE::Vector sigma1, NICE::Vector sigma2, NICE::Vector mu1, NICE::Vector mu2, double p ) const
{
  double d = mu1.size();

  double det1 = 1.0;
  double det2 = 1.0;
  double det3 = 1.0;
  double eval = 0.0;
  for ( int i = 0; i < d; i++ )
  {
    det1 *= sigma1[i];
    det2 *= sigma2[i];
    double sigma = 1.0 / ( ( 1.0 / sigma1[i] + 1.0 / sigma2[i] ) * p );
    det3 *= sigma;
    double mu = p * ( mu1[i] * sigma1[i] + mu2[i] * sigma2[i] );
    eval += 0.5 * mu * sigma * mu - ( p * mu1[i] * mu1[i] ) / ( 2.0 * sigma1[i] ) - ( p * mu2[i] * mu2[i] ) / ( 2.0 * sigma2[i] );
  }

  return ( pow ( 2.0*M_PI, ( ( 1.0 - 2.0*p ) *d ) / 2.0 ) * pow ( det1, -p / 2.0 ) * pow ( det2, -p / 2.0 ) * pow ( det3, 0.5 ) * exp ( eval ) );
}

double GMM::compareTo2ndGMM() const
{
  double distkij = 0.0;
  double distkjj = 0.0;
  double distkii = 0.0;
  for ( int i = 0; i < i_numOfGaussians; i++ )
  {
    for ( int j = 0; j < i_numOfGaussians; j++ )
    {
      double kij = kPPK ( sparse_sigma[i], sparse_sigma2[j], mu[i], mu2[j], 0.5 );
      double kii = kPPK ( sparse_sigma[i], sparse_sigma[j], mu[i], mu[j], 0.5 );
      double kjj = kPPK ( sparse_sigma2[i], sparse_sigma2[j], mu2[i], mu2[j], 0.5 );
      kij = priors[i] * priors2[j] * kij;
      kii = priors[i] * priors[j] * kii;
      kjj = priors2[i] * priors2[j] * kjj;
      distkij += kij;
      distkii += kii;
      distkjj += kjj;
    }
  }
  return ( distkij / ( sqrt ( distkii ) *sqrt ( distkjj ) ) );
}

void GMM::setCompareTo2ndGMM ( const bool & _compareTo2ndGMM )
{
  this->b_compareTo2ndGMM = _compareTo2ndGMM;
}

int GMM::getNumberOfGaussians() const
{
  return this->i_numOfGaussians;
}



///////////////////// INTERFACE PERSISTENT /////////////////////
// interface specific methods for store and restore
///////////////////// INTERFACE PERSISTENT ///////////////////// 

void GMM::restore ( std::istream & is, int format )
{
  //delete everything we knew so far...
  this->clear();
  
  
  if ( is.good() )
  {
    
    std::string tmp;
    is >> tmp; //class name 
    
    if ( ! this->isStartTag( tmp, "GMM" ) )
    {
      std::cerr << " WARNING - attempt to restore GMM, but start flag " << tmp << " does not match! Aborting... " << std::endl;
      throw;
    }   
    
    bool b_endOfBlock ( false ) ;
    
    while ( !b_endOfBlock )
    {
      is >> tmp; // start of block 
      
      if ( this->isEndTag( tmp, "GMM" ) )
      {
        b_endOfBlock = true;
        continue;
      }      
      
      tmp = this->removeStartTag ( tmp );  
   
      
      if ( tmp.compare("i_numOfGaussians") == 0 )
      {
        is >> this->i_numOfGaussians;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("dim") == 0 )
      {
        is >> this->dim;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("mu") == 0 )
      {
        this->mu.clear();          
        this->mu.setIoUntilEndOfFile ( false );
        this->mu.restore ( is, format );
      
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }       
      else if ( tmp.compare("sparse_sigma") == 0 )
      {
        this->sparse_sigma.clear();   
        this->sparse_sigma.setIoUntilEndOfFile ( false );
        this->sparse_sigma.restore ( is, format );
        
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("priors") == 0 )
      {
        int sizeOfPriors;
        is >> sizeOfPriors;
        
        this->priors.resize ( sizeOfPriors );
        for ( std::vector< double >::iterator itPriors = this->priors.begin();
             itPriors != this->priors.end(); 
             itPriors++
            )
        {
          is >> *itPriors;
        }
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("sparse_inv_sigma") == 0 )
      {
        this->sparse_inv_sigma.clear();          
        this->sparse_inv_sigma.setIoUntilEndOfFile ( false );
        this->sparse_inv_sigma.restore ( is, format );
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }         
      else if ( tmp.compare("log_det_sigma") == 0 )
      {
        int sizeOfLogDetSigma;
        is >> sizeOfLogDetSigma;
        
        this->log_det_sigma.resize ( sizeOfLogDetSigma );
        for ( std::vector< double >::iterator itLogDetSigma = this->log_det_sigma.begin(); 
              itLogDetSigma != this->log_det_sigma.end();
              itLogDetSigma++
            )
        {
          is >> *itLogDetSigma;
        }
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("mu2") == 0 )
      {
        this->mu2.clear();          
        this->mu2.setIoUntilEndOfFile ( false );
        this->mu2.restore ( is, format );

        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("sparse_sigma2") == 0 )
      {
        this->sparse_sigma2.clear();   
        this->sparse_sigma2.setIoUntilEndOfFile ( false );
        this->sparse_sigma2.restore ( is, format );
        
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }       
      else if ( tmp.compare("priors2") == 0 )
      {
        int sizeOfPriors2;
        is >> sizeOfPriors2;
        
        this->priors2.resize ( sizeOfPriors2 );
        for ( std::vector< double >::iterator itPriors2 = this->priors2.begin();
             itPriors2 != this->priors2.end(); 
             itPriors2++
            )
        {
          is >> *itPriors2;
        }
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("b_compareTo2ndGMM") == 0 )
      {
        is >> this->b_compareTo2ndGMM;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }           
      else if ( tmp.compare("maxiter") == 0 )
      {
        is >> this->maxiter;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }          
      else if ( tmp.compare("featsperclass") == 0 )
      {
        is >> this->featsperclass;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }       
      else if ( tmp.compare("cdimval") == 0 )
      {
        is >> this->cdimval;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }        
      else if ( tmp.compare("tau") == 0 )
      {
        is >> this->tau;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }      
      else if ( tmp.compare("pyramid") == 0 )
      {
        is >> this->pyramid;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else
      {
      std::cerr << "WARNING -- unexpected GMM object -- " << tmp << " -- for restoration... aborting" << std::endl;
      throw;
      }
    }
  }
  else
  {
    std::cerr << "GMM::restore -- InStream not initialized - restoring not possible!" << std::endl;
    throw;
  }
}

void GMM::store ( std::ostream & os, int format ) const
{ 
  if (os.good())
  {
    // show starting point
    os << this->createStartTag( "GMM" ) << std::endl;
    
    os << this->createStartTag( "i_numOfGaussians" ) << std::endl;
    os << this->i_numOfGaussians << std::endl;
    os << this->createEndTag( "i_numOfGaussians" ) << std::endl;  
    
    if ( this->dim != -1 )
    {
      os << this->createStartTag( "dim" ) << std::endl;
      os << this->dim << std::endl;
      os << this->createEndTag( "dim" ) << std::endl;
    }
    
    if ( this->mu.size() > 0 )
    {
      os << this->createStartTag( "mu" ) << std::endl;
      this->mu.store ( os, format );
      os << this->createEndTag( "mu" ) << std::endl;
    }
    
    if ( this->sparse_sigma.size() > 0 )
    {
      os << this->createStartTag( "sparse_sigma" ) << std::endl;
      this->sparse_sigma.store ( os, format );
      os << this->createEndTag( "sparse_sigma" ) << std::endl;
    }    
    
    if ( this->priors.size() > 0 )
    {
      os << this->createStartTag( "priors" ) << std::endl;
      os << this->priors.size () << std::endl;
      for ( std::vector< double >::const_iterator itPriors = this->priors.begin();
            itPriors != this->priors.end(); 
            itPriors++
          )
      {
        os << *itPriors;
      }
      os << std::endl;
      os << this->createEndTag( "priors" ) << std::endl;
    }
    
    if ( this->sparse_inv_sigma.size() > 0 )
    {
      os << this->createStartTag( "sparse_inv_sigma" ) << std::endl;
      this->sparse_inv_sigma.store ( os, format );
      os << this->createEndTag( "sparse_inv_sigma" ) << std::endl;
    }   
    
    if ( this->log_det_sigma.size() > 0 )
    {
      os << this->createStartTag( "log_det_sigma" ) << std::endl;
      os << this->log_det_sigma.size ();
      for ( std::vector< double >::const_iterator itLogDetSigma = this->log_det_sigma.begin(); 
            itLogDetSigma != this->log_det_sigma.end();
            itLogDetSigma++
          )
      {
        os << *itLogDetSigma;
      }
      os << std::endl;
      os << this->createEndTag( "log_det_sigma" ) << std::endl;
    }    
    
    if ( this->mu2.size() > 0 )
    {
      os << this->createStartTag( "mu2" ) << std::endl;
      this->mu2.store ( os, format );
      os << this->createEndTag( "mu2" ) << std::endl;
    }    
    
    if ( this->sparse_sigma2.size() > 0 )
    {
      os << this->createStartTag( "sparse_sigma2" ) << std::endl;
      this->sparse_sigma2.store ( os, format );
      os << this->createEndTag( "sparse_sigma2" ) << std::endl;
    }  
    
    if ( this->priors2.size() > 0 )
    {
      os << this->createStartTag( "priors2" ) << std::endl;
      os << this->priors2.size () << std::endl;
      for ( std::vector< double >::const_iterator itPriors2 = this->priors2.begin();
            itPriors2 != this->priors2.end(); 
            itPriors2++
          )
      {
        os << *itPriors2;
      }
      os << std::endl;
      os << this->createEndTag( "priors2" ) << std::endl;
    }     
      
    os << this->createStartTag( "b_compareTo2ndGMM" ) << std::endl;
    os << this->b_compareTo2ndGMM << std::endl;
    os << this->createEndTag( "b_compareTo2ndGMM" ) << std::endl;
    
    os << this->createStartTag( "maxiter" ) << std::endl;
    os << this->maxiter << std::endl;
    os << this->createEndTag( "maxiter" ) << std::endl;   
   
    os << this->createStartTag( "featsperclass" ) << std::endl;
    os << this->featsperclass << std::endl;
    os << this->createEndTag( "featsperclass" ) << std::endl;     
  
    if ( cdimval != -1 ) 
    {
      os << this->createStartTag( "cdimval" ) << std::endl;
      os << this->cdimval << std::endl;
      os << this->createEndTag( "cdimval" ) << std::endl;  
    }
    
    os << this->createStartTag( "tau" ) << std::endl;
    os << this->tau << std::endl;
    os << this->createEndTag( "tau" ) << std::endl;     
    
    os << this->createStartTag( "pyramid" ) << std::endl;
    os << this->pyramid << std::endl;
    os << this->createEndTag( "pyramid" ) << std::endl;  
    
    // done
    os << this->createEndTag( "GMM" ) << std::endl;    
  }
  else
  {
    std::cerr << "OutStream not initialized - storing not possible!" << std::endl;
  }
}

void GMM::clear ()
{ 
}