simulate.m

clc;
% Clear workspace unless running in test/CI mode
if ~strcmpi(getenv('MATLAB_TEST'), 'true') && ~strcmpi(getenv('CI'), 'true')
  clear;
end

% Add required paths for src functions and lib dependencies
scriptDir = fileparts(mfilename('fullpath'));

% Add src directory for supporting functions (config.m is now in main directory)
srcPath = fullfile(scriptDir, 'src');
if exist(srcPath, 'dir')
  addpath(srcPath);
  % Also add geometry subdirectory
  geometryPath = fullfile(srcPath, 'geometry');
  if exist(geometryPath, 'dir')
    addpath(geometryPath);
  end
end

% Load configuration parameters
% Can be overridden by setting CONFIG_FUNC and GEOMETRY_TYPE environment variables
config_func_name = getenv('CONFIG_FUNC');
geometry_type = getenv('GEOMETRY_TYPE');

if isempty(config_func_name)
  config_func_name = 'config';
end

% Check for configuration loading options
load_cfg_file = getenv('LOAD_CFG_FILE');

if ~isempty(load_cfg_file) && exist(load_cfg_file, 'file')
  % Load configuration from file
  load(load_cfg_file, 'cfg');
  fprintf('Configuration loaded from file: %s\n', load_cfg_file);
elseif ~exist('cfg', 'var') || isempty(cfg)
  % Load configuration from config() function
  if isempty(geometry_type)
    % Default behavior - call config function without arguments
    cfg = config();
  else
    % Pass geometry type to config function
    cfg = config(geometry_type);
  end
  fprintf('Configuration loaded from config() function\n');
else
  fprintf('Using pre-existing configuration (cfg already defined)\n');
end

%% 1) Setup environment and dependencies
[doPlot, isCI, isTest, Nt] = setup_environment(cfg, scriptDir);

% Extract domain parameters - define computational domain boundaries
x_min = cfg.domain.x_min;  % Left boundary of domain
x_max = cfg.domain.x_max;  % Right boundary of domain
y_min = cfg.domain.y_min;  % Bottom boundary of domain
y_max = cfg.domain.y_max;  % Top boundary of domain

%% 2) Generate geometry
G = build_geometry(cfg);

% Extract mesh data from geometry structure
xy = G.xy;                    % Interior velocity nodes
xy_s = G.xy_s;               % Interior pressure nodes
xt = G.xt;                   % Triangle connectivity

% Extract velocity grid boundaries
boundary_in = G.boundary_in;     % Inlet boundary nodes (V-grid)
boundary_out = G.boundary_out;   % Outlet boundary nodes (V-grid)
boundary_y = G.boundary_y;       % Wall boundary nodes (V-grid)
boundary_obs = G.boundary_obs;   % Obstacle boundary nodes (V-grid)

% Extract pressure grid boundaries
boundary_in_s = G.boundary_in_s;   % Inlet boundary nodes (P-grid)
boundary_out_s = G.boundary_out_s; % Outlet boundary nodes (P-grid)
boundary_y_s = G.boundary_y_s;     % Wall boundary nodes (P-grid)
boundary_obs_s = G.boundary_obs_s; % Obstacle boundary nodes (P-grid)

% Extract geometry-specific parameters (used for distance calculations)
if isfield(G, 'radius')
  radius = G.radius;           % Cylinder radius
  obs_char_length = radius;    % Characteristic length for distance calculations
elseif isfield(G, 'a') && isfield(G, 'b')
  a = G.a;                     % Ellipse semi-major axis
  b = G.b;                     % Ellipse semi-minor axis
  obs_char_length = min(a, b); % Use minimum axis as characteristic length
elseif isfield(G, 'rect_width') && isfield(G, 'rect_height')
  rect_width = G.rect_width;   % Rectangle width
  rect_height = G.rect_height; % Rectangle height
  obs_char_length = min(rect_width, rect_height) / 2; % Use half of smaller dimension
elseif isfield(G, 'chord_length') && isfield(G, 'naca_digits')
  chord_length = G.chord_length; % Airfoil chord length
  naca_digits = G.naca_digits;   % NACA 4-digit parameters
  % Calculate maximum thickness as characteristic length
  thickness_percent = naca_digits(3) * 10 + naca_digits(4); % Thickness as percentage
  max_thickness = (thickness_percent / 100) * chord_length;  % Maximum thickness in units
  obs_char_length = max_thickness / 2; % Use half of maximum thickness
elseif isfield(G, 'obstacles') && isfield(G, 'num_obstacles')
  % Multi-obstacle geometry: use minimum characteristic length of all obstacles
  obs_char_length = inf;
  for i = 1:G.num_obstacles
    obs = G.obstacles(i);
    switch lower(obs.type)
      case 'cylinder'
        char_len = obs.params.radius;
      case 'ellipse'
        char_len = min(obs.params.a, obs.params.b);
      case 'rectangle'
        char_len = min(obs.params.width, obs.params.height) / 2;
      case 'airfoil'
        thickness_percent = obs.params.naca_digits(3) * 10 + obs.params.naca_digits(4);
        max_thickness = (thickness_percent / 100) * obs.params.chord_length;
        char_len = max_thickness / 2;
      otherwise
        char_len = 0.5; % Default fallback
    end
    obs_char_length = min(obs_char_length, char_len);
  end
else
  error('Geometry structure missing expected parameters');
end

% Combine boundary nodes in specific order for compatibility
boundary_s = [boundary_y_s; boundary_out_s; boundary_in_s; boundary_obs_s];
boundary = [boundary_y; boundary_out; boundary_in; boundary_obs];

% Initial mesh visualization (only in non-CI mode)
if doPlot && ~isCI && ~isTest
  fprintf('Creating mesh visualization...\n');
  fig = figure('Name', 'Multi-Obstacle Mesh', 'Position', [100, 100, 1000, 600]);

  % Plot interior nodes (sample for performance if needed)
  if size(xy, 1) > 10000
    % Sample for very large meshes
    sample_rate = max(1, floor(size(xy, 1) / 8000));
    scatter(xy(1:sample_rate:end, 1), xy(1:sample_rate:end, 2), 4, 'k.', 'DisplayName', ...
            sprintf('Interior nodes (sampled %d/%d)', length(1:sample_rate:size(xy, 1)), size(xy, 1)));
  else
    % Show all interior nodes for reasonable mesh sizes
    scatter(xy(:, 1), xy(:, 2), 3, 'k.', 'DisplayName', sprintf('Interior nodes (%d)', size(xy, 1)));
  end
  hold on;

  % Plot pressure grid interior nodes (lighter color)
  if size(xy_s, 1) > 5000
    % Sample pressure nodes for very large meshes
    p_sample_rate = max(1, floor(size(xy_s, 1) / 4000));
    scatter(xy_s(1:p_sample_rate:end, 1), xy_s(1:p_sample_rate:end, 2), 2, [0.7 0.7 0.7], '.', ...
            'DisplayName', sprintf('Pressure nodes (sampled %d/%d)', length(1:p_sample_rate:size(xy_s, 1)), size(xy_s, 1)));
  else
    scatter(xy_s(:, 1), xy_s(:, 2), 2, [0.7 0.7 0.7], '.', ...
            'DisplayName', sprintf('Pressure nodes (%d)', size(xy_s, 1)));
  end

  % Plot boundary nodes with different colors and markers
  scatter(boundary_in(:, 1), boundary_in(:, 2), 25, 'b', 's', 'filled', 'DisplayName', sprintf('Inlet (%d)', length(boundary_in)));
  scatter(boundary_y(:, 1), boundary_y(:, 2), 25, 'r', '^', 'filled', 'DisplayName', sprintf('Walls (%d)', length(boundary_y)));
  scatter(boundary_out(:, 1), boundary_out(:, 2), 25, 'g', 'v', 'filled', 'DisplayName', sprintf('Outlet (%d)', length(boundary_out)));

  % Color-code obstacle boundaries if multi-obstacle
  if isfield(G, 'boundary_obs_ids') && isfield(G, 'num_obstacles')
    colors = {[1 0 1], [0 1 1], [1 1 0], [0.8 0.4 0], [0.6 0.2 0.8], [0.2 0.8 0.2], [0.8 0.2 0.2]};
    for obs_id = 1:G.num_obstacles
      obs_mask = G.boundary_obs_ids == obs_id;
      if any(obs_mask)
        color_idx = mod(obs_id - 1, length(colors)) + 1;
        color = colors{color_idx};
        scatter(G.boundary_obs_s(obs_mask, 1), G.boundary_obs_s(obs_mask, 2), ...
                35, color, 'o', 'filled', 'LineWidth', 1, 'MarkerEdgeColor', 'k', ...
                'DisplayName', sprintf('Obstacle %d (%d pts)', obs_id, sum(obs_mask)));
      end
    end
  else
    scatter(boundary_obs(:, 1), boundary_obs(:, 2), 35, [1 0 1], 'o', 'filled', ...
            'LineWidth', 1, 'MarkerEdgeColor', 'k', ...
            'DisplayName', sprintf('Obstacle (%d pts)', length(boundary_obs)));
  end

  axis equal;
  grid on;

  % Calculate total node counts
  total_v_nodes = size(xy, 1) + length(boundary_in) + length(boundary_y) + length(boundary_out) + length(boundary_obs);
  total_p_nodes = size(xy_s, 1) + length(G.boundary_y_s) + length(G.boundary_out_s) + length(G.boundary_in_s) + length(G.boundary_obs_s);
  if isfield(G, 'num_obstacles')
    num_obstacles = G.num_obstacles;
  else
    num_obstacles = 1; % Single obstacle case
  end

  title(sprintf('Multi-Obstacle Mesh: %d obstacles, V-grid: %d nodes, P-grid: %d nodes', ...
                num_obstacles, total_v_nodes, total_p_nodes));
  xlabel('x');
  ylabel('y');
  legend('Location', 'best', 'FontSize', 8);

  % Save figure and display
  try
    saveas(fig, 'multi_obstacle_mesh.png');
    fprintf('Mesh visualization saved as multi_obstacle_mesh.png\n');
  catch
    % Ignore save errors
  end

  % Force figure to front and pause briefly to ensure visibility
  figure(fig);
  drawnow;
  fprintf('Mesh visualization displayed. Continuing with simulation...\n');
  pause(1); % Brief pause to let user see the mesh
elseif isCI || isTest
  fprintf('Skipping mesh visualization (CI/test mode)\n');
end

% Combine interior and boundary nodes to create complete grids
xy1 = [xy; boundary];      % Complete velocity grid (V-grid): interior + boundary
xy1_s = [xy_s; boundary_s]; % Complete pressure grid (P-grid): interior + boundary

% Extract coordinates for convenience
x1 = xy1(:, 1);   % x-coordinates of velocity nodes
y1 = xy1(:, 2);   % y-coordinates of velocity nodes
x1_s = xy1_s(:, 1); % x-coordinates of pressure nodes
y1_s = xy1_s(:, 2); % y-coordinates of pressure nodes
x0 = xy(:, 1);    % x-coordinates of interior velocity nodes only
y0 = xy(:, 2);    % y-coordinates of interior velocity nodes only
x0_s = xy_s(:, 1); % x-coordinates of interior pressure nodes only
y0_s = xy_s(:, 2); % y-coordinates of interior pressure nodes only

% Define distance thresholds for special RBF treatment near boundaries
x_min_dist = cfg.distances.x_min;  % Distance threshold from left boundary
x_max_dist = cfg.distances.x_max;  % Distance threshold from right boundary
y_min_dist = cfg.distances.y_min;  % Distance threshold from bottom boundary
y_max_dist = cfg.distances.y_max;  % Distance threshold from top boundary
r_dist = cfg.mesh.refine_a2 * cfg.mesh.edge_multiplier;  % Distance threshold from cylinder

%% 3) Build stencils for RBF-FD method
S = build_stencils(xy1, xy1_s, xy, xy_s, G, cfg);

%% 4) Build pressure system and boundary conditions
P = build_pressure_system(G, xy_s, xy1_s, cfg);
L_inv_s = P.L_inv_s;
D0_21_x_obs = P.D0_21_x_obs;
D0_21_y_obs = P.D0_21_y_obs;

%% 5) Build inter-grid operators (P-grid <-> V-grid)
[D0_21_x, D0_21_y, D0_12_x, D0_12_y] = build_intergrid_ops(G, xy, xy1, xy_s, xy1_s, S, cfg);

%% 6) Build velocity operators and boundary conditions
Vops = build_velocity_operators(G, xy, xy1, boundary_y, boundary_out, S, cfg, cfg.distances);

Dx = Vops.Dx;
Dy = Vops.Dy;
L0 = Vops.L0;
Dy_b_0 = Vops.Dy_b_0;
Dx_b_0 = Vops.Dx_b_0;
Dy_b = Vops.Dy_b;
Dy_b_1 = Vops.Dy_b_1;

%% 7) Precompute velocity system solvers
[L_u_inv, L_v_inv] = precompute_velocity_solvers(L0, Dy_b_0, Dx_b_0, xy, boundary_y, boundary_out, cfg);

%% 8) Setup time-stepping parameters
nu = cfg.simulation.viscosity;  % Kinematic viscosity (1/Reynolds number)
dt = cfg.simulation.time_step;  % Time step size
dt_original = dt;               % Store original time step for adaptive control
dt_reductions = 0;              % Counter for number of time step reductions

% Time tracking for time-based simulation control
current_time = 0.0;             % Current simulation time
if cfg.simulation.use_end_time
  target_end_time = cfg.simulation.end_time;
  if isCI || isTest
    target_end_time = cfg.simulation.end_time_ci;
  end
  fprintf('Time-based simulation: target end time = %.3f\n', target_end_time);
else
  target_end_time = Nt * dt_original;  % Equivalent end time for step-based simulation
  fprintf('Step-based simulation: equivalent end time = %.3f\n', target_end_time);
end

%% 8.1) Precompute mesh spacing stats for CFL-like diagnostics
h_min = NaN;
h_med = NaN;
h_local = [];  % per-node spacing for local CFL (V-grid length)
if cfg.logging.enable || true  % Always compute for CFL stopping condition
  try
    [~, D2] = knnsearch(xy, xy, 2);  % 2nd neighbor distance on interior V-grid
    if size(D2, 2) >= 2
      d2 = D2(:, 2);
      h_min = min(d2);
      h_med = median(d2);
      fprintf('[diag] spacing: h_min=%.3g, h_med=%.3g (V-grid interior)\n', h_min, h_med);

      % Build per-node spacing for full V-grid (interior + boundary)
      h_local = ones(size(xy1, 1), 1) * h_min;  % conservative default for boundaries
      h_local(1:length(xy)) = d2;               % interior nodes use their own spacing
    end
  catch ME
    warning('%s', ['spacing precomp failed: ' ME.message]);
  end
end

%% 9) Initialize simulation state
% Calculate memory allocation needs for time-based simulations
if cfg.simulation.use_end_time
  Nt_alloc = 10 * Nt;  % Allow more steps for time-based simulation with adaptive dt
else
  Nt_alloc = Nt;       % Traditional step limit
end
[W, p0] = init_state(xy1, xy1_s, boundary_obs, Nt_alloc);

% Initialize tracers (seed outside obstacles)
tracers = seed_tracers(cfg, G, isCI, isTest);

% Initialize particle velocities for Stokes dynamics
if strcmpi(cfg.tracers.dynamics, 'stokes') && ~isempty(tracers)
  % Extract initial velocity field components
  NV = size(xy1, 1);
  U0 = W(1:NV, 1);           % u-velocity at t=0
  V0 = W(NV + 1:end, 1);     % v-velocity at t=0

  % Initialize particle velocities based on configuration
  if strcmpi(cfg.tracers.vel_init, 'fluid')
    % Initialize particle velocities to local fluid velocity
    Fu = scatteredInterpolant(xy1(:, 1), xy1(:, 2), U0, 'linear', 'nearest');
    Fv = scatteredInterpolant(xy1(:, 1), xy1(:, 2), V0, 'linear', 'nearest');
    tracer_vel = [Fu(tracers(:, 1), tracers(:, 2)), Fv(tracers(:, 1), tracers(:, 2))];
    fprintf('Initialized %d Stokes particles with fluid velocity\n', size(tracers, 1));
  else
    % Initialize particle velocities to zero
    tracer_vel = zeros(size(tracers));
    fprintf('Initialized %d Stokes particles with zero velocity\n', size(tracers, 1));
  end

  % Report Stokes parameters
  nu = cfg.simulation.viscosity;
  rho_f = cfg.tracers.rho_f;
  rho_p = cfg.tracers.rho_p;
  dp = cfg.tracers.diameter;
  mu = nu * rho_f;
  tau_p = (rho_p * dp^2) / (18 * mu);
  fprintf('Stokes parameters: tau_p = %.3e, rho_p/rho_f = %.1f, d_p = %.3e\n', tau_p, rho_p / rho_f, dp);

  if cfg.tracers.gravity_enable
    fprintf('Gravity enabled: g = [%.3f, %.3f]\n', cfg.tracers.g(1), cfg.tracers.g(2));
  end
else
  tracer_vel = [];
  if ~isempty(tracers)
    fprintf('Initialized %d tracer particles\n', size(tracers, 1));
  end
end

% Define useful index lengths for boundary handling
L_B = length(boundary_obs) + length(boundary_in);   % Total special boundaries
L_B_y = length(boundary_y);   % Wall boundaries
L_W = length(xy);             % Interior velocity nodes
L_B_obs = length(boundary_obs);   % Obstacle boundary nodes

% Define pressure boundary indices for easy access
Idx_y_s = length(xy_s) + 1:length(xy_s) + length(boundary_y_s);    % Wall pressure indices
Idx_in_s = length(xy1_s) + 1 - length(boundary_obs_s) - length(boundary_in_s): ...
           length(xy1_s) - length(boundary_obs_s);  % Inlet pressure indices
Idx_out_s = length(xy_s) + 1 + length(boundary_y_s): ...
            length(xy_s) + length(boundary_out_s) + length(boundary_y_s);  % Outlet pressure indices

%% 10) Main time-stepping loop: Fractional Step Method
j = 0;
max_steps = Nt_alloc;  % Use the same allocation limit as memory

while true
  j = j + 1;

  % Check termination conditions
  if cfg.simulation.use_end_time
    % Time-based termination: check if we've reached target time
    if current_time >= target_end_time
      fprintf('Time-based termination: reached target time %.3f at step %d\n', target_end_time, j - 1);
      break
    end
    % Safety check for time-based simulation
    if j > max_steps
      fprintf('WARNING: Exceeded maximum step limit (%d). Current time: %.3f, target: %.3f\n', ...
              max_steps, current_time, target_end_time);
      fprintf('This may indicate excessive time step reduction. Consider adjusting parameters.\n');
      break
    end
  else
    % Step-based termination: check if we've completed all steps
    if j > Nt
      break
    end
  end

  if cfg.simulation.show_progress && ~isCI && ~isTest
    if cfg.simulation.use_end_time
      disp(['Time step j = ' num2str(j) ', time = ' num2str(current_time, '%.3f') '/' num2str(target_end_time, '%.3f')]);
    else
      disp(['Time step j = ' num2str(j)]);
    end
  end

  try
    % Use different schemes for startup vs main integration
    if j < 3
      % First few steps: use simple first-order scheme for stability
      W(:, j + 1) = W(:, j);  % Copy previous solution for startup
    else
      % Main fractional step algorithm implemented in NS_2d_fractional_step_PHS
      % This performs: 1) Advection-diffusion step with Adams-Bashforth + Crank-Nicolson
      %                2) Pressure correction step to enforce incompressibility
      %                3) Velocity correction using pressure gradient
      [W(:, j + 1), p0] = NS_2d_fractional_step_PHS(dt, nu, W(:, j - 1), W(:, j), ...
                                                    Dy, Dx, L_inv_s, L_u_inv, L_v_inv, ...
                                                    L0, L_B, L_B_obs, L_W, L_B_y, ...
                                                    length(boundary_s), D0_12_x, D0_12_y, ...
                                                    D0_21_x, D0_21_y, Dy_b, Dy_b_1, ...
                                                    D0_21_x_obs, D0_21_y_obs, p0, W(:, 1), cfg);
    end
  catch ME
    warning('Step %d threw error: %s', j, ME.message);
    if cfg.logging.snapshot_on_nan
      stability_diagnostics('save_debug_snapshot', cfg, j, xy1, W, p0, dt, Dx, Dy, D0_12_x, D0_12_y, G);
    end
    rethrow(ME);
  end

  % Advect tracers with latest velocity field (Heun: uses W(:,j) and W(:,j+1))
  if cfg.tracers.enable && ~isempty(tracers)
    W_prev = W(:, j);
    W_now  = W(:, j + 1);
    domain_struct = struct('x_min', x_min, 'x_max', x_max, 'y_min', y_min, 'y_max', y_max);

    if strcmpi(cfg.tracers.dynamics, 'stokes')
      % Stokes particle dynamics with velocity evolution
      [tracers, tracer_vel] = advect_tracers(tracers, tracer_vel, dt, xy1, W_prev, W_now, cfg, G.fd_obs, domain_struct);
    else
      % Tracer particle dynamics (original behavior)
      tracers = advect_tracers(tracers, dt, xy1, W_prev, W_now, cfg, G.fd_obs, domain_struct);
    end
  end

  % Comprehensive instability detection
  if cfg.logging.immediate_nan_check || cfg.logging.comprehensive_instability_check
    instability_detected = false;
    instability_type = '';
    bad_idx = 1;

    % Extract current velocity components for detailed analysis
    n_nodes = length(xy1);
    U_current = W(1:n_nodes, j + 1);           % Current u-velocity
    V_current = W(n_nodes + 1:end, j + 1);     % Current v-velocity

    % Check for NaNs/Infs and near-zero collapse using comprehensive validator
    p_for_check = [];
    if exist('p0', 'var') && ~isempty(p0)
      p_for_check = p0;
    end

    [is_valid, failure_type, failure_details] = check_solution_validity(W(:, j + 1), p_for_check, 1e-12);

    % Additional explicit checks for zero velocity fields (like visualization does)
    u_max = max(abs(U_current));
    v_max = max(abs(V_current));
    vel_magnitude_max = max(sqrt(U_current.^2 + V_current.^2));

    % Optional debug output to show detection is running every step
    if cfg.logging.enable && j <= 5  % Show for first few steps only
      fprintf('[debug step %d] Instability check: %s (vel_max=%.3e, u_max=%.3e, v_max=%.3e, |V|_max=%.3e)\n', ...
              j, string(is_valid), max(abs(W(:, j + 1))), u_max, v_max, vel_magnitude_max);
    end

    % Explicit zero velocity field checks (same as visualization warnings)
    if ~is_valid
      % Already caught by comprehensive validator
    elseif u_max == 0 && v_max == 0
      is_valid = false;
      failure_type = 'Complete velocity field collapse - all U and V components are zero';
      instability_detected = true;
      instability_type = failure_type;
    elseif vel_magnitude_max == 0
      is_valid = false;
      failure_type = 'Velocity magnitude field collapse - all |V| components are zero';
      instability_detected = true;
      instability_type = failure_type;
    elseif u_max < 1e-15 && v_max < 1e-15
      is_valid = false;
      failure_type = 'Near-complete velocity field collapse - all components below machine precision';
      instability_detected = true;
      instability_type = failure_type;
    end

    if ~is_valid
      instability_detected = true;
      instability_type = failure_type;

      % Immediate notification of detection
      fprintf('\n[ALERT] IMMEDIATE DETECTION at step %d: %s\n', j, failure_type);

      % Find representative bad index based on failure type
      if contains(failure_type, 'NaN/Inf in velocity')
        bad_idx = find(~isfinite(W(:, j + 1)), 1, 'first');
      elseif contains(failure_type, 'NaN/Inf in pressure')
        bad_idx = find(~isfinite(p0), 1, 'first') + length(W(:, j + 1));
      elseif contains(failure_type, 'collapsed to zero') || contains(failure_type, 'mostly collapsed')
        bad_idx = 1;  % Representative index for collapse
      else
        bad_idx = 1;  % Default
      end

      % Add detailed failure information to output
      fprintf('\nDetailed failure analysis:\n');
      fprintf('  Velocity nodes: %d\n', failure_details.velocity_nodes);
      if failure_details.velocity_nan_count > 0
        fprintf('  Velocity NaN/Inf count: %d (%.1f%%)\n', failure_details.velocity_nan_count, ...
                100 * failure_details.velocity_nan_count / failure_details.velocity_nodes);
      end
      if failure_details.velocity_zero_count > 0
        fprintf('  Velocity near-zero count: %d (%.1f%%) [tolerance: %.0e]\n', ...
                failure_details.velocity_zero_count, ...
                100 * failure_details.velocity_zero_count / failure_details.velocity_nodes, ...
                failure_details.tolerance);
      end
      fprintf('  Velocity field: max=%.3e, mean=%.3e\n', failure_details.velocity_max, failure_details.velocity_mean);

      if ~isempty(p_for_check)
        fprintf('  Pressure nodes: %d\n', failure_details.pressure_nodes);
        if isfield(failure_details, 'pressure_nan_count') && failure_details.pressure_nan_count > 0
          fprintf('  Pressure NaN/Inf count: %d (%.1f%%)\n', failure_details.pressure_nan_count, ...
                  100 * failure_details.pressure_nan_count / failure_details.pressure_nodes);
        end
        fprintf('  Pressure field: max=%.3e, mean=%.3e\n', failure_details.pressure_max, failure_details.pressure_mean);
      end

      % Check for velocity explosion (separate from validity check)
    elseif max(abs(W(:, j + 1))) > cfg.logging.velocity_explosion_threshold
      instability_detected = true;
      instability_type = 'Velocity explosion';
      [~, bad_idx] = max(abs(W(:, j + 1)));

      % Check for pressure explosion (if pressure is available)
    elseif exist('p0', 'var') && max(abs(p0)) > cfg.logging.pressure_explosion_threshold
      instability_detected = true;
      instability_type = 'Pressure explosion';
      [~, bad_idx] = max(abs(p0));
      bad_idx = bad_idx + length(W(:, j + 1));  % Adjust index for pressure grid
    end

    if instability_detected
      [loc_str, pt] = stability_diagnostics('classify_node_idx', bad_idx, L_W, boundary_y, boundary_out, boundary_in, boundary_obs, xy1);

      fprintf('\n*** SIMULATION INSTABILITY DETECTED ***\n');
      fprintf('Type: %s\n', instability_type);
      if isnan(pt(1)) || isnan(pt(2))
        fprintf('Step: %d, Index: %d, Location: %s (coordinates not available)\n', j, bad_idx, loc_str);
      else
        fprintf('Step: %d, Index: %d, Location: %s at (%.3g, %.3g)\n', j, bad_idx, loc_str, pt(1), pt(2));
      end
      fprintf('Time: %.6f, dt: %.3e\n', current_time, dt);

      % Report current solution statistics
      vel_max = max(abs(W(:, j + 1)));
      vel_mean = mean(abs(W(:, j + 1)));
      fprintf('Velocity stats: max=%.3e, mean=%.3e\n', vel_max, vel_mean);
      if exist('p0', 'var')
        p_max = max(abs(p0));
        p_mean = mean(abs(p0));
        fprintf('Pressure stats: max=%.3e, mean=%.3e\n', p_max, p_mean);
      end

      % Check if this might be related to CFL conditions
      if ~isempty(h_local)
        try
          cfl_conv_max = stability_diagnostics('compute_max_cfl', W(:, j), dt, h_local);  % Use previous step
          cfl_visc_max = stability_diagnostics('compute_max_cfl_viscous', dt, nu, h_local);
          fprintf('CFL conditions at failure: conv=%.3f, visc=%.3f\n', cfl_conv_max, cfl_visc_max);

          if cfl_conv_max > 1.0 || cfl_visc_max > 1.0
            fprintf('LIKELY CAUSE: CFL condition violation (>1.0 indicates instability)\n');
            if ~cfg.adaptive_dt.enable
              fprintf('RECOMMENDATION: Enable adaptive time step (cfg.adaptive_dt.enable = true)\n');
            else
              fprintf('RECOMMENDATION: Lower CFL thresholds or reduce initial time step\n');
            end
          elseif strcmp(instability_type, 'Velocity collapse (all velocities -> 0)')
            fprintf('LIKELY CAUSE: Numerical damping or boundary condition issues\n');
            fprintf('RECOMMENDATION: Check boundary conditions and reduce viscosity\n');
          elseif contains(instability_type, 'explosion')
            fprintf('LIKELY CAUSE: Numerical instability or insufficient time step\n');
            fprintf('RECOMMENDATION: Reduce time step significantly\n');
          end
        catch
          fprintf('Could not compute CFL conditions for analysis\n');
        end
      end

      % Perform detailed instability analysis
      stability_diagnostics('analyze_instability_cause', j, W, p0, bad_idx, loc_str, pt, dt, Dx, Dy, D0_12_x, D0_12_y, xy1, h_min, h_med, cfg);

      if cfg.logging.snapshot_on_nan
        stability_diagnostics('save_debug_snapshot', cfg, j, xy1, W, p0, dt, Dx, Dy, D0_12_x, D0_12_y, G);
      end
      break  % Stop simulation immediately
    end
  end

  % Diagnostics every N steps
  if cfg.logging.enable && (mod(j, cfg.logging.step_frequency) == 0 || j <= 3)
    stability_diagnostics('log_step_stats', j, W(:, j), W(:, j + 1), dt, Dx, Dy, D0_12_x, D0_12_y, xy1, h_min, h_med, cfg);
  end

  % Local CFL check every N steps (uses per-node spacing with both convective and viscous)
  if cfg.logging.enable && cfg.logging.cfl_local_enable && ~isempty(h_local) && ...
     mod(j, cfg.logging.cfl_check_frequency) == 0
    stability_diagnostics('check_cfl_combined', j, W(:, j + 1), dt, nu, h_local, xy1, cfg);
  end

  % Periodic health check for gradual instabilities
  if cfg.logging.periodic_health_check && mod(j, cfg.logging.health_check_frequency) == 0
    vel_max = max(abs(W(:, j + 1)));
    vel_mean = mean(abs(W(:, j + 1)));
    vel_std = std(abs(W(:, j + 1)));

    fprintf('[health %5d] t=%.3f, vel: max=%.3e, mean=%.3e, std=%.3e', j, current_time, vel_max, vel_mean, vel_std);

    % Check for concerning trends
    if vel_max > cfg.logging.velocity_explosion_threshold * 0.1  % 10% of explosion threshold
      fprintf(' [WARNING: High velocities detected]');
    elseif vel_max < 1e-8 && j > 100  % Very low velocities after startup
      fprintf(' [WARNING: Very low velocities]');
    elseif vel_std / vel_mean > 100  % High variability
      fprintf(' [WARNING: High velocity variability]');
    end

    if exist('p0', 'var')
      p_max = max(abs(p0));
      fprintf(', p_max=%.3e', p_max);
      if p_max > cfg.logging.pressure_explosion_threshold * 0.1
        fprintf(' [WARNING: High pressures]');
      end
    end

    fprintf('\n');
  end

  % CFL condition check and adaptive time step control (always active)
  if ~isempty(h_local)
    cfl_conv_max = stability_diagnostics('compute_max_cfl', W(:, j + 1), dt, h_local);
    cfl_visc_max = stability_diagnostics('compute_max_cfl_viscous', dt, nu, h_local);

    % Check both convective and viscous CFL conditions
    cfl_violation = false;
    violation_type = '';

    if cfg.adaptive_dt.enable
      if isfinite(cfl_conv_max) && cfl_conv_max > cfg.adaptive_dt.cfl_threshold
        cfl_violation = true;
        violation_type = sprintf('Convective CFL: %.3f > %.3f', cfl_conv_max, cfg.adaptive_dt.cfl_threshold);
      elseif isfinite(cfl_visc_max) && cfl_visc_max > cfg.adaptive_dt.cfl_viscous_threshold
        cfl_violation = true;
        violation_type = sprintf('Viscous CFL: %.3f > %.3f', cfl_visc_max, cfg.adaptive_dt.cfl_viscous_threshold);
      end

      if cfl_violation
        % Calculate minimum allowed time step
        dt_min = dt_original * cfg.adaptive_dt.min_factor;

        % Check if we can still reduce the time step
        if dt > dt_min && dt_reductions < cfg.adaptive_dt.max_reductions
          % Reduce time step
          dt_new = dt * cfg.adaptive_dt.reduction_factor;
          dt_new = max(dt_new, dt_min);  % Don't go below minimum

          fprintf('\n*** ADAPTIVE TIME STEP REDUCTION ***\n');
          fprintf('Step: %d, %s\n', j, violation_type);
          fprintf('Reducing time step: %.3e -> %.3e (factor: %.2f)\n', dt, dt_new, dt_new / dt);
          fprintf('Reductions so far: %d/%d\n', dt_reductions + 1, cfg.adaptive_dt.max_reductions);

          dt = dt_new;
          dt_reductions = dt_reductions + 1;

          % Recompute this time step with new dt
          % Need to redo the fractional step with smaller time step
          try
            if j < 3
              W(:, j + 1) = W(:, j);  % Copy previous solution for startup
            else
              [W(:, j + 1), p0] = NS_2d_fractional_step_PHS(dt, nu, W(:, j - 1), W(:, j), ...
                                                            Dy, Dx, L_inv_s, L_u_inv, L_v_inv, ...
                                                            L0, L_B, L_B_obs, L_W, L_B_y, ...
                                                            length(boundary_s), D0_12_x, D0_12_y, ...
                                                            D0_21_x, D0_21_y, Dy_b, Dy_b_1, ...
                                                            D0_21_x_obs, D0_21_y_obs, p0, W(:, 1), cfg);
            end

            % Check for NaNs and near-zero collapse in recomputed solution - STOP IMMEDIATELY if found
            if cfg.adaptive_dt.nan_check_enable
              p_for_check = [];
              if exist('p0', 'var') && ~isempty(p0)
                p_for_check = p0;
              end

              [is_valid, failure_type, failure_details] = check_solution_validity(W(:, j + 1), p_for_check, 1e-12);

              if ~is_valid
                fprintf('\n[FATAL] ERROR: %s after time step reduction!\n', failure_type);
                fprintf('Step %d: Time step was reduced from %.3e to %.3e but solution is still corrupted.\n', ...
                        j, dt / cfg.adaptive_dt.reduction_factor, dt);
                fprintf('This indicates the solution cannot be recovered by time step reduction alone.\n');

                % Save debug information
                if cfg.logging.snapshot_on_nan
                  fprintf('Saving debug snapshot for analysis...\n');
                  stability_diagnostics('save_debug_snapshot', cfg, j, xy1, W, p0, dt, Dx, Dy, D0_12_x, D0_12_y, G);
                end

                % Provide detailed diagnostic information
                fprintf('\nDetailed diagnostic information:\n');
                fprintf('  Velocity nodes: %d\n', failure_details.velocity_nodes);
                if failure_details.velocity_nan_count > 0
                  fprintf('  Velocity NaN/Inf: %d (%.1f%%)\n', failure_details.velocity_nan_count, ...
                          100 * failure_details.velocity_nan_count / failure_details.velocity_nodes);
                end
                if failure_details.velocity_zero_count > 0
                  fprintf('  Velocity near-zero: %d (%.1f%%) [tolerance: %.0e]\n', ...
                          failure_details.velocity_zero_count, ...
                          100 * failure_details.velocity_zero_count / failure_details.velocity_nodes, ...
                          failure_details.tolerance);
                end
                fprintf('  Velocity field: max=%.3e, mean=%.3e\n', failure_details.velocity_max, failure_details.velocity_mean);

                if ~isempty(p_for_check)
                  fprintf('  Pressure nodes: %d\n', failure_details.pressure_nodes);
                  if isfield(failure_details, 'pressure_nan_count') && failure_details.pressure_nan_count > 0
                    fprintf('  Pressure NaN/Inf: %d (%.1f%%)\n', failure_details.pressure_nan_count, ...
                            100 * failure_details.pressure_nan_count / failure_details.pressure_nodes);
                  end
                  fprintf('  Pressure field: max=%.3e, mean=%.3e\n', failure_details.pressure_max, failure_details.pressure_mean);
                end

                fprintf('  Current time: %.6f\n', current_time);
                fprintf('  Time step reductions attempted: %d\n', dt_reductions);
                fprintf('  Final time step: %.3e\n', dt);

                % Stop simulation immediately - do not attempt further reductions
                error('SIMULATION TERMINATED: %s at step %d. Solution is corrupted and cannot be recovered by time step reduction.', failure_type, j);
              end
            end

            % Check CFL again with new time step (only if solution is valid)
            cfl_conv_new = stability_diagnostics('compute_max_cfl', W(:, j + 1), dt, h_local);
            cfl_visc_new = stability_diagnostics('compute_max_cfl_viscous', dt, nu, h_local);
            fprintf('New CFL after reduction: conv=%.3f, visc=%.3f\n', cfl_conv_new, cfl_visc_new);

          catch ME
            fprintf('Error with reduced time step: %s\n', ME.message);
            if cfg.logging.snapshot_on_nan
              stability_diagnostics('save_debug_snapshot', cfg, j, xy1, W, p0, dt, Dx, Dy, D0_12_x, D0_12_y, G);
            end
            rethrow(ME);
          end

        else
          % Cannot reduce time step further - stop simulation
          fprintf('\n*** SIMULATION STOPPED: CFL CONDITION VIOLATED ***\n');
          fprintf('Step: %d, %s\n', j, violation_type);
          fprintf('Convective CFL: %.3f, Viscous CFL: %.3f\n', cfl_conv_max, cfl_visc_max);
          if dt <= dt_min
            fprintf('Time step at minimum limit: dt = %.3e (%.1f%% of original)\n', dt, 100 * dt / dt_original);
          end
          if dt_reductions >= cfg.adaptive_dt.max_reductions
            fprintf('Maximum reductions reached: %d/%d\n', dt_reductions, cfg.adaptive_dt.max_reductions);
          end
          fprintf('Final recommendation: Reduce mesh size or increase viscosity\n');
          break
        end
      end  % End of if cfl_violation

    elseif ~cfg.adaptive_dt.enable && ((isfinite(cfl_conv_max) && cfl_conv_max > cfg.logging.cfl_stop_threshold) || ...
                                       (isfinite(cfl_visc_max) && cfl_visc_max > 0.5))
      % Original stopping behavior when adaptive time step is disabled
      fprintf('\n*** SIMULATION STOPPED: CFL CONDITION VIOLATED ***\n');
      fprintf('Step: %d, Convective CFL: %.3f, Viscous CFL: %.3f\n', j, cfl_conv_max, cfl_visc_max);
      if cfl_conv_max > cfg.logging.cfl_stop_threshold
        fprintf('Convective CFL %.3f > %.3f (threshold)\n', cfl_conv_max, cfg.logging.cfl_stop_threshold);
      end
      if cfl_visc_max > 0.5
        fprintf('Viscous CFL %.3f > 0.5 (standard limit)\n', cfl_visc_max);
      end
      fprintf('Recommendation: Reduce time step or enable adaptive time step\n');
      break
    end
  end

  % Live max-|V| heatmap every N steps (interactive only)
  if doPlot && ~isCI && ~isTest && isfield(cfg.visualization, 'live_enable') && cfg.visualization.live_enable && ...
     mod(j, cfg.visualization.live_frequency) == 0
    if strcmpi(cfg.tracers.dynamics, 'stokes') && exist('tracer_vel', 'var')
      visualization('live_heatmap', cfg, xy1, W(:, j + 1), j, x_min, x_max, y_min, y_max, tracers, tracer_vel);
    else
      visualization('live_heatmap', cfg, xy1, W(:, j + 1), j, x_min, x_max, y_min, y_max, tracers);
    end
  end

  % Advance simulation time (using current dt, which may have been adapted)
  current_time = current_time + dt;
end

% Extract final velocity field for potential continuation
W0 = W(:, j);  % Use last computed step, not end of allocated array

% Report simulation completion
fprintf('\n=== SIMULATION COMPLETED ===\n');
if cfg.simulation.use_end_time
  fprintf('Time-based simulation: reached time %.3f (target: %.3f)\n', current_time, target_end_time);
  fprintf('Completed %d time steps with final dt=%.3e\n', j, dt);
  if dt ~= dt_original
    fprintf('Time step adapted from %.3e to %.3e (%d reductions)\n', dt_original, dt, dt_reductions);
  end
else
  fprintf('Step-based simulation: completed %d/%d steps\n', j, Nt);
  fprintf('Final simulation time: %.3f (dt=%.3e)\n', current_time, dt);
  if dt ~= dt_original
    fprintf('Time step adapted from %.3e to %.3e (%d reductions)\n', dt_original, dt, dt_reductions);
  end
end
fprintf('=============================\n\n');

%% Final solution validation before declaring success
fprintf('=== FINAL SOLUTION VALIDATION ===\n');

% CRITICAL: Check velocity field immediately before validation to catch corruption
n_nodes = length(xy1);
U_final = W(1:n_nodes, j);           % Final u-velocity
V_final = W(n_nodes + 1:end, j);     % Final v-velocity
u_max_final = max(abs(U_final));
v_max_final = max(abs(V_final));
vel_magnitude_max_final = max(sqrt(U_final.^2 + V_final.^2));

fprintf('Pre-validation velocity check:\n');
fprintf('  u_max=%.3e, v_max=%.3e, |V|_max=%.3e\n', u_max_final, v_max_final, vel_magnitude_max_final);

% IMMEDIATE CHECK: If velocities are zero here, stop before formal validation
if u_max_final == 0 && v_max_final == 0
  error(['SIMULATION FAILED: Complete velocity field collapse detected before final validation at step %d.\n' ...
         'All U and V components are exactly zero. This indicates the solution was corrupted after the last time step.'], j);
elseif vel_magnitude_max_final == 0
  error(['SIMULATION FAILED: Velocity magnitude collapse detected before final validation at step %d.\n' ...
         'All |V| components are exactly zero. This indicates the solution was corrupted after the last time step.'], j);
elseif u_max_final < 1e-15 && v_max_final < 1e-15
  error(['SIMULATION FAILED: Near-complete velocity collapse detected before final validation at step %d.\n' ...
         'All velocity components are below machine precision (< 1e-15). This indicates the solution was corrupted ' ...
         'after the last time step.'], j);
end

% Use comprehensive validation for both velocity and pressure
p_for_check = [];
if exist('p0', 'var') && ~isempty(p0)
  p_for_check = p0;
end

% Use W(:, j) since j was incremented but the final step was not computed
[is_valid, failure_type, failure_details] = check_solution_validity(W(:, j), p_for_check, 1e-12);

if ~is_valid
  fprintf('[CRITICAL] %s in final solution!\n', failure_type);
  fprintf('The simulation appeared to complete but the solution is corrupted.\n');

  % Detailed failure analysis
  fprintf('\nDetailed final solution analysis:\n');
  fprintf('  Velocity nodes: %d\n', failure_details.velocity_nodes);
  if failure_details.velocity_nan_count > 0
    fprintf('  Velocity NaN/Inf: %d (%.1f%%)\n', failure_details.velocity_nan_count, ...
            100 * failure_details.velocity_nan_count / failure_details.velocity_nodes);
  end
  if failure_details.velocity_zero_count > 0
    fprintf('  Velocity near-zero: %d (%.1f%%) [tolerance: %.0e]\n', ...
            failure_details.velocity_zero_count, ...
            100 * failure_details.velocity_zero_count / failure_details.velocity_nodes, ...
            failure_details.tolerance);
  end
  fprintf('  Velocity field: max=%.3e, mean=%.3e\n', failure_details.velocity_max, failure_details.velocity_mean);

  if ~isempty(p_for_check)
    fprintf('  Pressure nodes: %d\n', failure_details.pressure_nodes);
    if isfield(failure_details, 'pressure_nan_count') && failure_details.pressure_nan_count > 0
      fprintf('  Pressure NaN/Inf: %d (%.1f%%)\n', failure_details.pressure_nan_count, ...
              100 * failure_details.pressure_nan_count / failure_details.pressure_nodes);
    end
    fprintf('  Pressure field: max=%.3e, mean=%.3e\n', failure_details.pressure_max, failure_details.pressure_mean);
  end

  % Save debug information
  if cfg.logging.snapshot_on_nan
    fprintf('Saving debug snapshot for analysis...\n');
    stability_diagnostics('save_debug_snapshot', cfg, j, xy1, W, p0, dt, Dx, Dy, D0_12_x, D0_12_y, G);
  end

  error('SIMULATION FAILED: %s at step %d. Solution is corrupted.', failure_type, j);
else
  fprintf('[PASS] Final solution validation: PASSED\n');
  fprintf('   Velocity nodes: %d (all valid)\n', failure_details.velocity_nodes);
  fprintf('   Velocity field: max=%.3e, mean=%.3e\n', failure_details.velocity_max, failure_details.velocity_mean);
  if ~isempty(p_for_check)
    fprintf('   Pressure nodes: %d (all valid)\n', failure_details.pressure_nodes);
    fprintf('   Pressure field: max=%.3e, mean=%.3e\n', failure_details.pressure_max, failure_details.pressure_mean);
  end
end

fprintf('=====================================\n\n');

%% 11) Visualization of final results
if strcmpi(cfg.tracers.dynamics, 'stokes') && exist('tracer_vel', 'var')
  visualization('final', cfg, doPlot, xy1, W0, Nt, x_min, x_max, y_min, y_max, Dx, Dy, tracers, tracer_vel);
else
  visualization('final', cfg, doPlot, xy1, W0, Nt, x_min, x_max, y_min, y_max, Dx, Dy, tracers);
end