a=5/8;
b=11/8;
cv=0.05;

%% Exercise 6.1
N_p = 2;
N = 2;
tau_max = 80;
u_max = 2;

g61 = estimate_g(N, N_p, tau_max, u_max, a, b, cv, true, true);

%% Exercise 6.2

N_p = 1;
N = 1;
tau_max = 80;
u_max = 2;
g0 = estimate_g(N, N_p, tau_max, u_max, a, b, cv, false, true);

err = zeros(30, 1);
for N_p=2:30
    g = estimate_g(N, N_p, tau_max, u_max, a, b, cv, true, true);
    err(N_p) = norm(g - g0);
end

plot(2:30, err(2:end), 'b');
xlabel("Number of Periods")
ylabel("||g_i - g_0||_2")
grid on
hold on

%% Exercise 6.3

N = 1;
tau_max = 80;
u_max = 2;
err = zeros(30, 1);
for N_p=2:30
    g = estimate_g(N, N_p, tau_max, u_max, a, b, cv, true, false);
    err(N_p) = norm(g - g0);
end

plot(2:30, err(2:end))

%% Exercise 6.4

N_p = 2;
tau_max = 80;
u_max = 2;

err = zeros(30, 1);
for N=2:30
    g = estimate_g(N, N_p, tau_max, u_max, a, b, cv, true, true);
    err(N) = norm(g - g0);
end

plot(2:30, err(2:end))


%% Exercise function
function g=estimate_g(N, N_p, tau_max, u_max, a, b, cv, noise, PRBS)
    g = zeros(tau_max, 1);
    for j=1:N
        % Compute the noise vector
        if noise
            v = sqrt(cv) * randn(N_p * tau_max, 1);
        else
            v = zeros(N_p * tau_max, 1);
        end

        % Compute the input vector
        if PRBS
            u = u_max * idinput(tau_max, 'PRBS');
        else
            u = u_max * (rand(tau_max, 1) - 0.5) * 2.0;
        end
        u = repmat(u, N_p, 1);

        % Compute the outputs of the system
        y = zeros(size(u));
        y(1) = v(1);
        for i=2:size(y, 1)
            y(i) = a * y(i-1) + b * u(i-1) + v(i);
        end

        % Build the Phi matrix
        % phi = [toeplitz(y, zeros(tau_max, 1)), toeplitz(u, zeros(tau_max, 1))];
        phi = toeplitz(u, zeros(tau_max, 1));

        % Solve for g
        g = g + phi \ y;
    end
    g = g / N;
end