% CHAIN Input-Output connects an ordered set of system by given input and output names. Other input and output names
% will occur in order of appearance and system position.
%
% SPDX-FileCopyrightText: 2023 Deutsches Elektronen-Synchrotron DESY, MSK, LbSync <lbsync-expert@desy.de>
% SPDX-License-Identifier: GPL-3.0-or-later
%
% Author: Maximilian Schütte <maximilian.schuette@desy.de>

function sys = chain(sys, opts)
    arguments (Repeating)
        sys ss
    end
    arguments
        opts.InputName string = "r"
        opts.OutputName string = "y"
    end

    %#ok<*AGROW>

    % collect input and output names
    in_names = cellfun(@(x) string(x.InputName), sys, UniformOutput=false); in_names = vertcat(in_names{:});
    in_names = unique(extractBefore(in_names + "(", "("), "stable");
    out_names = cellfun(@(x) string(x.OutputName), sys, UniformOutput=false); out_names = vertcat(out_names{:});
    out_names = unique(extractBefore(out_names + "(", "("), "stable");

    % successively connect subsystems
    sys1 = sys{1};
    for sys2 = sys(2:end)
        sys2 = sys2{1}; %#ok<FXSET>
        assert(strcmp(sys2.TimeUnit, sys1.TimeUnit));

        nx1 = order(sys1);
        nx2 = order(sys2);

        idx1in = arrayfun(@(x) startsWith(sys1.InputName, x), in_names, UniformOutput=false); % output system input indicies
        %idx1in = horzcat(idx1in{:});
        idx1out = arrayfun(@(x) startsWith(sys1.OutputName, x), out_names, UniformOutput=false); % output system output indicies
        %idx1out = horzcat(idx1out{:});
        idx2in = arrayfun(@(x) startsWith(sys2.InputName, x), in_names, UniformOutput=false);  % input system input indicies
        %idx2in = horzcat(idx2in{:});
        idx2out = arrayfun(@(x) startsWith(sys2.OutputName, x), out_names, UniformOutput=false); % input system output indicies
        %idx2out = horzcat(idx2out{:});

        Cy1 = sys1.C(idx1out{out_names == opts.OutputName}, :);
        Br2 = sys2.B(:, idx2in{in_names == opts.InputName});
        Dy1 = sys1.D(idx1out{out_names == opts.OutputName}, :);
        Dr2 = sys2.D(:, idx2in{in_names == opts.InputName});

        A = [sys1.A, zeros(nx1, nx2);
             Br2*Cy1, sys2.A];

        B = [];
        in_names_new = [];
        for i = 1:length(in_names)
            in_name = in_names(i);

            B1 = sys1.B(:, idx1in{i});
            B2 = sys2.B(:, idx2in{i});
            Dyi1 = Dy1(:, idx1in{i});

            B = [B, [B1;
                     Br2 * Dyi1]];
            nin = size(B1, 2);
            if in_name ~= opts.InputName
                B = [B, [zeros(size(B1, 1), size(B2, 2));
                         B2]];
                nin = nin + size(B2, 2);
            end
            in_names_new = [in_names_new, compose("%s(%d)", in_name, 1:nin)];
        end

        C = [];
        out_names_new = [];
        for i = 1:length(out_names)
            out_name = out_names(i);

            C1 = sys1.C(idx1out{i}, :);
            C2 = sys2.C(idx2out{i}, :);
            Dir2 = Dr2(idx2out{i}, :);

            nout = 0;
            if out_name ~= opts.OutputName
                C = [C;
                     [C1, zeros(size(C1, 1), size(C2, 2))]];
                nout = nout + size(C1, 1);
            end
            C = [C;
                 [Dir2 * Cy1, C2]];
            nout = nout + size(C2, 1);
            out_names_new = [out_names_new, compose("%s(%d)", out_name, 1:nout)];
        end

        D = [];
        for i = 1:length(out_names)
            out_name = out_names(i);

            Drow = [];
            for j = 1:length(in_names)
                in_name = in_names(j);

                D1 = sys1.D(idx1out{i}, idx1in{j});
                D2 = sys2.D(idx2out{i}, idx2in{j});
                Dir2 = Dr2(idx2out{i}, :);
                Dyj1 = Dy1(:, idx1in{j});

                if in_name == opts.InputName
                    D2 = D2(:, []);
                end
                if out_name == opts.OutputName
                    D1 = D1([], :);
                end

                Drow = [Drow, [D1, zeros(size(D1, 1), size(D2, 2));
                               Dir2*Dyj1, D2]];
            end
            D = [D;
                 Drow];
        end


        sys1 = ss(A, B, C, D, InputName=in_names_new, OutputName=out_names_new, TimeUnit=sys1.TimeUnit);
    end

    sys = sys1;
end

