#include "hmm.h"

hmm::hmm()
{
  hmmodel.A = NULL;
  hmmodel.gem = NULL;
}

hmm::~hmm()
{
  cleardata();
  clearmodel();
  cleartrainlog();
}


int hmm::cleardata()
{
  for(unsigned int i = 0; i < datapts.size(); i++)
    {
      if(datapts[i].alpha != NULL)
        {
          for( int j = 0; j < datapts[i].datasize; j++)
            delete [] datapts[i].alpha[j];
          delete [] datapts[i].alpha;
        }

      if(datapts[i].beta != NULL)
        {
          for( int j = 0; j < datapts[i].datasize; j++)
            delete [] datapts[i].beta[j];
          delete [] datapts[i].beta;
        }
      if(datapts[i].gamma != NULL)
        {
          for(int j = 0; j < datapts[i].datasize-1; j++)
            delete [] datapts[i].gamma[j];
          delete [] datapts[i].gamma;
        }
      if(datapts[i].xi != NULL)
        {
          for(int j = 0; j < datapts[i].datasize-1; j++)
            {
              for(int k = 0; k < nmodels; k++)
                delete [] datapts[i].xi[j][k];
              delete [] datapts[i].xi[j];
            }
          delete [] datapts[i].xi;
        }
      if(datapts[i].data != NULL)
        delete [] datapts[i].data;
    }
  datapts.clear();
  return 0;
}

int hmm::clearmodel()
{
  if(hmmodel.A != NULL)
    {
      for(int i = 0; i < hmmodel.states; i++)
        delete [] hmmodel.A[i];
      delete [] hmmodel.A;
      hmmodel.A = NULL;
    }
  hmmodel.pi.clear();
  if(hmmodel.gem != NULL)
    delete [] hmmodel.gem;
  hmmodel.gem = NULL;
  return 0;
}

int hmm::cleartrainlog()
{
  for( unsigned int i = 0; i < trainsteps.size(); i++ )
    {
      if(trainsteps[i].A != NULL)
        {
          for(int j = 0; j < trainsteps[i].states; j++)
            delete [] trainsteps[i].A[j];
          delete [] trainsteps[i].A;
          trainsteps[i].A = NULL;
        }
      trainsteps[i].pi.clear();
      if(trainsteps[i].gem != NULL)
        delete [] trainsteps[i].gem;
      trainsteps[i].gem = NULL;
    }
  trainsteps.clear();
  return 0;
}

int hmm::adddata( std::vector< double *> &data, int s)
{
  dseq myseq;

  myseq.data = new matrix [data.size()];
  for(unsigned int i = 0; i < data.size(); i++)
    {
      myseq.data[i].init(data[i], s);
    }

  myseq.alpha = NULL;
  myseq.beta = NULL;
  myseq.datasize = data.size();
  datapts.push_back(myseq);
  return 0;
}

int hmm::allocatememory()
{
  for(unsigned int i = 0;
      i < datapts.size(); i++)
    {
      datapts[i].alpha = new  double* [datapts[i].datasize];
      datapts[i].beta = new  double* [datapts[i].datasize];
      datapts[i].gamma = new  double* [datapts[i].datasize-1];
      datapts[i].c = new double [datapts[i].datasize];
      datapts[i].xi = new double **[datapts[i].datasize-1];

      for(int j = 0; j < datapts[i].datasize; j++)
        {
          datapts[i].alpha[j] = new  double [nmodels];
          datapts[i].beta[j] = new  double [nmodels];
        }
      for(int j = 0; j < datapts[i].datasize-1; j++)
        {
          datapts[i].gamma[j] = new  double [nmodels];
          datapts[i].xi[j] = new  double *[nmodels];
          for(int k = 0; k < nmodels; k++)
            datapts[i].xi[j][k] = new double [nmodels];
        }
    }
  return 0;
}

int hmm::calcalpha( dseq &data )
{
  //  std::cout << " calcalpha\n";
  double scale;
  scale = 0;
  for(int i = 0; i < nmodels; i++)
    {
      data.alpha[0][i] = hmmodel.pi[i] *
        emissionprob( data.data[0], i ) ;
      scale = scale + data.alpha[0][i];
    }
  //scale with scaling factor
  data.c[0] = 1/scale;

  for(int i = 0; i < nmodels; i++)
    data.alpha[0][i] = data.alpha[0][i] * data.c[0];

#ifdef __DEBUG_HMM__
  scale = 0;
  for(int i = 0; i < nmodels; i++)
    scale = scale + data.alpha[0][i];
#endif

  for(int i = 1; i < data.datasize; i++)
    {
      double s;
      scale = 0;
      for(int j = 0; j < nmodels; j++)
        {
          s = 0;
          for(int k = 0; k < nmodels; k++)
            s = s + data.alpha[i-1][k] * hmmodel.A[k][j];
          data.alpha[i][j] = s * emissionprob( data.data[i], j );

#ifdef __DEBUG_NAN__
          if(isnan(data.alpha[i][j]) | isinf(data.alpha[i][j]))
            {
              std::cout << "i:" << i << " j: " << j << " ";
              std::cout << "ERROR alpha calc\n";
              std::cout << data.alpha[i][j] << "\n";
              std::cout << "alpha\n\n";
              for(int s = 0; s < 10; s++)
                {
                  for(int t = 0; t < nmodels; t++)
                    std::cout << data.alpha[s][t] <<  " ";
                  std::cout << "\n";
                }
              exit(0);
            }
#endif

          scale = scale + data.alpha[i][j];
        }
      data.c[i] = 1/scale;
      //rescale
      s = 0;
      for(int j = 0; j < nmodels; j++)
        {
          data.alpha[i][j] = data.alpha[i][j] * data.c[i];
#ifdef __DEBUG_HMM__
          s = s + data.alpha[i][j];
#endif
        }
#ifdef __DEBUG_HMM__
      if(fabs(1-s) > 0.0000001)
        {
          std::cout << "alpha calculation hmm check error\n";
          std::cout << "does not add up to 1\n";
          exit(0);
        }
#endif
    }
  return 0;
}

int hmm::calcbeta( dseq &data )
{
  //  std::cout << " calcbeta\n";
  for(int i = 0; i < nmodels; i++)
    data.beta[ data.datasize -1][i] = 1;
  for(int t = data.datasize-2; t >= 0; t--)
    {
      for(int i = 0; i < nmodels; i++)
        {
          double s;
          s = 0;
          for(int j = 0; j < nmodels; j++)
            {
              double normal;
              normal = emissionprob( data.data[t+1], j );
              normal = normal * data.c[t];
              ///check equation
              s = s + (data.beta[t+1][j]) * normal * hmmodel.A[i][j];
#ifdef __DEBUG_NAN__
              if(isnan(s) || isinf(s))
                {
                  std::cout << "ERROR beta calc\n";
                  std::cout << normal << "\n";
                  std::cout << s << "\n";
                  std::cout << "beta i+1: " << data.beta[t+1][j] << "\n";
                  std::cout << "trans: " <<  hmmodel.A[i][j] << "\n";
                  std::cout << "t i j " << t << " " << i << " " << j << "\n";
                  exit(0);
                }
#endif
            }
          data.beta[t][i] = s;
        }
    }
  return 0;
}

int hmm::calcgamma( dseq &data )
{
  //  std::cout << " calcgamma\n";
  for(int t = 0; t < data.datasize-1; t++)
    {
      for(int i = 0; i < nmodels; i++)
        {
          data.gamma[t][i] = 0;
          for(int j = 0; j < nmodels; j++)
            {
              data.gamma[t][i] = data.gamma[t][i] + data.xi[t][i][j];
#ifdef __DEBUG_NAN__
              if(isnan(data.gamma[t][i]) )
                {
                  std::cout << "ERROR gamma calc\n";
                  std::cout << data.gamma[t][i] << "\n";
                  exit(0);
                }
#endif
            }
        }

      //renormaize the gamma so it is a probability measure
      double ts;
      ts = 0;
      for(int i = 0; i < nmodels; i++)
        ts = ts + data.gamma[t][i];
      for(int i = 0; i < nmodels; i++)
        data.gamma[t][i] = data.gamma[t][i]/ts;
    }
  return 0;
}

int hmm::calcxi(dseq &s)
{
  for(int t = 0; t < s.datasize-1; t++)
    for(int i = 0; i < nmodels; i++)
      for(int j = 0; j < nmodels; j++)
        s.xi[t][i][j] =  s.alpha[t][i] * emissionprob(s.data[t+1], j)
          * hmmodel.A[i][j] * s.beta[t+1][j];
  return 0;
}

int hmm::calcalpha()
{
  for(unsigned int i = 0; i < datapts.size(); i++)
    calcalpha( datapts[i] );
  return 0;
}

int hmm::calcbeta()
{
  for(unsigned int i = 0; i < datapts.size(); i++)
    calcbeta( datapts[i] );
  return 0;
}

int hmm::calcgamma()
{
  for(unsigned int i = 0; i < datapts.size(); i++)
    calcgamma( datapts[i] );
  return 0;
}

int hmm::calcxi()
{
  for(unsigned int i = 0; i < datapts.size(); i++)
    calcxi( datapts[i] );
  return 0;
}

int hmm::estep()
{
  calcalpha();
  calcbeta();
  calcxi();
  calcgamma();
  return 0;
}

int hmm::mstep(int updategem)
{
  double s;
  s = 0;  
  
  for(int k = 0; k < nmodels; k++)
    {
      hmmodel.pi[k] = 0;
      for(unsigned int r = 0; r < datapts.size(); r++)
        hmmodel.pi[k] = hmmodel.pi[k] + datapts[r].gamma[0][k];
      s = s + hmmodel.pi[k];
    }
  for(int i = 0; i < nmodels; i++)
    {
      hmmodel.pi[i] = hmmodel.pi[i]/s;
#ifdef __DEBUG_NAN__
      if(isnan(hmmodel.pi[i]))
        {
          std::cout << "pi em calc error\n";
          exit(0);
        }
#endif
    }
  
  ///calculate the transition matrix

  for(int i = 0; i < nmodels; i++)
    {
      s = 0;
      for(int j = 0; j < nmodels; j++)
        {
          hmmodel.A[i][j] = 0;
          double txi;
          for(unsigned int n = 0; n < datapts.size(); n++)
            {
              for( int t = 0; t < datapts[n].datasize-1; t++)
                {
                  txi = datapts[n].xi[t][i][j];
                  hmmodel.A[i][j] = hmmodel.A[i][j] + txi;
#ifdef __DEBUG_NAN__
                  if(isnan(txi) || isnan(hmmodel.A[i][j]))
                    {
                      std::cout << "xi calc error\n";
                      exit(0);
                    }
#endif
                }
            }
          s = s + hmmodel.A[i][j];
        }
      //      std::cout << "S:" << s << "\n";
      for(int j = 0; j < nmodels; j++)
        {
          hmmodel.A[i][j] = hmmodel.A[i][j] / s;
#ifdef __DEBUG_NAN__
          if(isnan( hmmodel.A[i][j] ))
            {
              std::cout << "A calc error\n";
              std::cout << s << "\n";
              std::cout << hmmodel.A[i][j] << "\n";
              exit(0);
            }
#endif
        }
    }
  
  ///m step for mean
  if(updategem)
    {
      matrix tm, tc;
      for(int k = 0; k < nmodels; k++)
        {
          hmmodel.gem[k].getmean(tm);
          tm.zero();
          s = 0;
          for(unsigned int r = 0; r < datapts.size(); r++)
            {
              for(int n = 0; n < datapts[r].datasize-1; n++)
                {
                  s = s + datapts[r].gamma[n][k];
                  tm.add( datapts[r].data[n] * datapts[r].gamma[n][k] );
                }
            }
          tm.divide(s);
          hmmodel.gem[k].setmean(tm);
        }
      
      matrix tmp, tmp2;
      for(int j = 0; j < nmodels; j++)
        {
          s = 0;
          //zero the stuff
          hmmodel.gem[j].getvariance(tc);
          hmmodel.gem[j].getmean(tm);
          tc.zero();
          
          for(unsigned int n = 0; n < datapts.size(); n++)
            {
              for(int t = 0; t < datapts[n].datasize-1; t++)
                {
                  tmp = datapts[n].data[t] - tm;
                  tmp2 = tmp * tmp.transpose();
                  tmp2 = tmp2 * datapts[n].gamma[t][j];
                  tc.add(tmp2);
                  s = s + datapts[n].gamma[t][j];
                }
            }
          tc.divide( s );
          hmmodel.gem[j].setvariance(tc);
        }
    }
  return 0;
}

int hmm::train( int n )
{
  nmodels = n;
 
  std::cout << "starting model training\n";
  std::cout << "number of sequences: " << datapts.size() << "\n";
  std::cout << "parameters in each data point: " << datapts[0].data[0].rows() << "\n";
  std::cout << "number of hidden variables: " << nmodels << "\n";
  //set initial hmm conditions
  inithmm();
  int iter = 0;
  std::cout << "starting forward backward\n";
  //  print();
  long double told = -100000000;
  told = told * 1000000;
  int flag = 2;
  timer t_iter, t_mstep, t_estep;
#ifdef __QUICK_TRAIN__
  int traincycles = 1;
#else
  int traincycles = 50;
#endif
  model emptymodel;
  emptymodel.A = NULL;
  emptymodel.gem = NULL;
  long double bestscore;
  int bestiter;
  bestiter = 0;
  bestscore = told;
  cleartrainlog();
  while(flag > 0)
    {

      t_iter.start();
      t_estep.start();
      //e step
      estep();
      t_estep.stop();

      t_mstep.start();
      //m step
      mstep(1);
      t_mstep.stop();

      long double tscore;
      tscore = 0;
      calcalpha();

      for(unsigned int i = 0; i < datapts.size(); i++)
        {
          double pts;
          pts = 0;
          for(int t = 0; t < datapts[i].datasize; t++)
            {
              pts = pts + logl(datapts[i].c[t]);
              if( isnan(pts) )
                {
                  std::cout << "ERROR loglikely calc is nan\n";
                  std::cout << datapts[i].c[t] << " t: " << t << "\n";
                  exit(0);
                }
            }
          pts = -pts;
          tscore = tscore + pts;
        }
      tscore = tscore / ( double(datapts.size()) );

      // save the model generated by iteration
      if( trainsteps.size() != (unsigned int)iter )
        {
          std::cout << "ERROR hmm train iteration count not synchronized\n";
          exit(0);
        }
      trainsteps.push_back( emptymodel );
      copymodel( hmmodel, trainsteps[iter] );
      trainsteps[iter].ll = tscore;

      if( trainsteps[iter].ll > bestscore )
        {
          bestscore = trainsteps[iter].ll;
          bestiter = iter;
          std::cout << "*";
        }
      else
        std::cout << " ";

      iter++;
      std::cout.precision(10);
      std::cout << "iter: " << iter << " " << tscore << " ";
      std::cout << tscore - told;
      std::cout.precision(5);

      if( iter - bestiter > 5 )
        flag = -1;
      
      if(iter > traincycles || isnan( tscore ) || isinf( tscore ))
        flag = -1;

      told = tscore;
      t_iter.stop();
      std::cout << " time I: " << t_iter.getseconds() 
                << "s E: " << t_estep.getseconds()
                <<"s M: " << t_mstep.getseconds() << "s\n";
      t_iter.reset();
      t_mstep.reset();
      t_estep.reset();
    }
  copymodel( trainsteps[bestiter], hmmodel );
  cleartrainlog();
  print();
  return 0;
}

int hmm::inithmm()
{
  int totalpts;
  double **d;
  
  totalpts = 0;

  for(unsigned int i = 0; i <  datapts.size(); i++)
    totalpts = totalpts + datapts[i].datasize;

  d = new double * [totalpts];
  for(int i = 0; i < totalpts; i++)
    d[i] = new double [datapts[0].data[0].rows()];
  int c = 0;

  for(unsigned int i = 0; i < datapts.size(); i++)
    {
      for(int j = 0; j < datapts[i].datasize; j++)
        {
          for(int k = 0; k < datapts[i].data[j].rows(); k++)
            {
              d[c][k] = datapts[i].data[j].get(k, 0);
            }
          c++;
        }
    }

  em.loaddata( d, totalpts, datapts[0].data[0].rows(), 2);
  std::cout << "starting initial training " << nmodels << "\n";
  em.run(nmodels);
  std::cout << "extracted models\n";
  em.printmodels();
  std::cout << "---------------\n";
  std::vector<gaussianmodel *> eminit;
  eminit = em.getmodels();

  for(int i = 0; i < totalpts; i++)
    delete [] d[i];
  delete [] d;

  clearmodel();
  allocatememory();

  hmmodel.pi.clear();
  for(int i = 0; i < nmodels; i++)
    hmmodel.pi.push_back( em.getpi(i) );
  //// initialize the hmm model
  hmmodel.A = new double *[nmodels];
  for(int i = 0; i < nmodels; i++)
    {
      hmmodel.A[i] = new double [nmodels];

      for(int j = 0; j < nmodels; j++)
        {
          hmmodel.A[i][j] = 1/( (double) nmodels );
        }
    }

  hmmodel.states = nmodels;

  hmmodel.gem = new gaussianmodel [nmodels];

  ///copy the initialized hmmodels
  for(int i = 0; i < nmodels; i++)
    {
      hmmodel.gem[i] =  *eminit[i];
      delete eminit[i];
    }
  return 0;
}

int hmm::copymodel( model &src, model &des )
{
  //delete des model 
  if( des.A != NULL )
    {
      for( int i = 0; i < des.states; i++ )
        delete [] des.A[i];
      delete [] des.A;
      des.A = NULL;
    }
  if( des.gem != NULL )
    {
      delete [] des.gem;
      des.gem = NULL;
    }
  des.pi.clear();
  des.states = src.states;
  des.pi = src.pi;
  des.gem = new gaussianmodel [ des.states ];
  des.A = new double *[des.states];
  for( int i = 0; i < des.states; i++ )
    des.A[i] = new double [des.states];
  for( int i = 0; i < des.states; i++ )
    {
      for( int j = 0; j < des.states; j++ )
        {
          des.A[i][j] = src.A[i][j];
        }
      des.gem[i] = src.gem[i];
    }
  return 0;
}

int hmm::print()
{
  std::cout << "hmmodel:::\n";
  std::cout << "pi\n  ";
  for(unsigned int i = 0; i < hmmodel.pi.size(); i++)
    std::cout << hmmodel.pi[i] << " ";
  std::cout << "\ntransition matrix\n";
  for(int i = 0; i < nmodels; i++)
    {
      for(int j = 0; j < nmodels; j++)
        {
          std::cout << hmmodel.A[i][j] << " \t";
        }
      std::cout << "\n";
    }
  std::cout << "GEM::\n";
  for(int i = 0; i < nmodels; i++)
    {
      std::cout << hmmodel.gem[i];
      std::cout << "-----\n";
    }

  return 0;
}

double hmm::emissionprob( matrix &m, int k)
{
  double b = hmmodel.gem[k].prob(m);
  return b;
}

double hmm::emissionprob( matrix &m, int k, long double *calc_buffer)
{
  double b = hmmodel.gem[k].prob_t(m, calc_buffer);
  return b;
}

double hmm::logemissionprob( matrix &m, int k)
{
  double b = log(hmmodel.gem[k].prob(m ));
  return b;
}

int hmm::serialize( hmmserialized &d )
{
  std::cout << "writing model\n";
  print();
  matrix m, c;
  hmmodel.gem[0].getmean(m);
  d.nparam = m.rows();
  d.nstates = nmodels;
  int sz;
  sz = (int)pow(d.nstates, 2) + d.nstates *
    (int)(d.nparam + (int)pow(d.nparam, 2));
  d.data = new char [ sz * sizeof(double) + 2 * sizeof(int) ];
  d.size = sz * sizeof(double) + 2 * sizeof(int);
  std::cout << "calculated data size: " << d.size << "\n";
  memcpy( d.data, &(d.nstates), sizeof(int) );
  memcpy( d.data+sizeof(int), &(d.nparam), sizeof(int) );

  char *data = d.data + ( 2 * sizeof(int) );

  //copy the transistion matrix
  for(int i = 0; i < nmodels; i++)
    {
      for(int j = 0; j < nmodels; j++)
        {
          memcpy(data, &(hmmodel.A[i][j]), sizeof(double) );
          data = data + (sizeof(double));
        }
    }

  //copy each of the models
  for(int n = 0; n < nmodels; n++)
    {
      hmmodel.gem[n].getmean(m);
      hmmodel.gem[n].getvariance(c);
      //mean
      for(int i = 0; i < d.nparam; i++)
        {
          double t;
          t = m.get(i, 0);
          memcpy(data, &t, sizeof(double));
          data = data + (sizeof(double));
        }
      //covariance
      for(int i = 0; i < d.nparam; i++)
        {
          for(int j = 0; j < d.nparam; j++)
            {
              double t;
              t = c.get(i,j);
              memcpy(data, &t, sizeof(double));
              data = data + (sizeof(double));
            }
        }
    }

  unsigned long int size = ( (unsigned long int)data ) - 
    ( (unsigned long int) d.data);
  std::cout << "actural data size: " << size << "\n";
  if(size != (unsigned long int) d.size)
    exit(-1);
  return 0;
}

int hmm::deserialize( hmmserialized &d )
{
  memcpy( &(d.nstates), d.data, sizeof(int) );
  memcpy( &(d.nparam), d.data+sizeof(int), sizeof(int) );
  nmodels = d.nstates;
  hmmodel.states = nmodels;
  cleardata();
  clearmodel();
  char *data = d.data + ( 2 * sizeof(int) );
  std::cout << "deserializing " << d.nstates << " states\n";
  hmmodel.A = new double * [d.nstates];
  hmmodel.gem = new gaussianmodel [d.nstates];
#ifdef __DEBUG__
  std::cout << "deserializing " << nmodels << " states\n";
  std::cout << "em parameters " << d.nparam << "\n";
  std::cout << "trans diagram\n";
#endif
  for(int i = 0; i < d.nstates; i++)
    {
      hmmodel.A[i] = new double [d.nstates];
      for(int j = 0; j < d.nstates; j++)
        {
          memcpy( &(hmmodel.A[i][j]), data , sizeof(double) );
          if(isnan(hmmodel.A[i][j]) || isinf(hmmodel.A[i][j]))
          {
            std::cout << "model read error\n";
            exit(0);
          }
          data = data + sizeof(double);
        }
      hmmodel.pi.push_back( 1.0/( (double)d.nstates ) );
    }
  matrix m,c;
  for(int n = 0; n < d.nstates; n++)
    {
      hmmodel.gem[n].init(d.nparam);
      hmmodel.gem[n].getmean(m);
      hmmodel.gem[n].getvariance(c);
      //mean
      for(int i = 0; i < d.nparam; i++)
        {
          double t;
          memcpy(&t, data, sizeof(double));
          m.set(t, i, 0);
          data = data + (sizeof(double));
          if( isnan(t) || isinf(t) )
            {
              std::cout << "ERROR:: INF in read\n";
              exit(0);
            }
        }
      //covariance
      for(int i = 0; i < d.nparam; i++)
        {
          for(int j = 0; j < d.nparam; j++)
            {
              double t;
              memcpy(&t, data, sizeof(double));
#ifdef __DEBUG_NAN__
              if( isnan(t) || isinf(t) )
                {
                  std::cout << "ERROR:: INF in read\n";
                  exit(0);
                }
#endif
              c.set(t, i, j);
              data = data + (sizeof(double));
            }
        }
      hmmodel.gem[n].setmean(m);
      hmmodel.gem[n].setvariance(c);

    }
  return 0;
}

int hmm::printdot( std::string fn )
{
  std::ofstream fh;
  fh.open( fn.c_str() );

  unsigned int mc[nmodels];
  //create a list of colors
  mc[0] = 0x0F0F0F;
  for(int i = 1; i < nmodels; i++)
    {
      mc[i] = mc[i-1] + (16777216-mc[0]) / (nmodels+2);
    }

  matrix m, c; 

  fh << "digraph hmm {";
  //print the nodes
  for(int i = 0; i < nmodels; i++)
    {
      hmmodel.gem[i].getmean(m);
      hmmodel.gem[i].getvariance(c);
      fh << "node [shape=record] s" << i << " [label=\"{";
      //mean
      for(int j = 0; j < m.rows(); j++)
        {
          fh << m.get(j, 0);
          if(j < c.rows() - 1)
            fh << "|";
        }
      fh << "}||";
      //covariance
      for(int j = 0; j < c.rows(); j++)
        {
          fh << "{";
          for(int k = 0; k < c.rows(); k++)
            {
              fh << c.get(k,j);
              if( k < c.rows() - 1)
                fh << "|";
            }
          fh << "}";
          if( j < c.rows() - 1)
            fh << "|";
        }
      fh << "\" color=\"#" <<std::hex <<mc[i] << std::dec << "\"];\n";
    }
  //print the links
  for(int i = 0; i < nmodels; i++)
    for(int j = 0; j < nmodels; j++)
      if(hmmodel.A[i][j] > 0.000001)
        fh << "s" << i << " -> s" << j << "[ label = \"" << hmmodel.A[i][j] <<  "\" color=\"#" << std::hex << mc[j] << std::dec << "\" fontcolor=\"#" << std::hex << mc[j] << std::dec << "\"];\n";

  fh << "}\n";
  fh.close();

  return 0;
}

int hmm::prepthread()
{
  for( int i = 0; i < hmmodel.states; i++ )
    hmmodel.gem[i].cachecholesky();
  return 0;
}

double hmm::match_t( std::vector< matrix > &data, long double *smatrix,
                     long double *calc_buffer)
{
  long double score;
  long double ts;
  ts = 0;
  score = 0;
  for(int i = 0; i < hmmodel.states; i++)
    {
      smatrix[i] = emissionprob( data[0], i, calc_buffer );
      if(isnan(smatrix[i]) || isinf(smatrix[i]))
        {
          std::cout << "MATCH nan/inf error\n";
          exit(0);
        }
      if(smatrix[i] < powl(2, -1000))
        smatrix[i] = powl(2, -1000);
      ts = ts + smatrix[i];
    }

  ts = 1/ts;
  score = score - logl(ts);
  if(isnan(score) || isinf(score))
    {
      std::cout << "MATCH nan/inf error\n";
      std::cout << score << " " << ts << "\n";
      exit(0);
    }
  for(int i = 0; i < hmmodel.states; i++)
    smatrix[i] = smatrix[i] * ts;

  for(unsigned int i = 1; i < data.size(); i++)
    {
      ts = 0;
      for(int j = 0; j < hmmodel.states; j++)
        {
          long double s;
          s = 0;
          for(int k = 0; k < hmmodel.states; k++)
            s = s + smatrix[ (i-1)*hmmodel.states+k ] * hmmodel.A[k][j];
          smatrix[i*hmmodel.states+j] = 
            s*emissionprob(data[i], j, calc_buffer);
          
          //check to prevent the probability to not be too low
          if(smatrix[i*hmmodel.states+j] < powl(2, -1000))
            smatrix[i*hmmodel.states+j] = powl(2, -1000);

          ts = ts + smatrix[i*hmmodel.states+j];
        }
      ts = 1/ts;
      score = score - logl(ts);
      for(int j = 0; j < hmmodel.states; j++)
        smatrix[i*hmmodel.states+j] = smatrix[i*hmmodel.states+j] * ts;
      if(isnan(score) || isinf(score))
        {
          std::cout << "MATCH nan/inf error " << i << "\n";
          std::cout << ts << "\n";
          std::cout << logl(ts) << "\n";
          std::cout << score << "\n";
          exit(0);
        }
    }

  return score;
}

double hmm::match( std::vector< matrix > &data )
{
  long double score;
  long double smatrix[data.size()][hmmodel.states];
  long double ts;
  ts = 0;
  score = 0;
  for(int i = 0; i < hmmodel.states; i++)
    {
      smatrix[0][i] = emissionprob( data[0], i );
      if(isnan(smatrix[0][i]) || isinf(smatrix[0][i]))
        {
          std::cout << "MATCH nan/inf error\n";
          exit(0);
        }
      if(smatrix[0][i] < powl(2, -1000))
        smatrix[0][i] = powl(2, -1000);
      ts = ts + smatrix[0][i];
    }

  ts = 1/ts;
  score = score - logl(ts);
  if(isnan(score) || isinf(score))
    {
      std::cout << "MATCH nan/inf error\n";
      std::cout << score << " " << ts << "\n";
      exit(0);
    }
  for(int i = 0; i < hmmodel.states; i++)
    smatrix[0][i] = smatrix[0][i] * ts;

  for(unsigned int i = 1; i < data.size(); i++)
    {
      ts = 0;
      for(int j = 0; j < hmmodel.states; j++)
        {
          long double s;
          s = 0;
          for(int k = 0; k < hmmodel.states; k++)
            s = s + smatrix[i-1][k] * hmmodel.A[k][j];
          smatrix[i][j] = s*emissionprob(data[i], j);
          
          //check to prevent the probability to not be too low
          if(smatrix[i][j] < powl(2, -1000))
            smatrix[i][j] = powl(2, -1000);

          ts = ts + smatrix[i][j];
        }
      ts = 1/ts;
      score = score - logl(ts);
      for(int j = 0; j < hmmodel.states; j++)
        smatrix[i][j] = smatrix[i][j] * ts;
      if(isnan(score) || isinf(score))
        {
          std::cout << "MATCH nan/inf error " << i << "\n";
          std::cout << ts << "\n";
          std::cout << logl(ts) << "\n";
          std::cout << score << "\n";
          exit(0);
        }
    }

  return score;
}

double hmm::match( matrix *data, int size )
{
  double score;
  score = 0;
  long double smatrix[size][hmmodel.states];
  long double ts;
  ts = 0;
  for(int i = 0; i < hmmodel.states; i++)
    {
      smatrix[0][i] = hmmodel.pi[i] * emissionprob( data[0], i );
      ts = ts + smatrix[0][i];
    }
  ts = 1/ts;
  score = score - logl(ts);

  for(int i = 0; i < hmmodel.states; i++)
    smatrix[0][i] = smatrix[0][i] * ts;

  for(int i = 1; i < size; i++)
    {
      ts = 0;
      for(int j = 0; j < hmmodel.states; j++)
        {
          long double s;
          s = 0;
          for(int k = 0; k < nmodels; k++)
            s = s + smatrix[i-1][k] * hmmodel.A[k][j];
          smatrix[i][j] = s*emissionprob(data[i], j);
          ts = ts + smatrix[i][j];
        }
      ts = 1/ts;
      score = score - logl(ts);
  
      for(int j = 0; j < hmmodel.states; j++)
        smatrix[i][j] = smatrix[i][j] * ts;
    }

  return score;
}

int hmm::size( int &mynstates, int &myvecsize )
{
  mynstates = hmmodel.states;
  myvecsize = hmmodel.gem[0].size();
  return 0;
}
