classdef ess < ss
% sys = ess(A,B,C,D,L,Q,R,M,...)
% sys = ess(ss,L,Q,R,M,...)
%
% Model for discrete system
% x(k+1) = A x(k) + B u(k) + L w(k)
% z(k)  = C x(k) + D u(k) + n(k)
% Noise statistics
% w~N(0,Q)
% n~N(0,R)
% E[w*n']=M(k)
%
% This class extends ss. See 'help ss' for a list of additional
% arguments.
    
% adapt number of meta-properties in getMetaProperties if
% datastructure is changed!
    properties
        l                               % noise input
        q                               % process noise
        r                               % measurement noise
        m                               % noise correlations
    end

    properties (Hidden = true, SetAccess = protected)
        isnoisy                         % system has noisy input
                                        % (boolean)
    end
    

    methods
        
        %%% constructor
        function obj = ess(varargin)
        % parse arguments
            ssvarlen = 4;
            essvarlen = 8;
            addvarlen = essvarlen-ssvarlen;
            newArgs = cell(1,addvarlen); superArgs = {};
                
            % copy from ss object
            if nargin>0 && isa(varargin{1},'StateSpaceModel') || isa(varargin{1},'tf')
                % create empty ss object and call copy constructor
                switch class(varargin{1})
                  case 'ss'
                    S = varargin{1};
                  case 'tf'
                    S = ss(varargin{1});
                  case 'idss'
                    S = ss(varargin{1});
                  case 'ess'
                    % already ess model
                    S = varargin{1};
                end
                
                superArgs{1} = [];
                [copyNames,copyArgs] = getProperties(S);
                    
                % fields not set in ss object
                if nargin > 1
                    newArgs(1:nargin-1) = varargin(2:nargin);
                end

                if isa(varargin{1},'idss')
                    newArgs{1} = varargin{1}.k; % set l = k
                    newArgs{2} = varargin{1}.NoiseVariance; % set q = NoiseVariance
                    newArgs{3} = 0; % set r = 0 such thats isNoisy is true
                end

            % create ess object from matrices
            else
                copyArgs = [];
                % only old fields
                if nargin == ssvarlen
                    superArgs = varargin;
                % including new fields
                elseif essvarlen >= nargin && nargin > ssvarlen
                    superArgs = varargin(1:ssvarlen);
                    newArgs(1:length(varargin(ssvarlen+1:end))) = varargin(ssvarlen+1:end);
                % new fields + additional args
                elseif nargin > essvarlen
                    % take variables for ss
                    superArgs(1:ssvarlen) = varargin(1:ssvarlen);
                    superArgs(ssvarlen+1:nargin-addvarlen) = varargin(essvarlen+1:end);
                    % take variables for ess only (l,q,r,m)
                    newArgs = varargin(ssvarlen+1:essvarlen);
                end
            end
            
            % call superclass constructor
            obj = obj@ss(superArgs{:});
            
            % copy properties
            clen = length(copyArgs);
            if clen > 0
                for ii = 1:clen
                    obj.(copyNames{ii}) = copyArgs{ii};
                end
            end
            
            % set new fields
            if length(newArgs) > 0
                [obj.l,obj.q,obj.r,obj.m] = newArgs{:};
                if obj.isnoisy() && isempty(obj.m)
                    obj = fillM(obj);
                end
            end
            argC = essdata(obj,'CellOutput');
            h = checkdim(argC{:},[],[],[],[]);

            if ~h
                error('Model dimensions are not valid!')
            end
            
        end
        
        function nobj = cleanNoise(obj)
        % empty all noise matrices if noiseTest fails
            nobj = obj;
            if noiseTest(obj)
                nobj.l = []; nobj.q = []; nobj.r = []; nobj.m = [];
            end
        end
                 
        %% meta-properties 
        % extract object meta-properties = all except matrices
        % adapt number of meta-properties if datastructure is changed!
        function props = getMetaProperties(ssObj)
            nMeta = 10;
            props = struct();
            names = properties(ssObj);
            names = names(nMeta:end);
            for ii=1:length(names)
                props.(names{ii})=ssObj.(names{ii});
            end
        end
        % set object meta-properties
        function ssObjout = setMetaProperties(ssObj,props)
            names = fieldnames(props);
            ssObjout = ssObj;
            for ii = 1:length(names)
                ssObjout.(names{ii}) = props.(names{ii});
            end
        end
        
        
        %% set: these toggle isnoisy if required
        function obj = set.q(obj,Value)
            tmpq = obj.q;
            obj.q = Value;
            obj = toggleIsnoisy(obj);
            if ~checkdim(obj)
                obj.q = tmpq;
                error('Dimension mismatch')
            end
        end

        function obj = set.r(obj,Value)
            tmpr = obj.r;
            obj.r = Value;
            obj = toggleIsnoisy(obj);
            if ~checkdim(obj)
                obj.r = tmpr;
                error('Dimension mismatch')
            end
        end

        function obj = set.l(obj,Value)
            tmpl = obj.l;
            obj.l = Value;
            obj = toggleIsnoisy(obj);
            if ~checkdim(obj)
                obj.l = tmpl;
                error('Dimension mismatch')
            end
        end

        % extend ss saveobj
        function S = saveobj(obj)
        % separately save ss system, extra ess matrices, and all meta
        % properties into struct
            S.ss = ss(obj);             % ss system
            S.ess = localEssData(obj);  % extra ess matrices (l,q,r,m)
            S.prop = getMetaProperties(obj); % meta properties
        end
        
    end
    
    methods (Static)
        % extend ss saveobj
        function obj = loadobj(S)
        % restore ess object from structure saved in saveobj
        % S.ess has to be in correct order to be passed to constructor
            obj = ess(loadobj@ss(S.ss),S.ess{:});
            obj = setMetaProperties(obj,S.prop);
        end
    end

    methods (Access = protected)
        function Value = rnotempty(obj)
        % If r==[] return zero matrix with appropriate dimensions.
        % This is needed as there is no transfer matrix before the
        % measurement noise term in our model.
            if isempty(obj.r)
                Value = zeros(size(obj.c,1));
            else
                Value = obj.r;
            end
        end
    end
end

%% private functions

% toggle isnoisy
% called by set function to set isnoisy if condition is fulfiled
function obj = toggleIsnoisy(obj)
    if noiseTest(obj)
        obj.isnoisy = false;
    else
        obj.isnoisy = true;
        % if m = [] set zero it with appropriate dimensions
        if isempty(obj.m)
            obj = fillM(obj);
        end
    end
end

% test used by toggleIsnoisy
function h = noiseTest(obj)
    h = isempty(obj.q) || isempty(obj.l) || isempty(obj.r);
end

% make m a zero matrix with appropriate dimensions
function obj = fillM(obj)
    obj.m = zeros(size(obj.q,1),size(obj.r,2));
end

% extract object properties
function [names,props] = getProperties(ssObj)
    props={};
    names = properties(ssObj);
    for ii=1:length(names)
        props{ii}=ssObj.(names{ii});
    end
end

% extra ess properties
function c = extraEssProperties(obj)
% return name of properties not in ss, e.g. l,q,r,m
% takes ess obj as input
    sp = properties('ss'); ep = properties(obj);
    % note order of input: ep must be before!!
    % 'stable' is needed such that original order 
    % of arguments is preserved for the ess constructor
    c = setdiff(ep,sp,'stable');                 
end

% local essdata
function c = localEssData(obj)
% only return properties not in ss, e.g. l,q,r,m
    strListC = extraEssProperties(obj);
    extF = @(str) obj.(str);
    % must be in correct order to be passed to constructor
    c = cellfun(extF,strListC,'UniformOutput',false);
end