Проверьте алгоритм на VBA

[Amigo]

Добрый день!
Проверьте пожалуйста алгоритм приведения матрицы к ступенчатому виду на языке
VBA.




Sub Syst()
ReDim A(4, 4) As Double
Dim i As Integer
Dim j As Integer
Dim k As Integer
Dim p As Integer
Dim t As Integer
Dim m As Integer
Dim c As Double

//Инициализируем матрицу
A(0, 0) = 0
A(0, 1) = 0
A(0, 2) = 2
A(1, 0) = 0
A(1, 1) = 1
A(1, 2) = 1
A(2, 0) = 0
A(2, 1) = 1
A(2, 2) = 0



n = 2 //количество столбцов
m = 2//количество строк
k = 1
r = -1//Число, смысл которого состоит в том,что бы определить номер столбца в котором находится текущий ненулевой элемент
For i = 0 To m - 1
2: q = 0
r = r + 1

If r > n Then GoTo 3

For p = i To m //Цикл, реализующий поиск ненулевого элемента в столбце
If A(p, r) <> 0 Then
For t = 0 To n //цикл, осуществляющий элементарное преобразование 3-го типа. т.е. меняет строки местами если найден ненулевой элемент в просматриваемой предыдущим циклом строке
A(m + 1, t) = A(i, t)
A(i, t) = A(p, t)
A(p, t) = A(m + 1, t)
Next t
q = 1
GoTo 1
End If
Next p


1: If q = 0 Then GoTo 2

For j = i To (m - 1)
c = -(A(j + 1, r)) / A(i, r)
For k = r To m
A(j + 1, k) = c * A(i, k) + A(j + 1, k)
Next k
Next j
Next i

3:
End Sub

-----------------------------------------------------------------------------
Проблема в том, что я запутался в массе текущих индексов, и немогу в голове удержать
правильное их расположение, хотя все проверенные мною примеры давали правильный результат при использовании алгоритма. Кстати, существует ли несколько контрольных примеров, в результате выполнения которых можно было бы наверняка убедится, что алгоритм верен?

И ещё одна проблема: пока думал над этой задачей, у меня в голове графически представлялась одна картина. Теперь мне нужно осуществить "обратный ход" т.е.
привести матрицу не к ступенчатому а к диагональному виду. Я совершенно немогу это в силу инерции сознания осуществить. Не могли бы Вы мне подсказать, что именно мне нужно поменять в текущем алгоритме, что бы осуществить проход не сверху вниз, а наоборот - снизу вверх?