clc; clear; close all;

%% 1. Identification
freq = [0.01 0.0167 0.0278 0.0464 0.0774 0.1292 0.2154 0.3594 0.5995 1];
mag = [52.9239 27.9229 12.0631 4.6756 1.7268 0.6236 0.2255 0.081 0.0291 0.0104];
phase = [-180.0033 -180.0653 -180.132 -180.261 -180.4691 -180.8925 -181.3380 -182.3202 -183.9136 -186.5078];

resp = mag .* exp(1j*deg2rad(phase));
data = idfrd(resp, freq, 0);

sys = tfest(data, 2, 0);

disp('Identified Transfer Function:');
sys

%% 2. Lead-Lag Controller Design
s = tf('s');

% Given plant (from your model)
G = 0.01055/(s^2 + 6.924e-06*s - 9.933e-05);

% Tuned controller (stable version)
K = 40;
z_lead = 1.5;
p_lead = 25;
z_lag = 0.1;
p_lag = 0.01;

C = K * ((s + z_lead)/(s + p_lead)) * ((s + z_lag)/(s + p_lag));

[num, den] = tfdata(C, 'v');

fprintf('\n--- SIMULINK UPDATE --- \n');
fprintf('Numerator: [%s]\n', num2str(num, '%.6f '));
fprintf('Denominator: [%s]\n', num2str(den, '%.6f '));
fprintf('------------------------\n');

%% 3. Run Simulation
modelName = 'ball_and_beam_student';

% Make sure model is loaded
load_system(modelName);

simData = sim(modelName, 'StopTime', '10');

%% 4. Data Extraction (robust)

try
    % --- REAL SYSTEM ---
    sig = simData.simout;

    if isprop(sig, 'Values')
        y_real = sig.Values.Data;
        t_sim = sig.Values.Time;
    else
        y_real = sig.Data;
        t_sim = sig.Time;
    end

    % --- MODEL OUTPUT ---
    sig1 = simData.simout1;   % ⚠️ MUST match Simulink block name

    if isprop(sig1, 'Values')
        y_model = sig1.Values.Data;
    else
        y_model = sig1.Data;
    end

catch
    error(['Simulink output variables not found.\n' ...
           'Make sure To Workspace blocks are named:\n' ...
           '  simout (real system)\n' ...
           '  simout1 (model)\n']);
end

%% 5. Metric (RMSE)
compare_idx = find(t_sim <= 5);

rmse = sqrt(mean((y_real(compare_idx) - y_model(compare_idx)).^2));

fprintf('\nFINAL METRIC: RMSE = %f\n', rmse);

%% 6. Plot
figure('Name', 'Model Validation');
plot(t_sim, y_real, 'b', 'LineWidth', 1.5); hold on;
plot(t_sim, y_model, 'r--', 'LineWidth', 1.5);

title('Model Validation: Real vs Identified');
xlabel('Time (s)');
ylabel('Ball Position (m)');
legend('Real System', 'TF Model');
grid on;

%% 2. PID Design
% We target a Crossover Frequency (wc) and Phase Margin (pm)
% For the Ball & Beam, a lower wc is safer for SimScape physics
wc = 1.5; 
pm = 60; 
opts = pidtuneOptions('PhaseMargin', pm);
[C_pid, info_pid] = pidtune(G, 'PID', wc, opts);

% Extract PID Gains
Kp = C_pid.Kp;
Ki = C_pid.Ki;
Kd = C_pid.Kd;
N = 100; % Filter coefficient (Standard for D-action)

fprintf('\n--- PID GAINS FOR SIMULINK --- \n');
fprintf('Kp: %.4f\n', Kp);
fprintf('Ki: %.4f\n', Ki);
fprintf('Kd: %.4f\n', Kd);
fprintf('Filter Coefficient (N): %.4f\n', N);
fprintf('------------------------------\n');

% Closed-loop for Math Model
T_pid = feedback(C_pid*G, 1);
step_info = stepinfo(T_pid);

fprintf('\nPID Math Model Results:\n');
fprintf('Settling Time: %.4f s\n', step_info.SettlingTime);

%% 3. Run Simulation
modelName = 'ball_and_beam_student'; 
simData = sim(modelName, 'StopTime', '10');

%% 4. Data Extraction
try
    y_real = simData.simout.Data; 
    t_sim = simData.simout.Time;
    y_model = simData.simout1.Data;
    
    rmse_pid = sqrt(mean((y_real - y_model).^2));
    fprintf('\nPID VALIDATION: RMSE = %f\n', rmse_pid);

    figure('Name', 'PID Validation');
    plot(t_sim, y_real, 'b', t_sim, y_model, 'r--');
    title('PID Control: Real vs Model');
    legend('Real System','TF Model'); grid on;
catch
    disp('Extraction failed. Ensure To Workspace blocks match names.');
end