#include <stdio.h>
#include <string.h>
#include <math.h>
#include "mat.h"

#include <stdlib.h>	/* for malloc() and calloc() */

#define A(i,j) a[i][j]
#define B(i,j) b[i][j]
#define X(i,j) x[i][j]
#define Q(i,j) q[i][j]
#define R(i,j) r[i][j]
#define Y(i,j) y[i][j]
#define Z(i,j) z[i][j]

#define V(i)   v[i]

char *strdup( const char *s)
{
    char *p;

    if( (p = (char *) malloc( strlen(s)+1)) != NULL)
	strcpy( p, s);

    return p;
}

/*
 * geteps: determine the machine precision
 *
 * The machine precision, eps, is the smallest floating point number
 * such that  1 + eps > 1
 */
double geteps( void)
{
    static double eps = 1.0;	/* machine precision result */
    double t;			/* dummy */

    if( eps == 1.0) do {
	eps = eps / 2.0;	/* keep dividing by 2 until  1+eps/2 == 1 */
	t = 1.0 + eps/2.0;
    } while( t != 1.0);

    return eps;
}

void matprint(
	FILE *fp,
	char *format,
	double *a[],
	int m, int n,
	double tol,	/* tolerance for zero */
	int io,		/* index origin */
	int num)	/* max number per line */
{
    int i, j, start, stop;

    start = 0;
    stop = n > num ? num : n;

    while( start < stop) {

	putc('\n',fp);

	if( n > num)
	    if( start+1 == stop)
		fprintf(fp,"\tcolumn %d\n", start + io);
	    else
		fprintf(fp,"\tcolumns %d to %d\n", start + io, stop + io - 1);

	for (i = 0; i < m; i++) {
	    for (j = start; j < stop; j++)
		fprintf(fp,format, (tol && fabs(A(i,j)) < tol) ? 0.0 : A(i,j));
	putc('\n',fp);
	}

    start += num;
    stop += num;
    stop = stop > n ? n : stop;
    }
}

void matcopy( double *a[], double *b[], int m, int n) /* a = b */
{
    int i, j;

    for( i = 0; i < m; i++)
	for( j = 0; j < n; j++)
	    A(i,j) = B(i,j);
}


void mattrans( double *b[], double *a[], int m, int n)
{
    int i, j;

    for( i = 0; i < m; ++i)
	for( j = 0; j < n; ++j)
	    B(j,i) = A(i,j);
}

void mattrans1( double *a[], int m) /* transpose in place for square matrix */
{
    int i, j;
    double t;

    for( i = 0; i < m; ++i)
	for( j = i+1; j < m; ++j) {
	    t = A(i,j);
	    A(i,j) = A(j,i);
	    A(j,i) = t;
    }
}

void matmul( 		/* x = y * z */
	double *x[],
	double *y[],
	double *z[],
	int m, int n, int p)
{
    int i, j, k;

    for( i = 0; i < m; i++)
	for( j = 0; j < p; j++) {
	    X(i,j) = 0;
	    for( k = 0; k < n; k++)
		X(i,j) += Y(i,k) * Z(k,j);
	}
}

double dot( 		/* r = y(0,:) * z(:,0) */
	double *y[],
	double *z[],
	int n)
{
    int i;
    double r = 0.0;

    for( i = 0; i < n; i++)
	r += Y(0,i) * Z(i,0);

    return r;
}

/*
 * matcreate: allocates space for m-by-n matrix, returns pointer if
 *  successful, NULL if not.  The matrix elements are initialized to zero.
 *  The matrix elements are contiguous, since space for all of the rows
 *  is obtained with one calloc() call, thus the matrix can be passed
 *  to f77 or pascal routines. a[-1] (as well as a[0]) is set to point to the
 *  storage so that rows may be interchanged by swapping row pointers, and
 *  matfree() can still reference the storage through a[-1] even if a[0] was
 *  changed.
 */
double **matcreate(
	int m,	/* number of rows */
	int n)	/* number of columns */
{
    double **a;		/* temporary pointer */
    int i;		/* row index */

    if( m < 1 || n < 1)
	return NULL;
    /*
     * first, get space for m+1 pointers to double
     */
    if( (a = (double **) malloc( (m+1) * sizeof(double *)) ) == NULL)
	return NULL;
    /*
     * get space for all of the rows, m*n doubles
     */
    if( (a[0] = a[1] = (double *) calloc( m*n, sizeof(double)) ) == NULL) {
	free(a);
	return NULL;
    }
    /*
     * set the pointers a[1]..a[m-1] to point to their row
     */
    ++a;
    for( i = 1; i < m; i++)
	a[i] = a[i-1] + n;

    return a;
}

void matfree( double **a)
{
    if( a) {
	free(a[-1]);	/* free the matrix data */
	free(a-1);	/* free the row pointers */
    }
}

double norm(double *a[], int m, int n) /* just sqrt of sum-of-squares for now */
{
    double r = 0;
    int i, j;

    for( i = 0; i < m; i++)
	for( j = 0; j < n; j++)
	    r += A(i,j) * A(i,j);

    return sqrt(r);
}

void ones( double *a[], int m, int n)
{
    int i, j;

    for( i = 0; i < m; i++)
	for( j = 0; j < n; j++)
	    A(i,j) = 1.0;
}

void zeros( double *a[], int m, int n)
{
    int i, j;

    for( i = 0; i < m; i++)
	for( j = 0; j < n; j++)
	    A(i,j) = 0.0;
}

void eye( double *a[], int m, int n) /* put 1's on diagonal of zero matrix */
{
    int i;

    m = m < n ? m : n;

    for( i = 0; i < m; i++)
	A(i,i) = 1.0;
}

/*
 * QR: orthogonalization using Housholder Transformations,
 *	based on algorithms 3.3-1 and 3.3-2 from Golub and Van Loan
 *
 * On entry, q must be either an m-by-m identity matrix, or NULL if computation
 * of q is not desired, and r is the m-by-n matrix to be decomposed which is
 * overwritten with the result.
 */
void qr( double *q[], double *r[], int m, int n)
{
    int i, j, k, p;
    double alpha, beta, s, t;
    double *v;

    if( (v = (double *) malloc(m*sizeof(double))) == NULL)
	error("malloc() failed in qr()","");

    p = n < m-1 ? n : m-1;

    for( k = 0; k < p; k++) {	/* go across the columns of r */

	t = fabs(R(k,k));
	for( i = k+1; i < m; i++)
	    t = fabs(R(i,k)) > t ? fabs(R(i,k)) : t;

	if( t == 0.0)
	    continue;

	alpha = 0;
	for( i = k; i < m; i++) {
	    V(i) = R(i,k) / t;
	    alpha += V(i) * V(i);
	}

	alpha = sqrt(alpha);
	beta = alpha * (alpha + fabs(V(k)));
	V(k) += V(k) > 0 ? alpha : -alpha;

	R(k,k) = V(k) > 0 ? -t * alpha : t * alpha;
	for( i = k+1; i < m; ++i)
	    R(i,k) = 0;

	for( j = k+1; j < n; j++) {		/* update r, by columns */
	    s = 0;
	    for( i = k; i < m; i++)
		s += V(i) * R(i,j);
	    s /= beta;
	    for( i = k; i < m; i++)
		R(i,j) -= s * V(i);
	}

	if( q) for( i = 0; i < m; i++) {	/* update q, by rows */
	    s = 0;
	    for( j = k; j < m; j++)
		s += V(j) * Q(i,j);
	    s /= beta;
	    for( j = k; j < m; j++)
		Q(i,j) -= s * V(j);
	}
    }
    free(v);
}