

static char *rcsid = "matrix.c";


#include "matrix.h"

#include <stdio.h>
#include <math.h>


#ifdef   MATRIX_SAVE_FREE
static matrix saved_matrices[MATRIX_SAVE_FREE+1][MATRIX_SAVE_FREE+1];
static int initialize_saved_matrices = 1;
#endif   

static void matSwap(matrix a, matrix b)
{
  int w, h;
  double **mat;

  assert(a != matNULL);
  assert(b != matNULL);

  w = a->w;      a->w = b->w;     b->w = w;
  h = a->h;      a->h = b->h;     b->h = h;
  mat = a->mat;  a->mat = b->mat; b->mat = mat;
}

matrix matAddR(matrix r, matrix a, matrix b)
{
  double *rp, *re, *ap, *bp;
  
  assert(a != matNULL);
  assert(b != matNULL);
  assert(a->w == b->w);
  assert(a->h == b->h);

  if (r == matNULL) {
    r = matNew(a->h, a->w);
  }
  else { 
    assert(a->w == r->w);
    assert(a->h == r->h);
  }

  for (rp=r->mat[0], re=rp+r->w*r->h, ap=a->mat[0], bp=b->mat[0]; rp < re; )
    *(rp++) = *(ap++) + *(bp++);

  return r;  
}

matrix matSubR(matrix r, matrix a, matrix b)
{
  double *rp, *re, *ap, *bp;
  
  assert(a != matNULL);
  assert(b != matNULL);
  assert(a->w == b->w);
  assert(a->h == b->h);

  if (r == matNULL) {
    r = matNew(a->h, a->w);
  }
  else { 
    assert(a->w == r->w);
    assert(a->h == r->h);
  }

  for (rp=r->mat[0], re=rp+r->w*r->h, ap=a->mat[0], bp=b->mat[0]; rp < re; )
    *(rp++) = *(ap++) - *(bp++);

  return r;  
}

matrix matMulR(matrix r, matrix a, matrix b)
{
  int i, j, k;
  int r_eq_a = (a == r);
  int r_eq_b = (b == r);
  double sum;
  
  assert(a != matNULL);
  assert(b != matNULL);
  assert(a->w == b->h);

  if (r == matNULL  ||  r_eq_a  ||  r_eq_b) {
    r = matNew(a->h, b->w);
  }
  else { 
    assert(r->h == a->h);
    assert(r->w == b->w);
  }

  for (j = 0;  j < r->h; j++) {
    for (i = 0; i < r->w; i++) {
      sum = 0.0;
      
      for (k = 0; k < a->w; k++) {
        sum += matRef(a, j, k) * matRef(b, k, i);
      }

      matRef(r, j, i) = sum;
    }
  }

  if (r_eq_a || r_eq_b) {
    matSwap(r, r_eq_a ? a : b);
    matFree(r);
    r = r_eq_a ? a : b;
  }
  
  return r;
}


                                             
static int ludcmp(matrix lu, int *indx, double *vv) /* from "Numerical Recipes In C" */
{
  int i, imax=-1, j, k;
  double big, dum, sum, temp;
  int n = lu->h;
  

  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++) {
      if ((temp = fabs(matRef(lu, i, j))) > big) {
        big = temp;
      }
    }
    if (big == 0.0) {
      return 1;                              /* singular */
    }
    
    vv[i] = 1.0/big;
  }

  for (j = 0; j < n; j++) {
    for (i = 0; i < j; i++) {
      sum = matRef(lu, i, j);
      for (k = 0; k < i; k++) {
        sum -= matRef(lu, i, k) * matRef(lu, k, j);
      }
      matRef(lu, i, j) = sum;
    }

    big = 0.0;
    for (i = j; i < n; i++) {
      sum = matRef(lu, i, j);
      for (k = 0; k < j; k++) {
        sum -= matRef(lu, i, k) * matRef(lu, k, j);
      }
      matRef(lu, i, j) = sum;

      if ((dum = vv[i] * fabs(sum)) >= big) {
        big = dum;
        imax = i;
      }
    }

    if (j != imax) {
      for (k = 0; k < n; k++) {
        dum = matRef(lu, imax, k);
        matRef(lu, imax, k) = matRef(lu, j, k);
        matRef(lu, j, k) = dum;
      }

      vv[imax] = vv[j];
    }

    indx[j] = imax;

    if (matRef(lu, j, j) == 0.0) {
      return 1;                              /* singular */
    }

    if (j != n-1) {
      dum = 1.0 / matRef(lu, j, j);
      for (i = j + 1; i < n; i++) {
        matRef(lu, i, j) *= dum;
      }
    }
  }

  return 0;                                  /* not singular */
}

static int lubksb(matrix lu, int *indx, double *b)
{
  int i, ii = -1, ip, j;
  double sum;
  int n = lu->h;

  for (i = 0; i < n; i++) {
    ip = indx[i];
    sum = b[ip];
    b[ip] = b[i];
    if (ii >= 0) {
      for (j = ii; j < i; j++) {
        sum -= matRef(lu, i, j) * b[j];
      }
    }
    else if (sum != 0.0) {
      ii = i;
    }

    b[i] = sum;
  }

  for (i = n-1; i >= 0; i--) {
    sum = b[i];
    for (j = i + 1; j < n; j++) {
      sum -= matRef(lu, i, j) * b[j];
    }
    b[i] = sum / matRef(lu, i, i);
  }

  return 0;
}

matrix matInvR(matrix r, matrix a)
{
  matrix lu;
  static int *indx = NULL;
  static int indxsize = 0;
  static double *col=NULL;
  int i, j;
  int n;


  assert(a != matNULL);
  assert(a->w == a->h);
  
  lu = matNewCopy(a);

  n = a->h;
  if (indxsize < n) {
    if (indxsize > 0) {
      free(indx);
      free(col);
    }
    indx = (int *)    malloc(n * sizeof(int));
    col  = (double *) malloc(n * sizeof(double));
    indxsize = n;

    if (indx == NULL  ||  col == NULL) {
      indxsize = 0;
      return matNULL;
    }
  }

  if (ludcmp(lu, indx, col)) {
    matFree(lu);
    return matNULL;
  }

  if (r == matNULL) {
    r = matNew(a->h, a->w);
  }
  else {
    assert(r->h == a->h);
    assert(r->w == a->w);
  }
  
  if (r == matNULL) {
    return matNULL;
  }
  
  for (j = 0; j < n; j++) {
    for (i = 0; i < n; i++) {
      col[i] = 0.0;
    }
    col[j] = 1.0;

    if (lubksb(lu, indx, col)) {
      matFree(lu);
      return matNULL;
    }
    
    for (i = 0; i < n; i++) {
      matRef(r, i, j) = col[i];
    }
  }

  matFree(lu);
  return r;
}


matrix matDivR(matrix r, matrix b, matrix a)
{
  matrix ainv;

  ainv = matInv(a);
  r = matMulR(r, b, ainv);
  matFree(ainv);
  return r;
}

matrix matSetVal(matrix m, double val)
{
  int i, j, w, h;

  assert(m != matNULL);

  w = matWidth(m);
  h = matHeight(m);
  for (i = 0; i < h; i++)
    for (j = 0; j < w; j++)
      matRef(m, i, j) = val;

  return m;
}

matrix matSet(matrix m, double *vals)
{
  int i, j, w, h;

  assert(m != matNULL);

  w = matWidth(m);
  h = matHeight(m);
  for (i = 0; i < h; i++)
    for (j = 0; j < w; j++)
      matRef(m, i, j) = *(vals++);

  return m;
}

matrix matSetSparse(matrix m, int nvals, MatrixSparseValue *vals)
{
  assert(m != matNULL);

  for ( ; nvals; nvals--, vals++)
    matRef(m, vals->i, vals->j) = vals->v;

  return m;
}

matrix matTransR(matrix r, matrix a)
{
  int i, j;
  assert(a != matNULL);

  if (r == matNULL) {
    r = matNew(a->w, a->h);
  }
  else if (r != a) { 
    assert(a->w == r->h);
    assert(a->h == r->w);
  }

  if (r == a) {
    if (a->h == a->w) {
      double t;
      
      for (j = 0;  j < r->h; j++) {
        for (i = 0; i < j; i++) {
          t = matRef(r, j, i);
          matRef(r, j, i) = matRef(r, i, j);
          matRef(r, i, j) = t;
        }
      }
    }
    else {
      matrix tm;

      tm = matTransR(matNULL, r);
      matSwap(tm, r);
      matFree(tm);
    }
  }
  else {
    for (j = 0;  j < r->h; j++) {
      for (i = 0; i < r->w; i++) {
        matRef(r, j, i) = matRef(a, i, j);
      }
    }
  }
  
  return r;
}

matrix matCopy(matrix r, matrix a)
{
  int i, j;
  assert(a != matNULL);

  if (r == matNULL) {
    r = matNew(a->h, a->w);
  }
  else { 
    assert(a->w == r->w);
    assert(a->h == r->h);
  }

  for (j = 0;  j < r->h; j++) {
    for (i = 0; i < r->w; i++) {
      matRef(r, j, i) = matRef(a, j, i);
    }
  }
  
  return r;
}

matrix matIdent(int w)
{
  matrix m;
  int i, j;
  
  if ((m = matNew(w, w)) == NULL) {
    return m;
  }

  for (j = 0; j < m->h; j++) {
    for (i = 0; i < m->w; i++) {
      matRef(m, j, i) = (i == j) ? 1.0 : 0.0;
    }
  }
  
  return m;
}

matrix matNew(int h, int w)
{
  matrix m=NULL;
  double **rows=NULL, *data=NULL;
  int i;
  
  if (w <= 0  ||  h <= 0) {
    return matNULL;
  }

#ifdef   MATRIX_SAVE_FREE
  if (initialize_saved_matrices) {
    int x, y;

    for (x = 0; x <= MATRIX_SAVE_FREE; x++)
      for (y = 0; y <= MATRIX_SAVE_FREE; y++)
        saved_matrices[x][y] = matNULL;
    initialize_saved_matrices = 0;
  }  
  
  if (w <= MATRIX_SAVE_FREE  &&
      h <= MATRIX_SAVE_FREE  &&
      (m = saved_matrices[w][h]) != (matrix) NULL) {
    saved_matrices[w][h] = m->freelink;
    m->freelink = matNULL;
    return m;
  }
#endif   
  
  if ((data = (double *)  malloc(w*h*sizeof(double))) == NULL  ||  
      (rows = (double **) malloc(h*sizeof(double *))) == NULL  ||
      (m = (matrix) malloc(sizeof(*m))) == NULL) {
    if (data)  free(data);
    if (rows)  free(rows);
    if (m)     free(m);

    return matNULL;
  }

  for (i = 0; i < h; i++) {
    rows[i] = data + i*w;
  }

  m->w = w;
  m->h = h;
  m->mat = rows;

  return m;
}

void matFree(matrix m)
{
  if (m == matNULL)
    return ;
  
  if (m->mat) {

#ifdef   MATRIX_SAVE_FREE
    if (m->w <= MATRIX_SAVE_FREE  &&  m->h <= MATRIX_SAVE_FREE) {
      m->freelink = saved_matrices[m->w][m->h];
      saved_matrices[m->w][m->h] = m;      
    }
    else 
#endif  
    {
      free(m->mat[0]);
      free(m->mat);
      free(m);
    }
  }
}

void matPrint(matrix m)
{
  int i, j;
  assert(m != matNULL);
  if (m->mat) {
    for (j = 0; j < m->h; j++) {
      for (i = 0; i < m->w; i++) {
        printf("%10.5f  ", matRef(m, j, i));
      }
      printf("\n");
    }
  }
}


#if      0

main()
{
  matrix a, b, c, d, e, f, g, h, i;
  double vals2[2][2] = {{ 1, 1 }, { 1, 2 }};
  double vals[4][4] = {{ 1, 2, 3, 4 },
                       { 5, 6, 7, 8 },
                       { 9, 0, 1, 2 },
                       { 3, 4, 1, 6 }};
  
  a = matNew(3, 3);
  b = matNew(1, 3);
  c = matNew(3, 1);

  d = matNew(2, 2);
  matSet(d, (double *) vals2);
  matPrint(d); printf("\n\n");
  e = matInv(d);
  matPrint(e); printf("\n\n");
  
  f = matNew(4, 4);
  matSet(f, (double *) vals);
  matPrint(f); printf("\n\n");
  g = matInv(f);
  matPrint(g); printf("\n\n");
  
  h = matIdent(4);
  i = matDiv(h, f);
  matPrint(i); printf("\n\n");

  a = matMul(i, f);
  matPrint(a);
  
  return 0;
}


#endif   
