%% =========================================================
%  Least Squares Identification with Delay Search
%% =========================================================
load Data
Ts       = 0.1;              % Sampling interval
maxDelay = round(10/Ts);     % search up to 10 sec
N = length(u_values);
bestFit = -inf;

for d_test = 0:maxDelay
    
    k_start = d_test + 2;
    k_end   = N;
    
    if k_start >= k_end
        continue
    end
    
    Y = y_meas(k_start:k_end);
    
    Phi = [ y_meas(k_start-1:k_end-1), ...
            u_values(k_start-d_test:k_end-d_test) ];
    
    if rank(Phi) < 2
        continue
    end
    
    theta = Phi \ Y;
    y_hat = Phi * theta;
    
    fit = 100*(1 - norm(Y - y_hat)/norm(Y - mean(Y)));
    
    if fit > bestFit
        bestFit = fit;
        best_d = d_test;
        best_theta = theta;
    end
end

a_est = best_theta(1);
b_est = best_theta(2);

%% =========================================================
%  Convert Discrete → Continuous FOPDT
%% =========================================================
if a_est <= 0 || a_est >= 1
    warning('Unstable or invalid discrete pole detected.')
end

T_est = -Ts / log(a_est);
K_est = b_est / (1 - a_est);
L_est = best_d * Ts;

fprintf('\n===== Identification Result =====\n');
fprintf('True K = %.3f | Estimated K = %.3f\n', K_true, K_est);
fprintf('True T = %.3f | Estimated T = %.3f\n', T_true, T_est);
fprintf('True L = %.3f | Estimated L = %.3f\n', L_true, L_est);
fprintf('Model Fit = %.2f %%\n', bestFit);

%% =========================================================
%  Simulate Identified Model
%% =========================================================
y_est = zeros(N,1);

for k = 2:N
    if k > best_d
        y_est(k) = a_est*y_est(k-1) + ...
                   b_est*u_values(k-best_d);
    else
        y_est(k) = a_est*y_est(k-1);
    end
end

%% =========================================================
% Plot Results
%% =========================================================
figure;

subplot(3,1,1)
stairs(t,u_values,'LineWidth',1.2)
ylabel('Input')
title('PRBS Excitation')
grid on

subplot(3,1,2)
plot(t,y_meas,'b')
ylabel('Measured Output')
title('Simulated Plant Output')
grid on

subplot(3,1,3)
plot(t,y_meas,'k','DisplayName','Measured'); hold on
plot(t,y_est,'r--','LineWidth',1.5,'DisplayName','Identified')
xlabel('Time (s)')
ylabel('Output')
title(['Model Validation (Fit = ' num2str(bestFit,'%.1f') ' %)'])
legend
grid on