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

n_digits = 4
verbose = FALSE

## Initialize the W matrix:
##
W_orig = matrix(data=c(3e-4, 1.566, 1.569, 0.3454, -2.436, 1.018), nrow=2, ncol=3, byrow=TRUE) ## <- the matrix in Example 4.4
W_orig = matrix(data=c(8e-4, 1.566, 1.569, 0.3454e-3, -2.436, 1.018), nrow=2, ncol=3, byrow=TRUE) ## <- this matrix seems to show the desired behavior
n = dim(W_orig)[1]

print('Initial W matrix:')
print(W_orig)
x_true = solve(W_orig[, 1:n], W_orig[, n+1])
print('True solution x=')
print(x_true)

##
## Scaled partial pivoting: 
## 
W = W_orig 

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)
if( verbose ){
    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
if( verbose ){
    print(sprintf('kk= %d pivot selection.  ps=', kk))
    print(ps)
}

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

W = exchange_two_rows(W, kk, pivot_row_indx)
if( verbose ){
    print(sprintf('Exchanging row kk= %d with row= %d:', 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)
if( verbose ){
    print('Final W before backsubstitution:')
    print(W)
}

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

## Backsubstitution:
##
kk = 2
x[kk] = signif(W[kk, n+1]/W[kk, kk], n_digits)
if( verbose ){
    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)

print(sprintf('Using scaled partial pivoting (n_digits in signif= %d); x=', n_digits) )
print(x)
print(sprintf('error in x= %10.6f', sum((x-x_true)^2)))


##
## Total pivoting: 
## 
W = W_orig

n = dim(W)[1]

pivot_benefit = abs(W[1:n, 1:n])
print('pivot_benefit=')
print(pivot_benefit)

## Based on the above we perform elimination of the variable 2 in the non-pivot rows using the row 2:
##
ps_kk = 2
qs_kk = 2

ri = 1 
m = W[ri, qs_kk] = signif(W[ri, qs_kk] / W[ps_kk, qs_kk], n_digits)
W[ri, 1] = signif(W[ri, 1] - m * W[ps_kk, 1], n_digits) ## <- do the variables for this row
W[ri, n+1] = signif(W[ri, n+1] - m * W[ps_kk, n+1], n_digits) ## <- do the right-hand-side for this row
##print('Final W before backsubstitution:')
##print(W)

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

## Backsubstitution:
##
kk = 1
x[kk] = W[1, n+1] / W[kk, kk]

kk = 2
known_x = x[1]
xoefficients = W[2, 1]
x[kk] = ( W[kk, n+1] - sum(xoefficients * known_x) ) / W[kk, kk]

print(sprintf('Using total pivoting (n_digits in signif= %d); x=', n_digits))
print(x)
print(sprintf('error in x= %10.6f', sum((x-x_true)^2)))