## 4.3-4
##
## Work this problem "by hand" i.e. perform Algorithm 4.2 "by hand"
##
source('utils.R')

n_digits = 3

## Initialize the W matrix:
##
W = matrix(data=c(0.21, 0.32, 0.12, 0.31, 0.96, 0.1, 0.15, 0.24, 0.22, 0.71, 0.2, 0.24, 0.46, 0.36, 1.26, 0.61, 0.4, 0.32, 0.2, 1.53), nrow=4, ncol=5, byrow=TRUE)
W_orig = W
print('Initial W matrix:')
print(W_orig)

n = dim(W)[1]

ps = 1:n ## ps[kk] is the pivot equation used in the kk-th step

## Compute the size d_i of each row i of A:
##
di = apply(abs(W[, 1:n]), 1, max, na.rm=TRUE)
print('di=')
print(di)

kk = 1 ## <- kk is now the 'step' index

## Select the pivot row for the kk-th step:
##
pivot_row_indx = which.max(abs(W[kk:n, kk])/di[kk:n]) + (kk-1)
old = ps[kk] ## <- exchange these two rows
ps[kk] = ps[pivot_row_indx]
ps[pivot_row_indx] = old
print(sprintf('kk= %d pivot selection:', kk))
print(ps)

old = di[kk]
di[kk] = di[pivot_row_indx]
di[pivot_row_indx] = old

print(sprintf('Exchanging row kk= %d with row= %d:', kk, pivot_row_indx))
W = exchange_two_rows(W, kk, pivot_row_indx)
print(W)

##
## Use the first row to eliminate all rows below it:
##

## The second row:
##
ii = 2
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('After second row elimination:')
print(W)

## The third row:
##
ii = 3
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('After third row elimination:')
print(W)

## The fourth row:
##
ii = 4
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('After fourth row elimination:')
print(W)


kk = 2

## Select the pivot row for the kk-th step:
##
pivot_row_indx = which.max(abs(W[kk:n, kk])/di[kk:n]) + (kk-1) ## <- from the choices kk:n
old = ps[kk] ## <- exchange these two rows
ps[kk] = ps[pivot_row_indx]
ps[pivot_row_indx] = old
print(sprintf('kk= %d pivot selection:', kk))
print(ps)

old = di[kk]
di[kk] = di[pivot_row_indx]
di[pivot_row_indx] = old
print(sprintf('Exchanging row kk= %d with row= %d:', kk, pivot_row_indx))

W = exchange_two_rows(W, kk, pivot_row_indx)
print(W)

##
## Use the second row to eliminate all rows below it:
##

## The third row:
##
ii = 3
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('After third row elimination:')
print(W)

## The fourth row:
##
ii = 4
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('After fourth row elimination:')
print(W)


kk = 3

## Select the pivot row for the kk-th step:
##
pivot_row_indx = which.max(abs(W[kk:n, kk])/di[kk:n]) + (kk-1) ## <- from the choices kk:n
old = ps[kk] ## <- exchange these two rows
ps[kk] = ps[pivot_row_indx]
ps[pivot_row_indx] = old
print(sprintf('kk= %d pivot selection:', kk))
print(ps)

old = di[kk]
di[kk] = di[pivot_row_indx]
di[pivot_row_indx] = old

print(sprintf('Exchanging row kk= %d with row= %d:', kk, pivot_row_indx) )
W = exchange_two_rows(W, kk, pivot_row_indx)
print(W)

##
## Use the second row to eliminate all rows below it:
##

## The fourth row:
##
ii = 4
m = W[ii, kk] = signif(W[ii, kk] / W[kk, kk], n_digits)
jj = (kk+1):(n+1)
W[ii, jj] = signif(W[ii, jj] - m * W[kk, jj], n_digits)
print('final W before backsubstitution:')
print(W)

x = matrix(data=0, nrow=n, ncol=1, byrow=TRUE)

## Backsubstitution:
##
kk = 4
x[kk] = signif(W[kk, n+1]/W[kk, kk], n_digits)
print(x)

kk = 3
x[kk] = signif((W[kk, n+1] - sum(W[kk, (kk+1):n] * x[(kk+1):n]) )/W[kk, kk], n_digits)
print(x)

kk = 2
x[kk] = signif(( W[kk, n+1] - sum(W[kk, (kk+1):n] * x[(kk+1):n]) )/W[kk, kk], n_digits)
print(x)

kk = 1
x[kk] = signif((W[kk, n+1] - sum(W[kk,(kk+1):n] * x[(kk+1):n]))/W[kk, kk], n_digits)

## Restore the correct order of the solutions:
##
x = x[ps]
print(sprintf('n_digits in signif= %d', n_digits) )
print(x)