class Matrix {

    /* Assumes aMat is symmetric.  Performs a cholesky
       factorization, leaving the result in the upper triangle
       of aMat. 
    */
    public static void cholesky(double [2d] a) {
	int high = a.domain().max()[1];
	int low = a.domain().min()[1];
	double sum, diff;
	int i, j, l; 

	for (i = low; i <= high; i++) {
	    sum = 0.0;
	    for (j=low; j < i; j++) {
		sum += a[j,i]*a[j,i];
	    }
	    diff = a[i,i] - sum;
	    if (diff <= 0.0) {
		System.out.println("Matrix is not positive definite");
		throw new IllegalArgumentException();
	    }

	    a[i,i] = Math.sqrt(diff);
	    for (l = i+1; l <= high; l++) {
		sum = 0.0;
		for (j=low; j < i; j++) {
		    sum += a[j,l]*a[j,i];
		}
		a[i,l] = (a[i,l] - sum) / a[i,i];
	    }
	}
    }

    /* Solves for x in: u * x = y, assuming u is an upper triangular 
       matrix. The argument x is used as the right-hand-side and is 
       overwritten with the result. Also assumes that the domain of x 
       is the same as the domain of both dimensions of u. */
    public static void triangSolveUpper(double [2d] u, double [1d] x) {
	// view x in the same domain as a column/row of u
	Point <1> diff = [u.domain().min()[1]] - x.domain().min();
	x = x.translate(diff); 
	int low = x.domain().min()[1];
	int high = x.domain().max()[1];
	int i, j;
	for (i = high; i >= low; i--) {
	    double sum = 0.0;
	    for (j = i+1; j <= high; j++) {
		sum += u[i,j]*x[j];
	    }
	    x[i] = (x[i] - sum) / u[i,i];
	}
    }

    /* Solves for x in: u * x = y, assuming m is a lower triangular 
       matrix. The argument x is used as the right-hand-side and is 
       overwritten with the result. Also assumes that the size of x 
       is the same as the both dimensions of m. */
    public static void triangSolveLower(double [2d] m, double [1d] x) {
	// view x in the same domain as a column/row of m
	Point <1> diff = [m.domain().min()[1]] - x.domain().min();
	x = x.translate(diff); 
	int low = x.domain().min()[1];
	int high = x.domain().max()[1];
	int i, j;
	for (i = low; i<= high; i++) {
	    double sum = 0.0;
	    for (j = low; j < i; j++) {
		sum += m[i,j]*x[j];
	    }
	    x[i] = (x[i] - sum) / m[i,i];
	}
    }

    public static String toString (double [2d] a) {
	int lowX = a.domain().min()[1];
	int lowY = a.domain().min()[2];
	int highX = a.domain().max()[1];
	int highY = a.domain().max()[2];
	String result = "[";
	for (int i = lowX; i < highX; i++) {
	    for (int j = lowY; j < highY; j++) {
		result += a[i,j] + ", ";
	    }
	    result += a[i,highY] + "\n ";
	}
	for (int j = lowY; j < highY; j++) {
	    result += a[highX,j] + ", ";
	}
	result += a[highX,highY] + "]\n";
	return result;
    }

    public static void tester (String [] args)
    {
	int n = 7;
	if (args.length > 0) {
	    n = Integer.parseInt(args[0]);
	}
	double [2d] u;
	double [1d] x; 

	System.out.println("Test 1, 1D Poisson:");
	u = (new Poisson1D(n)).toDense(); 
	System.out.println(toString(u));
	System.out.println("After factoring:");
	cholesky(u);
	System.out.println(toString(u));
	System.out.println("RHS for solve:");
	x = new double [0:n-1];
	x.set(1.0);
	System.out.println(Vector.toString(x));
	System.out.println("After solve:");
	triangSolveUpper(u,x);
	System.out.println(Vector.toString(x));
	System.out.println("Transposed factor:");
	double [2d] l = new double [0:n-1,0:n-1];
	l.copy(u.permute([2,1]));
	System.out.println(toString(l));
	x.set(1.0);
	System.out.println("Solve again:");
	triangSolveLower(l,x);
	System.out.println(Vector.toString(x));
	System.out.println("After low/up solve:");
	triangSolveUpper(u,x);
	System.out.println(Vector.toString(x));

	System.out.println("Test 2, 1D Poisson submatrix (n/4-elem border):");
	u = (new Poisson1D(2*n)).toDense(); 
	double [2d] interior = u.restrict(u.domain().shrink(n/2));
	System.out.println(toString(u));
	cholesky(interior);
	System.out.println("After factoring:");
	System.out.println(toString(u));
	x = new double [u.domain().shrink(n/2).slice(1)];
	x.set(1.0);
	System.out.println(Vector.toString(x));
	System.out.println("After solve:");
	triangSolveUpper(interior,x);
	System.out.println(Vector.toString(x));

    }
}