#include "gaussianmodel.h"

gaussianmodel::gaussianmodel()
{
#ifdef __THREADING__
  pthread_mutexattr_t ma;
  pthread_mutexattr_init( &ma );
  pthread_mutexattr_setpshared( &ma, PTHREAD_MUTEX_RECURSIVE );
  pthread_mutex_init( &lock, &ma );
  pthread_mutexattr_destroy( &ma );
#endif
  dsize = -1;
  cacheflag = 0;
  calcb = NULL;
}

gaussianmodel::~gaussianmodel()
{
  if(calcb != NULL)
    delete [] calcb;
#ifdef __THREADING__
  pthread_mutex_destroy( &lock );
#endif
}

int gaussianmodel::wigglecovariance()
{
  for( int i = 0; i < dsize; i++ )
    {
      int z;
      z = random()%1000;
      double p;
      p = ( (double)z ) / 1000000.0;
      covariance.set( p + covariance.get( i, i ), i, i);
    }
  return 0;
}

int gaussianmodel::cachecholesky()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  if(cacheflag != 1)
    {
      int wiggleiter = 0;
      while( covariance.cholesky(chol) != 0 )
        {
          wigglecovariance();
          wiggleiter++;
          if( wiggleiter > 5 )
            {
              std::cout << "covariance wiggled 5 times with out success\n";
              exit(0);
            }
        }
      ichol = chol.invertU();
      gc = -(double(dsize)/2 * log(2*PI)) - chol.logdiagsum();
      cacheflag = 1;
    }
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
  return 0;
}

long double gaussianmodel::prob_t(matrix &d, long double *tmparray)
{
  long double res= 0;
  //1d case
  if(dsize == 1)
    {
      res = 1/sqrt(covariance.get(0,0)*2*PI);
      res = res * exp(-pow(d.get(0,0)-mean.get(0,0),2)/(2*covariance.get(0,0)) );
      return res;
    }

  long double t;
  t = 0;
  int delflag;
  if( tmparray == NULL )
    {
      tmparray = new long double[ dsize ];
      delflag = 1;
    }
  else
    delflag = 0;

  for(int i = 0; i < dsize; i++)
    tmparray[i] = d.get(i,0) - mean.get(i, 0);
    
  //below replaces y = z.transpose() * m.ichol;
  //since that will allocate useless memory
  double ts;
  for(int i = 0; i < dsize; i++)
    {
      ts = 0;
      for(int j = 0; j < dsize; j++)
        ts = ts + tmparray[j] * ichol.get(j,i);
      t = t + pow(ts, 2);
    }
 
  res = exp(gc - 0.5*t);

#ifdef __DEBUG_NAN__
  if(isnan(res) || isinf(res) )
    {
      std::cout << "normal inf error\n";
      std::cout << res << "\n";
      std::cout << gc << " - 0.5 * " << t << "\n";
      std::cout << ichol << "\n--\n";
      std::cout << chol << "\n--\n";
      std::cout << covariance << "\n";
      std::cout << "data points:\n";
      std::cout << d << "\n";
      std::cout << "mean\n";
      std::cout << mean << "\n";
      exit(0);
    }
#endif

  if( delflag == 1 )
    delete [] tmparray;
  if( res < 0.00000000000001 )
    res = 0.00000000000001;
  return res;
}

long double gaussianmodel::prob(matrix &d)
{
  long double res= 0;
  //1d case
  if(dsize == 1)
    {
      res = 1/sqrt(covariance.get(0,0)*2*PI);
      res = res * exp(-pow(d.get(0,0)-mean.get(0,0),2)/(2*covariance.get(0,0)) );
      return res;
    }
  //caching check
  cachecholesky();

  long double t;
  t = 0;

#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  for(int i = 0; i < dsize; i++)
    calcb[i] = d.get(i,0) - mean.get(i, 0);
    
  //below replaces y = z.transpose() * m.ichol;
  //since that will allocate useless memory
  double ts;
  for(int i = 0; i < dsize; i++)
    {
      ts = 0;
      for(int j = 0; j < dsize; j++)
        ts = ts + calcb[j] * ichol.get(j,i);
      t = t + pow(ts, 2);
    }
 
  res = exp(gc - 0.5*t);

#ifdef __DEBUG_NAN__
  if(isnan(res) || isinf(res) )
    {
      std::cout << "normal inf error\n";
      std::cout << res << "\n";
      std::cout << gc << " - 0.5 * " << t << "\n";
      std::cout << ichol << "\n--\n";
      std::cout << chol << "\n--\n";
      std::cout << covariance << "\n";
      std::cout << "data points:\n";
      std::cout << d << "\n";
      std::cout << "mean\n";
      std::cout << mean << "\n";
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
      exit(0);
    }
#endif

#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif

  if( res < 0.00000000000001 )
    res = 0.00000000000001;
  return res;
}

long double gaussianmodel::prob(double *d)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  long double res= 0;
  //1d case
  if(dsize == 1)
    {
      res = 1/sqrt(covariance.get(0,0)*2*PI);
      res = res * exp(-pow(d[0]-mean.get(0,0),2)/(2*covariance.get(0,0)) );
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
      return res;
    }
  //caching check
  if(cacheflag != 1)
    {
      int wiggleiter = 0;
      while( covariance.cholesky(chol) != 0 )
        {
#ifdef __THREADING__
          pthread_mutex_unlock(&lock);
#endif
          wigglecovariance();
          wiggleiter++;
          if( wiggleiter > 5 )
            {
              std::cout << "covariance wiggled 5 times with out success\n";
              exit(0);
            }
        }
      ichol = chol.invertU();
      gc = -(double(dsize)/2 * log(2*PI)) - chol.logdiagsum();
      cacheflag = 1;
    }
  long double t;
  t = 0;

  for(int i = 0; i < dsize; i++)
    calcb[i] = d[i] - mean.get(i, 0);
    
  //below replaces y = z.transpose() * m.ichol;
  //since that will allocate useless memory
  double ts;
  for(int i = 0; i < dsize; i++)
    {
      ts = 0;
      for(int j = 0; j < dsize; j++)
        ts = ts + calcb[j] * ichol.get(j,i);
      t = t + pow(ts, 2);
    }
 
  res = exp(gc - 0.5*t);

#ifdef __DEBUG_NAN__
  if(isnan(res) || isinf(res) )
    {
      std::cout << "normal inf error\n";
      std::cout << res << "\n";
      std::cout << gc << " - 0.5 * " << t << "\n";
      std::cout << ichol << "\n--\n";
      std::cout << chol << "\n--\n";
      std::cout << covariance << "\n";
      std::cout << "data points:\n";
      std::cout << d << "\n";
      std::cout << "mean\n";
      std::cout << mean << "\n";
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
      exit(0);
    }
#endif

#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif

  if( res < 0.00000000000001 )
    res = 0.00000000000001;
  return res;
}

int gaussianmodel::init(int d)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  if(calcb != NULL)
    delete [] calcb;
  calcb = new long double [d];
  mean.init(d, 1);
  covariance.init(d,d);
  cacheflag = 0;
  dsize = d;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int gaussianmodel::setmean(matrix &m)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  mean = m;
  cacheflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int gaussianmodel::setvariance(matrix &v)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  covariance = v;
  cacheflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int gaussianmodel::getmean(matrix &m)
{
  m = mean;
  return 0;
}

int gaussianmodel::isgood()
{
  if( mean.notgood() )
    return 0;
  if( covariance.notgood() )
    return 0;
  return 1;
}

int gaussianmodel::getvariance(matrix &v)
{
  v = covariance;
  return 0;
}

int gaussianmodel::size()
{
  return dsize;
}

int gaussianmodel::fixcovariance()
{
  for( int i = 0; i < dsize; i++ )
    {
      if( covariance.get(i,i) < 0.0001 )
        {
#ifdef __DEBUG__
          std::cout << "fixing covariance\n" << std::flush;
#endif
          covariance.set( 0.0001, i, i );
        }
    }
  return 0;
}

gaussianmodel& gaussianmodel::operator=(const gaussianmodel &g)
{
  if(this == &g)
    return *this;
#ifdef __THREADING__
  if( (unsigned long int) this < (unsigned long int) (&g) )
    {
      pthread_mutex_lock(&lock);
      pthread_mutex_lock( (pthread_mutex_t*)&(g.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(g.lock) );
      pthread_mutex_lock(&lock);
    }
#endif
  if(calcb != NULL)
    delete [] calcb;
  dsize = g.dsize;
#ifdef __DEBUG__
  std::cout << "gauss cpy allocating memory: " << dsize << "\n";
#endif
  calcb = new long double [dsize];
  mean = g.mean;
  covariance = g.covariance;
  cacheflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(g.lock) );
#endif
  return *this;
}

int gaussianmodel::copy( gaussianmodel &g )
{
#ifdef __THREADING__
  if( (unsigned long int) this < (unsigned long int) (&g) )
    {
      pthread_mutex_lock(&lock);
      pthread_mutex_lock( (pthread_mutex_t*)&(g.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(g.lock) );
      pthread_mutex_lock(&lock);
    }
#endif
  g.init(dsize);
  g.mean = mean;
  g.covariance = covariance;
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(g.lock) );
#endif
  return 0;
}

int gaussianmodel::split(gaussianmodel &o)
{
#ifdef __THREADING__
  if( (unsigned long int) this < (unsigned long int) (&o) )
    {
      pthread_mutex_lock(&lock);
      pthread_mutex_lock( (pthread_mutex_t*)&(o.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(o.lock) );
      pthread_mutex_lock(&lock);
    }
#endif
  o.covariance = covariance;
  o.cacheflag = 0;
  matrix t;
  t.init( dsize, 1);
  for(int i = 0; i < dsize; i++)
    t.set( sqrt(fabs(covariance.get(i,i))), i, 0 );
  o.mean = mean - t;
  mean.add(t);
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(o.lock) );
#endif
  return 0;
}

int gaussianmodel::getserializedsize()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  int res = 0;
  res = dsize*sizeof(double) + dsize*dsize * sizeof(double);
  res = res + sizeof(int);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return res;
}

int gaussianmodel::serialize(char *buffer)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  memcpy(buffer, &dsize, sizeof(int));
  char *t;
  t = buffer + sizeof(int);
  for(int i = 0; i < dsize; i++)
    {
      double v;
      v = mean.get(i, 0);
      memcpy(t, &v, sizeof(double));
      t = t + sizeof(double);
    }
  for(int i = 0; i < dsize; i++)
    {
      for(int j = 0; j < dsize; j++)
        {
          double v;
          v = covariance.get(i,j);
          memcpy(t, &v, sizeof(double));
          t = t + sizeof(double);
        }
    }
  unsigned long int p = (unsigned long int) t - (unsigned long int) buffer;
  if(p != (unsigned long int) getserializedsize())
    {
      std::cout << "ERROR unexpected size in serializing...\n";
      exit(0);
    }
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 0;
}

int gaussianmodel::deserialize(char *buffer)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  memcpy(&dsize, buffer, sizeof(int));
  char *t;
  t = buffer + sizeof(int);

  init(4);

  cacheflag = 0;
  for(int i = 0; i < dsize; i++)
    {
      double v;
      memcpy(&v, t, sizeof(double));
      v = mean.set(v, i, 0);
      t = t + sizeof(double);
    }
  for(int i = 0; i < dsize; i++)
    {
      for(int j = 0; j < dsize; j++)
        {
          double v;
          memcpy(&v, t, sizeof(double));
          covariance.set(v, i,j);
          t = t + sizeof(double);
        }
    }
  unsigned long int p = (unsigned long int) t - (unsigned long int) buffer;
  if(p != (unsigned long int) getserializedsize())
    {
      std::cout << "ERROR unexpected size in serializing...\n";
      exit(0);
    }
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int gaussianmodel::zero()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  mean.zero();
  covariance.zero();

  cacheflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

std::ostream& operator <<(std::ostream &o, gaussianmodel &m)
{
  matrix mean, c;
  m.getmean(mean);
  m.getvariance(c);
  for(int i = 0; i < m.size(); i++)
    {
      std::cout << mean.get(i,0) << "\t";
      for(int j = 0; j < m.size(); j++)
        std::cout << c.get(i,j) << " ";
      std::cout << "\n";
    }
  return o;
}
