/****************************************************************************
MATRIX.C
Written in 2008-2009 by M. V. Iordache.
****************************************************************************/

#include "pns.h"

MatrElem* FindEl(const matrix* m, int i, int j, MatrElem** next, int dir) {
  /* THE FUNCTION TAKES IN ACCOUNT THE trans FIELD OF m!
     It finds a pointer to the element (i,j) if it has a MatrElem object in the
     matrix. Otherwise it returns zero. The search is done in the direction
     indicated by 'dir', which can be one of mDOWN, mUP, mRIGHT, or mLEFT. 
     The function writes to '*next' the element next to the (i,j) position in
     the direction indicated by 'dir'. If no other element is present, *next
     is made zero.  
  */

  int ii, jj, k = -1; /* k must be initialized to a value less than 0 */
  MatrElem* p;

  if(m->type != 3 || m->nc <= 0 || m->nr <= 0) {
    merror(0, "FindEl: The operation is not implemented.");
    return 0;
  }

  dir = dir^m->trans; /* xor with m->trans to correct direction */

  if(m->trans) {
    ii = j; jj = i;
  }
  else {
    ii = i; jj = j;
  }

  if(!(dir&1)) {/* that is, if(dir == mRIGHT || dir == mLEFT) */
    p = m->row[ii]; 
    if(p) {
      for( ; (k < p->column) && (jj > p->column); p = p->next[mRIGHT])
	k = p->column;

      if(p->column == jj) {
	*next = p->next[dir];
	return p;
      }
      if(dir == mLEFT) { /* else *next will be set to p in a subsequent line */
	*next = p->next[mLEFT];
	return 0;
      }
    }
  }
  else {/* that is, if(dir == mDOWN || dir == mUP) */
    p = m->column[jj]; 
    if(p) {
      for( ; (k < p->row) && (ii > p->row); p = p->next[mDOWN])
	k = p->row;

      if(p->row == ii) {
	*next = p->next[dir];
	return p;
      }
      if(dir == mUP) { /* else *next will be set to p in a subsequent line */
	*next = p->next[mUP];
	return 0;
      }
    }
  }
  *next = p;
  return 0;
}

inline mtype _matrel3(const matrix* m, int i, int j) {
  MatrElem *p, *next;

  p = FindEl(m, i, j, &next, mRIGHT);
  if(!p)
    return 0;
  return p->value;
}

mtype GetMatrixEl(const matrix* m, int i, int j) {
  switch(m->type) {
  case 1:
    return m->trans?m->elem[j*m->nc+i]:m->elem[i*m->nc+j];
  case 2:
    return m->trans?m->ar[j][i]:m->ar[i][j];
  case 3:
    return _matrel3(m, i, j);
  default:
    merror(0, "MATRIX: The operation is not implemented.");
  }
  return 0;
}



inline void _setmatrel3(const matrix* m, int i, int j, mtype x) {
  int ii, jj;
  int dir;
  MatrElem *p, *next, *next2;

  /* If an already allocated element is set to zero it is not deleted 
    (deallocated) in this version of the function. */

  p = FindEl(m, i, j, &next, mRIGHT); /* signals error if m->nr or m->nc = 0 */
  if(p) { /* if the element has been already allocated */
    p->value = x;
    return;
  }

  /* The following lines executed if element (i,j) not allocated in matrix. */

  if(!x) /* nothing to do if the element is not allocated and x = 0 */
    return;

  /* Create and initialize new element of the matrix */

  p = tmalloc(sizeof(*p));
  p->value = x;
  if(m->trans) {/*ii and jj: row and column number in internal representation*/
    ii = j; jj = i;
  }
  else {
    ii = i; jj = j;
  }
  p->column = jj;
  p->row = ii;

  /* Place element in matrix */

  if(!next) {
    if(m->trans){/*if transposed, search to mRIGHT in FindEl is to mDOWN in m*/
      m->column[jj] = p;
      p->next[mDOWN] = p;
      p->next[mUP] = p;
    }
    else {
      m->row[ii] = p;
      p->next[mRIGHT] = p;
      p->next[mLEFT] = p;
    }
  }
  else {
    dir = mRIGHT^(m->trans);/*next obtained by a search in the mRIGHT direction*/
    p->next[dir] = next; /*dir = mDOWN or mRIGHT depending on trans = 1 or 0 */
    p->next[dir^2] = next->next[dir^2]; /* dir^2 = mUP or mLEFT */
    next->next[dir^2]->next[dir] = p;
    next->next[dir^2] = p;
  }

  FindEl(m, i, j, &next2, mDOWN);
  if(!next2) {
    if(m->trans){/*if transposed, search to mDOWN in FindEl is to mRIGHT in m*/
      m->row[ii] = p;
      p->next[mRIGHT] = p;
      p->next[mLEFT] = p;
    }
    else {
      m->column[jj] = p;
      p->next[mDOWN] = p;
      p->next[mUP] = p;
    }
  }
  else {
    dir = mDOWN^(m->trans);/*next2 obtained by a search in the DOWN direction*/
    p->next[dir] = next2; /*dir = mDOWN or mRIGHT depending on trans = 1 or 0*/
    p->next[dir^2] = next2->next[dir^2]; /* dir^2 = mUP or mLEFT */
    next2->next[dir^2]->next[dir] = p;
    next2->next[dir^2] = p;
  }

  /* update row and column to point to the first element in chain */
  if(m->row[ii]->column > p->column)
    m->row[ii] = p;
  if(m->column[jj]->row > p->row)
    m->column[jj] = p;
}

void SetMatrixEl(matrix* m, int i, int j, mtype x) {
  switch(m->type) {
  case 1:
    if(m->trans)
      m->elem[j*m->nc+i] = x;
    else
      m->elem[i*m->nc+j] = x;
    break;
  case 2:
    if(m->trans)
      m->ar[j][i] = x;
    else
      m->ar[i][j] = x;
    break;
  case 3:
    _setmatrel3(m, i, j, x);
    break;
  default:
    merror(0, "MATRIX: The operation is not implemented.");
  }
}


inline MatrElem* NextEl(const matrix* m, int dir, int pos, MatrElem* current) {
  /* Finds the next element on the row or column 'pos' (depending on 'dir'). 
     Returns 0 if no other element is in the specified direction. */
  /* This function is for type 3 matrices in which not all elements are 
     allocated in memory. Thus, the function returns the next allocated 
     allocated element. */
  /* If called within a loop, the function will give the right result if 
     the matrix chains are not modified between calls. */
  /* It does not verify that pos and current->row/column are consistent! */
  /* 'dir' is one of mRIGHT, mLEFT, mUP, mDOWN; the next element is searched 
     on the row 'pos' if 'dir' is mRIGHT or mLEFT and else on the column 'pos'.
  */
  /* Here is an example on how this function could be used in a loop:
	 p = 0;
         for(p = NextEl(&(pn1.in),mUP,i,p); p; p = NextEl(&(pn1.in),mUP,i,p)) {
           if(pn1.in.trans) // pn1 is transposed!
             SetMatrixEl(&(pn.in), p->column, i, p->value);
           else
             SetMatrixEl(&(pn.in), p->row, i, p->value);
         }
  */
  MatrElem *p, *pt, **pbase;

  if(m->type != 3) {
    merror(0, "NextEl: The operation is not implemented.");
    return 0;
  }
  dir = dir^m->trans; /* xor with m->trans to correct direction */

  if(!(dir&1)) /* that is, if(dir == mRIGHT || dir == mLEFT) */
    pbase = m->row;
  else  /* that is, if(dir == mUP || dir == mDOWN) */
    pbase = m->column;

  p = pbase[pos];
  if(!current)
    return p;
  if(!p)
    return 0; /* no (allocated) elements on the row, so return 0. */
  pt = current->next[dir];
  if(p == pt)
    return 0; /* all elements of the chain have been visited, so return 0 */
  return pt;
}


void AllocateMatrix(matrix* m, int nr, int nc) {
  if(nr*nc <= 100) 
    AllocateMatrixType(1, m, nr, nc);
  else 
    AllocateMatrixType(2, m, nr, nc);
}


void AllocateMatrixType(int type, matrix* m, int nr, int nc) {
  int i;

  memset(m, 0, sizeof(matrix)); /* Initializes to zero the structure */
  m->nr = nr;
  m->nc = nc;
  m->type = type;

  if(!nr || !nc)
    return;

  switch(type) {
  case 1:
    m->elem = tcalloc(nc*nr, sizeof(mtype));
    return;
  case 2:
    m->ar = tcalloc(nr, sizeof(mtype*));
    for(i = 0; i < nr; i++)
      m->ar[i] = tcalloc(nc, sizeof(mtype));
    return;
  case 3:
    m->row = tcalloc(nr, sizeof(*(m->row)));
    m->column = tcalloc(nc, sizeof(*(m->column)));
    return;
  default:
    merror(0, "MATRIX: The operation is not implemented.");
  }
}



void Copy2ZeroColumn(const matrix* src, int i, matrix* dst, int k, int offset){
  /* Copies the allocated elements of column i of src to the column k of dst. 
     Namely, it does dst[offset + j][k] = src[j][i] for all j such that 
     src[j][i] is allocated. Now, IF the column k of dst is zero, this means
     that we copy the column i of src to the column k of dst. This function
     is intended for type 3 matrices. 

     Example:  When a Petri net object pn1 is copied to a new Petri net object
     pn, we could copy the column i of the input matrix of pn1 as follows: 
     Copy2ZeroColumn(&(pn1.in),i,&(pn.in),i,0);
  */

  MatrElem* p = 0; /* it is essential that p is initialized to zero */
  int j;

  if(src->type == 3) {
    for(p = NextEl(src,mUP,i,p); p; p = NextEl(src,mUP,i,p)) {
      if(src->trans) /*  */
	SetMatrixEl(dst, offset + (p->column), k, p->value);
      else
	SetMatrixEl(dst, offset + (p->row), k, p->value);
    }
  }
  else 
    for(j = 0; j < NumberOfRows(*src); j++) 
      SetMatrixEl(dst, offset + j, k, GetMatrixEl(src,j,i));
}


void MakeZeroColumn(matrix* dst, int col) {
  /* Makes zero the column col of dst. The implementation could be optimized if
     needed. */

  int i, n;
  n = NumberOfRows(*dst);
  for(i = 0; i < n; i++)
    SetMatrixEl(dst, i, col, 0);
}

void MakeZeroRow(matrix* dst, int num) {
  /* Makes zero the row num of dst. The implementation could be optimized if 
     needed. */

  int i, n;
  n = NumberOfColumns(*dst);
  for(i = 0; i < n; i++)
    SetMatrixEl(dst, num, i, 0);
}


void CopyBlock(int nx, int ny, const matrix* src, int xs, int ys,\
               matrix* dst, int xd, int yd) {
  /* Copies a block of nx by ny elements of src to dst. Specifically, it 
     copies the elements (xs+i, ys+j) of src to (xd+i, yd+j) of dst for all
     i = 0...nx-1 and j = 0...ny-1. */
  int i, j;

 /* This is a plain implementation; it could be optimized in the future */

  switch(src->type) {
  case 3: 
    /*    if(dst->type != 3) {
	  for(i = 0; i < nx; i++) {
	  p = NextEl(src, mRIGHT, i+xs, 0);
	  if(!p) { // all elements are zero
          for(j = 0; j < ny; j++)
	  SetMatrixEl(dst, i+xd, j+yd, 0);
	  }
	  ... 
	  for(j = 0; j < ny; j++) {
	  if(src->trans) { // rows actually columns 
	  if(j <= (p->row - yd))
	  }
	  }
	  }
	  } */
  default:
    for(i = 0; i < nx; i++)
      for(j = 0; j < ny; j++)
	SetMatrixEl(dst, i+xd, j+yd, GetMatrixEl(src, i+xs, j+ys)); 
  }
}


void CopyMatrix(const matrix* src, matrix* dst) {
  CopyBlock(NumberOfRows(*src), NumberOfColumns(*src), src, 0, 0, dst, 0, 0);
}


int** matrix2array(matrix mat) {
/* Converts a matrix object to an array of integers: result[i][j] = elem(i,j)*/
  int** res, i, j;
  res = tmalloc(sizeof(int*)*mat.nr);
  for(i = 0; i < mat.nr; i++) {
    res[i] = tmalloc(sizeof(int)*mat.nc);
    for (j = 0; j < mat.nc; j++)
      res[i][j] = GetMatrixEl(&mat, i, j);
  }
  return res;
}


int** vector2array(int* v, int nr, int nc) {
  int** res, i, j;
  res = tmalloc(sizeof(int*)*nr);
  for(i = 0; i < nr; i++) {
    res[i] = tmalloc(sizeof(int)*nc);
    for (j = 0; j < nc; j++)
      res[i][j] = v[i*nc + j];
  }
  return res;
}


matrix array2matrix(int** ar, int nr, int nc) {
  matrix m;
  int i, j, k;

  AllocateMatrix(&m, nr, nc);
  for(i = 0; i < nr; i++)
    for(j = 0; j < nc; j++)
      SetMatrixEl(&m, i, j, ar[i][j]);

  return m;
}

matrix vector2matrix(int* v, int nr, int nc) {
  matrix m;
  int i, j, k;

  AllocateMatrix(&m, nr, nc);
  for(i = 0, k = 0; i < nr; i++)
    for(j = 0; j < nc; j++, k++)
      SetMatrixEl(&m, i, j, v[k]);

  return m;
}



int findIndices(void* vect, int n, int esize, int** answer ) {
/* Finds the indices of the nonzero elements of 'vect'. Here, 'vect' is a 
   vector of 'n' elements. Each element of 'vect' has the size 'esize'. The
   return value is the number of nonzero indices. Further, the indices are
   stored in 'answer'. Note that *answer is a pointer to memory allocated 
   in this function. Memory is always allocated, even if there are no indices. 

   This function is similar to the find function of Matlab. */

  int count, i, j;
  void* p; 

  *answer = tcalloc(n, sizeof(int));

  if(sizeof(char) != 1)
    merror(0, "findIndices has to be rewritten!");

  for(count = 0, i = 0, p = vect; i < n; i++, p+= esize) 
    for(j = 0; j < esize; j++) 
      if( *(char*)( p + j ) ) {
	(*answer)[count++] = i;
	break;
      }

  return count;
}



inline int TestCondition(int v, int type, int w) {
/* Checks whether 'v' and 'w' (in this order) are in the relationship 
   described by 'type', where 'type' may have one of the values in the 
   range GREATER, ..., LESS. If the relationship is satisfied, the 
   function returns 1. Else, it returns 0. */

  int a, b;

  if(type < 0) {
    type = -type;
    a = v;
    b = w;
  }
  else {
    a = w;
    b = v;
  }
  switch(type) {
  case GREATER:
    if(a >= b)
      return 0;
    break;
  case GREATER_OR_EQUAL:
    if(a > b)
      return 0;
    break;
  case EQUAL:
    if(a != b)
      return 0;
    break;
  default:
    return 0;
  }
  return 1;
}

int TestExpression(const int* v, int c, const int* w, int len) {
/*  The vectors 'v' and 'w' are compared for the first 'len' elements.
    'c' may have one of the values in the range GREATER, ..., LESS. 
    The return value is as follows:
    * If 'len' <= 0 or v = 0 or w = 0 the function returns a nonzero value;
    * Else if 'c' is none of the above, the function returns zero;
    * Else if the expression is satisfied for each pair of elements v[i] and 
      w[i], the function returns a nonzero value;
    * Else the function returns zero. */

  const int *a, *b;
  int i;

  if(!v || !w || len <= 0)
    return 1;
  if(c < 0) {
    a = v;
    b = w;
    c = -c;
  }
  else {
    a = w;
    b = v;
  }
  switch(c) {
  case GREATER:
    for(i = 0; i < len; i++)
      if(a[i] >= b[i])
	return 0;
    break;
  case GREATER_OR_EQUAL:
    for(i = 0; i < len; i++)
      if(a[i] > b[i])
	return 0;
    break;
  case EQUAL:
    for(i = 0; i < len; i++)
      if(a[i] != b[i])
	return 0;
    break;
  default:
    return 0;
  }
  return 1;
}


void free2Darray(int** ar, int nr) {
/* Used to deallocate an array created with matrix2array or vector2array; nr is the number of rows */

  int i;
  for(i = 0; i < nr; i++)
    free(ar[i]);
  free(ar);
}


void inline _delmatr3(matrix* m) {
  int i;
  MatrElem* p, *pt;
  for (i = 0; i < (m->nr); i++) {
    p = m->row[i];
    if(p) {
      p->next[mLEFT]->next[mRIGHT] = 0;
      while(p) {
	pt = p->next[mRIGHT];
	free(p);
	p = pt;
      }
    }
  }
  free(m->row);
  free(m->column);
}

void DeallocateMatrix(matrix* m) {
  switch(m->type) {
  case 1:
    if(m->nr && m->nc && m->elem)
      free(m->elem);
    break;
  case 2:
    if(m->nr && m->nc)
      free2Darray((int**) (m->ar), (int) m->nr);
    break;
  case 3:
    if(m->nr && m->nc) {
      _delmatr3(m);
    }
    break;
  default:
    merror(0, "MATRIX: The operation is not implemented.");
  }
  memset(m, 0, sizeof(*m));
}

inline void InvertSignOfMatrix(matrix* m) {
  int i, j, r, n;
  if(!m) return;
  r = NumberOfRows(*m);
  n = NumberOfColumns(*m);
  for(i = 0; i < r; i++)
    for(j = 0; j < n; j++)
      SetMatrixEl(m, i, j, -GetMatrixEl(m, i, j)); 
}

inline void InvertSignOfVector(int* v, int len) {
  int i;
  if(!v) return;
  for(i = 0; i < len; i++)
    v[i] = -v[i];
}

/* To do:
    - _setmatrel3 does not deallocate elements set to zero
    - consider better matrix representations. For instance, for type 3 
      matrices, consider better representations of row and column than
      int*. The improvements are to be intended for a more effient 
      representation of very large matrices.
    - CopyBlock could be optimized for type 3 matrices.  
*/
