function [t,y,breakIndices,xx] = runSSystem( Lss,simData)
%RUNSSYSTEM integrates a linear switched system with time-dependent switching
% Given a sequence of subsystems and time points at which the switching
% happens, this function integrates a linear switched system with a given
% input u based on MATLAB's internal ode45 and ode23 routines
%
% Input arguments:
%   Lss:        Struct containing the switched system
%       Lss.dim         (int) state dimension (n) of the system matrices 
%                       (must be the same for all subsystems)
%       Lss.dimInput    (int) dimension (m) of the input function 
%                       (must be the same for all subsystems)
%       Lss.dimOutput   (int) dimension (p) of the output function 
%                       (must be the same for all subsystems)
%       Lss.nSystems    (int) number of subsystems
%       Lss.As          cell array of length FOM.nSystems containing the A
%                       matrices, i.e. FOM.As = {A_1,...,A_{nSystems}}
%       Lss.Bs          cell array of length FOM.nSystems containing the B
%                       matrices, i.e. FOM.Bs = {B_1,...,B_{nSystems}}
%       Lss.Cs          cell array of length FOM.nSystems containing the C
%                       matrices, i.e. FOM.Cs = {C_1,...,C_{nSystems}}
%   simData:    Struct containing all information for the time simulation
%       simData.switchSequence:     1d array containing the switching order
%                                   Ex: [1 2 1] start with mode 1, switch
%                                   to mode 2 and then back to mode 1
%       simData.Tint:               1d array containing the start and end
%                                   points for each subsystem
%                                   Ex: [0 0.5 0.7 1] simulate the system
%                                   from 0 to 1 with switches at 0.5 and
%                                   0.7, i.e. use mode 1 in [0 0.5], mode 2
%                                   in [0.5 0.7], and mode 1 in [0.7 1].
%       simData.u:                  the control function for the LSS
%       simData.initialCondition:   the initial condition
%
% Output arguments:
%   t:              the time points used in the integration
%   y:              the approximation of the ouput at the time points
%   breakIndices:   the indices at which the switching happend
%   xx:             the approximation of the state variable at the time
%                   points
%
% Authors:
%   Philipp Schulze, pschulze@math.tu-berlin.de
%   Benjamin Unger, unger@math.tu-berlin.de
%
% Version:
%   1.0 (January 08, 2018)

    %% create shortcuts for the required data
    As          = Lss.As ;
    Bs          = Lss.Bs ;
    Cs          = Lss.Cs ;
    Ks          = Lss.Ks ;
    E           = [] ;
    
    sequence    = simData.switchSequence ;
    Tint        = simData.Tint ;
    u           = simData.u ;
    x0          = simData.initialCondition ;
    
    % it is also possible to simulation the switched system with a leading
    % nonsingular E matrix, which must be the same for all subsystems
    if(isfield(Lss,'Es'))
        Es = Lss.Es ;
        if issparse(Es{1}) 
            for i=1:length(Es)-1
                if nnz(Es{1}-Es{i+1})>0 
                    error('The E matrix in the linear switched system must be constant for all subsystems.');
                end
            end
        else
            Ediff = zeros(length(Es)-1,1);
            for i=1:length(Es)-1
                Ediff(i) = norm(Es{1}-Es{i+1});
            end
            if sum(Ediff)>1e-16
                error('The E matrix in the linear switched system must be constant for all subsystems.');
            end
        end
        E = Es{1} ;
    end
    
    %% perform time integration
    y               = Cs{sequence(1)}*x0 ;
    t               = [] ;
    breakIndices    = zeros(size(sequence)) ;
    xx              = [] ;
    for i=1:length(sequence)
        odefun      = @(t,x) As{sequence(i)}*x + Bs{sequence(i)}*u(t) ;
        if(~isempty(E))
            options = odeset('Mass',E) ;
            [tt,x]  = ode23(odefun,[Tint(i) Tint(i+1)],x0,options) ;
        else
            [tt,x]  = ode45(odefun,[Tint(i) Tint(i+1)],x0) ;
        end
        xx          = [xx; x] ;
        y           = [y Cs{sequence(i)}*x'] ;
        t           = [t;tt] ;
        if(i<length(sequence))
            x0      = transpose(Ks{sequence(i)}{sequence(i+1)}*x(end,:)') ;
        end
        breakIndices(i) = length(t);
    end
end

