#include "matrix.h"

double point_dis( matrix &p1,
                  matrix &p2 )
{
  double d;
  d = 0;
  for( int i = 0; i < p1.rows(); i++)
    {
      d = d + pow( p1.get(i,0)-p2.get(i,0), 2);
    }
  return sqrt(d);
}

int matrix::notgood()
{
  for( int i = 0; i < row*col; i++ )
    if( isnan( d[i] ) || isinf( d[i] ) )
      return 1;
  return 0;
}

matrix::matrix()
{
#ifdef __THREADING__
  pthread_mutex_init( &lock, NULL );
#endif
  d = NULL;
  col = row = -1;
  detflag = 0;
}

matrix::matrix(const matrix &m)
{
#ifdef __THREADING__
  pthread_mutex_init( &lock, NULL );
#endif
  d = NULL;

  col = row = -1;
  detflag = 0;
  init(m.d, m.row, m.col);
}

matrix::~matrix()
{
#ifdef __THREADING__
  pthread_mutex_destroy( &lock );
#endif
  if(d != NULL)
    {
      delete [] d;
    }
  d = NULL;
  row = col = -1;
}

int matrix::serialize( char **buf, int &size )
{
  size = col*row*sizeof(double) + 2*sizeof(int);
  char *b = new char[size];
  memcpy( b, &col, sizeof(int) );
  memcpy( b+sizeof(int), &row, sizeof(int) );
  memcpy( b + (sizeof(int)*2), d, sizeof(double)* row*col );
  *buf = b;
  return 0;
}

int matrix::deserialize( char *buf )
{
  memcpy( &col, buf, sizeof(int) );
  memcpy( &row, buf+sizeof(int), sizeof(int) );
  init(row, col);
  memcpy( d, buf + (sizeof(int)*2), sizeof(double)* row*col );
  return 0;
}

int matrix::transposemult( const matrix &m )
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );  
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( &lock );  
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
#endif
#ifndef __FAST_MATRIX__
  if( col != m.col )
    {
      std::cout << "matrix multiply size error\n";
      exit(0);
    }
#endif

  double *myd;
  myd = new double [row*m.row];

  for(int i = 0; i < row; i++)
    {
      for(int j = 0; j < m.row; j++)
        {
          myd[i*m.row+j] = 0;
          for(int k = 0; k < m.col; k++)
            {
              myd[i*m.row+j] = myd[i*m.row+j] + d[i*col+k] * m.d[j*m.col+k];
            }
        }
    }

  delete [] d;
  d = myd;
  col = row;
  row = m.row;
  detflag = 0;

#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return 0;
}

int matrix::transposemult( const matrix &a, const matrix &b )
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&a) )
    {
      if( (unsigned long int)(&b) < (unsigned long int)this )
        {
          pthread_mutex_lock( (pthread_mutex_t*)&(b.lock) );
          pthread_mutex_lock( &lock );  
          pthread_mutex_lock( (pthread_mutex_t*)&(a.lock) );
        }
      else if( (unsigned long int)(&a) < (unsigned long int)(&b))
        {
          pthread_mutex_lock( &lock );  
          pthread_mutex_lock( (pthread_mutex_t*)&(a.lock) );
          pthread_mutex_lock( (pthread_mutex_t*)&(b.lock) );
        }
      else
        {
          pthread_mutex_lock( &lock );
          pthread_mutex_lock( (pthread_mutex_t*)&(b.lock) );
          pthread_mutex_lock( (pthread_mutex_t*)&(a.lock) );
        }
    }
#endif
#ifndef __FAST_MATRIX__
  if( a.cols() != b.cols() )
    {
      std::cout << "matrix multiply size error\n";
      exit(0);
    }
#endif

  init_nolock(a.row, b.row);
  int brs;
  brs = b.row;
  for(int i = 0; i < row; i++)
    {
      for(int j = 0; j < col; j++)
        {
          d[i*col+j] = 0;
          for(int k = 0; k < b.col; k++)
            {
              d[i*col+j] = d[i*col+j] + a.d[i*a.col+k] * b.d[j*b.col+k];
            }
        }
    }

#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
  pthread_mutex_unlock( (pthread_mutex_t*)&(b.lock) );
  pthread_mutex_unlock( (pthread_mutex_t*)&(a.lock) );
#endif
  return 0;
}

int matrix::del()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  if(d != NULL)
    delete [] d;
  d = NULL;
  row = col = -1;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

matrix::matrix(int r, int c)
{
#ifdef __THREADING__
  pthread_mutex_init( &lock, NULL );
#endif
  d = new double [r*c];
  row = r;
  col = c;
  detflag = 0;
}

int matrix::initrand( int r, int c )
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(r, c);
  for(int i = 0; i < r; i++)
    for(int j = 0; j < c; j++)
      d[i*c+j] = random()/100;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int matrix::init(int r, int c)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(r, c);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int matrix::init_nolock(int r, int c)
{
  if(d == NULL)
    {
      d = new double [r*c];
    }
  else
    {
      //if there is not enough memory declare some more
      if(row*col < r*c)
        {
          delete [] d;
          d = new double[r*c];
        }
    }

  row = r;
  col = c;
  detflag = 0;
  return 0;
}

int matrix::initidentity(double v, int s)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(s,s);
  for(int i = 0; i < s; i++)
    {
      for(int j = 0; j < s; j++)
        {
          if(i == j)
            d[i*col+j] = v;
          else
            d[i*col+j] = 0;
        }
    }
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 0;
}

int matrix::init(double *md, int r)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(md, r);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int matrix::init_nolock(double *md, int r)
{

  if(d == NULL)
    {
      d = new double [r];
    }
  else
    {
      //if there is not enough memory declare some more
      if(row*col < r)
        {
          delete [] d;
          d = new double[r];
        }
    }

  row = r;
  col = 1;
  detflag = 0;

  for(int i = 0; i < r; i++)
    d[i*col] = md[i];
  detflag = 0;
  return 0;
}

int matrix::init(double **md, int r, int c)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(md, r, c);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int matrix::init_nolock(double **md, int r, int c)
{
  if(md != NULL)
    {
      if(d == NULL)
        {
          d = new double [r*c];
        }
      else
        {
          //if there is not enough memory declare some more
          if(row*col < r*c)
            {
              delete [] d;
              d = new double[r*c];
            }
        }
      
      row = r;
      col = c;
      detflag = 0;

      for(int i = 0; i < r; i++)
        for(int j = 0; j < c; j++)
          d[i*col+j] = md[i][j];
    }
  else
    {
      d = NULL;
      row = col = -1;
    }
  detflag = 0;
  return 0;
}

int matrix::init(double *md, int r, int c)
{

#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  init_nolock(md, r, c);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

int matrix::init_nolock(double *md, int r, int c)
{
  if(md != NULL)
    {

      if(d == NULL)
        {
          d = new double [r*c];
        }
      else
        {
          //if there is not enough memory declare some more
          if(row*col < r*c)
            {
              delete [] d;
              d = new double[r*c];
            }
        }
      
      row = r;
      col = c;
      detflag = 0;

      memcpy(d, md, sizeof(double)*r*c);
    }
  else
    {
      d = NULL;
      row = col = -1;
    }
  detflag = 0; 
  return 0;
}

matrix& matrix::operator=(const matrix &m)
{
  if(this == &m)
    return *this;
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif
  init_nolock(m.d, m.row, m.col);

  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
  pthread_mutex_unlock( &lock );
#endif
  return *this;
}

matrix matrix::transpose()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  matrix c(col, row);
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      c.d[j*row+i] = d[i*col+j];
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return c;
}

int matrix::transpose( matrix &c ) const
{
#ifdef __THREADING__
  pthread_mutex_lock( (pthread_mutex_t*)&lock );
#endif
  transpose_nolock( c );
#ifdef __THREADING__
  pthread_mutex_unlock( (pthread_mutex_t*)&lock );
#endif
  return 0;
}

int matrix::transpose_nolock( matrix &c ) const
{
  c.init(col, row);
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      c.d[j*row+i] = d[i*col+j];
  return 0;
}

int matrix::cholesky(matrix &c)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&c) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(c.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(c.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif
#ifdef __DEBUG_THREAD_1__
  std::cout << "cholesky locks acquired calculating det..\n";
#endif
  if(determinant_nolock() < 0)
    {
#ifdef __DEBUG_THREAD_1__
      std::cout << "ERROR non positive definite\n";
      std::cout << *this;
#endif

#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
      pthread_mutex_unlock( (pthread_mutex_t*)&(c.lock) );
#endif
      return -1;
    }
#ifdef __DEBUG_THREAD_1__
  std::cout << "det finished init new memory\n";
#endif


  c.init_nolock(row, col);
  c.zero_nolock();
#ifdef __DEBUG_THREAD_1__
  std::cout << "memory init done\n";
#endif
  /*
  timer mytimer;
  mytimer.reset();
  mytimer.start();
  */
  for(int i = 0; i < col; i++)
    {
      for(int j = 0; j < i; j++)
        {
          double s;
          s = 0;
          for(int k = 0; k < j; k++)
            s = s + c.d[i*col+k] * c.d[j*col+k];
          c.d[i*col+j] = 1/(c.d[j*col+j]) * (d[i*col+j] - s);
        }
      c.d[i*col+i] = d[i*col+i];
      for(int k = 0; k < i; k++)
        c.d[i*col+i] = c.d[i*col+i] - pow(c.d[i*col+k], 2);
      c.d[i*col+i] = sqrt(c.d[i*col+i]);
    }
  /*
  mytimer.stop();
  
  std::cout << "cholesky of " << row << "x" << col 
            << " matrix  finished in " << mytimer.getseconds() << "\n" << std::flush;
  */

  //transpose the results so it will be upper triangle
  for(int i = 0; i < row; i++)
    {
      for(int j = i+1; j < row; j++)
        {
          double t;
          t = c.d[i*col+j];
          c.d[i*col+j] = c.d[j*col+i];
          c.d[j*col+i] = t;
          if( isnan( c.d[j*col+i] ) || isnan(c.d[i*col+j]) ||
              isinf( c.d[j*col+i] ) || isinf(c.d[i*col+j]) )
            return -1;
        }
    }
#ifdef __DEBUG_THREAD_1__
  std::cout << "calc copy done\n";
#endif

#ifdef __DEBUG_MATRIX_2__
  matrix p;
  matrix t;
  std::cout << "checking...\n" << std::flush;
  p = c.transpose();
  std::cout << "check transponse finished\n" 
            << std::flush;
  t = p*c;
  double ss = 0;
  for(int i = 0; i < row; i++)
    for(int j = 0; j < row; j++)
      ss = ss + pow(t.d[i*col+j] - d[i*col+j], 2);

  if(ss > 0.0000001)
    {
      std::cout << "MATRIX cholesky error\n";
      exit(0);
    }
#endif

#ifdef __THREADING__
  pthread_mutex_unlock( (pthread_mutex_t*)&lock );
  pthread_mutex_unlock( (pthread_mutex_t*)&(c.lock) );
#endif

#ifdef __DEBUG_THREAD_1__
  std::cout << "unlocked\n";
#endif
  return 0;
}

const matrix matrix::operator*(const matrix &m)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif

#ifndef __FAST_MATRIX__
  if(col != m.row)
    {
      std::cout << "ERROR matrix size on multiply\n";
      std::cout << row << "x" << col << " * ";
      std::cout << m.row << "x" << m.col << "\n";

      exit(0);
    }
#endif

  matrix c(row, m.col);
  if( m.col*m.row < 100 )
    {
      for(int i = 0; i < row; i++)
        {
          for(int j = 0; j < m.col; j++)
            {
              c.d[i*m.col+j] = 0;
              for(int k = 0; k < col; k++)
                {
                  c.d[i*m.col+j] = c.d[i*m.col+j] + d[i*col+k] * m.d[k*m.col+j];
                }
            }
        }
    }
  else
    {
      double *tm;
      tm = new double [m.col*m.row];
      for(int i = 0; i < m.row; i++)
        for(int j = 0; j < m.col; j++)
          tm[j*m.row+i] = m.d[i*m.col+j];
      
      for(int i = 0; i < row*m.col; i++)
        c.d[i] = 0;
      
      int msize;
      msize = m.col*m.row*sizeof(double);
      int nblocks;
      nblocks = msize/125000;
      
      if( nblocks == 0 )
        nblocks = 1;
      
      int cpblock;
      cpblock = (int)ceil( ((double)m.col) / ((double)nblocks) );
      
      for( int block = 0; block < nblocks; block++)
        {
          int s, e;
          s = block*cpblock;
          e = (block+1)*cpblock;
          if( m.col < e )
            e = m.col;
          for(int i = 0; i < row; i++)
            {
              for(int j = s; j < e; j++)
                {
                  for(int k = 0; k < col; k++)
                    {
                      c.d[i*m.col+j] = c.d[i*m.col+j] + d[i*col+k] * tm[j*m.row+k];
                    }
                }
            }
        }

      delete [] tm;
    }
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return c;
}

int matrix::multiply( std::vector< matrix *> &ml )
{
  matrix tmp, tmp1;
  matrix *om, *nm, *tm;
  double *buf;
  buf = NULL;
  int bufsize;
  bufsize = 0;
  if( ml.size() == 2 )
    multiply( ml[0], ml[1], this, &buf, bufsize );
  else
    {
      multiply( ml[0], ml[1], &tmp, &buf, bufsize );
      om = &tmp;
      nm = &tmp1;
      for( unsigned i = 2; i < ml.size(); i++ )
        {
          multiply( om, ml[i], nm, &buf, bufsize );
          tm = om;
          om = nm;
          nm = tm;
        }
      del();
      d = om->d;
      row = om->row;
      col = om->col;
      om->d = NULL;
      om->row = om->col = -1;
    }
  if( buf != NULL )
    delete [] buf;
  return 0;
}

int matrix::multiply( matrix *a, matrix *b, matrix *c, double **buffer, int &bufsize )
{
#ifndef __FAST_MATRIX__
  if(a->col != b->row)
    {
      std::cout << "ERROR matrix size on multiply\n";
      std::cout << a->row << "x" << a->col << " * ";
      std::cout << b->row << "x" << b->col << "\n";

      exit(0);
    }
#endif

  c->init(a->row, b->col);
  double *tm;
  if( buffer == NULL )
    {
      tm = new double [b->col*b->row];
      std::cout << "creating buffer 1\n";
    }
  else if( *buffer == NULL )
    {
      *buffer = new double [b->col*b->row];
      tm = *buffer;
      bufsize = b->col*b->row;
      std::cout << "creating buffer 2\n";
    }
  else if( bufsize < b->col*b->row )
    {
      delete [] *buffer;
      *buffer = new double [b->col*b->row];
      tm = *buffer;
      bufsize = b->col*b->row;
      std::cout << "creating buffer 3\n";
    }
  else
    tm = *buffer;

  for(int i = 0; i < b->row; i++)
    for(int j = 0; j < b->col; j++)
      tm[j*b->row+i] = b->d[i*b->col+j];

  for(int i = 0; i < a->row*b->col; i++)
    c->d[i] = 0;

  int msize;
  msize = b->col*b->row*sizeof(double);
  int nblocks;
  nblocks = msize/125000;
  
  if( nblocks == 0 )
    nblocks = 1;
  std::cout << "using " << nblocks << " blocks\n";
  int cpblock;
  cpblock = (int)ceil( ((double)b->col) / ((double)nblocks) );

  for( int block = 0; block < nblocks; block++)
    {
      int s, e;
      s = block*cpblock;
      e = (block+1)*cpblock;
      if( b->col < e )
        e = b->col;
      for(int i = 0; i < a->row; i++)
        for(int j = s; j < e; j++)
          for(int k = 0; k < a->col; k++)
            c->d[i*b->col+j] = c->d[i*b->col+j] + a->d[i*a->col+k] * tm[j*b->row+k];
    }

  if( buffer == NULL )
    delete [] tm;

  return 0;
}

const matrix matrix::operator*(const double m)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );  
#endif
  matrix n(row, col);
  int t = row*col;
  for(int i = 0; i < t; i++)
      n.d[i] = d[i] * m;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
#endif
  return n;
}

const matrix matrix::operator/(const double m)
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );  
#endif
  matrix n(row, col);
  int t = row*col;
  for(int i = 0; i < t; i++)
      n.d[i] = d[i] / m;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
#endif
  return n;
}

const matrix matrix::operator+(const matrix &m)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif

#ifndef __FAST_MATRIX__
  if(m.row != row || m.col != col)
    {
      std::cout << "ERROR matrix size on add\n";
      std::cout << m.row << " " << row << ":" << m.col << " " << col << "\n";
      exit(0);
    }
#endif

  matrix c(row, col);
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      c.d[i*col+j] = d[i*col+j] + m.d[i*col+j];
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return c;
}

const matrix matrix::operator-(const matrix &m)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif

#ifndef __FAST_MATRIX__
  if(m.row != row || m.col != col)
    {
      std::cout << "ERROR matrix size on add\n";
      exit(0);
    }
#endif

  matrix c(row, col);
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      c.d[i*col+j] = d[i*col+j] - m.d[i*col+j];
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return c;
}

int matrix::add(const matrix &m)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif

#ifndef __FAST_MATRIX__
  if(m.row != row || m.col != col)
    {
      std::cout << "ERROR matrix size on add\n";
      std::cout << m.row << " " << row << ":" << m.col << " " << col << "\n";
      exit(0);
    }
#endif

  int t = row*col;
  for(int i = 0; i < t; i++)
      d[i] = d[i] + m.d[i];

  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return 0;
}

int matrix::addmult(const matrix &m, const double c)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif

#ifndef __FAST_MATRIX__
  if(m.row != row || m.col != col)
    {
      std::cout << "ERROR matrix size on add\n";
      std::cout << m.row << " " << m.col << ":" << m.row << " " << m.col << "\n";
      exit(0);
    }
#endif

  int t = row*col;
  for(int i = 0; i < t; i++)
      d[i] = d[i] + m.d[i] * c;

  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return 0;
}

int matrix::divide(double m)
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
  int t = row*col;
  for(int i = 0; i < t; i++)
    d[i] = d[i] / m;
  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 0;
}

int matrix::addtranspose(matrix &m)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      d[i*col+j] = d[i*col+j] + m.d[i]*m.d[j];
  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
#endif
  return 0;
}

int matrix::addtransposemult(const matrix &m, const double c)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      d[i*col+j] = d[i*col+j] + m.d[i]*m.d[j]*c;
  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
  pthread_mutex_unlock( &(lock) );
#endif
  return 0;
}

int matrix::diag( matrix &c)
{
  c.initidentity(1, col );
  return 0;
}

matrix matrix::invertU()
{
  // assert that matrix is lower triangle
  for( int i = 0; i < row; i++ )
    {
      for( int j = 0; j < i; j++ )
        {
          if( fabs( d[i*col+j] ) > 0.000000001 )
            {
              std::cout << "invertL assert error\n";
              std::cout << *this << "\n";
              exit(0);
            }
        }
    }
  matrix c;
  c.initidentity( 1, row );
  double *dd;
  dd = new double[ row*col ];
  memcpy( dd, d, sizeof(double)*col*row );

  for( int i = 0; i < row; i++ )
    {
      c.d[i*col+i] = c.d[i*col+i] / dd[i*col+i];
      for( int j = i+1; j < row; j++ )
        dd[i*col+j] = dd[i*col+j]/dd[i*col+i];
      dd[i*col+i] = 1;
    }
  for( int r = row-1; r > 0; r-- )
    {
      for( int i = r+1; i < row; i++ )
        {
          c.d[i*col+r] = -dd[i*col+r];
        }
    }

  delete [] dd;

#ifdef __DEBUG_MATRIX__
  matrix ttt;
  std::cout << "testing inverted matrix\n";
  ttt = (*this) * c;
  if( !ttt.isidentity())
    {
      std::cout << "MATRIX INVERSION ERROR\n";
      std::cout << c << "\n\n";
      std::cout << *this << "\n";
      exit(0);
    }
  std::cout << "invert passed\n";
#endif

  return c;
}

matrix matrix::invertL()
{
  // assert that matrix is lower triangle
  for( int i = 0; i < row; i++ )
    {
      for( int j = i+1; j < col; j++ )
        {
          if( fabs( d[i*col+j] ) > 0.000000001 )
            {
              std::cout << "invertL assert error\n";
              std::cout << *this << "\n";
              exit(0);
            }
        }
    }
  matrix c;
  c.initidentity( 1, row );
  double *dd;
  dd = new double[ row*col ];
  memcpy( dd, d, sizeof(double)*col*row );

  for( int i = 0; i < row; i++ )
    {
      c.d[i*col+i] = c.d[i*col+i] / dd[i*col+i];
      for( int j = 0; j < i; j++ )
        dd[i*col+j] = dd[i*col+j]/dd[i*col+i];
      dd[i*col+i] = 1;
    }
  for( int r = 0; r < row; r++ )
    {
      for( int i = r+1; i < row; i++ )
        {
          c.d[i*col+r] = -dd[i*col+r];
        }
    }

  delete [] dd;

#ifdef __DEBUG_MATRIX__
  matrix ttt;
  std::cout << "testing inverted matrix\n";
  ttt = (*this) * c;
  if( !ttt.isidentity())
    {
      std::cout << "MATRIX INVERSION ERROR\n";
      std::cout << c << "\n\n";
      std::cout << *this << "\n";
      exit(0);
    }
  std::cout << "invert passed\n";
#endif

  return c;
}

matrix matrix::invert()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif

  matrix c;
  double **nm;
  nm = new double *[row];
#ifdef __DEBUG_THREAD_1__
  std::cout << "inverting " << row << "x" << col << " matrix\n";
#endif
  for(int i = 0; i < row; i++)
    {
      nm[i] = new double [2*col];
      for(int j = 0; j < col; j++)
        {
          nm[i][j] = d[i*col+j];
          nm[i][j+col] = 0;
        }
    }
  for(int i = 0; i < col; i++)
    nm[i][i+col] = 1;

  for(int i = 0; i < row; i++)
    pivot(nm, i, i, row, col * 2);

  c.init_nolock(row, col);

  ///copy the matrix back
  for(int i = 0; i < row; i++)
    for(int j = 0; j < col; j++)
      c.d[i*col+j] = nm[i][j+col];

#ifdef __DEBUG_MATRIX__
  matrix ttt;
  ttt = (*this) * c;
  if( !ttt.isidentity())
    {
      std::cout << "MATRIX INVERSION ERROR\n";
      std::cout << c << "\n\n";
      std::cout << *this << "\n";
      exit(0);
    }
#endif

  for(int i = 0; i < row; i++)
    delete [] nm[i];
  delete [] nm;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return c;
}

///pivot the matrix
int matrix::pivot(double **dd, int pi, int pj, int sr, int sc) 
{
  double pv = dd[pi][pj];
  for(int i = 0; i < sc; i++)
    dd[pi][i] = dd[pi][i] / pv;

  for(int i = 0; i < sr; i++)
    {
      if(i != pi)
        {
          pv = dd[i][pj];
          for(int j = 0; j < sc; j++)
            {
              dd[i][j] = dd[i][j] - dd[pi][j]*pv;
            }
        }
    }
  return 0;
}

int matrix::rows() const
{
  return row;
}


int matrix::cols() const
{
  return col;
}

int matrix::set(double v, int r, int c)
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
#ifdef __DEBUG_MATRIX__
  if(r < 0 || r >= row || c < 0 || c >= col)
    {
      std::cout << "ERROR MATRIX SIZE on set\n";
      std::cout << r << " " << c << "\n"
                << row << " " << col << "\n";
      exit(0);
    }
#endif

  d[r*col+c] = v;
  detflag = 0;
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 0;
}

int matrix::initminor_nolock(matrix &m, int i, int j)
{
  init_nolock(m.row-1, m.col-1);
      
  int ai,aj,ak,al;
  ai = aj = ak = al = 0;
  while(ai < row)
    {
      aj = al = 0;
      if(ak == i)
        ak++;
      while(aj < col)
        {
          if(al == j)
            al++;
          d[ai*col+aj] = m.d[ak*m.col+al];
          al++;
          aj++;
        }
      ai++;
      ak++;
    }

  return 0;
}

int matrix::initminor(matrix &m, int i, int j)
{
#ifdef __THREADING__
  if( (unsigned long int)this < (unsigned long int)(&m) )
    {
      pthread_mutex_lock( &lock );
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );
    }
  else
    {
      pthread_mutex_lock( (pthread_mutex_t*)&(m.lock) );  
      pthread_mutex_lock( &lock );
    }
#endif
  initminor_nolock( m, i, j);
#ifdef __THREADING__
  pthread_mutex_unlock( (pthread_mutex_t*)&(m.lock) );
  pthread_mutex_unlock( &lock );
#endif
  return 0;
}

double matrix::determinant_nolock()
{
  //  std::cout << "started det calc\n";
  if(detflag == 1)
    {
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
      return cdeterminant;
    }

#ifndef __FAST_MATRIX__
  if(row != col)
    {
      std::cout << "ERROR det matrix\n";
      exit(0);
    }
#endif

  //  std::cout << "calculating determinate ";
  if(row == 2 && col == 2)
    return d[0]*d[3] - d[1]*d[2];
  double res;
  res = 0;
  matrix m;
  for(int i = 0; i < col; i++)
    {
      m.initminor_nolock( *this, 0, i );
      res = res + d[i] * pow(-1, i) * m.determinant_nolock();
    }
  detflag = 1;
  cdeterminant = res;
  //  std::cout << res << "\n";
 
  return res;
}

double matrix::determinant()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );
#endif
  if(detflag == 1)
    {
#ifdef __THREADING__
      pthread_mutex_unlock( &lock );
#endif
      return cdeterminant;
    }

#ifndef __FAST_MATRIX__
  if(row != col)
    {
      std::cout << "ERROR det matrix\n";
      exit(0);
    }
#endif

  double res;
  res = determinant_nolock();
#ifdef __THREADING__
  pthread_mutex_unlock( &lock );
#endif
  return res;
}

int matrix::zero()
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
  zero_nolock();
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 0;
}

int matrix::zero_nolock()
{
  int rc = row*col;
  for(int i = 0; i < rc; i++)
      d[i] = 0;
  detflag = 0;
  return 0;
}

int matrix::isidentity()
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
  if(row != col)
    {
#ifdef __THREADING__
      pthread_mutex_unlock(&lock);
#endif
      return 0;
    }
  for(int i = 0; i < row; i++)
    {
      if(fabs(d[i*col+i] - 1) > 0.00000001)
        {
#ifdef __THREADING__
          pthread_mutex_unlock(&lock);
#endif
          return 0;
        }
    }
  for(int i = 0; i < row; i++)
    {
      for(int j = 0; j < col; j++)
        {
          if( i != j)
            {
              if(fabs(d[i*col+j]) > 0.00000001)
                {
#ifdef __THREADING__
                  pthread_mutex_unlock(&lock);
#endif
                  return 0;
                }
            }
        }
    }
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return 1;
}

double matrix::diagsum()
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
  if(row != col)
    {
#ifdef __THREADING__
      pthread_mutex_unlock(&lock);
#endif
      return 0;
    }
  double s;
  s = 0;
  for(int i = 0; i < col; i++)
    s = s + d[i*col+i];
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return s;
}

double matrix::logdiagsum()
{
#ifdef __THREADING__
  pthread_mutex_lock(&lock);
#endif
  if(row != col)
    {
#ifdef __THREADING__
      pthread_mutex_unlock(&lock);
#endif
      return 0;
    }
  double s;
  s = 0;
  for(int i = 0; i < col; i++)
    s = s + log(d[i*col+i]);
#ifdef __THREADING__
  pthread_mutex_unlock(&lock);
#endif
  return s;
}

double matrix::get(int r, int c) const
{
  double res;
#ifdef __DEBUG_THREAD_1__
  std::cout << "getting from matrix: " << r << "," << c << "\n";
#endif
#ifdef __THREADING__
  pthread_mutex_lock( ( pthread_mutex_t* )(&lock) );
#endif

#ifdef __DEBUG_MATRIX__
  if( r < 0 || r >= row ||
      c < 0 || c >= col )
    {
      std::cout << "ERROR matrix get..\n";
      std::cout << r << " " << c << "\n"
                << row << " " << col << "\n";
      exit(0);
    }
#endif

  res = d[r*col+c];
#ifdef __THREADING__
  pthread_mutex_unlock( ( pthread_mutex_t* )(&lock) );
#endif
#ifdef __DEBUG_THREAD_1__
  std::cout << "get matrix unlocked\n";
#endif
  return res;
}

double matrix::sum()
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );  
#endif
  double s = 0;
  double t = row*col;
  for(int i = 0; i < t; i++)
    s = s + fabs(d[i]);
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
#endif
  return s;
}

double matrix::maxdiag()
{
  double max;
  max = d[0];
  for( int i = 1; i < row; i++ )
    {
      if( d[i*col+i] > max )
        max = d[i*col+i];
    }
  return max;
}

int matrix::multiply( double m )
{
#ifdef __THREADING__
  pthread_mutex_lock( &lock );  
#endif
  int t = row*col;
  for(int i = 0; i < t; i++)
      d[i] = d[i] * m;
#ifdef __THREADING__
  pthread_mutex_unlock( &lock);
#endif
  return 0;
}



std::ostream& operator <<(std::ostream &o, const matrix &m)
{

  for(int i = 0; i < m.rows(); i++)
    {
      for(int j = 0; j < m.cols(); j ++)
        {
          o << m.get(i, j) << " ";
        }
      o << "\n";
    }

  return o;
}

