%% Optimize vHS phase_lead Robust NCF for minimum J2
% Same configuration and robust-NCF workflow as
% optimize_vHS_phase_lead_robust_ncf, but feasible candidates minimize
% J2 = 100*std(dm_filtfilt - d_n)/std(d_n) on the healthy state.

clearvars -except phase_lead_robust_config ...
    nominal_ncf_target robust_ncf_target section_count_override ...
    minimum_stability_margin_override notch_count_override ...
    fem_file_override specimen_file_override ...
    h_dc_dm_model_file_override pid_gain_override ...
    phase_lead_file_name_override; clc; close all;

script_dir = fileparts(mfilename('fullpath'));
addpath(fullfile(script_dir, 'utils_HS'));
project_root = fileparts(fileparts(script_dir));
addpath(fullfile(project_root, 'helpers', 'utils'));
addpath(fullfile(project_root, 'control_and_HS', 'c_fem_modeling', ...
    'utils_fem'));
addpath(fullfile(project_root, 'results_analysis'));
addpath(fullfile(project_root, 'control_and_HS', 'b_system_id', ...
    'id_utils'));

auth = 0.6;
config = struct( ...
    ... % --- primary tuning controls (edit in this order)
    'objective_metric', 'J2', ...
    'pid_gain_override', [0.15*auth 15*auth 0.0015*auth 512], ... % [] uses saved exp_tf PID; e.g. [0.15 20 0.0015 512]
    'target_ncf_margin', 0.40, ...
    'robust_ncf_target', 0.25, ...
    'phase_lead_section_count', 1, ...
    'notch_section_count', 1, ...
    'template_warp_scenarios', [0.9 1.1], ...
    'seed_phase_leads', {{'pl_7000_robust_notch.mat'}}, ...
    'minimum_coupled_pole_damping', 0.02, ...
    'coupled_pole_damping_band_hz', [7 16], ...
    'minimum_stability_margin', 1e-4, ...
    ... % --- model and record
    'specimen_file', fullfile(project_root, 'saved_results', ...
        'specimens', 'sp1_7000.mat'), ...
    'h_dc_dm_model_file', fullfile(project_root, 'saved_results', ...
        'exp_tf', 'tf_sp1_7000_20260627_210411.mat'), ...
    'fem_file', fullfile(project_root, 'saved_results', 'fem', ...
        'fem_sp1_7000.mat'), ...
    'damage_states', struct( ...
        'label', {'5k'}, ...
        'specimen_file', {fullfile(project_root, 'saved_results', ...
            'specimens', 'sp1_5000.mat')}, ...
        'h_dc_dm_model_file', {fullfile(project_root, 'saved_results', ...
            'exp_tf', 'tf_sp1_5000_20260627_233333.mat')}), ...
    ... % 1 lead + 1 notch, warped +-10% to avoid overfitting the
    ... % measured resonance. For lead+lag use section count 2 and
    ... % notch count 0.
    'compare_phase_lead_files', ...
        {{'pl_7000_robust.mat', 'pl_7000_ncf40_v2.mat'}}, ...
    'phase_lead_file_name', 'pl_7000_robust_J2', ...
    'save_results', true, ...
    'save_best_checkpoint', true, ...
    'plot_results', true);

if exist('phase_lead_robust_config', 'var') ...
        && ~isempty(phase_lead_robust_config)
    override_fields = fieldnames(phase_lead_robust_config);
    for override_id = 1:numel(override_fields)
        config.(override_fields{override_id}) = ...
            phase_lead_robust_config.(override_fields{override_id});
    end
end
config.objective_metric = 'J2';

if exist('nominal_ncf_target', 'var') && ~isempty(nominal_ncf_target)
    config.target_ncf_margin = nominal_ncf_target;
end
if exist('robust_ncf_target', 'var') && ~isempty(robust_ncf_target)
    config.robust_ncf_target = robust_ncf_target;
end
if exist('minimum_stability_margin_override', 'var') ...
        && ~isempty(minimum_stability_margin_override)
    config.minimum_stability_margin = minimum_stability_margin_override;
end
if exist('section_count_override', 'var') ...
        && ~isempty(section_count_override)
    config.phase_lead_section_count = section_count_override;
end
if exist('notch_count_override', 'var') ...
        && ~isempty(notch_count_override)
    config.notch_section_count = notch_count_override;
end
if exist('fem_file_override', 'var') && ~isempty(fem_file_override)
    config.fem_file = fem_file_override;
end
if exist('specimen_file_override', 'var') ...
        && ~isempty(specimen_file_override)
    config.specimen_file = specimen_file_override;
end
if exist('h_dc_dm_model_file_override', 'var') ...
        && ~isempty(h_dc_dm_model_file_override)
    config.h_dc_dm_model_file = h_dc_dm_model_file_override;
end
if exist('pid_gain_override', 'var') && ~isempty(pid_gain_override)
    config.pid_gain_override = pid_gain_override;
end

fprintf('\nDefault files used by this J2 optimization:\n')
fprintf('  Objective:          %s\n', config.objective_metric)
fprintf('  Nominal NCF target: %.4g\n', config.target_ncf_margin)
fprintf('  Robust NCF target:  %.4g\n', config.robust_ncf_target)
fprintf('  Stability margin:   %.4g\n', config.minimum_stability_margin)
fprintf('  Healthy specimen: %s\n', config.specimen_file)
fprintf('  Healthy exp TF:   %s\n', config.h_dc_dm_model_file)
fprintf('  FEM:              %s\n', config.fem_file)
for damage_id = 1:numel(config.damage_states)
    fprintf('  Damage state %s specimen: %s\n', ...
        config.damage_states(damage_id).label, ...
        config.damage_states(damage_id).specimen_file)
    fprintf('  Damage state %s exp TF:   %s\n', ...
        config.damage_states(damage_id).label, ...
        config.damage_states(damage_id).h_dc_dm_model_file)
end
for seed_id = 1:numel(config.seed_phase_leads)
    fprintf('  Warm-start seed phase_lead: %s\n', ...
        config.seed_phase_leads{seed_id})
end
for compare_id = 1:numel(config.compare_phase_lead_files)
    fprintf('  Comparison phase_lead: %s\n', ...
        config.compare_phase_lead_files{compare_id})
end
if config.save_best_checkpoint
    fprintf(['  Best checkpoint: saved to saved_results\\phase_lead\\' ...
        'vHS_phase_lead_robust_best_checkpoint.mat unless overridden\n'])
end
fprintf(['Use the numbered file tree to choose the healthy specimen, ' ...
    'healthy exp TF, and FEM unless an override variable is set.\n\n'])

if ~exist('specimen_file_override', 'var') || isempty(specimen_file_override)
    fprintf('\nSelect healthy specimen file:\n')
    [~, config.specimen_file, ~] = load_saved_result_file( ...
        project_root, 'specimens');
end
if ~exist('h_dc_dm_model_file_override', 'var') ...
        || isempty(h_dc_dm_model_file_override)
    fprintf('\nSelect healthy experimental transfer-function file:\n')
    [~, config.h_dc_dm_model_file, ~] = load_saved_result_file( ...
        project_root, 'exp_tf');
end
if ~exist('fem_file_override', 'var') || isempty(fem_file_override)
    fprintf('\nSelect FEM file:\n')
    [~, config.fem_file, ~] = load_saved_result_file(project_root, 'fem');
end

config.phase_lead_file_name = '';
while isempty(config.phase_lead_file_name)
    config.phase_lead_file_name = strtrim(input( ...
        'Enter phase_lead output file name: ', 's'));
end

phase_lead_robust_result = optimize_vHS_phase_lead_robust(config);