function [D_rv, D_flux, DvDt, solutionPenalties, residualPenalties] = constructDiffOps(scheme, g, schemeOrder, residualOrder, schemeParams, opSet, BCs)
    %% DiffOps for solution vector
    [D, solutionPenalties] = constructFluxDiffOp(scheme, g, schemeOrder, schemeParams, opSet, BCs);
    D2 = constructSymmetricD2Operator(g, schemeOrder, opSet);
    D_rv = @(v,Viscosity)(D(v) + D2(Viscosity)*v);

    %% DiffOps for residual viscosity
    [D_flux, residualPenalties] = constructFluxDiffOp(scheme, g, residualOrder, schemeParams, opSet, BCs);
    % TODO: Construct D_flux without closures when using bdfs.
    % diffOp = scheme(g, residualOrder, schemeParams{:}, opSet);
    % if ~isa(diffOp.D, 'function_handle')
    %     D_flux = @(v) diffOp.D*v;
    % else
    %     D_flux = diffOp.D;
    % end

    % DiffOp for flux in residual viscosity. Due to sign conventions of the implemented schemes, we need to
    % change the sign.
    D_flux = @(v) -D_flux(v);
    lim = {g.x{1}(1), g.x{1}(end)};
    o = 2;
    h = g.scaling();
    gam = 1;
    if isequal(opSet,@sbp.D1Upwind)
        ops = sbp.D1Upwind(g.m(), lim, o);
        Hi = ops.HI;
        e_r = ops.e_r;
        e_l = ops.e_l;
        B = e_r*e_r' - e_l*e_l';
        M =  ops.Dm - Hi*B;
        D2 = M*ops.Dp;
    else
        ops = sbp.D2Standard(g.m(), lim, o);
        D2 = ops.D2;
    end
    D2_low = gam*h^(residualOrder)*D2;
    D2_low = @(v) D2_low*v;
    % DiffOp for time derivative in residual viscosity
    DvDt = @(v) D(v)+D2_low(v);
    %DvDt = @(v) D(v);
end

function [D, penalties] = constructFluxDiffOp(scheme, g, order, schemeParams, opSet, BCs)
    diffOp = scheme(g, order, schemeParams{:}, opSet);
    [D, penalties]  = addClosuresToDiffOp(diffOp, BCs);
end

function [D, penalties] = addClosuresToDiffOp(diffOp, BCs)
    if ~isa(diffOp.D, 'function_handle')
        D = @(v) diffOp.D*v;
    else
        D = diffOp.D;
    end
    penalties = cell(size(BCs));
    for i = 1:size(BCs,1)
        for j = 1:size(BCs,2)
            [closure, penalties{i,j}] = diffOp.boundary_condition(BCs{i,j}.boundary, BCs{i,j}.type);
            if ~isa(closure, 'function_handle')
                closure = @(v) closure*v;
            end
            D = @(v) D(v) + closure(v);
        end
    end
end

function D2 = constructSymmetricD2Operator(g, order, opSet)
    m = g.size();
    ops = cell(g.D(),1);
    I = cell(g.D(),1);
    for i = 1:g.D()
       lim = {g.x{i}(1), g.x{i}(end)};
       ops{i} = opSet(m(i), lim, order);
       I{i} = speye(m(i));
    end

    switch g.D()
        case 1
            e_r = ops{1}.e_r;
            e_l = ops{1}.e_l;
            Hi = ops{1}.HI;
            B = e_r*e_r' - e_l*e_l';
            if isequal(opSet,@sbp.D1Upwind)
                Dm = ops{1}.Dm;
                Dp = ops{1}.Dp;
                M =  Dm - Hi*B;
                D2 = @(Viscosity) M*Viscosity*Dp;
            else
                % TODO: Fix closure for D2Variable
                % TODO: Fix Viscosity not being vector
                D2 = @(Viscosity)ops{1}.D2(diag(Viscosity));
            end
        case 2
            % TODO: 
            % Currently only implemented for upwind operators.
            % Remove this part once the time-dependent D2 operator is implemented for other opSets
            % or if it is decided that it should only be supported for upwind operators.
            assert(isequal(opSet,@sbp.D1Upwind))
            e_e = kron(ops{1}.e_r,I{2});
            e_w = kron(ops{1}.e_l,I{2});
            Dm_x = kron(ops{1}.Dm,I{2});
            Dp_x  = kron(ops{1}.Dp,I{2});
            H_x  = kron(ops{1}.HI,I{2});
            B_x = e_e*e_e' - e_w*e_w';
            M_x = Dm_x-H_x*B_x;
            D2_x = @(Viscosity) M_x*Viscosity*Dp_x;

            e_n = kron(I{1},ops{2}.e_r);
            e_s = kron(I{1},ops{2}.e_l);
            Dm_y = kron(I{1},ops{2}.Dm);
            Dp_y  = kron(I{1},ops{2}.Dp);
            H_y = kron(I{1},ops{2}.HI);
            B_y = e_n*e_n' - e_s*e_s';
            M_y = Dm_y-H_y*B_y;
            D2_y = @(Viscosity) M_y*Viscosity*Dp_y;
            D2 = @(Viscosity) D2_x(Viscosity) + D2_y(Viscosity);
        otherwise
            error('3D not yet implemented')
    end
end