#include "Matrix.h"
#define E_DISPLAY

/**********************************/
/* V E C T O R  F U N C T I O N S */
/**********************************/

/*****************************************************************************
Function : Vector_Create
Purpose  : Dynamically creates a vector of arbitrary size.
Inputs   : Vector dimension
Outputs  : Pointer to struct Vector where struct.s points to space for FltN
           vector of dimension r
******************************************************************************/

Vector* Vector_Create(int r)
{
  Vector* a = (Vector*) malloc(sizeof(Vector));
  a->name = NULL;
  a->r = r; 
  a->state = COLUMN;
  a->s = (FltN *) malloc((unsigned) a->r * sizeof(FltN));
  Vector_Zero(a);
  return a;
}


/*****************************************************************************
Function : Vector_Duplicate
Purpose  : Dynamically creates a vector of size v and copies v into it.
Inputs   : Vector* v
Outputs  : Pointer to struct Vector where struct.s points to space for FltN
           vector of dimension r
******************************************************************************/

Vector* Vector_Duplicate(Vector* v)
/*********************************/
{
  Vector* a = (Vector*) malloc(sizeof(Vector));
  a->name = NULL;
  a->r = v->r; 
  a->state = COLUMN;
  a->s = (FltN *) malloc((unsigned) a->r * sizeof(FltN));
  Vector_Copy(a,v);
  return a;
}


/*****************************************************************************
Function : Vector_Destroy
Purpose  : Frees memory used by vector.
Inputs   : Pointer to vector
Outputs  : None
******************************************************************************/

void Vector_Destroy(Vector* a)
{
  free(a->s);
  free(a);
}


/*****************************************************************************
Function : Vector_Print
Purpose  : Prints vector
Inputs   : Pointer to vector
Outputs  : None
******************************************************************************/

void Vector_Print(Vector* a)
{
  int i;
  if (a->name != NULL) printf("%s\n", a->name);
  if (a->state == ROW){
    for (i = 0; i < a->r; i++){
#ifdef E_DISPLAY
      printf("%10.2e", a->s[i]);
#else
      printf("%8.4f", a->s[i]);
#endif
    }
    printf("\n");
  }
  else
    for (i = 0; i < a->r; i++){
#ifdef E_DISPLAY
      printf("%10.2e\n", a->s[i]);
#else
      printf("%8.4f\n", a->s[i]);
#endif
    }
}


/*****************************************************************************
Function : Vector_Copy
Purpose  : Copies vector b into vector a
Inputs   : Two pointers to vectors of equal dimension
Outputs  : Vector b copied to vector a; vector b unchanged
******************************************************************************/

void Vector_Copy(Vector* a, Vector* b)
{
  int i;
  a->state = b->state;
  for (i = 0; i < a->r; i++)
    a->s[i] = b->s[i];
}


/*****************************************************************************
Function : Vector_Norm
Purpose  : Returns the squared norm of vector a.
Inputs   : Pointer to vector a
Outputs  : Norm squared
******************************************************************************/

FltN Vector_Norm(Vector* a)
/**************************/
{
  int i;
  FltN norm = 0.0;
  for (i = 0; i < a->r; i++)
    norm += a->s[i] * a->s[i];
  return norm;
}


/*****************************************************************************
Function : Vector_Zero
Purpose  : Zeros vector a
Inputs   : Pointer to vector a
Outputs  : Zeroed vector
******************************************************************************/

void Vector_Zero(Vector* a)
{
  int i;
  for (i = 0; i < a->r; i++) a->s[i] = 0.0;
}


/*****************************************************************************
Function : Vector_Init
Purpose  : Loads all b values into vector a
Inputs   : Pointer to vector a, float value b
Outputs  : 
******************************************************************************/

void Vector_Init(Vector* a, FltN b)
{
  int i;
  for (i = 0; i < a->r; i++) a->s[i] = b;
}


/*****************************************************************************
Function : Vector_Addition
Purpose  : Adds c = a + b
Inputs   : Pointers to a, b, c
Outputs  : c contains sum; a, b unchanged
******************************************************************************/

void Vector_Addition(Vector* a, Vector* b, Vector* c)
{
  int i;
  for (i = 0; i < a->r; i++) c->s[i] = a->s[i] + b->s[i];
}


/*****************************************************************************
Function : Vector_Subtraction
Purpose  : Subtracts c = a - b
Inputs   : Pointers to a, b, c
Outputs  : c contains difference; a, b unchanged
******************************************************************************/

void Vector_Subtraction(Vector* a, Vector* b, Vector* c)
{
  int i;
  for (i = 0; i < a->r; i++) c->s[i] = a->s[i] - b->s[i];
}


/*****************************************************************************
Function : Vector_Multiplication
Purpose  : Multiplies point-by-point vectors a and b and returns c
Inputs   : Pointers to a, b, c
Outputs  : c contains answer; a, b unchanged
******************************************************************************/

void Vector_Multiplication(Vector* a, Vector* b, Vector* c)
{
  int i;
  for (i = 0; i < a->r; i++) c->s[i] = a->s[i] * b->s[i];
}


/*****************************************************************************
Function : Vector_Scalar_Multiplication
Purpose  : Multiplies point-by-point vector a by scalar p
Inputs   : Pointer to vector a, scalar p
Outputs  : vector b
******************************************************************************/

void Vector_Scalar_Multiplication(Vector* a, FltN p, Vector* b)
{
  int i;
  for (i = 0; i < a->r; i++) b->s[i] = a->s[i] * p;
}


#ifndef VX_WORKS
void Vector_Randomize_Uniform(Vector* a, FltN v)
{
  int i;
  for (i = 0; i < a->r; i++)
      a->s[i] = Random_Uniform(v);
}
  

void Vector_Randomize_Gaussian(Vector* a, FltN v)
{
  int i;
  for (i = 0; i < a->r; i++)
      a->s[i] = Random_Gaussian(v);
}
#endif  


/*****************************************************************************
Function : Vector_Insert
Purpose  : Inserts vector b into vector a at location n
Inputs   : Pointers to a, b, and location n
Outputs  : 
Notes    : Checks the size of vector a to see if its big enough to insert b 
******************************************************************************/

void Vector_Insert(Vector* a, Vector* b, int n)
{
  int i;

#ifdef DIMENSION_CHECK
  if (n+b->r > a->r){
    printf("Vector_Insert : error\n"); 
    printf("vector too big to insert at location %d\n", n);
    printf("(%d x 1) -> ", b->r);
    printf("(%d x 1)\n", a->r);
    exit(0);
  }
#endif

  for (i = 0; i < b->r; i++)
      a->s[(n + i)] = b->s[i];
}


/*****************************************************************************
Function : Vector_Extract
Purpose  : Extracts from vector a at location n a vector the size of b and
           puts it in b
Inputs   : Pointers to a, b, and location n
Outputs  : 
Notes    : Checks the size of vector a to see if its big enough to extract b 
******************************************************************************/

void Vector_Extract(Vector* a, Vector* b, int n)
{
  int i;

#ifdef DIMENSION_CHECK
  if (n+b->r > a->r){
    printf("Vector_Extract : error\n"); 
    printf("vector too big to extract at location %d\n", n);
    printf("(%d x 1) -> ", a->r);
    printf("(%d x 1)\n", b->r);
    exit(0);
  }
#endif

  for (i = 0; i < b->r; i++)
      b->s[i] = a->s[(n + i)];
}




/***********************************/
/* M A T R I X   F U N C T I O N S */
/***********************************/

/*****************************************************************************
Function : Matrix_Create
Purpose  : Creates a matrix of dimension r x c and returns pointer
Inputs   : Dimensions r, c
Outputs  : Pointer to created matrix
******************************************************************************/

Matrix* Matrix_Create(int r, int c)
{
  int i;
  Matrix* a = (Matrix*) malloc(sizeof(Matrix));
  a->name = NULL;
  a->r = r; a->c = c; 
  a->state = NORMAL;
  a->s = (FltN **) malloc((unsigned) a->r * sizeof(FltN));
  for (i = 0; i < a->r; i++)
    a->s[i] = (FltN *) malloc((unsigned) a->c * sizeof(FltN));
  Matrix_Zero(a);
  return a;
}


/*****************************************************************************
Function : Matrix_Duplicate
Purpose  : Creates matrix a of dimension of matrix b, copies b into a
           and returns pointer to a
Inputs   : Pointer to matrix b
Outputs  : Pointer to created matrix a
******************************************************************************/

Matrix* Matrix_Duplicate(Matrix* b)
{
  int i;
  Matrix* a = (Matrix*) malloc(sizeof(Matrix));
  a->name = NULL;
  a->r = b->r; a->c = b->c; 
  a->state = b->state;
  a->s = (FltN **) malloc((unsigned) a->r * sizeof(FltN));
  for (i = 0; i < a->r; i++)
    a->s[i] = (FltN *) malloc((unsigned) a->c * sizeof(FltN));
  Matrix_Copy(a,b);
  return a;
}


/*****************************************************************************
Function : Matrix_Destroy
Purpose  : Frees memory used by matrix a
Inputs   : Pointer to matrix a
Outputs  :
******************************************************************************/

void Matrix_Destroy(Matrix* a)
{
  int i;
  for (i = 0; i < a->r; i++) free(a->s[i]);
  free(a->s);
  free(a);
}


/*****************************************************************************
Function : Matrix_Zero
Purpose  : Zeros matrix
Inputs   : Pointer to matrix a
Outputs  : None
******************************************************************************/

void Matrix_Zero(Matrix* a)
{
  Reg int i, j;
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++) 
      a->s[i][j] = 0.0;
}


/*****************************************************************************
Function : Matrix_Init
Purpose  : Loads all b values into matrix a
Inputs   : Pointer to matrix a, float value b
Outputs  : 
******************************************************************************/

void Matrix_Init(Matrix* a, FltN b)
{
  int i,j;
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++) 
      a->s[i][j] = b;
}


/*****************************************************************************
Function : Matrix_Copy
Purpose  : Copies matrix b into matrix a
Inputs   : Pointers to a, b
Outputs  : None
Notes    : Checks to see if either a or b are transposed and copies the
           elements accordingly.
******************************************************************************/

void Matrix_Copy(Matrix* a, Matrix* b)
{
 Reg  int i, j;

  if (b->state == TRANSPOSE){

#ifdef DIMENSION_CHECK
    if (a->r != b->c || a->c != b->r){
      printf("Matrix_Copy : dimension error\n");
      printf("(%d x %d) = (%d x %d)\n", a->r, a->c, b->c, b->r);
      exit(1);
    }
#endif

    for (i = 0; i < a->r; i++)
      for (j = 0; j < a->c; j++)
	a->s[i][j] = b->s[j][i];
  }
  else{

#ifdef DIMENSION_CHECK
    if (a->r != b->r || a->c != b->c){
      printf("Matrix_Copy : dimension error\n");
      printf("(%d x %d) = (%d x %d)\n", a->r, a->c, b->r, b->c);
      exit(1);
    }
#endif

    for (i = 0; i < a->r; i++)
      for (j = 0; j < a->c; j++)
	a->s[i][j] = b->s[i][j];
  }
}


/*****************************************************************************
Function : Matrix_Print
Purpose  : Prints matrix a to terminal
Inputs   : Pointer to a
Outputs  : None
Notes    : Checks to see if a is transposed and prints the elements 
           accordingly.
******************************************************************************/

void Matrix_Print(Matrix* a)
{
  Reg int i, j;
  if (a->state == TRANSPOSE){
    if (a->name != NULL) printf("%s^T\n", a->name);
    for (i = 0; i < a->c; i++){
      for (j = 0; j < a->r; j++)
#ifdef E_DISPLAY
	printf("%10.2e", a->s[j][i]);
#else
	printf("%8.4f", a->s[j][i]);
#endif
      printf("\n");
    }
  }
  else{
    if (a->name != NULL) printf("%s\n", a->name);
    for (i = 0; i < a->r; i++){
      for (j = 0; j < a->c; j++)
#ifdef E_DISPLAY
	printf("%10.2e", a->s[i][j]);
#else
	printf("%8.4f", a->s[i][j]);
#endif
      printf("\n");
    }
  }
  printf("\n");
}
  

/*****************************************************************************
Function : Matrix_Identity
Purpose  : Sets matrix to identity
Inputs   : Pointer to a
Outputs  : Identity matrix for a
******************************************************************************/

void Matrix_Identity(Matrix* a)
{
  Reg int i, j;
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++){
      if (i == j) a->s[i][j] = 1.0;
      else a->s[i][j] = 0.0;
    }
}
  

/*****************************************************************************
Function : Matrix_Multiplication
Purpose  : Multiplies d = a * b
Inputs   : Pointers to a, b, and d
Outputs  : Product in matrix d
Notes    : Checks for transposition in either a or b and adjusts algorithm
           for proper operation. If output matrix d is the same as either
           a or b, a scratch matrix is created to hold the product during
	   multiplication; the product is then copied to the output matrix d.
******************************************************************************/

void Matrix_Multiplication(Matrix* a, Matrix* b, Matrix* d)
/* multiplies d(m x l) = a(m x n) * b(n x l)             */
{
  Reg int i, j, k;

  Matrix* c;
  if (a == d || b == d) c = Matrix_Create(d->r,d->c);
  else c = d;

  if (a->state == TRANSPOSE){
    if (b->state == TRANSPOSE){

/* both a and b transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->c || a->c != c->r || b->r != c->c){
	printf("Matrix_Multiplication : dimension error\n");
	if (c->name != NULL) printf("%s", c->name);
	printf("(%d x %d) = ", c->r, c->c);
	if (a->name != NULL) printf("%s", a->name);
	printf("(%d x %d) * ", a->c, a->r);
	if (b->name != NULL) printf("%s", b->name);
	printf("(%d x %d)\n", b->c, b->r);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                  /* a and b transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = 0.0;
	  for (k = 0; k < a->c; k++){
	    c->s[i][j] += a->s[k][i] * b->s[j][k];
	  }
	}
      }
    }
    else{

/* only a transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->r || a->c != c->r || b->c != c->c){
	printf("Matrix_Multiplication : dimension error\n");
	if (c->name != NULL) printf("%s", c->name);
	printf("(%d x %d) = ", c->r, c->c);
	if (a->name != NULL) printf("%s", a->name);
	printf("(%d x %d) * ", a->c, a->r);
	if (b->name != NULL) printf("%s", b->name);
	printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                 /* only a transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = 0.0;
	  for (k = 0; k < a->c; k++){
	    c->s[i][j] += a->s[k][i] * b->s[k][j];
	  }
	}
      }
    }
  }
  else{
    if (b->state == TRANSPOSE){

/* only b transposed */
#ifdef DIMENSION_CHECK
      if (a->c != b->c || a->r != c->r || b->r != c->c){
	printf("Matrix_Multiplication : dimension error\n");
	if (c->name != NULL) printf("%s", c->name);
	printf("(%d x %d) = ", c->r, c->c);
	if (a->name != NULL) printf("%s", a->name);
	printf("(%d x %d) * ", a->r, a->c);
	if (b->name != NULL) printf("%s", b->name);
	printf("(%d x %d)\n", b->c, b->r);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                /* only b transposed */
	for (j = 0; j < b->r; j++){
	  c->s[i][j] = 0.0;
	  for (k = 0; k < a->c; k++){
	    c->s[i][j] += a->s[i][k] * b->s[j][k];
	  }
	}
      }
    }
    else{

/* a and b normal */
#ifdef DIMENSION_CHECK
      if (a->c != b->r || a->r != c->r || b->c != c->c){
	printf("Matrix_Multiplication : dimension error\n");
	if (c->name != NULL) printf("%s", c->name);
	printf("(%d x %d) = ", c->r, c->c);
	if (a->name != NULL) printf("%s", a->name);
	printf("(%d x %d) * ", a->r, a->c);
	if (b->name != NULL) printf("%s", b->name);
	printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif
      for (i = 0; i < a->r; i++){               /* both a and b normal */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = 0.0;
	  for (k = 0; k < a->c; k++){
	    c->s[i][j] += a->s[i][k] * b->s[k][j];
	  }
	}
      }
    }
  }
  if (a == d || b == d){
    Matrix_Copy(d,c);
    Matrix_Destroy(c);
  }
}


/*****************************************************************************
Function : Matrix_Vector_Multiplication
Purpose  : Multiplies vector by matrix d(m x 1) = a(m x n) * b(n x 1)
Inputs   : Pointers to a, b, and d
Outputs  : Product in matrix d
Notes    : Checks for transposition in a and adjusts algorithm for proper 
           operation. If output vector d is the same as vector b, a scratch 
	   vector c is created to hold the product during multiplication; 
	   the product is then copied to the output vector d.
******************************************************************************/

void Matrix_Vector_Multiplication(Matrix* a, Vector* b, Vector* d)
/* multiplies d(m) = a(m x n) * b(n)                            */
{
  Reg int i, j;
  Vector* c;
  if (b == d) c = Vector_Create(d->r);
  else c = d;

  if (a->state == TRANSPOSE){

/* a transposed */
#ifdef DIMENSION_CHECK
    if (a->r != b->r || a->c != c->r){
      printf("Matrix_Vector_Multiplication : dimension error\n");
      printf("(%d x 1) = (%d x %d) * (%d x 1)\n",
	     c->r, a->c, a->r, b->r);
      if (b == d) Vector_Destroy(c);
      exit(1);
    }
#endif

    for (i = 0; i < a->c; i++){                  
      c->s[i] = 0.0;
      for (j = 0; j < b->r; j++){
	c->s[i] += a->s[j][i] * b->s[j];
      }
    }
  }
  else{

/* no transpose */
#ifdef DIMENSION_CHECK
    if (a->c != b->r || a->r != c->r){
      printf("Matrix_Vector_Multiplication : dimension error\n");
      printf("(%d x 1) = (%d x %d) * (%d x 1)\n",
	     c->r, a->r, a->c, b->r);
      if (b == d) Vector_Destroy(c);
      exit(1);
    }
#endif

    for (i = 0; i < a->r; i++){                 
      c->s[i] = 0.0;
      for (j = 0; j < b->r; j++){
	c->s[i] += a->s[i][j] * b->s[j];
      }
    }
  }
  if (b == d){
    Vector_Copy(d,c);
    Vector_Destroy(c);
  }
}


/*****************************************************************************
Function : Matrix_Addition
Purpose  : Adds two matrices c(m x n) = a(m x n) + b(m x n)
Inputs   : Pointers to a, b, and c
Outputs  : Sum in matrix c
Notes    : Checks for transposition in a and adjusts algorithm for proper 
           operation. 
******************************************************************************/

void Matrix_Addition(Matrix* a, Matrix* b, Matrix* d)
/* multiplies c(m x l) = a(m x l) + b(m x l)       */
{
  Reg int i, j;

  Matrix* c;
  if (a == d || b == d) c = Matrix_Create(d->r,d->c);
  else c = d;

  if (a->state == TRANSPOSE){
    if (b->state == TRANSPOSE){

/* both a and b transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->r || a->c != b->c || a->c != c->r || b->r != c->c){
	printf("Matrix_Addition : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) + ", a->name, a->r, a->c);
	else printf("(%d x %d) + ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < c->r; i++){                  /* a and b transposed */
	for (j = 0; j < c->c; j++){
	  c->s[i][j] = a->s[j][i] + b->s[j][i];
	}
      }
    }
    else{

/* only a transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->c || a->c != b->r || a->c != c->r || b->c != c->c){
	printf("Matrix_Addition : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) + ", a->name, a->r, a->c);
	else printf("(%d x %d) + ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                 /* only a transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[j][i] + b->s[i][j];
	}
      }
    }
  }
  else{
    if (b->state == TRANSPOSE){

/* only b transposed */
#ifdef DIMENSION_CHECK
      if (a->c != b->r || a->r != b->c || a->r != c->r || b->r != c->c){
	printf("Matrix_Addition : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) + ", a->name, a->r, a->c);
	else printf("(%d x %d) + ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                /* only b transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[i][j] + b->s[j][i];
	}
      }
    }
    else{

/* a and b normal */
#ifdef DIMENSION_CHECK
      if (a->c != b->c || a->r != b->r || a->r != c->r || b->c != c->c){
	printf("Matrix_Addition : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) + ", a->name, a->r, a->c);
	else printf("(%d x %d) + ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){               /* both a and b normal */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[i][j] + b->s[i][j];
	}
      }
    }
  }

  if (a == d || b == d){
    Matrix_Copy(d,c);
    Matrix_Destroy(c);
  }
}


/*****************************************************************************
Function : Matrix_Subtraction
Purpose  : Subtracts two matrices d(m x n) = a(m x n) - b(m x n)
Inputs   : Pointers to a, b, and d
Outputs  : Difference in matrix d
Notes    : Checks for transposition in a and adjusts algorithm for proper 
           operation. 
******************************************************************************/

void Matrix_Subtraction(Matrix* a, Matrix* b, Matrix* d)
{
  Reg int i, j;

  Matrix* c;
  if (a == d || b == d) c = Matrix_Create(d->r,d->c);
  else c = d;

  if (a->state == TRANSPOSE){
    if (b->state == TRANSPOSE){

/* both a and b transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->r || a->c != b->c || a->c != c->r || b->r != c->c){
	printf("Matrix_Subtraction : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) - ", a->name, a->r, a->c);
	else printf("(%d x %d) - ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < c->r; i++){                  /* a and b transposed */
	for (j = 0; j < c->c; j++){
	  c->s[i][j] = a->s[j][i] - b->s[j][i];
	}
      }
    }
    else{

/* only a transposed */
#ifdef DIMENSION_CHECK
      if (a->r != b->c || a->c != b->r || a->c != c->r || b->c != c->c){
	printf("Matrix_Subtraction : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) - ", a->name, a->r, a->c);
	else printf("(%d x %d) - ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                 /* only a transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[j][i] - b->s[i][j];
	}
      }
    }
  }
  else{
    if (b->state == TRANSPOSE){

/* only b transposed */
#ifdef DIMENSION_CHECK
      if (a->c != b->r || a->r != b->c || a->r != c->r || b->r != c->c){
	printf("Matrix_Subtraction : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) - ", a->name, a->r, a->c);
	else printf("(%d x %d) - ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){                /* only b transposed */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[i][j] - b->s[j][i];
	}
      }
    }
    else{

/* a and b normal */
#ifdef DIMENSION_CHECK
      if (a->c != b->c || a->r != b->r || a->r != c->r || b->c != c->c){
	printf("Matrix_Subtraction : dimension error\n");
	if (c->name != 0) printf("%s(%d x %d) = ", c->name, c->r, c->c);
	else printf("(%d x %d) = ", c->r, c->c);
	if (a->name != 0) printf("%s(%d x %d) - ", a->name, a->r, a->c);
	else printf("(%d x %d) - ", a->r, a->c);
	if (b->name != 0) printf("%s(%d x %d)\n", b->name, b->r, b->c);
	else printf("(%d x %d)\n", b->r, b->c);
	if (a == d || b == d) Matrix_Destroy(c);
	exit(1);
      }
#endif

      for (i = 0; i < a->r; i++){               /* both a and b normal */
	for (j = 0; j < b->c; j++){
	  c->s[i][j] = a->s[i][j] - b->s[i][j];
	}
      }
    }
  }

  if (a == d || b == d){
    Matrix_Copy(d,c);
    Matrix_Destroy(c);
  }
}


void Matrix_Scalar_Multiplication(Matrix* a, FltN c, Matrix* b)
{
  Reg int i, j;

  for (i = 0; i < a->r; i++){  
    for (j = 0; j < a->c; j++){
      b->s[i][j] = a->s[i][j] * c;
    }
  }
}


/*****************************************************************************
Function : Matrix_Insert
Purpose  : Inserts matrix b into matrix a at location (n,m)
Inputs   : Pointers to a, b, and location (n,m)
Outputs  : 
Notes    : Checks the size of matrix a to see if its big enough to insert b 
           Checks if b is transposed
******************************************************************************/

void Matrix_Insert(Matrix* a, Matrix* b, int n, int m)
{
  Reg int i, j;

  if (b->state == TRANSPOSE){

#ifdef DIMENSION_CHECK
    if (n+b->c > a->r || m+b->r > a->c){
      printf("Matrix_Insert : error\n"); 
      printf("matrix too big to insert at location (%d, %d)\n", n, m);
      printf("(%d x %d) -> ", b->c, b->r);
      printf("(%d x %d)\n", a->r, a->c);
      exit(0);
    }
#endif

    for (i = 0; i < b->r; i++)
      for (j = 0; j < b->c; j++)
	a->s[(n+i)][(m+j)] = b->s[j][i];

  }else{

#ifdef DIMENSION_CHECK
    if (n+b->r > a->r || m+b->c > a->c){
      printf("Matrix_Insert : error\n"); 
      printf("matrix too big to insert at location (%d, %d)\n", n, m);
      printf("(%d x %d) -> ", b->r, b->c);
      printf("(%d x %d)\n", a->r, a->c);
      exit(0);
    }
#endif

    for (i = 0; i < b->r; i++)
      for (j = 0; j < b->c; j++)
	a->s[(n+i)][(m+j)] = b->s[i][j];
  }
}


/*****************************************************************************
Function : Matrix_Extract
Purpose  : Extracts from matrix a at location (n,m) a matrix the size of b 
           and puts it in b.
Inputs   : Pointers to a, b, and location (n,m)
Outputs  : Extracted matrix in b
Notes    : Checks the size of matrix a to see if its big enough to extract 
******************************************************************************/

void Matrix_Extract(Matrix* a, Matrix* b, int n, int m)
{
  Reg int i, j;

#ifdef DIMENSION_CHECK
  if (n+b->r > a->r || m+b->c > a->c){
    printf("Matrix_Extract : error\n"); 
    printf("matrix too big to extract from location (%d, %d)\n", n, m);
    printf("(%d x %d) -> ", a->r, a->c);
    printf("(%d x %d)\n", b->r, b->c);
    exit(0);
  }
#endif

  for (i = 0; i < b->r; i++)
    for (j = 0; j < b->c; j++)
      b->s[i][j] = a->s[(n+i)][(m+j)];
}


/*****************************************************************************
Function : Matrix_Power
Purpose  : Raises matrix a to power n and returns it in matrix b
Inputs   : Pointers to a, b, and power n
Outputs  : 
Notes    :  
******************************************************************************/

void Matrix_Power(Matrix* a, Matrix* b, int n)
{
  Reg int i;
  Matrix* d = Matrix_Create(a->r, a->c);
  Matrix_Identity(d);
  if (n == 0) return;
  for (i = 0; i < n; i++){
    Matrix_Multiplication(d,a,d);
  }
  Matrix_Copy(b,d);
  Matrix_Destroy(d);
}


/*****************************************************************************
Function : Matrix_Exponential
Purpose  : Computes exponential of matrix a and copies it to matrix b
Inputs   : Pointers to a, b
Outputs  : 
Notes    : Computes exponential to order MATRIX_EXPONENTIAL_ORDER
******************************************************************************/
#define MATRIX_EXPONENTIAL_ORDER 8
void Matrix_Exponential(Matrix* a, Matrix* b)
{
  Reg int i;
  Matrix* d = Matrix_Create(a->r, a->c);
  Matrix* c = Matrix_Create(a->r, a->c);
  Matrix_Identity(d);
  for (i = 1; i < MATRIX_EXPONENTIAL_ORDER; i++){
    Matrix_Power(a,c,i);
    Matrix_Scalar_Multiplication(c, 1.0/fact(i), c);
    Matrix_Addition(d,c,d);
  }
  Matrix_Copy(b,d);
  Matrix_Destroy(c);
  Matrix_Destroy(d);
}


/*****************************************************************************
Function : Matrix_Inverse
Purpose  : Inverts matrix m and copies it to matrix b
Inputs   : Pointers to m, b
Outputs  : 
Notes    :  
******************************************************************************/

void Matrix_Inverse(Matrix* m, Matrix* b)
{
  Reg int i, j;
  Matrix*  a = Matrix_Duplicate(m);
  int*     indx = (int*) malloc((unsigned) m->r * sizeof(int));
  double   d = 0.0;
  Vector* col = Vector_Create(m->r);

#ifdef DIMENSION_CHECK
  if (m->r != m->c){
    printf("inverse undefined\n");
    printf("(%d x %d) is not square\n", m->r, m->c);
    Matrix_Destroy(a);
    Vector_Destroy(col);
    exit(1);
  }
#endif

  ludcmp(a, m->r, indx, &d);

  for (j = 0; j < m->r; j++){
    Vector_Zero(col);
    col->s[j] = 1.0;
    lubksb(a, m->r, indx, col);
    for (i = 0; i < m->r; i++) b->s[i][j] = col->s[i];
  }
  free(indx);
  Matrix_Destroy(a);
  Vector_Destroy(col);
}


/*****************************************************************************
Function : ludcmp
Purpose  : From Numerical Recipes in C; used in Matrix_Inverse
Inputs   : 
Outputs  : 
Notes    :  
******************************************************************************/

void ludcmp(Matrix* a, int n, int* indx, double* d)
{
  register int i, imax, j, k;
  double big, dum, sum, temp;
  Vector* vv = Vector_Create(n);
  
  *d = 1.0;
  for (i = 0; i < n; i++){
    big = 0.0;
    for (j = 0; j < n; j++)
      if ( (temp = fabs(a->s[i][j])) > big ) big = temp;
    if (big == 0.0) 
      printf("singular matrix\n");
    vv->s[i] = 1.0/big;
  }
  for (j = 0; j < n; j++){
    for (i = 0; i < j; i++){
      sum = a->s[i][j];
      for (k = 0; k < i; k++) 
	sum -= a->s[i][k] * a->s[k][j];
      a->s[i][j] = sum;
    }
    big = 0.0;
    for (i = j; i < n; i++){
      sum = a->s[i][j];
      for (k = 0; k < j; k++)
	sum -= a->s[i][k] * a->s[k][j];
      a->s[i][j] = sum;
      if ( (dum = vv->s[i]*fabs(sum)) >= big ){
	big = dum;
	imax = i;
      }
    }
    if (j != imax){
      for (k = 0; k < n; k++){
	dum = a->s[imax][k];
	a->s[imax][k] = a->s[j][k];
	a->s[j][k] = dum;
      }
      *d = -(*d);
      vv->s[imax] = vv->s[j];
    }
    indx[j] = imax;
    if (a->s[j][j] == 0.0) a->s[j][j] = 1.0e-20;
    if (j != n){
      dum = 1.0/a->s[j][j];
      for (i = j+1; i < n; i++) a->s[i][j] *= dum;
    }
  }
  Vector_Destroy(vv);
}


/*****************************************************************************
Function : lubksb
Purpose  : From Numerical Recipes in C; used in Matrix_Inverse
Inputs   : 
Outputs  : 
Notes    :  
******************************************************************************/

void lubksb(Matrix* a, int n, int* indx, Vector* b)
{
  int i, ii=-1, ip, j;
  double sum;
  for (i = 0; i < n; i++){
    ip = indx[i];
    sum = b->s[ip];
    b->s[ip] = b->s[i];
    if (ii != (-1))
      for (j = ii; j < i; j++) sum -= a->s[i][j] * b->s[j];
    else if (sum) ii = i;
    b->s[i] = sum;
  }
  for (i = (n-1); i >= 0; i--){
    sum = b->s[i];
    for (j = i+1; j < n; j++) sum -= a->s[i][j] * b->s[j];
    b->s[i] = sum/a->s[i][i];
  }
}


/******************************************************************************/
/*  random numbers not supported by vxWorks                                   */
/******************************************************************************/

#ifndef VX_WORKS
void Matrix_Randomize_Uniform(Matrix* a, FltN v)
/***********************************************/
{
  Reg int i, j;
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++) 
      a->s[i][j] = Random_Uniform(v);
}
  

void Matrix_Randomize_Uniform_Symmetrical(Matrix* a, FltN v)
/***********************************************************/
{
  Reg int i, j;
  for (i = 0; i < a->r; i++)
    for (j = i; j < a->c; j++) 
      a->s[i][j] = a->s[j][i] = Random_Uniform(v);
}
  

void Matrix_Randomize_Gaussian(Matrix* a, FltN v)
/************************************************/
{
  Reg int i, j;
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++) 
      a->s[i][j] = Random_Gaussian(v);
}
#endif  


FltN Matrix_Trace(Matrix* a)
/***************************/
{
  int i;
  FltN t = 0;
  for (i = 0; i < a->r; i++) t += a->s[i][i];
  return t;
}
  

FltN Matrix_Norm(Matrix* a)
/**************************/
{
  FltN t = 0;
  Matrix* b = Matrix_Create(a->r, a->c);
  Matrix_Copy(b,a);
  b->state = TRANSPOSE;
  Matrix_Multiplication(a,b,b);
  t = Matrix_Trace(b);
  Matrix_Destroy(b);
  return t;
}
  

void Matrix_Output(Matrix* a, FILE* fp)
/*************************************/
{
  int i,j;
  fwrite((char*) a, sizeof(Matrix), 1, fp);
  for (i = 0; i < a->r; i++)
   for (j = 0; j < a->c; j++)
     fwrite((char*) &(a->s[i][j]), sizeof(char), 4, fp);
}


void Matrix_Symmetrize(Matrix* a)
/*******************************/
{
  int i,j;
  for (i = 0; i < a->c; i++)
    for (j = 0; j <= i; j++) a->s[i][j] = a->s[j][i];
}


/*****************************************************************************
Function : Matrix_Transpose
Purpose  : Transposes matrix a
Inputs   : Pointer to a
Outputs  : a matrix transposed
Notes    : 
******************************************************************************/

void Matrix_Transpose(Matrix* a)
/******************************/
{
  int i,j;
  FltN b;
  for (i = 1; i < a->r; i++)
    for (j = 0; j < i; j++){
      b = a->s[i][j];
      a->s[i][j] = a->s[j][i];
      a->s[j][i] = b; 
    }
}


Matrix* Matrix_Input(FILE* fp)
/****************************/
{
  int i,j;
  Matrix p;
  Matrix* a;

  fread((char*) &p, sizeof(Matrix), 1, fp);
  a = Matrix_Create(p.r, p.c);
  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++)
      fread((char*) &(a->s[i][j]), sizeof(Flt64), 1, fp);
  a->name = NULL;
  return a;
}


void Matrix_Read(Matrix* a, char* file)
/*************************************/
{
  FILE* fp;
  int i,j;
  Matrix p;

  printf("opening input file %s\n", file);
  if ( (fp = fopen(file, "rb")) == NULL ){
    printf("error opening input file - %s\n", file);
    exit(1);
  }
  fread((char*) &p, sizeof(Matrix), 1, fp);
  if (p.r != a->r || p.c != a->c){
    printf("Incorrect dimensions (%d x %d) in file %s\n", p.r, p.c, file);
    exit(1);
  }

  for (i = 0; i < a->r; i++)
    for (j = 0; j < a->c; j++)
      fread((char*) &(a->s[i][j]), sizeof(Flt64), 1, fp);
  a->name = NULL;
  fclose(fp);
}


#define DYNAMIC_RANGE 32768/2
#ifndef VX_WORKS
FltN Random_Uniform(FltN v)
/***************************/
{
/*  return(v * (2.0 * drand48() - 1.0)); */
  return( v * (rand() - DYNAMIC_RANGE) / DYNAMIC_RANGE );
}


FltN Random_Gaussian(FltN v)
/****************************/
{
  static FltN a;
  static int n = 0;
  FltN c, v1, v2;

  if (n == 0){
    do {
      v1 = Random_Uniform(1.0);
      v2 = Random_Uniform(1.0);
      c = v1 * v1 + v2 * v2;
    } while (c > 1.0);
    n = 1;
    c = sqrt(-2.0 * log(c) / c);
    a = v1 * c;
    return v2 * c * sqrt(v);
  }
  else{
    n = 0;
    return a * sqrt(v);
  }
}
#endif

int fact(int n)
/*************/
{
  int i, k=1;
  if (n == 0) return 1;
  for (i = 1; i <= n; i++) k *= i;
  return k;
}

FltN power(FltN a, int n)
/*************************/
{
  Reg int i;
  FltN b = 1.0;
  for (i = 0; i < n; i++) b *= a;
  return b;
}