clear;
close all;
clc;

% Define parameters
N = 1000000;
h = 0.4;

z_k = rand(1, N) - 0.5;
u_k = 2 * rand(1, N) - 1;

gamma_a = zeros(3, 3);
gamma_b = zeros(3, 1);

x_k = zeros(1,N);
mu_k = zeros(1, N);
y_k = zeros(1,N);

x_k_dash = zeros(1,N);
v_k_dash = zeros(1,N);
y_k_dash = zeros(1,N);

%% Gammas estimation
gamma_dash = cell(1, N);
for k = 1:N
    gamma_dash{k} = zeros(3, 1);
end

for k = 3:N
    x_k(k) = 3*u_k(k) + 2*u_k(k-1) + 1*u_k(k-2);
    mu_k(k) = mu(x_k(k));
    y_k(k) = mu_k(k) + z_k(k);

    fi_k =  [u_k(k); u_k(k-1); u_k(k-2)];
    gamma_a = gamma_a + fi_k * transpose(fi_k)*kernel(norm(fi_k), h);
    gamma_b = gamma_b + fi_k*y_k(k)*kernel(norm(fi_k), h);
    gamma_dash{k} = gamma_a^-1 * gamma_b;
end

disp(['Final estimated gamma_0: ' num2str(gamma_dash{N}(1))]);
disp(['Final estimated gamma_1: ' num2str(gamma_dash{N}(2))]);
disp(['Final estimated gamma_2: ' num2str(gamma_dash{N}(3))]);

N_values = 100:100:N;
gamma_0 = zeros(size(N_values));
gamma_1 = zeros(size(N_values));
gamma_2 = zeros(size(N_values));

for i = 3:length(N_values)
    index = N_values(i);
    gamma_0(i) = gamma_dash{index}(1);
    gamma_1(i) = gamma_dash{index}(2);
    gamma_2(i) = gamma_dash{index}(3);
end

figure(1);

subplot(1, 3, 1);
plot_subplot(N_values, 3, gamma_0, 'Estimation of true value $\\gamma_0=3$');

subplot(1, 3, 2);
plot_subplot(N_values, 2, gamma_1, 'Estimation of true value $\\gamma_1=2$');

subplot(1, 3, 3);
plot_subplot(N_values, 1, gamma_2, 'Estimation of true value $\\gamma_2=1$');

%% Regresion estimation from obtained gammas

u = linspace(-5, 5, 201);
R = zeros(1,length(u));
theoretical = u.^2 + u; % Change to yours
modelx = zeros(N,1);

for i=3:N
    modelx(i) = gamma_dash{N}(1)*u_k(i) + gamma_dash{N}(2)*u_k(i-1) + gamma_dash{N}(3)*u_k(i-2);
end

for i = 3:length(u)
    nom = 0;
    den = 0;
    for k = 1:N
        nom = nom + y_k(k)*rectangular_kernel(modelx(k), u(i), h);
        den = den + rectangular_kernel(modelx(k), u(i), h);
    end
    R(i) = nom/den;
end

figure(2);
plot(u(3:end), theoretical(3:end), 'LineWidth', 4,'DisplayName',"Nonlinear Characteristic of Static Component $\\mu(x)$");
hold on;
plot(u(3:end), R(3:end), 'LineWidth', 2,'DisplayName','Estimated Regression $\\hat{\\mu}(x)$');
legend('show',Location="best", Interpreter='latex');
title('Estimated and Theoretical Values', Interpreter='latex');
grid on;

%% Choose a proper fitting for your function
% Example: Polynomial Regression
degree = 2; % Choose an appropriate degree based on your data
p = polyfit(u, R, degree); % Fit a polynomial of the chosen degree
estimated_mu = @(x) polyval(p, x); % Anonymous function for estimation

%% Model v_k

u = linspace(-1, 1, 201);
modelx = zeros(1,length(u));
modely = zeros(1,length(u));
xk = zeros(1,length(u));
yk = zeros(1,length(u));

for i=3:length(u)
    xk(i) = 3 * u(i) + 2*u(i-1) + 1*u(i-2);
    modelx(i) = gamma_dash{N}(1)*u(i) + gamma_dash{N}(2)*u(i-1) + gamma_dash{N}(3)*u(i-2);
    
    yk(i) = mu(xk(i));
    modely(i) = estimated_mu(modelx(i));
end

figure(3);
plot(u(3:end), yk(3:end), 'LineWidth', 4,'DisplayName',"System $v_k$");
hold on;
plot(u(3:end), modely(3:end), 'LineWidth', 2,'DisplayName','Model $v_k$');
legend('show',Location="best",Interpreter='latex');
title('Undisturbed $v_k$ output signals comparison',Interpreter='latex');
grid on;

% Function Definition
function result = kernel(kernel_arg, h)
   valid_range = abs(kernel_arg) <= h;
    result = valid_range / 2*h;
end

function result = rectangular_kernel(u_k, u, h)
    valid_range = abs(u_k - u) <= h;
    result = valid_range / 2*h;
end

function result = mu(x)
    result = x^2+x; % Change to yours
end

function plot_subplot(N_values, true_value, estimated_values, title_str)
    plot(N_values, ones(size(N_values)) * true_value, 'LineWidth', 2, 'DisplayName', 'True Value', 'Color', '#EDB120');
    hold on;
    plot(N_values, estimated_values, 'LineWidth', 1.5, 'DisplayName', 'Estimated Value', 'Color', '#7E2F8E');
    grid on;
    legend('show', 'Location', 'best', 'FontSize', 14);
    title(title_str, Interpreter="latex");
    ylabel('Values');
    xlabel('Number of samples N');
end