/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	matrixFunc
 *
 * DESCRIPTION:	module which implements the various matrix functions and
 *		operators.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>
#include "cdefs.h"
#include "error.h"
#include "types.h"
#include "parameters.h"

matrix *talloc_matrix();

multiplyMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1, *m2;
	double x;
	int i, j, k;

	m1 = a[0].val.m;
	m2 = a[1].val.m;

	if (m1->col != m2->row)
	 { error (0, "matrix multiply -- row/column mismatch\n");
	   return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m2->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { x = 0.0;
	      for (k=0; k<m1->col; k++)
	       { x += m1->buf[i*m1->col+k] * m2->buf[k*m2->col+j];
	       }
	      mr->buf[i*mr->col+j] = x;
	    }
	 }
	r->val.m = mr;
	return (1);
}

addMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1, *m2;
	double x;
	int i, j;

	m1 = a[0].val.m;
	m2 = a[1].val.m;

	if (m1->col != m2->col ||
	    m1->row != m2->row)
	 { error (0, "matrix add -- row/column mismatch\n");
	   return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m2->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = m1->buf[i*m1->col+j] + 
				     m2->buf[i*m2->col+j];
	    }
	 }
	r->val.m = mr;
	return (1);
}

subtractMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1, *m2;
	double x;
	int i, j;

	m1 = a[0].val.m;
	m2 = a[1].val.m;

	if (m1->col != m2->col ||
	    m1->row != m2->row)
	 { error (0, "matrix subtract -- row/column mismatch\n");
	   return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m2->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = m1->buf[i*m1->col+j] - 
				     m2->buf[i*m2->col+j];
	    }
	 }
	r->val.m = mr;
	return (1);
}

invertMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	int i, j;

	m1 = a[0].val.m;

	if (m1->row != m1->col)
	 { error (0, "matrix invert -- matrix is not square\n");
	   return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m1->col)) == NULL)
	 { return (-1);
	 }
	if (invert_matrix (mr, m1) == NULL)
  	 { return (-1);
	 }
	r->val.m = mr;
	return (1);
}

multInvMatrixLeft (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *m1, *m2, *mr, *mt;
	double x;
	int i, j, k;

	m1 = a[0].val.m;
	m2 = a[1].val.m;

	if (m1->col != m1->row)
	 { error ("matrix inversion -- matrix is not square\n");
	 }
	if (m1->col != m2->row)
	 { error (0, "matrix multiply -- row/column mismatch\n");
	   return (-1);
	 }
	if ((mt = talloc_matrix (m1->row, m1->col)) == NULL)
	 { return (-1);
	 }
	if (invert_matrix (mt, m1) == NULL)
  	 { return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m2->col)) == NULL)
 	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { x = 0.0;
	      for (k=0; k<mt->col; k++)
	       { x += mt->buf[i*mt->col+k] * m2->buf[k*m2->col+j];
	       }
	      mr->buf[i*mr->col+j] = x;
	    }
	 }
	r->val.m = mr; 	
 	return (1);
}

multInvMatrixRight (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *m1, *m2, *mr, *mt;
	double x;
	int i, j, k;

	m1 = a[0].val.m;
	m2 = a[1].val.m;

	if (m2->col != m2->row)
	 { error ("matrix inversion -- matrix is not square\n");
	 }
	if (m1->col != m2->row)
	 { error (0, "matrix multiply -- row/column mismatch\n");
	   return (-1);
	 }
	if ((mt = talloc_matrix (m2->row, m2->col)) == NULL)
	 { return (-1);
	 }
	if (invert_matrix (mt, m2) == NULL)
  	 { return (-1);
	 }
	if ((mr = talloc_matrix (m1->row, m2->col)) == NULL)
 	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { x = 0.0;
	      for (k=0; k<m1->col; k++)
	       { x += m1->buf[i*m1->col+k] * mt->buf[k*mt->col+j];
	       }
	      mr->buf[i*mr->col+j] = x;
	    }
	 }
	r->val.m = mr; 	
 	return (1);
}

transposeMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	int i, j;

	m1 = a[0].val.m;

	if ((mr = talloc_matrix (m1->col, m1->row)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = m1->buf[j*m1->col+i];
	    }
	 }
	r->val.m = mr;
	return (1);
}

scaleMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	double d;
	int i, j;

	if (a[0].type == DOUBLE && a[1].type == MATRIX)
	 { d = a[0].val.d;
	   m1 = a[1].val.m;
	 }
	else if (a[0].type == MATRIX && a[1].type == DOUBLE)
	 { d = a[1].val.d;
	   m1 = a[0].val.m;
	 }
	else
	 { error (0, "scale matrix -- illegal type\n");
	   return (-1);
	 }

	if ((mr = talloc_matrix (m1->row, m1->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = d * m1->buf[i*m1->col+j];
	    }
	 }
	r->val.m = mr;
	return (1);
}

divideMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	double d;
	int i, j;

	if (a[0].type == DOUBLE && a[1].type == MATRIX)
	 { d = a[0].val.d;
	   m1 = a[1].val.m;
	 }
	else if (a[0].type == MATRIX && a[1].type == DOUBLE)
	 { d = a[1].val.d;
	   m1 = a[0].val.m;
	 }
	else
	 { error (0, "divide matrix -- illegal type\n");
	   return (-1);
	 }

	if (fabs(d) < SMALLEST)
  	 { error (0, "divide matric -- divide by zero detected\n");
	   return (-1);
	 }

	if ((mr = talloc_matrix (m1->row, m1->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = m1->buf[i*m1->col+j] / d;
	    }
	 }
	r->val.m = mr;
	return (1);
}

negateMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	int i, j;

	m1 = a[0].val.m;

	if ((mr = talloc_matrix (m1->row, m1->col)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<mr->row; i++)
	 { for (j=0; j<mr->col; j++)
	    { mr->buf[i*mr->col+j] = - m1->buf[i*m1->col+j];
	    }
	 }
	r->val.m = mr;
	return (1);
}

zeroMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	int i, j, row, col;
	matrix *mr;

	if ((row = a[0].val.i) < 0 || (col = a[1].val.i) < 0)
	 { error (0, "%s() -- bad matrix dimensions\n", r->name);
	   return (-1);
	 }
	else
 	 { if ((mr = talloc_matrix(row, col)) == NULL)
	    { return (-1);
	    }
	   for (i=0; i<row; i++)
	    { for (j=0; j<col; j++)
	       { ELEMENT (mr, i, j) = 0.0;
	       }
	    }
	   r->val.m = mr;
	   r->type = MATRIX;
	   return (1);
	 }
}

diagonalMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr, *m1;
	int i, j, size;

	m1 = a[0].val.m;
	if (m1->row != 1 && m1->col != 1)
	 { error (0, "%s() -- requires vector argument\n");
	   return (-1);
	 }
 	size = m1->row*m1->col;
 	
	if ((mr = talloc_matrix(size, size)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<size; i++)
	 { for (j=0; j<size; j++)
	    { if (i == j)
	       { ELEMENT (mr, i, j) = m1->buf[i];
	       }
	      else
	       { ELEMENT (mr, i, j) = 0.0;
	       }
	    }
	 }
	r->val.m = mr;
	r->type = MATRIX;
	return (1);
}

identityMatrix (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *mr;
	int i, j, size;

	if ((size = a[0].val.i) <= 1)
	 { r->val.d = 1.0;
	   r->type = DOUBLE;
	   return (1);
	 }
	else
 	 { if ((mr = talloc_matrix(size, size)) == NULL)
	    { return (-1);
	    }
	   for (i=0; i<size; i++)
	    { for (j=0; j<size; j++)
	       { if (i == j)
		  { ELEMENT (mr, i, j) = 1.0;
		  }
		 else
		  { ELEMENT (mr, i, j) = 0.0;
		  }
	       }
	    }
	   r->val.m = mr;
	   r->type = MATRIX;
	   return (1);
	 }
}

euclidianNorm (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *m;
	int i, j;
	double d;
	
	m = a[0].val.m;

	d = 0.0;
	for (i=0; i<m->row; i++)
	 { for (j=0; j<m->col; j++)
	    { d += ELEMENT (m, i, j) * ELEMENT (m, i, j);
	    }
	 }
	r[0].val.d = sqrt (d);
	return (1);
}
