%% Q3b
% Name: William Qi
% Date: Nov. 13, 2017
% Assignment 4

% Set up A matrices with varying epsilon values in the lower rows
A1 = [1 1 1; 2*sqrt(eps) 0 0; 0 2*sqrt(eps) 0; 0 0 2*sqrt(eps)];
A2 = [1 1 1; sqrt(eps) 0 0; 0 sqrt(eps) 0; 0 0 sqrt(eps)];
A3 = [1 1 1; 2*eps 0 0; 0 2*eps 0; 0 0 2*eps];
A4 = [1 1 1; eps 0 0; 0 eps 0; 0 0 eps];
b = [1; 0; 0; 0];

% Set up solutions based off of exact solution from 3A
x1 = 1/(3 + (2*sqrt(eps))^2)
x2 = 1/(3 + (sqrt(eps))^2)
x3 = 1/(3 + (2*eps)^2)
x4 = 1/(3 + (eps)^2)

% i
x1_i_err = norm(normalSolve(A1, b) - x1);
x2_i_err = norm(normalSolve(A2, b) - x2);
x3_i_err = norm(normalSolve(A3, b) - x3);
x4_i_err = norm(normalSolve(A4, b) - x4);

% ii
x1_ii_err = norm(classicalGSSolve(A1, b) - x1);
x2_ii_err = norm(classicalGSSolve(A2, b) - x2);
x3_ii_err = norm(classicalGSSolve(A3, b) - x3);
x4_ii_err = norm(classicalGSSolve(A4, b) - x4);

% iii
x1_iii_err = norm(modifiedGSSolve(A1, b) - x1);
x2_iii_err = norm(modifiedGSSolve(A2, b) - x2);
x3_iii_err = norm(modifiedGSSolve(A3, b) - x3);
x4_iii_err = norm(modifiedGSSolve(A4, b) - x4);

% iv
x1_iv_err = norm(householderSolve(A1, b) - x1);
x2_iv_err = norm(householderSolve(A2, b) - x2);
x3_iv_err = norm(householderSolve(A3, b) - x3);
x4_iv_err = norm(householderSolve(A4, b) - x4);

% v
x1_v_err = norm(givensSolve(A1, b) - x1);
x2_v_err = norm(givensSolve(A2, b) - x2);
x3_v_err = norm(givensSolve(A3, b) - x3);
x4_v_err = norm(givensSolve(A4, b) - x4);

% vi
x1_vi_err = norm(qrSolve(A1, b) - x1);
x2_vi_err = norm(qrSolve(A2, b) - x2);
x3_vi_err = norm(qrSolve(A3, b) - x3);
x4_vi_err = norm(qrSolve(A4, b) - x4);

% vii
x1_vii_err = norm(backslashSolve(A1, b) - x1);
x2_vii_err = norm(backslashSolve(A2, b) - x2);
x3_vii_err = norm(backslashSolve(A3, b) - x3);
x4_vii_err = norm(backslashSolve(A4, b) - x4);

% viii
x1_viii_err = norm(svdSolve(A1, b) - x1);
x2_viii_err = norm(svdSolve(A2, b) - x2);
x3_viii_err = norm(svdSolve(A3, b) - x3);
x4_viii_err = norm(svdSolve(A4, b) - x4);

%ix
x1_ix_err = norm(tsvdSolve(A1, b) - x1);
x2_ix_err = norm(tsvdSolve(A2, b) - x2);
x3_ix_err = norm(tsvdSolve(A3, b) - x3);
x4_ix_err = norm(tsvdSolve(A4, b) - x4);

%% Function Definitions
% i
function x = normalSolve(A, b)
    % Set up system of normal equations and use backslash to solve
    x = (A.' * A) \ (A.' * b);
end

% ii
function x = classicalGSSolve(A, b)
    % Compute QR decomposition using classical Gram-Schmidt orthgonalization
    [~, n] = size(A);
    R = zeros(n);

    % r_11 = ||q_1||
    R(1,1) = norm(A(:,1));
    % q_1 = q_1/r_11
    Q(:,1) = A(:,1)/R(1,1);
    for j = 2:n
        R(1:j-1, j) = Q(:, 1:j-1)' * A(:, j);
        Q(:, j) = A(:, j) - Q(:, 1:j-1) * R(1:j-1, j);
        % r_jj = ||q_j||
        R(j, j)= norm(Q(:, j));
        % q_j = q_j/r_jj
        Q(:, j) = Q(:, j) / R(j, j);
    end

    % Solve system Rx=Q^T*b
    y = Q.' * b;
    x = R \ y;
end

% iii
function x = modifiedGSSolve(A, b)
    % Compute QR decomposition using modified Gram-Schmidt orthgonalization
    [m, n] = size(A);
    Q = zeros(m, n);
    R = zeros(n, n);

    for j = 1:n
        % q_j = a_j
        Q(:, j) = A(:, j);
        for i = 1:j-1
            % r_ij = <q_j, q_i>
            R(i, j) = Q(:,i)' * Q(:, j);
            % q_j = q_j = r_ij * q_i
            Q(:, j) = Q(:, j) - R(i, j) * Q(:, i);
        end
        % r_jj = ||q_j||
        R(j, j) = norm(Q(:, j));
        % q_j = q_j/r_jj
        Q(:, j) = Q(:, j) / R(j, j);
    end

    % Solve system Rx=Q^T*b
    y = Q.' * b;
    x = R \ y;
end

% iv
function x = householderSolve(A, b)
    [m, n] = size(A);
    p = zeros(1, n);

    % Compute QR decomposition using Householder reflections
    for k = 1:n
        % Define u of length = m-k+1
        z = A(k:m, k);
        e1 = [1; zeros(m-k, 1)];
        u = z + sign(z(1))*norm(z)*e1;
        u = u / norm(u);
        % Update nonzero part of A by I-2uu^T
        A(k:m, k:n) = A(k:m, k:n) - 2*u*(u'*A(k:m, k:n));
        % Store u
        p(k) = u(1);
        A(k+1:m, k) = u(2:m-k+1);
    end

    % Use QR decomposition to solve least squares
    y = b(:);
    % Transform b
    for k=1:n
        u = [p(k);A(k+1:m, k)];
        y(k:m) = y(k:m) - 2*u*(u'*y(k:m));
    end
    % Form upper triangular R and solve
    R = triu(A(1:n, :));
    x = R \ y(1:n);
end

% v
function x = givensSolve(A, b)
    [m,n] = size(A);
    Q = eye(m);
    R = A;

    % Use Givens rotation to zero out a single entry at a time
    for j = 1:n
        for i = m:-1:(j+1)
            G = eye(m);
            % Set up G_ij by computing c, s
            [c,s] = givensrotation(R(i-1, j), R(i, j));
            G([i-1, i],[i-1, i]) = [c -s; s c];

            % Apply G_ij on A
            R = G' * R;
            Q = Q * G;
        end
    end

    % Solve system Rx=Q^T*b
    y = Q' * b;
    x = R \ y;
end

function [c,s] = givensrotation(a,b)
  % Check if a_ij is 0
  if b == 0
    c = 1;
    s = 0;
  else
    % r = sqrt(a^2 + b^2)
    % c = a/r
    % s = b/r
    if abs(b) > abs(a)
      r = a / b;
      s = 1 / sqrt(1 + r^2);
      c = s*r;
    else
      r = b / a;
      c = 1 / sqrt(1 + r^2);
      s = c*r;
    end
  end
end

% vi
function x = qrSolve(A, b)
    % Obtain QR decomposition using matlab builtin function
    [Q,R] = qr(A);

    % Solve system Rx=Q^T*b
    y = Q' * b;
    x = R \ y;
end

% vii
function x = backslashSolve(A, b)
    % Solve least squares using backslash
    x = A \ b;
end

% viii
function x = svdSolve(A, b)
    % Obtain SVD decomposition using matlab builtin function
    [U, S, V] = svd(A);

    % Format y and z with the correct dimensions
    y = diag(S).^-1;
    z = U' * b;
    z = z(1:numel(y));

    % x = V * S^-1 * U^T * b
    x = V * (diag(y) * z);
end

% ix
function x = tsvdSolve(A, b)
    % Obtain SVD decomposition using matlab builtin function
    [U, S, V] = svd(A);
    % Set singluar value truncation threshold to 10^-6
    threshold = 10^-6;

    % Find the index of the last element in S that is above the threshold
    r = find(diag(S) < threshold, 1) - 1;

    % Format y, z, and V with the correct truncated dimensions
    y = diag(S).^-1;
    z = U' * b;
    y = y(1:r);
    z = z(1:r);
    V = V(:, 1:r);

    % x = V * S^-1 * U^T * b with truncated dimensions
    x = V * (diag(y) * z);
end