%mathpiper,def="MatrixSolve"

//Retract("MatrixSolve",*);

10 # MatrixSolve(matrix_IsDiagonal,b_IsVector) <--
[
    If(InVerboseMode(),Tell("   MatrixSolve_diag",{matrix,b}));
	Local(rowsm,rowsb,x);
    rowsm:=Length(matrix);
	rowsb:=Length(b);
	Check(rowsm=rowsb, "Argument", "MatrixSolve: Matrix and vector must have same number of rows");
	x:=ZeroVector(rowsb);
	ForEach(i,1 .. rowsb)
		x[i]:=b[i]/matrix[i][i];
	x;
];

// Backward Substitution
15 # MatrixSolve(matrix_IsUpperTriangular,b_IsVector) <--
[
    If(InVerboseMode(),Tell("   MatrixSolve_ut",{matrix,b}));
    Local(rowsm,rowsb,x,s);
    rowsm:=Length(matrix);
    rowsb:=Length(b);
    Check(rowsm=rowsb, "Argument", "MatrixSolve: Matrix and vector must have same number of rows");
    x:=ZeroVector(rowsb);

	x[rowsb]:=b[rowsb]/matrix[rowsb][rowsb];
	If(InVerboseMode(),Echo({"set x[",rowsb,"] = ",b[rowsb]/matrix[rowsb][rowsb]}));

	ForEach(i,(rowsb-1) .. 1 )[
		s:=b[i];
		ForEach(j,i+1 .. rowsb )[
			s:= s - matrix[i][j]*x[j];
		];
		x[i]:= s/matrix[i][i];
		If(InVerboseMode(),Echo({"set x[",i,"] = ",s/matrix[i][i]}));
	];
        x;
];

// Forward Substitution
15 # MatrixSolve(matrix_IsLowerTriangular,b_IsVector) <--
[
    If(InVerboseMode(),Tell("   MatrixSolve_lt",{matrix,b}));
	Local(rowsm,rowsb,x,s);
	rowsm:=Length(matrix);
	rowsb:=Length(b);
	Check(rowsm=rowsb, "Argument", "MatrixSolve: Matrix and vector must have same number of rows");
	x:=ZeroVector(rowsb);

	x[1]:=b[1]/matrix[1][1];
	If(InVerboseMode(),Echo({"set x[1] = ",b[1]/matrix[1][1]}));

	ForEach(i,2 .. rowsb )[
		s:=b[i];
		ForEach(j,1 .. (i-1) )[
			s:= s - matrix[i][j]*x[j];
		];
		x[i]:= s/matrix[i][i];
		If(InVerboseMode(),Echo({"set x[",i,"] = ",s/matrix[i][i]}));
	];
	x;
];
// Gaussian Elimination and Back Substitution
// pivoting not implemented yet
20 # MatrixSolve(matrix_IsMatrix,b_IsVector) <--
[
    If(InVerboseMode(),Tell("   MatrixSolve",{matrix,b}));
	Local(aug,rowsm,rowsb,x,s);
    rowsm:=Length(matrix);
    rowsb:=Length(b);
    Check(rowsm=rowsb, "Argument", "MatrixSolve: Matrix and vector must have same number of rows");
    aug:=ZeroMatrix(rowsb,rowsb+1);
	x:=ZeroVector(rowsb);

	// create augmented matrix
	ForEach(i, 1 .. rowsb )
		ForEach(j, 1 .. rowsb )
			aug[i][j] := matrix[i][j];
	ForEach(i, 1 .. rowsb )
		aug[i][rowsb+1] := b[i];

	// gaussian elimination
	ForEach(i, 1 .. (rowsb-1) )[
		// If our pivot element is 0 we need to switch
		// this row with a row that has a nonzero element
		If(aug[i][i] = 0, [
			Local(p,tmp);
			p:=i+1;
			While( aug[p][p] = 0 )[ p++; ];
			If(InVerboseMode(), Echo({"switching row ",i,"with ",p}) );
			tmp:=aug[i];
			aug[i]:=aug[p];
			aug[p]:=tmp;
		]);


		ForEach(k, (i+1) .. rowsb )[
			s:=aug[k][i];
			ForEach(j, i .. (rowsb+1) )[
				aug[k][j] := aug[k][j] - (s/aug[i][i])*aug[i][j];
				//Echo({"aug[",k,"][",j,"] =", aug[k][j]," - ",
				//	aug[k][i],"/",aug[i][i],"*",aug[i][j]," k i =", k,i  });
			];
		];
	];
	//PrettyForm(aug);
	x[rowsb]:=aug[rowsb][rowsb+1]/aug[rowsb][rowsb];
	If(InVerboseMode(),Echo({"set x[",rowsb,"] = ",x[rowsb] }));

	ForEach(i,(rowsb-1) .. 1 )[
		s:=aug[i][rowsb+1];
		ForEach(j,i+1 .. rowsb)[
			s := s - aug[i][j]*x[j];
		];
		x[i]:=Simplify(s/aug[i][i]);
		If(InVerboseMode(),Echo({"set x[",i,"] = ",x[i] }));
	];
	x;
];

%/mathpiper

    %output,preserve="false"
      Result: True
.   %/output



%mathpiper_docs,name="MatrixSolve",categories="User Functions;Solvers (Symbolic);Linear Algebra"
*CMD MatrixSolve --- solve a system of equations
*STD
*CALL
	MatrixSolve(A,b)

*PARMS

{A} -- coefficient matrix

{b} -- row vector

*DESC

{MatrixSolve} solves the matrix equations {A*x = b} using Gaussian Elimination
with Backward substitution. If your matrix is triangular or diagonal, it will
be recognized as such and a faster algorithm will be used.

*E.G.

In> A:={{2,4,-2,-2},{1,2,4,-3},{-3,-3,8,-2},{-1,1,6,-3}};
Result: {{2,4,-2,-2},{1,2,4,-3},{-3,-3,8,-2},{-1,1,6,-3}};
In> b:={-4,5,7,7};
Result: {-4,5,7,7};
In> MatrixSolve(A,b);
Result: {1,2,3,4};
%/mathpiper_docs