% Q2 Discontinuous Galerkin Method for 2D Compressible Euler Equations
% Accuracy Test with Isentropic Vortex (Smooth Solution)
% Computes L2 errors and convergence rates

clear all; close all; clc;

%% Physical and numerical parameters
gamma = 1.4;           % Ratio of specific heats
CFL = 0.1;             % CFL number (reduced for stability)

% Domain: [0, 10] x [0, 10] with periodic boundaries
x_min = 0; x_max = 10;
y_min = 0; y_max = 10;

% Test different mesh resolutions for convergence study
mesh_sizes = [10, 15, 20, 25];
num_tests = length(mesh_sizes);

% DG parameters
p = 2;    % Polynomial degree (Q2)
Np = (p+1)^2;  % Number of basis functions per element

fprintf('=== Q2 DG Accuracy Test: Isentropic Vortex ===\n');
fprintf('Polynomial degree: %d\n', p);
fprintf('Testing mesh sizes: ');
fprintf('%d ', mesh_sizes);
fprintf('\n\n');

% Storage for errors and convergence rates
L2_errors_rho = zeros(num_tests, 1);
L2_errors_u = zeros(num_tests, 1);
L2_errors_v = zeros(num_tests, 1);
L2_errors_p = zeros(num_tests, 1);
mesh_spacing = zeros(num_tests, 1);

%% Run convergence study
for test_idx = 1:num_tests
    Nx = mesh_sizes(test_idx);
    Ny = mesh_sizes(test_idx);

    fprintf('--- Test %d: %d x %d mesh ---\n', test_idx, Nx, Ny);

    % Vortex advects one full period (returns to initial position)
    T_final = 2.0;  % Final time (reduced for stability testing)

    %% Generate rectangular mesh
    dx = (x_max - x_min) / Nx;
    dy = (y_max - y_min) / Ny;
    mesh_spacing(test_idx) = dx;

    % Element centers
    x_centers = linspace(x_min + dx/2, x_max - dx/2, Nx);
    y_centers = linspace(y_min + dy/2, y_max - dy/2, Ny);

    %% Setup reference element and quadrature
    % Gauss-Legendre quadrature points and weights in 1D
    % Use p+1 points for exact integration of mass matrix (degree 2p)
    n_quad_1d = p + 1;  % 3 points for p=2
    [xi_1d, w_1d] = lgwt(n_quad_1d, -1, 1);

    % Tensor product for 2D
    [XI, ETA] = meshgrid(xi_1d, xi_1d);
    xi_quad = XI(:);
    eta_quad = ETA(:);
    [W1, W2] = meshgrid(w_1d, w_1d);
    w_quad = W1(:) .* W2(:);
    N_quad = length(w_quad);

    % Basis functions: Standard Legendre polynomials (unnormalized)
    L0 = @(x) ones(size(x));
    L1 = @(x) x;
    L2 = @(x) 0.5 * (3*x.^2 - 1);

    L_basis = {L0, L1, L2};
    dL_basis = {@(x) 0*x, @(x) ones(size(x)), @(x) 3*x};

    % Evaluate basis functions at quadrature points
    Phi = zeros(N_quad, Np);
    dPhi_xi = zeros(N_quad, Np);
    dPhi_eta = zeros(N_quad, Np);

    idx = 1;
    for j = 0:p
        for i = 0:p
            Phi(:, idx) = L_basis{i+1}(xi_quad) .* L_basis{j+1}(eta_quad);
            dPhi_xi(:, idx) = dL_basis{i+1}(xi_quad) .* L_basis{j+1}(eta_quad);
            dPhi_eta(:, idx) = L_basis{i+1}(xi_quad) .* dL_basis{j+1}(eta_quad);
            idx = idx + 1;
        end
    end

    % Mass matrix (on reference element)
    M = Phi' * diag(w_quad) * Phi;

    % Add small regularization for numerical stability
    M = M + 1e-14 * eye(Np);

    % Check condition number and use robust inversion
    cond_M = condest(M);
    if cond_M > 1e12
        fprintf('  Warning: Mass matrix poorly conditioned (cond ~ %.2e). Using pseudoinverse.\n', cond_M);
        M_inv = pinv(M, 1e-10);
    else
        % Use LU decomposition for better numerical stability
        [L, U_lu, P] = lu(M);
        M_inv = U_lu \ (L \ P);
    end

    % Verify the inversion worked
    if any(isnan(M_inv(:))) || any(isinf(M_inv(:)))
        error('Mass matrix inversion failed - contains NaN or Inf');
    end

    % Check that M * M_inv is close to identity
    residual = norm(M * M_inv - eye(Np), 'fro');
    if residual > 1e-6
        fprintf('  Warning: Mass matrix inversion residual = %.2e\n', residual);
    end

    %% Initialize solution with isentropic vortex
    U = zeros(Nx, Ny, Np, 4);

    % Isentropic vortex parameters
    vortex_strength = 1.0;  % Reduced for smoother solution
    x0 = 5.0;  % Initial vortex center
    y0 = 5.0;
    u_inf = 1.0;  % Background velocity
    v_inf = 1.0;
    rho_inf = 1.0;
    p_inf = 1.0;

    for i = 1:Nx
        for j = 1:Ny
            x_c = x_centers(i);
            y_c = y_centers(j);

            % Evaluate at cell center for initial cell average
            [rho, u, v, p] = isentropic_vortex(x_c, y_c, 0, vortex_strength, ...
                x0, y0, u_inf, v_inf, rho_inf, p_inf, gamma);

            E = p / (gamma - 1) + 0.5 * rho * (u^2 + v^2);

            % Verify positive quantities
            if rho <= 0 || p <= 0 || E <= 0
                error('Initial condition has negative values at cell (%d,%d): rho=%.4e, p=%.4e, E=%.4e', ...
                    i, j, rho, p, E);
            end

            % Set cell average (first basis function is constant)
            U(i, j, 1, 1) = rho;
            U(i, j, 1, 2) = rho * u;
            U(i, j, 1, 3) = rho * v;
            U(i, j, 1, 4) = E;
        end
    end

    % Compute initial pressure range for diagnostics
    p_init = zeros(Nx, Ny);
    for i = 1:Nx
        for j = 1:Ny
            rho_tmp = U(i, j, 1, 1);
            u_tmp = U(i, j, 1, 2) / rho_tmp;
            v_tmp = U(i, j, 1, 3) / rho_tmp;
            E_tmp = U(i, j, 1, 4);
            p_init(i, j) = (gamma - 1) * (E_tmp - 0.5 * rho_tmp * (u_tmp^2 + v_tmp^2));
        end
    end
    fprintf('  Initial condition: rho range [%.4f, %.4f], p range [%.4f, %.4f]\n', ...
        min(U(:,:,1,1),[],'all'), max(U(:,:,1,1),[],'all'), ...
        min(p_init(:)), max(p_init(:)));

    %% Time stepping with SSP-RK3
    t = 0;
    iter = 0;

    while t < T_final
        % Compute time step
        dt = compute_timestep(U, dx, dy, gamma, CFL, Nx, Ny, Np);
        if t + dt > T_final
            dt = T_final - t;
        end

        % SSP-RK3 time integration with positivity-preserving limiter
        U1 = U + dt * rhs_dg(U, dx, dy, gamma, Phi, dPhi_xi, dPhi_eta, w_quad, M_inv, Nx, Ny, Np, x_centers, y_centers);
        U1 = apply_positivity_limiter(U1, dx, dy, gamma, Phi, w_quad, Nx, Ny, Np, N_quad);

        % Check for NaN or Inf
        if any(isnan(U1(:))) || any(isinf(U1(:)))
            error('Solution became NaN or Inf at iteration %d, t = %.4f', iter, t);
        end

        U2 = 0.75*U + 0.25*(U1 + dt * rhs_dg(U1, dx, dy, gamma, Phi, dPhi_xi, dPhi_eta, w_quad, M_inv, Nx, Ny, Np, x_centers, y_centers));
        U2 = apply_positivity_limiter(U2, dx, dy, gamma, Phi, w_quad, Nx, Ny, Np, N_quad);

        if any(isnan(U2(:))) || any(isinf(U2(:)))
            error('Solution became NaN or Inf at iteration %d, t = %.4f', iter, t);
        end

        U = (1/3)*U + (2/3)*(U2 + dt * rhs_dg(U2, dx, dy, gamma, Phi, dPhi_xi, dPhi_eta, w_quad, M_inv, Nx, Ny, Np, x_centers, y_centers));
        U = apply_positivity_limiter(U, dx, dy, gamma, Phi, w_quad, Nx, Ny, Np, N_quad);

        if any(isnan(U(:))) || any(isinf(U(:)))
            error('Solution became NaN or Inf at iteration %d, t = %.4f', iter, t);
        end

        t = t + dt;
        iter = iter + 1;

        if mod(iter, 100) == 0
            fprintf('  Iter %4d: t = %.4f, dt = %.4e\n', iter, t, dt);
        end
    end

    fprintf('  Completed: %d iterations\n', iter);

    %% Compute L2 errors
    [err_rho, err_u, err_v, err_p] = compute_L2_error(U, x_centers, y_centers, t, ...
        vortex_strength, x0, y0, u_inf, v_inf, rho_inf, p_inf, gamma, Nx, Ny);

    L2_errors_rho(test_idx) = err_rho;
    L2_errors_u(test_idx) = err_u;
    L2_errors_v(test_idx) = err_v;
    L2_errors_p(test_idx) = err_p;

    fprintf('  L2 Error - Density: %.6e\n', err_rho);
    fprintf('  L2 Error - u-velocity: %.6e\n', err_u);
    fprintf('  L2 Error - v-velocity: %.6e\n', err_v);
    fprintf('  L2 Error - Pressure: %.6e\n\n', err_p);
end

%% Compute convergence rates
fprintf('=== Convergence Analysis ===\n');
fprintf('Mesh Size    dx         L2(rho)      Rate     L2(p)        Rate\n');
fprintf('--------------------------------------------------------------------\n');

for test_idx = 1:num_tests
    if test_idx == 1
        fprintf('%3dx%3d    %.4f    %.6e     -       %.6e     -\n', ...
            mesh_sizes(test_idx), mesh_sizes(test_idx), ...
            mesh_spacing(test_idx), L2_errors_rho(test_idx), L2_errors_p(test_idx));
    else
        rate_rho = log(L2_errors_rho(test_idx-1) / L2_errors_rho(test_idx)) / ...
                   log(mesh_spacing(test_idx-1) / mesh_spacing(test_idx));
        rate_p = log(L2_errors_p(test_idx-1) / L2_errors_p(test_idx)) / ...
                 log(mesh_spacing(test_idx-1) / mesh_spacing(test_idx));

        fprintf('%3dx%3d    %.4f    %.6e    %.2f     %.6e    %.2f\n', ...
            mesh_sizes(test_idx), mesh_sizes(test_idx), ...
            mesh_spacing(test_idx), L2_errors_rho(test_idx), rate_rho, ...
            L2_errors_p(test_idx), rate_p);
    end
end

%% Visualization
figure('Position', [100, 100, 1200, 800]);

% Convergence plot for density
subplot(2, 2, 1);
loglog(mesh_spacing, L2_errors_rho, 'bo-', 'LineWidth', 2, 'MarkerSize', 8);
hold on;
loglog(mesh_spacing, mesh_spacing.^(p+1), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('Mesh spacing h', 'FontSize', 12);
ylabel('L2 Error', 'FontSize', 12);
title('Density Error', 'FontSize', 13);
legend('Computed', sprintf('O(h^{%d})', p+1), 'Location', 'best');
set(gca, 'FontSize', 11);

% Convergence plot for pressure
subplot(2, 2, 2);
loglog(mesh_spacing, L2_errors_p, 'bo-', 'LineWidth', 2, 'MarkerSize', 8);
hold on;
loglog(mesh_spacing, mesh_spacing.^(p+1), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('Mesh spacing h', 'FontSize', 12);
ylabel('L2 Error', 'FontSize', 12);
title('Pressure Error', 'FontSize', 13);
legend('Computed', sprintf('O(h^{%d})', p+1), 'Location', 'best');
set(gca, 'FontSize', 11);

% Convergence plot for u-velocity
subplot(2, 2, 3);
loglog(mesh_spacing, L2_errors_u, 'go-', 'LineWidth', 2, 'MarkerSize', 8);
hold on;
loglog(mesh_spacing, mesh_spacing.^(p+1), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('Mesh spacing h', 'FontSize', 12);
ylabel('L2 Error', 'FontSize', 12);
title('u-velocity Error', 'FontSize', 13);
legend('Computed', sprintf('O(h^{%d})', p+1), 'Location', 'best');
set(gca, 'FontSize', 11);

% Convergence plot for v-velocity
subplot(2, 2, 4);
loglog(mesh_spacing, L2_errors_v, 'mo-', 'LineWidth', 2, 'MarkerSize', 8);
hold on;
loglog(mesh_spacing, mesh_spacing.^(p+1), 'r--', 'LineWidth', 1.5);
grid on;
xlabel('Mesh spacing h', 'FontSize', 12);
ylabel('L2 Error', 'FontSize', 12);
title('v-velocity Error', 'FontSize', 13);
legend('Computed', sprintf('O(h^{%d})', p+1), 'Location', 'best');
set(gca, 'FontSize', 11);

sgtitle(sprintf('Q2 DG Convergence Study (Expected Order: %d)', p+1), ...
    'FontSize', 14, 'FontWeight', 'bold');

fprintf('\nConvergence study complete!\n');
fprintf('Expected convergence rate for Q2: O(h^3)\n');

%% Helper Functions

function [rho, u, v, p] = isentropic_vortex(x, y, t, beta, x0, y0, u_inf, v_inf, rho_inf, p_inf, gamma)
    % Isentropic vortex exact solution
    % Vortex center moves with background velocity
    xc = x0 + u_inf * t;
    yc = y0 + v_inf * t;

    % Periodic boundary conditions
    dx = x - xc;
    dy = y - yc;

    % Apply periodicity (domain is [0, 10] x [0, 10])
    if dx > 5, dx = dx - 10; end
    if dx < -5, dx = dx + 10; end
    if dy > 5, dy = dy - 10; end
    if dy < -5, dy = dy + 10; end

    r = sqrt(dx^2 + dy^2);

    % Temperature perturbation
    dT = -(gamma - 1) * beta^2 * exp(1 - r^2) / (8 * gamma * pi^2);

    % Velocity perturbation
    du = -beta * dy * exp((1 - r^2) / 2) / (2 * pi);
    dv = beta * dx * exp((1 - r^2) / 2) / (2 * pi);

    % Total velocity
    u = u_inf + du;
    v = v_inf + dv;

    % Isentropic relations
    T_inf = p_inf / rho_inf;  % Assuming ideal gas
    T = T_inf + dT;
    rho = rho_inf * (T / T_inf)^(1 / (gamma - 1));
    p = p_inf * (T / T_inf)^(gamma / (gamma - 1));
end

function dt = compute_timestep(U, dx, dy, gamma, CFL, Nx, Ny, Np)
    % Compute stable time step
    max_speed = 0;
    for i = 1:Nx
        for j = 1:Ny
            rho = U(i, j, 1, 1);

            % Safety check for positive density
            if rho <= 0
                error('Negative or zero density detected at cell (%d, %d)', i, j);
            end

            u = U(i, j, 1, 2) / rho;
            v = U(i, j, 1, 3) / rho;
            E = U(i, j, 1, 4);
            p = (gamma - 1) * (E - 0.5 * rho * (u^2 + v^2));

            % Safety check for positive pressure
            if p <= 0
                error('Negative or zero pressure detected at cell (%d, %d)', i, j);
            end

            c = sqrt(gamma * p / rho);

            speed = sqrt(u^2 + v^2) + c;
            max_speed = max(max_speed, speed);
        end
    end

    h = min(dx, dy);
    dt = CFL * h / (max_speed + 1e-14);  % Add small epsilon for safety
end

function dU = rhs_dg(U, dx, dy, gamma, Phi, dPhi_xi, dPhi_eta, w_quad, M_inv, Nx, Ny, Np, x_centers, y_centers)
    % Compute RHS of DG scheme with periodic boundaries
    dU = zeros(size(U));
    N_quad = length(w_quad);

    for i = 1:Nx
        for j = 1:Ny
            % Volume integral
            rhs_vol = zeros(Np, 4);

            for q = 1:N_quad
                % Evaluate solution at quadrature point
                U_q = zeros(1, 4);
                for k = 1:Np
                    U_q = U_q + Phi(q, k) * squeeze(U(i, j, k, :))';
                end

                % Compute flux
                [F, G] = euler_flux(U_q, gamma);

                % Accumulate volume integral
                for k = 1:Np
                    rhs_vol(k, :) = rhs_vol(k, :) - w_quad(q) * ...
                        (dPhi_xi(q, k) * F + dPhi_eta(q, k) * G)' * (2/dx) * (2/dy);
                end
            end

            % Surface integrals with periodic boundaries
            rhs_surf = compute_surface_flux_periodic(U, i, j, dx, dy, gamma, Nx, Ny, Np);

            % Apply inverse mass matrix
            for m = 1:4
                dU(i, j, :, m) = M_inv * (rhs_vol(:, m) + rhs_surf(:, m)) * (dx*dy/4);
            end
        end
    end
end

function [F, G] = euler_flux(U, gamma)
    % Compute Euler flux vectors
    rho = U(1);
    u = U(2) / rho;
    v = U(3) / rho;
    E = U(4);
    p = (gamma - 1) * (E - 0.5 * rho * (u^2 + v^2));

    F = [rho*u; rho*u^2 + p; rho*u*v; u*(E + p)];
    G = [rho*v; rho*u*v; rho*v^2 + p; v*(E + p)];
end

function flux = compute_surface_flux_periodic(U, i, j, dx, dy, gamma, Nx, Ny, Np)
    % Surface flux with periodic boundary conditions
    flux = zeros(Np, 4);

    % Get cell averages
    U_center = squeeze(U(i, j, 1, :));

    % Periodic boundaries
    i_left = i - 1; if i_left < 1, i_left = Nx; end
    i_right = i + 1; if i_right > Nx, i_right = 1; end
    j_bottom = j - 1; if j_bottom < 1, j_bottom = Ny; end
    j_top = j + 1; if j_top > Ny, j_top = 1; end

    U_left = squeeze(U(i_left, j, 1, :));
    U_right = squeeze(U(i_right, j, 1, :));
    U_bottom = squeeze(U(i, j_bottom, 1, :));
    U_top = squeeze(U(i, j_top, 1, :));

    % Compute local wave speeds for Lax-Friedrichs flux
    % Left/Right faces (x-direction)
    rho_c = U_center(1);
    u_c = U_center(2) / rho_c;
    v_c = U_center(3) / rho_c;
    E_c = U_center(4);
    p_c = (gamma - 1) * (E_c - 0.5 * rho_c * (u_c^2 + v_c^2));
    c_c = sqrt(gamma * p_c / rho_c);

    rho_l = U_left(1);
    u_l = U_left(2) / rho_l;
    v_l = U_left(3) / rho_l;
    E_l = U_left(4);
    p_l = (gamma - 1) * (E_l - 0.5 * rho_l * (u_l^2 + v_l^2));
    c_l = sqrt(gamma * p_l / rho_l);

    rho_r = U_right(1);
    u_r = U_right(2) / rho_r;
    v_r = U_right(3) / rho_r;
    E_r = U_right(4);
    p_r = (gamma - 1) * (E_r - 0.5 * rho_r * (u_r^2 + v_r^2));
    c_r = sqrt(gamma * p_r / rho_r);

    % Maximum wave speed in x-direction
    alpha_x = max([abs(u_c) + c_c, abs(u_l) + c_l, abs(u_r) + c_r]);

    % Bottom/Top faces (y-direction)
    rho_b = U_bottom(1);
    u_b = U_bottom(2) / rho_b;
    v_b = U_bottom(3) / rho_b;
    E_b = U_bottom(4);
    p_b = (gamma - 1) * (E_b - 0.5 * rho_b * (u_b^2 + v_b^2));
    c_b = sqrt(gamma * p_b / rho_b);

    rho_t = U_top(1);
    u_t = U_top(2) / rho_t;
    v_t = U_top(3) / rho_t;
    E_t = U_top(4);
    p_t = (gamma - 1) * (E_t - 0.5 * rho_t * (u_t^2 + v_t^2));
    c_t = sqrt(gamma * p_t / rho_t);

    % Maximum wave speed in y-direction
    alpha_y = max([abs(v_c) + c_c, abs(v_b) + c_b, abs(v_t) + c_t]);

    [F_c, G_c] = euler_flux(U_center', gamma);
    [F_l, ~] = euler_flux(U_left', gamma);
    [F_r, ~] = euler_flux(U_right', gamma);
    [~, G_b] = euler_flux(U_bottom', gamma);
    [~, G_t] = euler_flux(U_top', gamma);

    % Lax-Friedrichs numerical flux with proper wave speeds
    flux(1, :) = (2/dx) * ((F_r - F_l)'/2 + alpha_x*(U_center' - (U_left' + U_right')/2)) + ...
                 (2/dy) * ((G_t - G_b)'/2 + alpha_y*(U_center' - (U_bottom' + U_top')/2));
end

function [err_rho, err_u, err_v, err_p] = compute_L2_error(U, x_centers, y_centers, t, ...
    vortex_strength, x0, y0, u_inf, v_inf, rho_inf, p_inf, gamma, Nx, Ny)
    % Compute L2 error against exact solution

    err_rho = 0;
    err_u = 0;
    err_v = 0;
    err_p = 0;

    for i = 1:Nx
        for j = 1:Ny
            x_c = x_centers(i);
            y_c = y_centers(j);

            % Computed solution (cell average)
            rho_h = U(i, j, 1, 1);
            u_h = U(i, j, 1, 2) / rho_h;
            v_h = U(i, j, 1, 3) / rho_h;
            E_h = U(i, j, 1, 4);
            p_h = (gamma - 1) * (E_h - 0.5 * rho_h * (u_h^2 + v_h^2));

            % Exact solution
            [rho_ex, u_ex, v_ex, p_ex] = isentropic_vortex(x_c, y_c, t, ...
                vortex_strength, x0, y0, u_inf, v_inf, rho_inf, p_inf, gamma);

            % Accumulate squared errors
            err_rho = err_rho + (rho_h - rho_ex)^2;
            err_u = err_u + (u_h - u_ex)^2;
            err_v = err_v + (v_h - v_ex)^2;
            err_p = err_p + (p_h - p_ex)^2;
        end
    end

    % Normalize by number of cells
    err_rho = sqrt(err_rho / (Nx * Ny));
    err_u = sqrt(err_u / (Nx * Ny));
    err_v = sqrt(err_v / (Nx * Ny));
    err_p = sqrt(err_p / (Nx * Ny));
end

function U_lim = apply_positivity_limiter(U, dx, dy, gamma, Phi, w_quad, Nx, Ny, Np, N_quad)
    % Zhang-Shu positivity-preserving limiter (simplified version)
    U_lim = U;
    eps_p = 1e-13;  % Small positive number

    for i = 1:Nx
        for j = 1:Ny
            % Cell average
            U_avg = squeeze(U(i, j, 1, :));
            rho_avg = U_avg(1);
            E_avg = U_avg(4);

            % Skip if cell average already violates positivity
            if rho_avg <= eps_p
                continue;
            end

            % Check positivity at quadrature points
            theta = 1.0;
            rho_min = rho_avg;
            p_min = inf;

            for q = 1:N_quad
                % Evaluate at quadrature point
                U_q = zeros(4, 1);
                for k = 1:Np
                    U_q = U_q + Phi(q, k) * squeeze(U(i, j, k, :));
                end

                rho_q = U_q(1);
                rho_min = min(rho_min, rho_q);

                % Check pressure positivity (with safeguard for division)
                if rho_q > 1e-15
                    u_q = U_q(2) / rho_q;
                    v_q = U_q(3) / rho_q;
                    E_q = U_q(4);
                    p_q = (gamma - 1) * (E_q - 0.5 * rho_q * (u_q^2 + v_q^2));
                    p_min = min(p_min, p_q);
                end
            end

            % Compute limiter parameter for density
            if rho_min < eps_p
                theta_rho = (rho_avg - eps_p) / max(rho_avg - rho_min, 1e-14);
                theta = min(theta, max(0, min(1, theta_rho)));
            end

            % Compute limiter parameter for pressure
            if p_min < eps_p
                u_avg = U_avg(2) / rho_avg;
                v_avg = U_avg(3) / rho_avg;
                p_avg = (gamma - 1) * (E_avg - 0.5 * rho_avg * (u_avg^2 + v_avg^2));

                if p_avg > eps_p
                    theta_p = (p_avg - eps_p) / max(p_avg - p_min, 1e-14);
                    theta = min(theta, max(0, min(1, theta_p)));
                else
                    theta = 0.0;  % Drastic limiting needed
                end
            end

            % Apply limiter: u_new = u_avg + theta * (u - u_avg)
            % Since phi_1 = 1 (constant), this means scaling higher modes by theta
            if theta < 0.999  % Only limit if necessary
                for k = 2:Np  % Keep cell average (k=1), scale higher modes
                    U_lim(i, j, k, :) = theta * U(i, j, k, :);
                end
            end
        end
    end
end

function [x, w] = lgwt(N, a, b)
    % Legendre-Gauss quadrature points and weights
    N = N - 1;
    N1 = N + 1;
    N2 = N + 2;

    xu = linspace(-1, 1, N1)';
    y = cos((2*(0:N)' + 1) * pi / (2*N + 2)) + (0.27/N1) * sin(pi*xu*N/N2);

    L = zeros(N1, N2);
    Lp = zeros(N1, N2);
    y0 = 2;

    while max(abs(y - y0)) > eps
        L(:, 1) = 1;
        L(:, 2) = y;

        for k = 2:N1
            L(:, k+1) = ((2*k - 1) * y .* L(:, k) - (k - 1) * L(:, k-1)) / k;
        end

        Lp = N2 * (L(:, N1) - y .* L(:, N2)) ./ (1 - y.^2);
        y0 = y;
        y = y0 - L(:, N2) ./ Lp;
    end

    x = (a * (1 - y) + b * (1 + y)) / 2;
    w = (b - a) ./ ((1 - y.^2) .* Lp.^2) * (N2/N1)^2;
end
