--- a/+multiblock/DiffOp.m	Thu Nov 22 10:44:11 2018 +0100
+++ b/+multiblock/DiffOp.m	Sat Nov 24 15:52:47 2018 +0100
@@ -201,33 +201,8 @@
             [blockClosure, blockPenalty] = obj.diffOps{I}.boundary_condition(name, type);
 
             % Expand to matrix for full domain.
-            div = obj.blockmatrixDiv;
-            if ~iscell(blockClosure)
-                temp = blockmatrix.zero(div);
-                temp{I,I} = blockClosure;
-                closure = blockmatrix.toMatrix(temp);
-            else
-                for i = 1:length(blockClosure)
-                    temp = blockmatrix.zero(div);
-                    temp{I,I} = blockClosure{i};
-                    closure{i} = blockmatrix.toMatrix(temp);
-                end
-            end
-
-            if ~iscell(blockPenalty)
-                div{2} = size(blockPenalty, 2); % Penalty is a column vector
-                p = blockmatrix.zero(div);
-                p{I} = blockPenalty;
-                penalty = blockmatrix.toMatrix(p);
-            else
-                % TODO: used by beam equation, should be eliminated. SHould only set one BC per call
-                for i = 1:length(blockPenalty)
-                    div{2} = size(blockPenalty{i}, 2); % Penalty is a column vector
-                    p = blockmatrix.zero(div);
-                    p{I} = blockPenalty{i};
-                    penalty{i} = blockmatrix.toMatrix(p);
-                end
-            end
+            closure = multiblock.local2globalClosure(blockClosure, obj.blockmatrixDiv, I);
+            penalty = multiblock.local2globalPenalty(blockPenalty, obj.blockmatrixDiv, I);
         end
 
         function [closure, penalty] = interface(obj,boundary,neighbour_scheme,neighbour_boundary)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/+multiblock/local2globalClosure.m	Sat Nov 24 15:52:47 2018 +0100
@@ -0,0 +1,10 @@
+% Takes the block-local closures and turns it into a global closure
+%   local -- The local closure
+%   div   -- block matrix division for the diffOp
+%   I     -- Index of blockmatrix block
+function closure = local2globalClosure(local, div, I)
+    closure_bm = blockmatrix.zero(div);
+    closure_bm{I,I} = local;
+
+    closure = blockmatrix.toMatrix(closure_bm);
+end

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/+multiblock/local2globalPenalty.m	Sat Nov 24 15:52:47 2018 +0100
@@ -0,0 +1,11 @@
+% Takes the block-local penalty and turns it into a global penalty
+%   local -- The local penalty
+%   div   -- block matrix division for the diffOp
+%   I     -- Index of blockmatrix block
+function penalty = local2globalPenalty(local, div, I)
+    penaltyDiv = {div{1}, size(local,2)};
+    penalty_bm = blockmatrix.zero(penaltyDiv);
+    penalty_bm{I,1} = local;
+
+    penalty = blockmatrix.toMatrix(penalty_bm);
+end

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/+scheme/+bc/closureSetup.m	Sat Nov 24 15:52:47 2018 +0100
@@ -0,0 +1,25 @@
+% Setup closure and penalty matrices for several boundary conditions at once.
+% Each bc is a struct with the fields
+%  * type     -- Type of boundary condition
+%  * boundary -- Boundary identifier
+%  * data     -- A function_handle for a function which provides boundary data.(see below)
+% Also takes S_sign which modifies the sign of the penalty function, [-1,1]
+% Returns a closure matrix and a penalty matrices for each boundary condition.
+%
+% The boundary data function can either be a function of time or a function of time and space coordinates.
+% In the case where it only depends on time it should return the data as grid function for the boundary.
+% In the case where it also takes space coordinates the number of space coordinates should match the number of dimensions of the problem domain.
+% For example in the 2D case: f(t,x,y).
+function [closure, penalties] = closureSetup(diffOp, bcs)
+    scheme.bc.verifyFormat(bcs, diffOp);
+
+    % Setup storage arrays
+    closure = spzeros(size(diffOp));
+    penalties = cell(1, length(bcs));
+
+    % Collect closures and penalties
+    for i = 1:length(bcs)
+        [localClosure, penalties{i}] = diffOp.boundary_condition(bcs{i}.boundary, bcs{i}.type);
+        closure = closure + localClosure;
+    end
+end

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/+scheme/+bc/forcingSetup.m	Sat Nov 24 15:52:47 2018 +0100
@@ -0,0 +1,86 @@
+% Setup the forcing function for the given boundary conditions and data.
+% Each bc is a struct with the fields
+%  * type     -- Type of boundary condition
+%  * boundary -- Boundary identifier
+%  * data     -- A function_handle for a function which provides boundary data.(see below)
+% S_sign allows changing the sign of the function to put on different sides in the system of ODEs.
+%   default is 1, which the same side as the diffOp.
+% Returns a forcing function S.
+%
+% The boundary data function can either be a function of time or a function of time and space coordinates.
+% In the case where it only depends on time it should return the data as grid function for the boundary.
+% In the case where it also takes space coordinates the number of space coordinates should match the number of dimensions of the problem domain.
+% For example in the 2D case: f(t,x,y).
+
+function S = forcingSetup(diffOp, penalties, bcs, S_sign)
+    default_arg('S_sign', 1);
+
+    assertType(bcs, 'cell');
+    assertIsMember(S_sign, [1, -1]);
+
+    scheme.bc.verifyFormat(bcs, diffOp);
+
+    [gridData, symbolicData] = parseAndSortData(bcs, penalties, diffOp);
+
+    % Setup penalty function
+    O = spzeros(size(diffOp),1);
+    function v = S_fun(t)
+        v = O;
+        for i = 1:length(gridData)
+            v = v + gridData{i}.penalty*gridData{i}.func(t);
+        end
+
+        for i = 1:length(symbolicData)
+            v = v + symbolicData{i}.penalty*symbolicData{i}.func(t, symbolicData{i}.coords{:});
+        end
+
+        v = S_sign * v;
+    end
+    S = @S_fun;
+end
+
+% Go through a cell array of boundary condition specifications and return cell arrays
+% of structs for grid and symbolic data.
+function [gridData, symbolicData] = parseAndSortData(bcs, penalties, diffOp)
+    gridData = {};
+    symbolicData = {};
+    for i = 1:length(bcs)
+        [ok, isSymbolic, data] = parseData(bcs{i}, penalties{i}, diffOp.grid);
+
+        if ~ok
+            continue % There was no data
+        end
+
+        if isSymbolic
+            symbolicData{end+1} = data;
+        else
+            gridData{end+1} = data;
+        end
+    end
+end
+
+function [ok, isSymbolic, dataStruct] = parseData(bc, penalty, grid)
+    if ~isfield(bc,'data') || isempty(bc.data)
+        isSymbolic = [];
+        dataStruct = struct();
+        ok = false;
+        return
+    end
+    ok = true;
+
+    nArg = nargin(bc.data);
+
+    if nArg > 1
+        % Symbolic data
+        isSymbolic = true;
+        coord = grid.getBoundary(bc.boundary);
+        dataStruct.penalty = penalty;
+        dataStruct.func = bc.data;
+        dataStruct.coords = num2cell(coord, 1);
+    else
+        % Grid data
+        isSymbolic = false;
+        dataStruct.penalty = penalty;
+        dataStruct.func = bc.data;
+    end
+end

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/+scheme/+bc/verifyFormat.m	Sat Nov 24 15:52:47 2018 +0100
@@ -0,0 +1,31 @@
+% Errors with a more or less detailed error message if there is a problem with the bc specification
+function verifyBcFormat(bcs, diffOp)
+    assertType(bcs, 'cell');
+    for i = 1:length(bcs)
+        assertType(bcs{i}, 'struct');
+        assertStructFields(bcs{i}, {'type', 'boundary'});
+
+        if ~isfield(bcs{i}, 'data') || isempty(bcs{i}.data)
+            continue
+        end
+
+        if ~isa(bcs{i}.data, 'function_handle')
+            error('bcs{%d}.data should be a function of time or a function of time and space',i);
+        end
+
+        % Find dimension of boundary
+        b = diffOp.grid.getBoundary(bcs{i}.boundary);
+        dim = size(b,2);
+
+        % Assert that the data function has a valid number of input arguments
+        if ~(nargin(bcs{i}.data) == 1 || nargin(bcs{i}.data) == 1 + dim)
+            error('sbplib:scheme:bcSetup:DataWrongNumberOfArguments', 'bcs{%d}.data has the wrong number of input arguments. Must be either only time or time and space.', i);
+        end
+
+        if nargin(bcs{i}.data) == 1
+            % Grid data (only function of time)
+            % Assert that the data has the correct dimension
+            assertSize(bcs{i}.data(0), 1, size(b,1));
+        end
+    end
+end

--- a/+scheme/bcSetup.m	Thu Nov 22 10:44:11 2018 +0100
+++ b/+scheme/bcSetup.m	Sat Nov 24 15:52:47 2018 +0100
@@ -1,10 +1,9 @@
-% function [closure, S] = bcSetup(diffOp, bc)
 % Takes a diffOp and a cell array of boundary condition definitions.
 % Each bc is a struct with the fields
 %  * type     -- Type of boundary condition
 %  * boundary -- Boundary identifier
 %  * data     -- A function_handle for a function which provides boundary data.(see below)
-% Also takes S_sign which modifies the sign of S, [-1,1]
+% Also takes S_sign which modifies the sign of the penalty function, [-1,1]
 % Returns a closure matrix and a forcing function S.
 %
 % The boundary data function can either be a function of time or a function of time and space coordinates.
@@ -16,97 +15,6 @@
     assertType(bcs, 'cell');
     assert(S_sign == 1 || S_sign == -1, 'S_sign must be either 1 or -1');
 
-    verifyBcFormat(bcs, diffOp);
-
-    % Setup storage arrays
-    closure = spzeros(size(diffOp));
-    gridData = {};
-    symbolicData = {};
-
-    % Collect closures, penalties and data
-    for i = 1:length(bcs)
-        [localClosure, penalty] = diffOp.boundary_condition(bcs{i}.boundary, bcs{i}.type);
-        closure = closure + localClosure;
-
-        [ok, isSymbolic, data] = parseData(bcs{i}, penalty, diffOp.grid);
-
-        if ~ok
-            % There was no data
-            continue
-        end
-
-        if isSymbolic
-            symbolicData{end+1} = data;
-        else
-            gridData{end+1} = data;
-        end
-    end
-
-    % Setup penalty function
-    O = spzeros(size(diffOp),1);
-    function v = S_fun(t)
-        v = O;
-        for i = 1:length(gridData)
-            v = v + gridData{i}.penalty*gridData{i}.func(t);
-        end
-
-        for i = 1:length(symbolicData)
-            v = v + symbolicData{i}.penalty*symbolicData{i}.func(t, symbolicData{i}.coords{:});
-        end
-
-        v = S_sign * v;
-    end
-    S = @S_fun;
+    [closure, penalties] = scheme.bc.closureSetup(diffOp, bcs);
+    S = scheme.bc.forcingSetup(diffOp, penalties, bcs, S_sign);
 end
-
-function verifyBcFormat(bcs, diffOp)
-    for i = 1:length(bcs)
-        assertType(bcs{i}, 'struct');
-        assertStructFields(bcs{i}, {'type', 'boundary'});
-
-        if ~isfield(bcs{i}, 'data') || isempty(bcs{i}.data)
-            continue
-        end
-
-        if ~isa(bcs{i}.data, 'function_handle')
-            error('bcs{%d}.data should be a function of time or a function of time and space',i);
-        end
-
-        b = diffOp.grid.getBoundary(bcs{i}.boundary);
-
-        dim = size(b,2);
-
-        if nargin(bcs{i}.data) == 1
-            % Grid data (only function of time)
-            assertSize(bcs{i}.data(0), 1, size(b,1));
-        elseif nargin(bcs{i}.data) ~= 1+dim
-           error('sbplib:scheme:bcSetup:DataWrongNumberOfArguments', 'bcs{%d}.data has the wrong number of input arguments. Must be either only time or time and space.', i);
-        end
-    end
-end
-
-function [ok, isSymbolic, dataStruct] = parseData(bc, penalty, grid)
-    if ~isfield(bc,'data') || isempty(bc.data)
-        isSymbolic = [];
-        dataStruct = struct();
-        ok = false;
-        return
-    end
-    ok = true;
-
-    nArg = nargin(bc.data);
-
-    if nArg > 1
-        % Symbolic data
-        isSymbolic = true;
-        coord = grid.getBoundary(bc.boundary);
-        dataStruct.penalty = penalty;
-        dataStruct.func = bc.data;
-        dataStruct.coords = num2cell(coord, 1);
-    else
-        % Grid data
-        isSymbolic = false;
-        dataStruct.penalty = penalty;
-        dataStruct.func = bc.data;
-    end
-end