#include <math.h>
#include "misc.h"
#include "matvec.h"
#include "stdlib.h"
#include "assert.h"
#include <stdarg.h>

static foint _DEADBEEF = {16045690984833335023ULL}; /* decimal rep. of DEADBEEFDEABFEEF */
/* A sparse matrix is marked by *every* row having its last element
 * equal to 0xDEADBEEFDEADBEEF
 */
Boolean MatIsSparse(int n, int m, const double A[n][m])
{
    int i;
    assert(_DEADBEEF.l == 0xDEADBEEF && _DEADBEEF.l2 == 0xDEADBEEF);
    if(n==0 || m==0) return false;
    for(i=0; i<n; i++)
	if(A[i][m-1] != _DEADBEEF.d) return false;
    return true;
}

Boolean MatSparseSanity(int n, int m, const double A[n][m])
{
    int i;
    assert(MatIsSparse(n,m,A));

    /* Now verify values */
    for(i=0;i<n;i++)
    {
	int nElem=A[i][0], j;

	/* Check if it's an integer */
	assert(nElem == A[i][0]);
	assert(0 <= nElem && nElem < (m-2)/2);
	    /*Fatal("Hmmm, this %dx%d matrix looks sparse but A[%d][0]=%d is not "
		    "a reasonable number of non-zero elements for row %d",
		    n,m, i, nElem, i);*/

	for(j=1;j<=nElem;j++)
	{
	    int col = A[i][j];
	    assert(A[i][j] == col); /* ensure it's an integer */
	    assert(0 <= col && col < m);
		/*Fatal("Hmmm, this %dx%d matrix looks sparse but A[%d][%d]=%d "
			"is out of bounds", n,m, i, j, col);*/
	    /* ensure column numbers are increasing */
	    if(j>1)
		assert(A[i][j-1] < col);
		/*Fatal("Hmmm, this %dx%d matrix looks sparse but "
			"(A[%d][%d]=%d) <= (A[%d][%d]=%g) is a non-increasing column number",
			n,m, i, j, col, i, j-1, A[i][j-1]);*/
	}
    }
    return true;
}

static void MatRawCopy(int n, int m, double dest[n][m], const double src[n][m])
{
    int i,j;
    for(i=0; i<n; i++)
	for(j=0; j<n; j++)
	    dest[i][j] = src[i][j];
}

Boolean MatMakeSparse(int n, int m, double S[n][m], const double A[n][m])
{
    int i;
    double T[n][m]; /* temp matrix */
    assert(!MatIsSparse(n,m,A));
    for(i=0;i<n;i++)
    {
	int nElem = 0, col, j;
	T[i][m-1] = _DEADBEEF.d;
	for(j=0;j<m;j++)
	    if(A[i][j]) nElem++;
	assert(nElem <= (m-2)/2);
	col=nElem;
	for(j=m-1; j>=0; j--)
	{
	    if(A[i][j])
	    {
		T[i][col] = j;
		T[i][nElem+col] = A[i][j];
		col--;
	    }
	}
	/* Before blasting A[i][0], ensure its element is either zero, or stored */
	assert(col==0 && (nElem == 0 || A[i][0] == 0.0 || (T[i][1]==0 && T[i][nElem+1] == A[i][0])));
	T[i][0] = nElem;
    }
    MatRawCopy(n,m,S,T);
    assert(MatIsSparse(n,m,S));
}

void MatMakeUnSparse(int n, int m, double A[n][m], const double S[n][m])
{
    int i;
    double T[n][m]; /* temp matrix */
    assert(MatIsSparse(n,m,S));
    for(i=0;i<n;i++)
    {
	int nElem = S[i][0], j;
	assert(nElem == S[i][0]);
	for(j=0;j<m;j++) T[i][j] = 0.0;
	for(j=1; j <=nElem; j++)
	{
	    int col = S[i][j];
	    assert(S[i][j] == col);
	    T[i][col] = S[i][nElem+j];
	}
    }
    MatRawCopy(n,m,A,T);
    assert(!MatIsSparse(n,m,A));
}


double *VecAssign(double *v, int n, ...)
{
    int i;
    va_list ap;

    va_start(ap, n);
    for(i=0; i<n; i++)
        v[i] = (double)va_arg(ap, double);
    va_end(ap);
    return v;
}

void VecPut(int dim, const double y[dim])
{
    int i;
    for(i=0; i<dim; i++)
	printf("%18.15g ", y[i]);
}

void MatPut(int n, int m, const double yy[n][m])
{
    int i;
    assert(!MatIsSparse(n,m,yy));
    for(i=0; i<n; i++)
    {
	printf("%2d: ", i);
	VecPut(m, yy[i]);
	printf("\n");
    }
    fflush(stdout);
}


void VecGet(int n, double yy[n])
{
    int i;
    for(i=0; i<n; i++)
	scanf("%lg", &(yy[i]));
}


void MatGet(int n, int m, double yy[n][m])
{
    int i;
    for(i=0; i<n; i++)
	VecGet(m, yy[i]);
    assert(!MatIsSparse(n,m,yy));
}


/*
** Random matrix with entries in [0,1].
*/
void MatRand(int n, int m, double A[n][m])
{
    int i,j;
    for(i=0; i<n; i++) for(j=0;j<m;j++)
	A[i][j] = drand48();
}
    
void MatCopy(int n, int m, double dest[n][m], const double src[n][m])
{
    int i,j;
    assert(!MatIsSparse(n,m,src));
    for(i=0; i<n; i++)
	for(j=0; j<n; j++)
	    dest[i][j] = src[i][j];
}

/*
** Compute matrix-matrix product A = B*C.  Any of A, B, C can be the same
** memory space.
*/
void MatMatMult(int n, int m, int p, double A[n][p],
    double const B[n][m], const double C[m][p])
{
    int i,j,k, numZeros=0;
    static Boolean warned = false;

    assert(!MatIsSparse(n,m,B) && !MatIsSparse(m,p,C));

#define MMMULT_COUNT_ZEROS 1
#if MMMULT_COUNT_ZEROS
    for(i=0; i<n; i++)
	for(j=0;j<m;j++)
	    if(B[i][j] == 0.0)
		numZeros++;

    for(j=0;j<m;j++)
	for(k=0; k<p; k++)
	    if(C[j][k] == 0.0)
		numZeros++;
    if(numZeros > 0.5*(n*m+m*p) && !warned)
    {
	Warning("MatMatMult: input matrices %g%% zero; consider sparse representation", 100.*numZeros/(n*m+m*p));
	warned = true;
    }
#endif

    if(A != B && A != C)
    {
	for(i=0; i<n; i++)
	    for(k=0; k<p; k++)
	    {
		A[i][k] = 0;
		for(j=0; j<m; j++)
		    A[i][k] += B[i][j] * C[j][k];
	    }
    }
    else    /* be careful */
    {
	double result[n][p];
	for(i=0; i<n; i++)
	    for(k=0; k<p; k++)
	    {
		result[i][k] = 0;
		for(j=0; j<m; j++)
		    result[i][k] += B[i][j] * C[j][k];
	    }
	for(i=0; i<n; i++)
	    for(k=0; k<p; k++)
		A[i][k] = result[i][k];
    }
}



/*
** Compute yy = A * xx.  Return pointer to array yy.
*/
double *MatVecMult(int rows, int cols, double yy[rows],
    const double A[rows][cols], const double xx[cols])
{
    int i;
    assert(!MatIsSparse(rows,cols,A));
    if(yy != xx)
    {
	for(i=0; i<rows; i++)
	{
	    int j;
	    yy[i] = 0;
	    for(j=0; j<cols; j++)
		yy[i] += A[i][j]*xx[j];
	}
    }
    else    /* be careful */
    {
	double xxx[cols];
	for(i=0; i<cols; i++)
	    xxx[i] = xx[i];
	for(i=0; i<rows; i++)
	{
	    int j;
	    yy[i] = 0;
	    for(j=0; j<cols; j++)
		yy[i] += A[i][j]*xxx[j];
	}
    }
    return yy;
}


/*
** FLOPS ~ 12M
*/
double VecDot(int dim, const double vec1[dim], const double vec2[dim])
{
    int i;
    double sum = 0;
    for(i=0; i<dim; i++)
	sum += vec1[i]*vec2[i];
    return sum;
}

/*
**  Vector cross product
*/
void VecCrossProd(int dim, double prod[dim], const double u[dim], const double v[dim])
{
    assert(dim==3);
    prod[0] = u[1]*v[2] - u[2]*v[1];
    prod[1] = u[2]*v[0] - u[0]*v[2];
    prod[2] = u[0]*v[1] - u[1]*v[0];
}

/*
** Yes I know I could just return sqrt(dot(v,v)), but it may be faster
** this way, since the compiler knows they're both the same vector.
** FLOPS ~ 18M
*/
double VecLength(int dim, const double vec[dim])
{
    int i;
    double sum = 0;
    for(i=0; i<dim; i++)
	sum += SQR(vec[i]);
    return sqrt(sum);
}

/*
** FLOPS ~ 24M
*/
double *VecNormalize(int dim, double dest[dim], const double vec[dim])
{
    double mag_ = 1./VecLength(dim, vec);
    if(mag_ != 1.0)
    {
	int i;
	for(i=0; i<dim; i++)
	    dest[i] = mag_ * vec[i]; /* this is faster than division */
    }
    return dest;
}

double VecNorm1(int n, const double y[n])
{
    double sum = 0;
    int i;
    for(i=0; i<n; i++)
	sum += ABS(y[i]);
    return sum;
}

double VecNormEucl(int n, const double y[n])
{
    double sum = 0;
    int i;
    for(i=0; i<n; i++)
	sum += SQR(y[i]);
    return sqrt(sum);
}

double MatNormEucl(int n, int m, const double A[n][m])
{
    assert(!MatIsSparse(n,m,A));
    return VecNormEucl(n*m, (const double*)A);
}

double *VecAdd(int dim, double result[dim], const double v1[dim], const double v2[dim])
{
    int i;
    for(i=0; i<dim; i++)
	result[i] = v1[i] + v2[i];
    return result;
}

double *VecDiff(int dim, double result[dim], const double v1[dim], const double v2[dim])
{
    int i;
    for(i=0; i<dim; i++)
	result[i] = v1[i] - v2[i];
    return result;
}

void MatDiff(int n,int m, double result[n][m], const double A1[n][m], const double A2[n][m])
{
    assert(!MatIsSparse(n,m,A1) && !MatIsSparse(n,m,A2));
    VecDiff(n*m, (double*)result, (const double*)A1, (const double*)A2);
}

double *VecScalMul(int dim, double result[dim], double k, const double v[dim])
{
    int i;
    for(i=0; i<dim; i++)
	result[i] = k*v[i];
    return result;
}

double *VecSetZero(int dim, double v[dim])
{
    int i;
    for(i=0; i<dim; i++)
	v[i] = 0.;
    return v;
}

double *VecCopy(int dim, double dest[dim], const double src[dim])
{
    int i;
    for(i=0; i<dim; i++)
	dest[i] = src[i];
    return dest;
}

/*
** Compute the matrix exponential e^A, using Taylor expansion, until
** the individual elements change by less than eps.
** result = 1 + A;
** term = A;
** k = 1;
** do {
**      k++;
**      term *= A/k;
**      result += term;
** } while(inftyNorm(term) > eps);
*/
void MatExpMat(int n, double result[n][n], const double A[n][n], double eps)
{
    double term[n][n], maxNorm, Ac[n][n];
    int i,j, k = 1;

    assert(!MatIsSparse(n,n,A));

    /* In case result == A.  Even if it isn't, who cares, this copy
    ** is almost free in comparison to the computation below.
    */
    for(i=0; i<n; i++) for(j=0; j<n; j++) Ac[i][j] = A[i][j];

    for(i=0; i<n; i++)
    {
	result[i][i] = 1 + Ac[i][i];
	term[i][i] = Ac[i][i];

	for(j=i+1; j<n; j++)
	{
	    result[i][j] = term[i][j] = Ac[i][j];
	    result[j][i] = term[j][i] = Ac[j][i];
	}
    }

    do
    {
	maxNorm = 0;
	k++;
	MatMatMult(n,n,n, term, term, Ac);
	for(i=0; i<n; i++) for(j=0; j<n; j++)
	{
	    term[i][j] /= k;
	    result[i][j] += term[i][j];
	    if(fabs(term[i][j]) > maxNorm)
		maxNorm = fabs(term[i][j]);
	}
    } while(maxNorm > eps);
}

void MatInverse(int n, int m, double AI[m][n], const double A[n][m])
{
    double L[n][n], U[n][n], tmp[n], e[n], B[n][n];
    int i;

    assert(!MatIsSparse(n,m,A));
    if(n!=m)
	Apology("n must= m in MatInverse");

    MatLUFact(n, L, U, A);
    for(i=0; i<n; i++)
    {
	VecSetZero(n,e);
	e[i] = 1;
	MatForwardSubst(n, tmp, L, e);
	MatBackSubst(n, B[i], U, tmp);
    }
    MatTranspose(n,n,AI,B);
}


void MatTranspose(int n, int m, double AT[m][n], const double A[n][m])
{
    double at[m][n];
    int i,j;
    assert(!MatIsSparse(n,m,A));
    for(i=0; i<n; i++) for(j=0; j<m; j++)
	at[j][i] = A[i][j];
    
    MatCopy(m, n, AT, at);
}

void MatLUFact(int n, double L[n][n], double U[n][n], const double A[n][n])
{
    int i, j, k;
    
    assert(!MatIsSparse(n,n,A));

    /* L := Id; U := A */
    for(i=0; i<n; i++) for(j=0; j<n; j++)
    {
	L[i][j] = (double)(i==j);
	U[i][j] = A[i][j];
    }

    for(k=0; k<n-1; k++)
    {
	for(i=k+1; i<n; i++)
	{
	    if(U[k][k] != 0.0)
	    {
		double c = U[i][k] / U[k][k];
		L[i][k] = c;
		for(j=k; j<n; j++)
		    U[i][j] -= c*U[k][j];
	    }
	}
    }
}

double *MatForwardSubst(int n, double y[n], const double L[n][n],
    double const b[n])
{
    int i,j;

    assert(!MatIsSparse(n,n,L));

    VecCopy(n, y, b);

    for(i=0; i<n; i++)
    {
	for(j=0; j<=i-1; j++)
	{
	    y[i] -= L[i][j] * y[j];
	}
	y[i] /= L[i][i];
    }

    return y;
}

double *MatBackSubst(int n, double x[n], const double U[n][n],
    const double b[n])
{
    int i,j;

    assert(!MatIsSparse(n,n,U));

    VecCopy(n, x, b);
    for(i=n-1; i>=0; i--)
    {
	for(j=i+1; j<n; j++)
	    x[i] -= U[i][j] * x[j];
	x[i] = x[i] / U[i][i];
    }
    return x;
}

double *MatSolve(int n, const double A[n][n], double x[n], const double b[n])
{
    double L[n][n], U[n][n], tmp[n];
    assert(!MatIsSparse(n,n,A));
    MatLUFact(n, L, U, A);
    MatForwardSubst(n, tmp, L, b);
    return MatBackSubst(n, x, U, tmp);
}

#if 0 /* TEST */
int main(int argc, char *argv[])
{
    int n= atoi(argv[1]), i;
    double A[n][n], B[n][n], C[n][n], I[n][n], tmpMat[n][n];

    VecSetZero(n*n, (double*)I);
    for(i=0; i<n;i++)
	I[i][i] = 1;

    for(i=0;i<10;i++)
    {
	MatRand(n,n,A);
	printf("Matrix:\n");
	MatInverse(n,n,B,A);

	MatMatMult(n,n,n,C,A,B);
	MatDiff(n,n,tmpMat,C,I);
	printf("%g\n", MatNormEucl(n,n,tmpMat));

	MatMatMult(n,n,n,C,B,A);
	MatDiff(n,n,tmpMat,C,I);
	printf("%g\n", MatNormEucl(n,n,tmpMat));
    }
    return 0;
}
#endif
