% Clear the environment and close all figures
clear;
close all;
clc;

% Number of samples
N = 1000000;

% Generate random noise and input signals
z_k = rand(1, N) - 0.5;  % Noise signal (uniformly distributed between -0.5 and 0.5)
u_k = 2 * rand(1, N) - 1; % Input signal (uniformly distributed between -1 and 1)

% Define constants c and gamma
c = [3; 2];
gamma = [3; 2; 1];

% Calculate Theta based on gamma and c
Theta = [gamma(1)*c(1);
         gamma(1)*c(2);
         gamma(2)*c(1);
         gamma(2)*c(2);
         gamma(3)*c(1);
         gamma(3)*c(2)];

% Initialize Phi for each sample
Phi = cell(1, N);
for i = 1:N
    Phi{i} = zeros(6, 1);
end

% Initialize output signal
y_k = zeros(1, N);

% Compute Phi and y_k for each sample
for k = 3:N
    Phi{k} = [f1(u_k(k)); f2(u_k(k)); f1(u_k(k-1)); f2(u_k(k-1)); f1(u_k(k-2)); f2(u_k(k-2))];
    y_k(k) = transpose(Theta) * Phi{k} + z_k(k);
end

% Transpose Phi for each sample
PHI = cell(N, 1);
for k = 1:N
    PHI{k} = transpose(Phi{k});
end

% Prepare data for system identification
Y = transpose(y_k);
PHI = vertcat(PHI{1:N});

% Perform system identification
Theta_ = (transpose(PHI) * PHI) ^ -1 * transpose(PHI) * Y;
Matrix = [Theta_(1), Theta_(3), Theta_(5);
          Theta_(2), Theta_(4), Theta_(6)];

% Singular Value Decomposition (SVD)
[P, D, Q] = svd(Matrix);

% Update constants c and gamma based on SVD results
c = [P(1,1); P(2,1)]
gamma = [Q(1,1); Q(2,1); Q(3,1)]
Theta_

% Function definitions
function result = f1(u)
    % Linear function
    result = u;
end

function result = f2(u)
    % Quadratic function
    result = u^2;
end