classdef CSDModel < modelpack.Model
    %modelpack.CSDModel class
    %   modelpack.CSDModel extends modelpack.Model.
    %

    %    modelpack.CSDModel properties:
    %       Description - Property is of type 'ustring'
    %
    %    modelpack.CSDModel methods:
    %       addLinearizationIO -  methods to add a linearization port to a SISOTOOL
    %       addOutput -  method to add an Output port to a SISOTOOL model object.
    %       convertValue -  CSDModel object method to convert a parameter value from one
    %       findInput -  method to find particular input(s)
    %       findLinearizationIO -  method to find particular linearization port(s)
    %       findOutput -  method to find particular output(s)
    %       findParameter -  method to find particular parameter(s)
    %       findState -  method to find particular state(s)
    %       getCurrentConfig -  method to return current model configuration
    %       getInputs -  method to return SISOTOOL model object's input port objects
    %       getLinearizationIOs -  method to return SISOTOOL model object's
    %       getName -  method to return the name of a SISOTOOL model object
    %       getOutputs -  method to return SISOTOOL model object's output port objects
    %       getParameters -  GETSTATES  method to return SISOTOOL model object's parameter objects
    %       getSensitivity - Computes sensitivity derivatives of model trajectories with
    %       getSpec -  Method to return a spec object for a modelpack ID object
    %       getStates -  method to return SISOTOOL model object's state objects
    %       getValue -  method to return the value of a CSD model object.
    %       isParameterIntegrator - method to check whether parameter is an integrator
    %       isValidPort - check that ports are valid for this model
    %       linearize -  method to return a linear model (defined by the ports
    %       removeLinearizationIO -  method to remove linearization ports from a
    %       removeOutput -   method to remove output ports from a SISOTOOL model
    %       setCurrentConfig -  method to set SISTOOL model configuration
    %       setValue -  method to set the value of a SISOTOOL model variable object.
    %       simulate -  method to perform time domain simulation of the SISTOOL object
    %       update -  method to update SISOTOOL model object, this synchronizes the
    %       utSimulate_LTI - method to simulate an LTI object that is derived

    % Copyright 2000-2023 The MathWorks, Inc.

    properties (Access=protected, SetObservable)
        %NAME Property is of type 'string'
        Name = '';
        %IOS Property is of type 'handle vector'
        IOs = [];
        %PARAMETERS Property is of type 'handle vector'
        Parameters = [];
        %LINEARIZATIONIOS Property is of type 'handle vector'
        LinearizationIOs = [];
        %STATES Property is of type 'handle vector'
        States = [];
        %MODEL Property is of type 'MATLAB array'
        Model = [];
    end


    methods  % constructor block
        function this = CSDModel(Model)
            % CSDModel  constructor for CSD model object

            % Call parent constructor
            this = this@modelpack.Model;

            % No argument constructor call
            if nargin == 0, return, end

            % Check number of arguments
            if nargin ~= 1
                error(message('SLControllib:modelpack:errNumArguments','1'))
            end

            % Check argument types
            if isa(Model,'ctrlguis.csdesignerapp.data.internal.DesignerData')
                name = Model.UniqueName;
            else
                error(message('SLControllib:modelpack:errArgumentType','Model','sisodata.loopdata'))
            end

            % First, ask the model manager if the model exists
            this.Model = Model;
            h = modelpack.ModelManager.addModel(this, name);

            if (this == h)
                % Model does not exist
                try
                    % Set properties
                    this.Name    = name;
                    this.Version = 1.0;
                    %this@modelpack.Model(Model); % converted super class constructor call

                    % Get model information
                    update(this)
                catch E
                    % Remove uninitialized model.
                    modelpack.ModelManager.removeModel(this);
                    rethrow(E);
                end
            else
                % Model already exists
                this = h;
            end

        end  % CSDModel

    end  % constructor block

    methods
        function set.Name(obj,value)
            % DataType = 'string'
            validateattributes(value,{'char'}, {'row'},'','Name')
            obj.Name = value;
        end

        function set.IOs(obj,value)
            % DataType = 'handle vector'
            %validateattributes(value,{'handle'}, {'vector'},'','IOs')
            obj.IOs = value;
        end

        function set.Parameters(obj,value)
            % DataType = 'handle vector'
            %validateattributes(value,{'handle'}, {'vector'},'','Parameters')
            obj.Parameters = value;
        end

        function set.LinearizationIOs(obj,value)
            % DataType = 'handle vector'
            %validateattributes(value,{'handle'}, {'vector'},'','LinearizationIOs')
            obj.LinearizationIOs = value;
        end

        function set.States(obj,value)
            % DataType = 'handle vector'
            %validateattributes(value,{'handle'}, {'vector'},'','States')
            obj.States = value;
        end

    end   % set and get functions

    methods  % public methods
        function hPort = addLinearizationIO(~,~)
            % ADDLINEARIZATIONIO  methods to add a linearization port to a SISOTOOL
            % model object.
            %
            % Inputs:
            %    Ports - a vector of SISOTOOL linearization ports
            %
            % As SISOTOOL linearization ports cannot be added to a SISOTOOL model this method
            % is a no-op, but is required for consistency with the model API.
            %

            hPort = [];
            warning(message('SLControllib:modelpack:stWarnAddLinearizationIO'))
        end  % addLinearizationIO

        function hPort = addOutput(~,~)
            % ADDOUTPUT  method to add an Output port to a SISOTOOL model object.
            %
            % Input:
            %     Port - a modelpack.STPort object
            %
            % As SISOTOOL output ports cannot be added to a SISOTOOL model this method
            % is a no-op, but is required for consistency with the model API.
            %

            hPort = [];
            warning(message('SLControllib:modelpack:stWarnAddOutput'))
        end  % addOutput

        function convertValue(this,Var,newFormat)
            % CONVERTVALUE  CSDModel object method to convert a parameter value from one
            % format to another
            %
            % Var = this.convertValue(Var,newFormat)
            %
            % Inputs:
            %   Var       - a modelpack.CSDParameterSpec or modelpack.ParameterValue
            %               object to convert
            %   newFormat - a numerical index or string giving the new parameter format,
            %               valid formats can be found from the 'FormatOptions' property, i.e.,
            %               this.FormatOptions
            %

            TunedBlocks = getTunableBlocks(this.Model);
            varID       = Var.getID;
            Path        = varID.getPath;
            idx         = strcmp(getIdentifier(TunedBlocks),Path);
            if ischar(newFormat) && isa(Var,'modelpack.CSDParameterSpec')
                newFormat = find(strcmp(newFormat,Var.FormatOptions));
                if isempty(newFormat), newFormat = inf; end
            end
            if ~isnumeric(newFormat) && isa(Var,'modelpack.ParameterSpec')
                error(message('SLControllib:modelpack:errArgumentType','newFormat','numeric index'))
            end
            if any(idx)
                %Found tuned block with this parameter, now find correct parameter and
                %Update min, max, etc. to match new format
                TB       = TunedBlocks(idx);
                ZPKSpecs = TB.getZPKParameterSpec;
                allPSpec = [ZPKSpecs.GainSpec; ZPKSpecs.PZGroupSpec];
                pID      = allPSpec.getID;
                if iscell(pID), pID = [pID{:}]; end
                found = false; idx = 1;
                while ~found && idx <= numel(pID)
                    found = varID.isSame(pID(idx));
                    if ~found, idx = idx + 1; end
                end
                if found && isa(Var,'modelpack.CSDParameterSpec')
                    if idx==1
                        %Gain parameter
                        Var.setFormat(TB,newFormat);
                    else
                        Var.setFormat(TB.PZGroup(idx-1),newFormat)
                    end
                end
                if found && isa(Var,'modelpack.ParameterValue')
                    if idx==1
                        %Gain parameter
                        Var.Value = TB.getGainValue(newFormat);
                    else
                        Var.Value = TB.PZGroup(idx).getValue(newFormat);
                    end
                end
                if ~found
                    %Did not find matching parameter value
                    error(message('SLControllib:modelpack:stErrorParameterNotFoundConvert',varID.getFullName))
                end
            else
                %Found no compensator with matching name that contains requested parameter
                error(message('SLControllib:modelpack:stErrorParameterNotFoundConvert',varID.getFullName))
            end
        end  % convertValue

        function Ports = findInput(this,Search,Exact)
            % FINDINPUT  method to find particular input(s)
            %
            % ports = this.findInput(Search,Exact)
            %
            % Input:
            %   Search - string used to find matching input (the inputs are searched on
            %            the output fullname) or numerical index of input to return
            %   Exact  - optional logical, if true an exact match is required, default
            %            is true
            %

            %Check number of arguments
            if nargin < 2 || nargin > 3
                error(message('SLControllib:modelpack:errNumArguments','1 or 2'));
            end

            %Check argument types
            if ~ischar(Search) && ~isnumeric(Search)
                error(message('SLControllib:modelpack:errArgumentType','Search','double or string'))
            end
            %Set Exact to default if omitted
            if nargin == 2, Exact = false; end
            if ~islogical(Exact)
                error(message('SLControllib:modelpack:errArgumentType','Exact','logical'))
            end

            idx = strcmp(this.IOs.getType,'Input');
            PortList = this.IOs(idx);
            if ~isempty(PortList)
                if isnumeric(Search)
                    Ports = PortList(Search(:));
                else
                    Ports = findPort(this,PortList,Search,Exact);
                end
            else
                Ports = [];
            end
        end  % findInput

        function Ports = findLinearizationIO(this,Search,Exact)
            % FINDLINEARIZATIONIO  method to find particular linearization port(s)
            %
            % ports = this.findLinearizationIO(Search,Exact)
            %
            % Input:
            %   Search - string used to find matching port (the linearization ports are
            %            searched on the port fullname) or numerical index of port
            %            to return
            %   Exact  - optional boolean, if true an exact match is required, default
            %            is true
            %

            %Check number of arguments
            if nargin < 2 || nargin > 3
                error(message('SLControllib:modelpack:errNumArguments','1 or 2'))
            end

            %Check argument types
            if ~ischar(Search) && ~isnumeric(Search)
                error(message('SLControllib:modelpack:errArgumentType','Search','double or string'))
            end
            %Set Exact to default if omitted
            if nargin == 2, Exact = false; end
            if ~islogical(Exact)
                error(message('SLControllib:modelpack:errArgumentType','Exact','logical'))
            end

            PortList = this.LinearizationIOs;
            if ~isempty(PortList)
                if isnumeric(Search)
                    Ports = PortList(Search(:));
                else
                    Ports = findPort(this,PortList,Search,Exact);
                end
            else
                Ports = [];
            end
        end  % findLinearizationIO

        function Ports = findOutput(this,Search,Exact)
            % FINDOUTPUT  method to find particular output(s)
            %
            % ports = this.findOutput(Search,Exact)
            %
            % Input:
            %   Search - string used to find matching output (the outputs are searched on
            %            the output fullname) or numerical index of output to return
            %   Exact  - optional boolean, if true an exact match is required, default
            %            is true
            %

            %Check number of arguments
            if nargin < 2 || nargin > 3
                error(message('SLControllib:modelpack:errNumArguments','1 or 2'));
            end

            %Check argument types
            if ~ischar(Search) && ~isnumeric(Search)
                error(message('SLControllib:modelpack:errArgumentType','Search','double or string'))
            end
            %Set Exact to default if omitted
            if nargin == 2, Exact = false; end
            if ~islogical(Exact)
                error(message('SLControllib:modelpack:errArgumentType','Exact','logical'))
            end

            idx = strcmp(this.IOs.getType,'Output');
            PortList = this.IOs(idx);
            if ~isempty(PortList)
                if isnumeric(Search)
                    Ports = PortList(Search(:));
                else
                    Ports = findPort(this,PortList,Search,Exact);
                end
            else
                Ports = [];
            end
        end  % findOutput

        function Param = findParameter(this,Search,Exact)
            % FINDPARAMETER  method to find particular parameter(s)
            %
            % param = this.findParameter(Search,Exact)
            %
            % Input:
            %   Search - string used to find matching parameter (the parameters are searched on
            %            the fullname) or numerical index of parameter to return
            %   Exact  - optional boolean, if true an exact match is required, default
            %            is true
            %

            %Check number of arguments
            if nargin < 2 || nargin > 3
                error(message('SLControllib:modelpack:errNumArguments','1 or 2'))
            end

            %Check argument types
            if ~ischar(Search) && ~isnumeric(Search)
                error(message('SLControllib:modelpack:errArgumentType','Search','double or string'));
            end
            %Set Exact to default if omitted
            if nargin == 2, Exact = false; end
            if ~islogical(Exact)
                error(message('SLControllib:modelpack:errArgumentType','Exact','logical'));
            end

            if ~isempty(this.Parameters)
                if isnumeric(Search)
                    Param = this.Parameters(Search(:));
                else
                    if Exact
                        idx = strcmp(this.Parameters.getFullName,Search);
                    else
                        %Look for full parameter names that contain the search string
                        idx = ~cellfun('isempty',strfind(this.Parameters.getFullName,Search));
                    end
                    Param = this.Parameters(idx);
                end
            else
                Param = [];
            end
        end  % findParameter

        function State = findState(this,Search,Exact)
            % FINDSTATE  method to find particular state(s)
            %
            % state = this.findState(Search,Exact)
            %
            % Input:
            %   Search - string used to find matching state (the states are searched on
            %            the states fullname) or numerical index of state to return
            %   Exact  - optional boolean, if true an exact match is required, default
            %            is true
            %

            %Check number of arguments
            if nargin < 2 || nargin > 3
                error(message('SLControllib:modelpack:errNumArguments','1 or 2'))
            end

            %Check argument types
            if ~ischar(Search) && ~isnumeric(Search)
                error(message('SLControllib:modelpack:errArgumentType','Search','double or string'));
            end
            %Set Exact to default if omitted
            if nargin == 2, Exact = false; end
            if ~islogical(Exact)
                error(message('SLControllib:modelpack:errArgumentType','Exact','logical'))
            end

            if ~isempty(this.States)
                if isnumeric(Search)
                    State = this.States(Search(:));
                else
                    if Exact
                        idx = strcmp(this.States.getFullName,Search);
                    else
                        %Look for full state names that contain the search string
                        idx = ~cellfun('isempty',strfind(this.States.getFullName,Search));
                    end
                    State = this.States(idx);
                end
            else
                State = [];
            end
        end  % findState

        function cfg = getCurrentConfig(this)
            % GETCURRENTCONFIG  method to return current model configuration
            %
            % As SISOTOOL does not have a configuration object this method returns a
            % default structure with the fields necessary to perform a SISOTOOL time
            % domain siumulation using the simulate model API method.
            %

            idx              = find(strcmp(this.IOs.getType,'Input'),1);
            cfg              = modelpack.CSDConfig;
            cfg.ActiveInputs = this.IOs(idx);
            cfg.InputType    = 'step';
        end  % getCurrentConfig

        function Inputs = getInputs(this,index)
            % GETINPUTS  method to return SISOTOOL model object's input port objects
            %
            % inputs = this.getInputs(index)
            %
            % Input:
            %   index - numerical index of inputs to return
            %

            Ports = this.IOs;
            if isempty(Ports)
                Inputs = [];
                return
            end
            idx = strcmpi(Ports.getType,'input');
            Inputs = Ports(idx);
            if nargin == 1
                %Quick return for all inputs
                return
            elseif all(isnumeric(index)) && all(isfinite(index))
                %Indexed
                Inputs = Inputs(index);
            else
                error(message('SLControllib:modelpack:errArgumentType','index','finite integer'))
            end

        end  % getInputs

        function Ports = getLinearizationIOs(this,index)
            % GETLINEARIZATIONIOS  method to return SISOTOOL model object's
            % linearization port objects.
            %
            % ports = this.getLinearizationIOs(index)
            %
            % Input:
            %   index - numerical index of linearization ports to return
            %

            Ports = this.LinearizationIOs;
            if isempty(Ports)
                return
            end
            if nargin == 1
                %Quick return for all outputs
                return
            elseif all(isnumeric(index)) && all(isfinite(index))
                %Indexed
                Ports = Ports(index);
            else
                error(message('SLControllib:modelpack:errArgumentType','index','finite integer'));
            end
        end  % getLinearizationIOs

        function Name = getName(this)
            % GETNAME  method to return the name of a SISOTOOL model object
            %

            Name = get(this,'Name');
        end  % getName

        function Outputs = getOutputs(this,index)
            % GETOUTPUTS  method to return SISOTOOL model object's output port objects
            %
            % ports = this.getOutputs(index)
            %
            % Input:
            %   index - numerical index of output ports to return
            %

            Ports = this.IOs;
            if isempty(Ports)
                Outputs = [];
                return
            end
            idx = strcmpi(Ports.getType,'output');
            Outputs = Ports(idx);
            if nargin == 1
                %Quick return for all outputs
                return
            elseif all(isnumeric(index)) && all(isfinite(index))
                %Indexed
                Outputs = Outputs(index);
            else
                error(message('SLControllib:modelpack:errArgumentType','index','finite integer'))
            end
        end  % getOutputs

        function Parameters = getParameters(this,index)
            % GETSTATES  method to return SISOTOOL model object's parameter objects
            %
            % params = this.getParameters(index)
            %
            % Input:
            %   index - numerical index of parameters to return
            %

            Parameters = this.Parameters;
            if nargin == 1
                %Quick return for all outputs
                return
            elseif all(isnumeric(index)) && all(isfinite(index))
                %Indexed
                Parameters = Parameters(index);
            else
                error(message('SLControllib:modelpack:errArgumentType','index','finite integer'))
            end
        end  % getParameters

        %----------------------------------------
        function [derivs, varargout] = getSensitivity(this, T, InputSignals, variables, Opt)
            % GETSENSITIVITY Computes sensitivity derivatives of model trajectories with
            % respect to parameter changes
            %
            % [derivs,info] = this.getSensitivity(time, inputs, variables, options)
            % [lResp, rResp, info] = this.getSensitivity(...)
            %
            % TIMESPAN is one of: TFinal, [TStart TFinal], or [TStart OutputTimes TFinal].
            % INPUTS is a cell array of TIMESERIES objects, one per model input.
            % VARIABLES one of CSDParameterID, ParameterSpec for a CSDParameterID, or a
            %           string with the variable full name
            % OPTIONS is a GRADOPTIONS object.
            %
            % DERIVS is a cell array of TIMESERIES objects, one per parameter per model
            % output.
            %

            dOut = nargout==2;            %Flag to compute and return finite difference

            %Check argument dimensions
            if ~ischar(variables)&&numel(variables)~=1
                error(message('SLControllib:general:InvalidArgument', ...
                    'VARIABLES', 'getSensitivity', ...
                    'modelpack.CSDModel.getSensitivity'))
            end

            %Check variables argument type
            haveSpec = false;
            switch class(variables)
                case {'modelpack.ParameterSpec', 'modelpack.CSDParameterSpec'}
                    haveSpec = true;
                    pSpec = variables;
                    nP    = numel(pSpec);
                    pID   = pSpec.getID;
                    if nP > 1, pID   = [pID{:}]; end   %Convert to vector from cell array
                    if ~isa(pID,'modelpack.CSDParameterID')
                        error(message('SLControllib:modelpack:errArgumentType','variables','modelpack.CSDParameterID'))
                    end
                case 'modelpack.CSDParameterID'
                    pID   = variables;
                    nP    = numel(pID);
                    pSpec = struct('Maximum',cell(nP,1),'Minimum',cell(nP,1));
                case 'char'
                    pID = this.findParameter(variables,true);  %Look for exact match
                    if isempty(pID)
                        error(message('SLControllib:modelpack:errParameterNotFound',variables))
                    end
                    nP    = numel(pID);
                    pSpec = struct('Maximum',cell(nP,1),'Minimum',cell(nP,1));
                otherwise
                    error(message('SLControllib:modelpack:errArgumentType','variables', ...
                        'modelpack.ParameterSpec, modelpack.CSDParameterID, or string'));
            end

            %Check the Opt argument
            if ~isa(Opt,'modelpack.gradoptions')
                error(message('SLControllib:modelpack:errArgumentType','options','modelpack.gradoptions'));
            end
            delta    = Opt.Perturbation;
            if isempty(delta), delta = 1e-3; end
            OutPorts = Opt.Outputs;
            InPorts  = Opt.Configuration.ActiveInputs;

            %Create storage space for outputs
            nOut = numel(OutPorts);               %Number of output ports
            nPScalar = prod(pID.getDimensions);   %Number of scalar parameters
            if dOut
                %Compute and return finite differences
                derivs = cell(nPScalar,nOut);
            else
                %Return perturbed signals
                yR = cell(nPScalar,nOut);
                yL = cell(nPScalar,nOut);
            end

            %Compute all perturbations
            if haveSpec
                nom = this.getValue(pSpec).Value;  %Nominal parameter value
            else
                nom = this.getValue(pID).Value;  %Nominal parameter value
            end
            if ~isempty(pSpec.Maximum)
                dR = min( nom.*(1+sign(nom)*delta), pSpec.Maximum);
            else
                dR = nom.*(1+sign(nom)*delta);
            end
            if ~isempty(pSpec.Minimum)
                dL = max( nom.*(1-sign(nom)*delta), pSpec.Minimum);
            else
                dL = nom.*(1-sign(nom)*delta);
            end
            %Set any perturbations with zero nominal
            idxZero     = nom==0;
            if all(idxZero)
                nNorm = 1;
            else
                nNorm = norm(nom);
            end
            dR(idxZero) = delta*nNorm;
            dL(idxZero) = -delta*nNorm;

            %Loop over each scalar perturbation
            pvR = nom;
            pvL = nom;
            for ct_pScalar=1:nPScalar
                %Set perturbation for a specific parameter
                pvR(ct_pScalar) = dR(ct_pScalar);
                pvL(ct_pScalar) = dL(ct_pScalar);

                %Get left and right perturbed systems

                %Right perturbation
                if haveSpec
                    this.setValue(pSpec,pvR);
                else
                    this.setValue(pID,pvR);
                end
                sysR = this.linearize([],[InPorts;OutPorts],[]);
                %Left perturbation
                if haveSpec
                    this.setValue(pSpec,pvL)
                else
                    this.setValue(pID,pvL)
                end
                sysL = this.linearize([],[InPorts;OutPorts],[]);

                if dOut
                    %Compute finite difference
                    [y,t] = utSimulate_LTI(this,sysR-sysL,T,Opt.Configuration.InputType,InputSignals);
                    yD = localConvert2TS(y,t,OutPorts);
                    yD = cellfun(@(x) x./(pvR(ct_pScalar)-pvL(ct_pScalar)),yD,'UniformOutput',false);
                    [derivs{ct_pScalar,:}] = yD{:};
                else
                    %Return perturbed signals directly
                    [y,t] = utSimulate_LTI(this,vertcat(sysR,sysL),T,Opt.Configuration.InputType,InputSignals);
                    yRL = localConvert2TS(y,t,OutPorts);
                    [yR{ct_pScalar,:}] = yRL{1:nOut};
                    [yL{ct_pScalar,:}] = yRL{nOut+1:2*nOut};
                end

                %Restore parameter perturbation
                pvR(ct_pScalar) = nom(ct_pScalar);
                pvL(ct_pScalar) = nom(ct_pScalar);
            end

            %Construct info argument to return
            rTypes = {'Perturbed signals','Finite differences'};
            info = struct('Perturbations',[dL(:), dR(:)],'ReturnType',rTypes{dOut+1});

            if dOut
                %Return computed Jacobian
                varargout{1} = info;
            else
                %Return perturbed trajectories
                derivs = yL;
                varargout{1} = yR;
                varargout{2} = info;
            end
        end  % getSensitivity


        function Spec = getSpec(this,Variable)
            % GETSPEC  Method to return a spec object for a modelpack ID object
            %
            % Spec = model.getSpec(ID)
            %

            %Handle cases for explicit input types
            processed = false;
            if isa(Variable,'modelpack.CSDStateID')
                %Want to get a state spec
                Spec = localGetSpec(this,Variable,'modelpack.StateSpec');
                processed = true;
            end
            if isa(Variable,'modelpack.CSDParameterID')
                %Want to get a parameter spec
                Spec = localGetSpec(this,Variable,'modelpack.CSDParameterSpec');
                processed = true;
            end
            if isa(Variable,'modelpack.CSDPortID')
                %Want to get a port spec
                Spec = localGetSpec(this,Variable,'PortSpec');
                processed = true;
            end

            %Handle case for more generic inputs
            if ischar(Variable)
                %Want to get a parameter parameter spec using the fullname
                Spec = localGetStringSpec(this,Variable);
                processed = true;
            end
            if ~processed && isa(Variable,'modelpack.VariableID')
                %Want to get a generic variable spec
                Spec = localGetVariableSpec(this,Variable);
                processed = true;
            end

            if ~processed
                error(message('SLControllib:modelpack:errArgumentType','ID', ...
                    'string, modelpack.CSDParameterID, modelpack.CSDPortID, or modelpack.CSDStateID'));
            end
        end  % getSpec


        function States = getStates(this,index)
            % GETSTATES  method to return SISOTOOL model object's state objects
            %
            % states = this.getStates(index)
            %
            % Input:
            %   index - numerical index of states to return
            %

            States = this.States;
            if nargin == 1
                %Quick return for all outputs
                return
            elseif all(isnumeric(index)) && all(isfinite(index))
                %Indexed
                States = States(index);
            else
                error(message('SLControllib:modelpack:errArgumentType','index','finite integer'));
            end
        end  % getStates

        function Value = getValue(this,Variable)
            % GETVALUE  method to return the value of a CSD model object.
            %
            % Value = this.getValue(Variable)
            %
            % Input:
            %   Variable - a vector or single VariableID object, valid objects are
            %              parameterID's, parameterSpecs, stateID's, stateSpecs or
            %              a string with the variable name
            %
            % Output:
            %   Value - a vector of VariableValue objects corresponding to the requested varaibales
            %           with the current value set to the model variable value
            %

            %Parse input types to check which local set methods to call
            processed = false;
            if ischar(Variable)
                %Want to set a parameter using the fullname
                Value = localGetStringValue(this,Variable);
                processed = true;
            end
            if isa(Variable,'modelpack.CSDStateID') || ...
                    isa(Variable,'modelpack.StateValue')
                %Want to get a state value
                Value = localGetStateValue(Variable);
                processed = true;
            end
            if isa(Variable,'modelpack.CSDParameterID') || ...
                    isa(Variable,'modelpack.ParameterValue') || ...
                    isa(Variable,'modelpack.ParameterSpec')
                %Want to get a parameter value
                Value = localGetParameterValue(this,Variable);
                processed = true;
            end

            if ~processed
                error(message('SLControllib:modelpack:errArgumentType','Variable', ...
                    'string, modelpack.CSDParameterID, or modelpack.CSDStateID'));
            end
        end  % getValue


        function isInt = isParameterIntegrator(this,pID)
            % ISPARAMETERINTEGRATOR method to check whether parameter is an integrator
            % or differentiator
            %
            % isInt = this.isParameterIntegrator(pID);
            %
            % Inputs:
            %    pID - nx1 vector of CSDParameterID objects
            %
            % Outputs:
            %    isInt - nx1 vector of {-1,0,1} with 1 indicating an integrator, -1 a
            %            differentiator
            %

            TunedBlocks = getTunableBlocks(this.Model);
            isInt       = zeros(size(pID));
            for ct = 1:numel(pID)
                Path = pID(ct).getPath;
                idx = strcmp(getIdentifier(TunedBlocks),Path);
                if any(idx)
                    %Found tuned block with this parameter, now find correct parameter
                    TB = TunedBlocks(idx);
                    dt = TB.getTs > 0;    %Discrete time model
                    ZPKSpecs = TunedBlocks(idx).getZPKParameterSpec;
                    if ~isempty(ZPKSpecs.PZGroupSpec)
                        %Check PZgroups for parameter
                        allPSpec = ZPKSpecs.PZGroupSpec;
                        allPID   = allPSpec.getID;
                        if iscell(allPID), pID = [allPID{:}]; end
                        idx = 1;
                        found = false;
                        while ~found && idx <= numel(allPID)
                            found = pID(ct).isSame(allPID(idx));
                            if ~found, idx = idx + 1; end
                        end
                        if found
                            PZ = TB.PZGroup(idx);
                            if isequal(PZ.getValue,dt)
                                if isempty(PZ.Pole)
                                    %Differentiator
                                    isInt(ct) = -1;
                                else
                                    %Integrator
                                    isInt(ct) = 1;
                                end
                            end
                        end
                    end
                end
            end
        end  % isParameterIntegrator

        function b = isValidPort(this,PortIDs)
            % ISVALIDPORT check that ports are valid for this model
            %
            % b = this.isValidPort(PortIDs)
            %
            % Inputs:
            %   PortIDs - a vector of modepack.CSDPortID objects
            % Outputs:
            %   b - a vector of logicals indicating whether the passed ports are valid
            %       for this model

            if ~isa(PortIDs,'modelpack.CSDPortID')
                error(message('SLControllib:modelpack:errArgumentType','PortIDs','modelpack.CSDPortID'))
            end

            %As ports cannot be added or removed via the modelPI need to check that the
            %ports are the same as any in the model.
            allPorts = [this.getOutputs; this.getInputs; this.getLinearizationIOs];
            nAll = numel(allPorts);
            b = false(size(PortIDs));
            for ct=1:numel(PortIDs)
                ctAll = 1;
                while ~b(ct) && ctAll <= nAll
                    if PortIDs(ct).isSame(allPorts(ctAll))
                        b(ct) = true;
                    else
                        ctAll = ctAll + 1;
                    end
                end
            end


        end  % isValidPort

        function sys = linearize(this,~,Ports,~)
            % LINEARIZE  method to return a linear model (defined by the ports
            % argument) from a SISOTOOL model object
            %
            % sys = this.linearize(Oppoint,Ports,LinOptions)
            %
            % Input:
            %    Oppoint    - an operating point object, as SISOTOOL only supports linear
            %                 systems this input is ignored and a zero-point operating point
            %                 assumed.
            %    Ports      - an optional vector of linearizationID objects that define the linear
            %                 system. This vector must contain at least one input and one output
            %                 port. If omitted the full SISOTOOL closed loop model is
            %                 returned.
            %    Linoptions - an optional linoptions object, as SISOTOOL only supports
            %                 linear systems this input is ignored.
            %

            %Check number of arguments
            if nargin < 2 || nargin > 4
                error(message('SLControllib:modelpack:errNumArguments','1 to 3'))
            end

            %Check input argument types
            if (nargin > 2)
                if ~isempty(Ports) && ~isa(Ports,'modelpack.CSDPortID')
                    error(message('SLControllib:modelpack:errArgumentType','Ports','modelpack.CSDLinearizationID'))
                end
            else
                Ports = [];
            end

            %Find the ports & path defining the linear model
            if isempty(Ports)
                %No ports defined, default to returning whole system
                Path     = '';
                namesIn  = {};
                namesOut = {};
            else
                %Find input and output ports
                Paths = Ports.getPath;
                %Make sure have consistent port paths
                if ~all(strcmp(Paths,Paths{1}))
                    error(message('SLControllib:modelpack:stErrorLinearizePortPaths'))
                else
                    Path = Paths{1};
                end
                Names = Ports.getName;
                Names = regexprep(Names,':1$',''); %Remove port number as always 1
                Type  = Ports.getType;
                idxI  = strcmp(Type,'Input')  | strcmp(Type,'OutIn');
                idxO  = strcmp(Type,'Output') | strcmp(Type,'OutIn');
                namesIn  = {Names{idxI}};
                namesOut = {Names{idxO}};

                %Check that have at least one input and one output
                if isempty(namesIn) || isempty(namesOut)
                    error(message('SLControllib:modelpack:stErrorInputOutputPort'))
                end
            end

            if ~isempty(Path)
                %Want system from TunedLoop or TunedBlock, can ignore port names
                TB = getTunableBlocks(this.Model);
                idx = strcmp(getIdentifier(TB),Path);
                if any(idx)
                    % REVISIT: Is ZPK the best? Should SS be used?
                    obj = zpk(TB(idx));
                else
                    %Check Response as no TunedBlock found
                    idx = strcmp(getIdentifier(this.Model.Responses),Path);
                    if any(idx)
                        obj = getModel(this.Model.Responses(idx));
                    end
                end
            end

            if isempty(obj)
                error(message('SLControllib:modelpack:stErrorLinearizePortDetails'));
            else
                %Convert ssdata object into state-space model
                sys = obj;
            end


        end  % linearize

        function removeLinearizationIO(~,~)
            % REMOVELINEARIZATIONIO  method to remove linearization ports from a
            % SISOTOOL model object
            %
            % Inputs:
            %     Ports -  a vector of indices of ports to remove or a vector of SISOTOOL
            %            linearization objects to remove.
            %
            % As SISOTOOL linearization ports cannot be removed from a SISOTOOL model this method
            % is a no-op, but is required for consistency with the model API.
            %

            warning(message('SLControllib:modelpack:stWarnRemoveLinearizationIO'))
        end  % removeLinearizationIO

        function removeOutput(~,~)
            % REMOVEOUTPUTS  method to remove output ports from a SISOTOOL model
            % object.
            %
            % Input:
            %    Ports - a vector of indices of ports to remove or a vector of SISOTOOL
            %            port objects to remove.
            %
            % As SISOTOOL output ports cannot be removed from a SISOTOOL model this method
            % is a no-op, but is required for consistency with the model API.
            %

            warning(message('SLControllib:modelpack:stWarnRemoveOutput'))
        end  % removeOutputs

        function setCurrentConfig(~,~)
            % SETCURRENTCONFIG  method to set SISTOOL model configuration
            %
            %Inputs:
            %     NewCfg -  a SISOTOOL configuration object.
            %
            % As SISOTOOL cannot be cobnfigured from the command this method
            % is a no-op, but is required for consistency with the model API.
            %

            warning(message('SLControllib:modelpack:stWarnSetConfiguration'))
        end  % setCurrentConfig

        function setValue(this,Variable,Value)
            % SETVALUE  method to set the value of a SISOTOOL model variable object.
            %
            % this.setValue(Variable,Value)
            %
            % Input:
            %   Variable - a vector or single VariableID object (valid ID objects are
            %              parameterID's, stateID's, ParameterSpec's and StateSpec's),
            %              or a string with the variable name
            %   Value    - a double value with the new value for the variable object,
            %              or a cell array of values when the Variable argument is a vector.
            %              If the Variable argument is a Spec object the value can be omitted
            %

            if nargin < 3, Value = {}; end

            %Parse input types to check which local set methods to call
            processed = false;
            if ischar(Variable)
                %Want to set a parameter using the fullname
                localSetStringValue(this,Variable,Value)
                processed = true;
            end
            if isa(Variable,'modelpack.CSDStateID') || ...
                    isa(Variable,'modelpack.StateValue')
                %Want to set a state value
                localSetStateValue(this,Variable,Value)
                processed = true;
            end
            if isa(Variable,'modelpack.CSDParameterID') || ...
                    isa(Variable,'modelpack.ParameterValue') || ...
                    isa(Variable,'modelpack.ParameterSpec')
                %Want to set a parameter value
                localSetParameterValue(this,Variable,Value)
                processed = true;
            end

            if ~processed
                error(message('SLControllib:modelpack:errArgumentType','Variable', ...
                    'modelpack.CSDParameterID, modelpack.CSDStateID, ParameterSpec, StateSpec, or a string'));
            end
        end
        
        function [out,info] = simulate(this,T,InputSignals,SimOpt)
            % SIMULATE  method to perform time domain simulation of the SISTOOL object
            % model.
            %
            % [out,info] = this.simulate(T,InputSignals,SimOpt)
            %
            % Input:
            %    T            - a double vector of simulation time points
            %    InputSignals - a vector of modelpack.CSDPortID input objects
            %    SimOpt       - a modelpack simoptions object

            %Check number of arguments
            if nargin ~=4
                error(message('SLControllib:modelpack:errNumArguments','4'))
            end

            %Check input argument types
            if ~isa(SimOpt,'modelpack.simoptions')
                error(message('SLControllib:modelpack:errArgumentType','SimOpt','modelpack.simoptions'))
            elseif ~isa(SimOpt.Configuration,'modelpack.CSDConfig')
                error(message('SLControllib:modelpack:stErrorSimConfiguration'))
            end
            if ~isempty(InputSignals) && ~isa(InputSignals,'timeseries')
                error(message('SLControllib:modelpack:errArgumentType','InputSignals','timeseries'));
            end

            %Find inputs and outputs defining system
            Outputs = SimOpt.Outputs;
            Inputs  = SimOpt.Configuration.ActiveInputs;
            %Get the linear system!
            sys     = this.linearize([],[Inputs;Outputs],[]);

            %Perform appropriate simulation on system
            [y,t] = utSimulate_LTI(this,sys,T,SimOpt.Configuration.InputType,InputSignals);

            %Format output into timeseries objects
            if ~isempty(y) && ~isempty(t)
                %Simulation succeeded
                nOut = numel(Outputs);
                out = cell(nOut,1);
                for ct = 1:nOut
                    %Note indexing y by column works since all SISOTOOL outputs have
                    %dimension 1
                    out{ct} = timeseries(y(:,ct),t,'name',Outputs(ct).getFullName);
                end
            else
                %Simulation failed
                error(message('SLControllib:modelpack:stErrorSimulatePortDetails'))
            end

            if nargout > 1
                %Requested simulation info output argument
                info = [];
            end

        end  % simulate

        function update(this,UpdateWhat)
            % UPDATE  method to update SISOTOOL model object, this synchronizes the
            % model object parameters, ports, and states with the SISOTOOL object.
            %
            % this.update(UpdateWhat)
            %
            % Inputs:
            %     UpdateWhat - an optional string indication what to update, accepted
            %     values are {'Parameters'|'Ports'|'LinearizationPorts'|'States'|'All'},
            %     if omitted the default 'All' is used.
            %

            %Check for optional arguments
            if nargin < 2, UpdateWhat = 'All'; end

            switch UpdateWhat
                case 'Parameters'
                    localUpdateParams(this)
                case 'Ports'
                    this.IOs = localUpdatePorts(this,this.IOs,'modelpack.CSDPortID');
                case 'LinearizationPorts'
                    this.LinearizationIOs = localUpdatePorts(this,...
                        this.LinearizationIOs,'modelpack.CSDLinearizationID');
                case 'States'
                    localUpdateStates(this);
                case 'All'
                    localUpdateParams(this)
                    this.IOs              = localUpdatePorts(this,this.IOs,'modelpack.CSDPortID');
                    this.LinearizationIOs = localUpdatePorts(this,...
                        this.LinearizationIOs,'modelpack.CSDLinearizationID');
                    localUpdateStates(this);
                otherwise
                    error(message('SLControllib:modelpack:errValueEnumerated','UpdateWhat', ...
                        '{''Parameters''|''Ports''|''LinearizationPorts''|''States''|''All''}'));
            end
        end  % update


        function [y,t] = utSimulate_LTI(~,sys,T,InputType,InputSignals)
            % UTSIMULATE_LTI method to simulate an LTI object that is derived
            % from a SISOTOOL model object
            %
            % Input:
            %   sys          - an LTI object
            %   T            - a TimeSpan vector
            %   InputType    - one of {'step'|'impulse'|'specified'}
            %   InputSignals - timeseries of inputs to use when input type is specified
            %

            % Method was private but made public to facilitate unit testing

            %Perform all calculations in units of system. At end convert back to
            %seconds.
            T = T*tunitconv('seconds',sys.TimeUnit);

            Ts       = sys.Ts;
            T        = T(:);                %Make sure specified times are in a column
            T        = T(T>=0);             %Can only simulate positive time

            %Determine simulation stop time
            if ~isfinite(T(end))
                %Infinite stop time specified, choose time twice last finite value.
                if numel(T) > 1
                    T(end) = 2*max(T(isfinite(T)));
                else
                    %No end time to base Tend on, choose zero as this will ensure we
                    %only simulate step/impulse without a specified end time, i.e., end
                    %time left to timegrid.m
                    T(end) = 0;
                end
            end
            Tend = T(end);

            if Ts > 0
                %Discrete system, make sure end time is multiple of Ts
                Tend = Tend-rem(Tend,Ts)+Ts;
            end

            %Perform simulation
            if any(strcmp(InputType,{'step','impulse'}))
                %Setp or impulse simulation required
                if strcmp(InputType,'step')
                    simFcn = @step;
                else
                    simFcn = @impulse;
                end
                [y,t] = simFcn(sys);  %Simulate to some finite end time
                if t(end) < Tend
                    %Default simulation stopped before requested end time, redo
                    %simulation with specified end time
                    [yInf,tInf] = simFcn(sys,Tend);
                    %Combine the two responses
                    [t,idx] = unique([t;tInf]);
                    Y = [y;yInf];
                    y = Y(idx,:);
                end
            elseif strcmp(InputType,'specified')
                %Simulation with specified input signal required
                if isempty(InputSignals) || ...
                        ~(isa(InputSignals,'timeseries') || ...
                        isa(InputSignals,'tscollection'))
                    error(message('SLControllib:modelpack:errArgumentType','InputSignals','timeseries'))
                end
                t = InputSignals.Time;
                u = localGetTSData(InputSignals);
                y = lsim(sys,u,t);
                %Filter out any requested time points that are beyond the input signal
                %time range
                T = T(T<=max(t));
            else
                %Unknown simulation input type
                t = []; y = [];
            end

            if any(isnan(y))
                %Output giving nan for unstable system, replace nans with largest
                %absolute value in signal
                y(isnan(y)) = max(abs(y(isfinite(y))));
            end

            %Make sure produce output for all requested times
            nT = numel(T);    %Number of specified time points
            if nT > 1
                %Use interpolation to find outputs at specified times
                if Ts > 0
                    %Discrete system, use zoh
                    yAdd = nan(nT-1,size(y,2));
                    for ct=1:nT
                        idx  = find(t<=T(ct),1,'last');
                        yAdd(ct,:) = y(idx,:);
                    end
                else
                    %Continuous system, use linear interpolation
                    yAdd = interp1(t,y,T,'linear');
                end
                %Combine interpolated points with original response
                t = [t; T];
                y = [y; yAdd];
                [t,idx] = unique(t); %Also sorts t
                y = y(idx,:);
            end

            %Convert time units to seconds
            t = t*tunitconv(sys.TimeUnit,'seconds');
        end


    end  % public methods

    %QE methods
    methods(Access = public, Hidden = true)
        function qeSetIOs(this,ios)
            this.IOs = ios;
        end
        function qeSetLinearizationIOs(this,ios)
            this.LinearizationIOs = ios;
        end
    end

end  % classdef

function out = localConvert2TS(y,t,Outputs)
%Helper function to convert numerical data to time series.
%
%Note that this function relies on the fact that all SISOTOOL output ports
%have a dimension equal to [1 1].

nSig = size(y,2);        %Number of signals from simulation
nOut = numel(Outputs);   %Number of system outputs

switch nSig
    case nOut
        %Results from computed finite difference
        out = cell(nSig,1);
        for ct = 1:nSig
            out{ct} = timeseries(y(:,ct),t,'name',Outputs(ct).getFullName);
        end
    case 2*nOut
        %Results from Left-Right simulation
        out = cell(nSig,1);
        for ct = 0:nSig-1
            out{ct+1} = timeseries(y(:,ct+1),t,'name',...
                Outputs(rem(ct,nOut)+1).getFullName);
        end
    otherwise
        error(message('SLControllib:modelpack:stErrorPortDimensionMismatch'))
end
end  % localConvert2TS

function Val = localGetSpec(this,VarID,type)
%Local sub-function to return a spec ot the appropriate type

nVar = numel(VarID);
Val = handle(nan(nVar,1));
for ct = 1:nVar
    Val(ct,1) = feval(type,this,VarID(ct));
    if strcmp(type,'modelpack.CSDParameterSpec')
        Val(ct,1).InitialValue = this.getValue(VarID(ct)).Value;
        Val(ct,1).TypicalValue = Val(ct,1).InitialValue;
    end
end
end  % localGetSpec

function Val = localGetStringSpec(this,Variable)
%Local sub-function to create a spec from a string

%Check parameters for match
ID = this.findParameter(Variable,true);
if ~isempty(ID)
    Val = localGetSpec(this,ID,'modelpack.CSDParameterSpec');
    %No need to search further, exit
    return
end

%Check states for match
ID = this.findState(Variable,true);
if ~isempty(ID)
    Val = localGetSpec(this,ID,'modelpack.StateSpec');
    %No need to search further, exit
    return
end

%Check input ports
ID = this.findInput(Variable,true);
if ~isempty(ID)
    Val = localGetSpec(this,ID,'modelpack.PortSpec');
    %No need to search further, exit
    return
end

%Check output ports
ID = this.findInput(Variable,true);
if ~isempty(ID)
    Val = localGetSpec(this,ID,'modelpack.PortSpec');
    %No need to search further, exit
    return
end

%No matching element found return empty handle
Val = handle(nan);
end  % localGetStringSpec

function Val = localGetVariableSpec(this,Variable)

%Mix of different Variable ID objects
nVal = numel(Variable);
Val  = handle(nan(nVal,1));
for ct = 1:nVal

    %Determine the type of spec to create
    switch class(Variable(ct))
        case 'modelpack.CSDPortID'
            type = 'modelpack.PortSpec';
        case 'modelpack.CSDStateID'
            type = 'modelpack.StateSpec';
        case 'modelpack.CSDParameterID'
            type = 'modelpack.CSDParameterSpec';
        otherwise
            %Unsupported type, skip spec creation
            type = [];
    end

    %Create the spec
    if ~isempty(type)
        Val(ct,1) = localGetSpec(this,Variable(ct),type);
    end
end
end  % localGetVariableSpec

function Val = localGetParameterValue(this,VarIDIn)
%Local sub-function to return a parameter value

TunedBlocks = getTunableBlocks(this.Model);
Val = [];
for ct = 1:numel(VarIDIn)
    %Check whether we have a Spec object, if so extract ID object
    Format = [];  %Some parameters have a format option
    if isa(VarIDIn(ct),'modelpack.ParameterSpec')
        if isa(VarIDIn(ct).getID,'modelpack.CSDParameterID')
            if isa(VarIDIn(ct),'modelpack.CSDParameterSpec')
                %Want parameter value in specific format
                Format = VarIDIn(ct).Format;
            end
            VarID = VarIDIn(ct).getID; 
        else
            error(message('SLControllib:modelpack:errArgumentType','Variable',...
                'modelpack.CSDParameterID or modelpack.CSDParameterSpec'));
        end
    elseif isa(VarIDIn(ct),'modelpack.ParameterID')
        VarID = VarIDIn(ct);
    end
    %Find TunedBlock that uses this parameter
    Path = VarID.getPath;
    ID = getIdentifier(TunedBlocks);
    idx = strcmp(ID,Path);
    if any(idx)
        %Found tuned block with this parameter, now find correct parameter and
        %get value
        found = false;
        TB = TunedBlocks(idx);
        if ~isempty(TB.getMaskParameterSpec)
            allPSpec = TB.getMaskParameterSpec.getID;
            if iscell(allPSpec), allPSpec = [allPSpec{:}]; end
            idx = 1;
            while ~found && idx <= numel(allPSpec)
                found = VarID.isSame(allPSpec(idx));
                if ~found, idx = idx + 1; end
            end
            if found
                %Match spec index to tunable parameter index
                Parameters = getParameters(TB);
                idxTunable = cumsum(strcmp({Parameters.Tunable},'on'));
                idxTBParam = find(idxTunable >= idx,1,'first');
                %Have index into parameters, get value
                Value = Parameters(idxTBParam).Value;
            end
        end
        if ~found
            ZPKSpecs = TB.getZPKParameterSpec;
            if ZPKSpecs.GainSpec.getID.isSame(VarID)
                %PZgroup gain parameter
                found = true;
                if isempty(Format), Format = 1; end
                Value = TB.getGainValue(Format);
            end
            if ~found && ~isempty(ZPKSpecs.PZGroupSpec)
                %Check PZgroups for parameter
                allPSpec = ZPKSpecs.PZGroupSpec;
                pID = allPSpec.getID;
                if iscell(pID), pID = [pID{:}]; end
                idx = 1;
                while ~found && idx <= numel(pID)
                    found = VarID.isSame(pID(idx));
                    if ~found, idx = idx + 1; end
                end
                if found
                    if isempty(Format)
                        Format = allPSpec(idx).Format;
                    end
                    Value = TB.PZGroup(idx).getValue(Format);
                end
            end
        end
        %Now find correct index into model API parameter list, set value
        %object and return
        if found
            if isempty(Format), Format = 1; end
            %Need to create a parameter value object
            ParaValue = modelpack.CSDParameterValue(VarID);
            ParaValue.Value  = Value;
            ParaValue.Format = Format;
            Val = [Val; ParaValue]; %#ok<AGROW>
        end
    end
end
end  % localGetParameterValue

function Val = localGetStateValue(VarID)
%Local sub-function to return a state value, note that for SISOTOOL all
%state values are column zeros

Val = [];
for ct = 1:numel(VarID)
    if isa(VarID(ct),'modelpack.StateSpec')
        if isa(VarID(ct).getID,'modelpack.CSDStateID')
            VarID(ct) = VarID(ct).getID;
        else
            error(message('SLControllib:modelpack:errArgumentType','Variable', ...
                'modelpack.CSDStateID or StateSpec'))
        end
    end
    Val = [Val; modelpack.StateValue(VarID(ct),[])]; %#ok<AGROW>
end
end  % localGetStateValue


function Val = localGetStringValue(this,VarName)
%Local sub-function to return a variable value based on a variable
%fullname. An exact match is used when looking for the variable name

Val = [];
idx = strcmp(this.Parameters.getFullName,VarName);
if any(idx)
    %Looking for a parameter value
    Val = localGetParameterValue(this,this.Parameters(idx));
else
    idx = strcmp(this.States.getFullName,VarName);
    if any(idx)
        %Looking for a state value
        Val = localGetStateValue(this.States(idx));
    end
end

if isempty(Val)
    error(message('SLControllib:modelpack:errParameterNotFound',VarName));
end
end  % localGetStringValue

function localSetStateValue(this,VarID,Val) %#ok<INUSD>
%Local function to set state value. As SISOTOOL states cannot be set this
%function is a no-op.

warning(message('SLControllib:modelpack:stWarnSetStates'))
end

function localSetParameterValue(this,VarID,Val)
%Local function to set parameter value

%Make sure new values are in a cell array
if ~iscell(Val), Val = {Val}; end

%Check input arguments
nVarID  = numel(VarID);
nVal    = numel(Val);
pvVarID = isa(VarID,'modelpack.ParameterValue');
if ~pvVarID && (pvVarID || ~(nVal==1 || nVal==nVarID))
    error(message('SLControllib:modelpack:stErrorSetDimensions'))
end

TunedBlocks = getTunableBlocks(this.Model);
for ct = 1:nVarID
    if pvVarID
        %Convert value object to ID and value
        if isempty(Val), ValIn = VarID(ct).Value; end
        ID = VarID(ct).getID;
        if isa(VarID(ct),'modelpack.CSDParameterValue')
            Format = VarID(ct).Format;
        else
            Format = [];
        end
    else
        if nVal > 1
            ValIn = Val{ct};
        else
            ValIn = Val{1};
        end
        if isa(VarID(ct),'modelpack.ParameterSpec')
            %Passed a spec object
            ID = VarID(ct).getID;
            if isa(VarID(ct),'modelpack.CSDParameterSpec')
                Format = VarID(ct).Format;
            else
                Format = [];
            end
        else
            ID = VarID(ct);
            Format = [];
        end
        %Check dimensions and type of new value
        if ~isnumeric(ValIn) || ~all(isfinite(ValIn))
            error(message('SLControllib:modelpack:stErrorNewValueType',ID.getFullName))
        end
        if prod(ID.getDimensions)~=numel(ValIn)
            error(message('SLControllib:modelpack:stErrorNewValueDimension',ID.getFullName))
        end
    end

    %Now ready to set the value
    Path = ID.getPath;
    idx = strcmp(getIdentifier(TunedBlocks),Path);
    if any(idx)
        %Found tunedblock with this parameter, now find correct parameter and
        %set value
        found = false;
        TB = TunedBlocks(idx);
        if ~isempty(TB.getMaskParameterSpec)
            %Mask parameter
            allPSpec = TB.getMaskParameterSpec.getID;
            if iscell(allPSpec), allPSpec = [allPSpec{:}]; end
            idx = 1;
            while ~found && idx <= numel(allPSpec)
                found = ID.isSame(allPSpec(idx));
                if ~found, idx = idx + 1; end
            end
            if found
                %Match spec index to tunable parameter index
                Params = getParameters(TB);
                idxTunable = cumsum(strcmp({Params.Tunable},'on'));
                idxTBParam = find(idxTunable >= idx,1,'first');
                %Have index into parameters, set value
                TB.setParameterValue(idxTBParam,ValIn)
            end
        end
        if ~found && TB.isTunable
            ZPKSpecs = TB.getZPKParameterSpec;
            if ZPKSpecs.GainSpec.getID.isSame(ID)
                %PZgroup gain parameter
                found = true;
                if isempty(Format), Format = 1; end
                TB.setGainValue(ValIn,Format);
            end
            if ~found && ~isempty(ZPKSpecs.PZGroupSpec)
                %Check PZgroups for parameter
                allPSpec = ZPKSpecs.PZGroupSpec;
                pID = allPSpec.getID;
                if iscell(pID), pID = [pID{:}]; end
                idx = 1;
                while ~found && idx <= numel(pID)
                    found = ID.isSame(pID(idx));
                    if ~found, idx = idx + 1; end
                end
                if found
                    if isempty(Format)
                        Format = allPSpec(idx).Format;
                    end
                    TB.PZGroup(idx).setValue(ValIn,Format);
                    %Clear all compensator dependencies
                    notifyValueChanged(TB);
                end
            end
        end
    end
end
end

function localSetStringValue(this,VarName,Val)
%Local function to set a variable value based on the variable name

idx = strcmp(this.Parameters.getFullName,VarName);
if any(idx)
    %Looking for a parameter value
    localSetParameterValue(this,this.Parameters(idx),Val);
else
    idx = strcmp(this.States.getFullName,VarName);
    if any(idx)
        %Looking for a state value
        localSetStateValue(this,this.States(idx),Val);
    end
end
end

function localUpdateParams(this)
%Function to make sure all SISOTOOL parameters have equivalent parameter ID
%objects as properties of the SISOTOOL model object.

Compensators = getTunableBlocks(this.Model);
Params       = this.Parameters;
FoundParams  = false(1,numel(Params));   %Keep track of parameters found
NewParams    = [];                       %Vector of newly found parameters

for ct=1:numel(Compensators)
    if isa(Compensators(ct),'ctrlguis.csdesignerapp.data.architectures.internal.TunableBlock')
        %Always has PZGroup specs
        if Compensators(ct).isTunable
            ZPKSpecs = Compensators(ct).getZPKParameterSpec;
        else
            ZPKSpecs = struct('GainSpec',[],'PZGroupSpec',[]);
        end
        MaskParamSpec = Compensators(ct).getMaskParameterSpec; %Only returns tunable mask parameters
        SISOParams = [...
            MaskParamSpec; ...
            ZPKSpecs.GainSpec; ...
            ZPKSpecs.PZGroupSpec];
    else
        %No PZGroup spec
        SISOParams = Compensators(ct).getMaskParameterSpec;
    end
    for ct_P = 1:numel(SISOParams)
        NewP  = SISOParams(ct_P).getID;
        found = false; idx = 1;
        while ~found && idx <= numel(this.Parameters)
            found = NewP.isSame(this.Parameters(idx));
            if ~found, idx = idx + 1; end
        end
        if ~found
            %New parameter, first make sure we have correct dimension and locations
            %then add to list of new parameters
            [~,Dim,Locations] = getParameterData(this,Compensators(ct),NewP);
            NewP.update(this,'Dimension',Dim,'Locations',Locations);
            NewParams = [NewParams; NewP]; %#ok<AGROW>
        else
            %Existing parameter
            if ~FoundParams(idx)
                %Haven't already found this parameter, update dimension, value and
                %location
                FoundParams(idx) = true;
                [~,Dim,Locations] = getParameterData(this,Compensators(ct),NewP);
                Params(idx).update(this,'Dimension',Dim,'Locations',Locations);
            end
        end
    end
end

%Remove any missing params and add new parameters
if any(~FoundParams)
    Params(~FoundParams).delete;
    Params = Params(ishandle(Params));
end
this.Parameters      = [Params; NewParams];
end  % localUpdateParams

function IOs = localUpdatePorts(this,IOs,objType)
%Function to make sure all SISOTOOL ports have equivalent port ID
%objects as properties of the SISOTOOL model object.
%
% Inputs:
%    IOs     - vector of currently known ports
%    objType - string with type of port object to create if new ports are
%              identified can be one of
%              {'modelpack.CSDPortID'|'modelpack.CSDLinearizationID'}

nIOs = numel(IOs);

%Collect list of all actual SISOTOOL inputs & outputs
% REVISIT
% ios = linearize.getModelIOPoints({this.Model.getArchitectureName});
% Inputs = ios(ismember({ios.Type},'input'));
if isempty(getTunableBlocks(this.Model))
    Compensators = [];
else
    Compensators = getIdentifier(getTunableBlocks(this.Model));
end

if ~iscell(Compensators)
    Compensators = {Compensators};
end
nComps       = numel(Compensators);
ValidLoops   = this.Model.Responses;
if isempty(ValidLoops)
    Loops = [];
else
    Loops = getIdentifier(ValidLoops);
end
if ~iscell(Loops)
    Loops = {Loops};
end
nLoops       = numel(Loops);
%Construct full names for comparison with known ports
InputsPaths  = repmat({''},0,1);
InPortNames  = repmat({'In'},nComps+nLoops,1);
InPortPaths  = vertcat(Compensators,Loops);
InFullNames  = vertcat(InputsPaths,strcat(InPortPaths,'/'));
InPortPaths  = vertcat(InputsPaths,InPortPaths);
InFullNames  = strcat(InFullNames,strcat(InPortNames,':1'));
OutputsPaths = repmat({''},0,1);
OutPortNames = repmat({'Out'},nComps+nLoops,1);
OutPortPaths = vertcat(Compensators,Loops);
OutFullNames = vertcat(OutputsPaths,strcat(OutPortPaths,'/'));
OutPortPaths = vertcat(OutputsPaths,OutPortPaths);
OutFullNames = strcat(OutFullNames,strcat(OutPortNames,':1'));

%Check the difference between known and actual ports
FoundInPorts  = false(1,nComps+nLoops);  %Keep track of ports found
FoundOutPorts = false(1,nComps+nLoops);
FoundIO       = false(1,nIOs);
for ct = 1:nIOs   %Loop over known ports
    FullName = IOs(ct).getFullName;
    idxIn  = strcmp(InFullNames,FullName);
    idxOut = strcmp(OutFullNames,FullName);
    if any(idxIn)
        FoundInPorts(idxIn) = true;
        FoundIO(ct) = true;
    end
    if any(idxOut)
        FoundOutPorts(idxOut) = true;
        FoundIO(ct) = true;
    end
end

%Create any new ports.
newIO = localNewPorts(...
    {InPortNames{~FoundInPorts}}, ...
    {InPortPaths{~FoundInPorts}}, ...
    objType, 'Input');
newIO = [newIO; localNewPorts(...
    {OutPortNames{~FoundOutPorts}}, ...
    {OutPortPaths{~FoundOutPorts}}, ...
    objType, 'Output')];

%Remove IOs not found and add new IOs
if any(~FoundIO), IOs(~FoundIO).delete, end
IOs = [IOs(FoundIO); newIO];
end  % localUpdatePorts

function localUpdateStates(this)
%Function to make sure all CSD states have equivalent CSDStateID
%objects as properties of the CSD model object.
%

States  = this.States;  %List of known states
nStates = numel(States);
StateNames = cell(0,1);
%Collect list of all actual CSD states
Compensators = getTunableBlocks(this.Model);
if ~isempty(Compensators)
    CompNames = getIdentifier(Compensators);
    StateNames = vertcat(StateNames,CompNames);
end
ValidLoops = this.Model.Responses;
if ~isempty(ValidLoops)
    Loops        = getIdentifier(ValidLoops);
    StateNames   = vertcat(StateNames,Loops);
end

%Check the difference between known and actual states
FoundState = false(1,numel(StateNames));  %Keep track of states found
Found      = false(1,nStates);
for ct = 1:nStates
    Name = States(ct).getName;
    idx  = strcmp(StateNames,Name);
    if any(idx)
        %Existing state,
        FoundState(idx) = true;
        if ~Found(ct)
            %Haven't yet found this state,update dimension and sampling time
            Found(ct) = true;
            [Ts,Dim]  = getStateData(this,StateNames{idx},'');
            States(ct).update(this,'Ts',Ts,'Dimension',Dim);
        end
    end
end

%Create any new states.
newState = [];
for idx = find(~FoundState)
    %Add new state
    [Ts,Dim] = getStateData(this,StateNames{idx},'');
    newState = [newState; modelpack.CSDStateID(...
        StateNames{idx},...
        Dim,...
        '',...
        Ts)]; %#ok<AGROW>
end

%Remove states not found and add new states
if any(~Found), States(~Found).delete, end
this.States = [States(Found); newState];
end  % localUpdateStates

function NewPorts = localNewPorts(PortNames,PortPaths,objType,Type)
%Helper function to create a vector of CSDPortID or CSDLinearizationID objects

NewPorts = [];
for ct=1:numel(PortNames)
    NewPorts = [NewPorts; feval(objType,...
        PortNames{ct}, [1 1], ...
        PortPaths{ct}, Type);]; %#ok<AGROW>
end
end  % localNewPorts

function data = localGetTSData(ts_collection)
%Helper function to return all the data from a ts collection in a matrix
%format

if isa(ts_collection,'tscollection')
    %Multiple inputs
    Names = gettimeseriesnames(ts_collection);
    for ct=1:numel(Names)
        data = [data, ts_collection.(Names{ct}).data]; %#ok<AGROW>
    end
elseif isa(ts_collection,'timeseries')
    %Single input
    data = ts_collection.data;
else
    data = [];
end
end

function isstab = localISStable(sys)

CanComputeStab = true;
if isa(sys,'ss')
    %Be careful of systems with internal delays
    iod = getIODelay(getPrivateData(sys));
    if any(isnan(iod(:)))
        CanComputeStab = false;
    end
end

if CanComputeStab
    isstab = isstable(sys);
else
    isstab = false; %Can not tell is system is stable, assume not. Forces extra time points
end
end
function Port = findPort(~,PortList,Name,Exact)
% FINDPORT  private method to return particular port(s)
%
% Input:
%   PortList - a vector of CSDPortID objects to search through
%   Name     - a string with the name of the port to find
%   Exact    - a boolean flag, when true an exact match is required
%

%Check number of arguments
if nargin ~= 4
    error(message('SLControllib:modelpack:errNumArguments','3'))
end

%Check argument types
if ~isa(PortList,'modelpack.CSDPortID')
    error(message('SLControllib:modelpack:errArgumentType','PortList','modelpack.CSDPortID'))
end
if ~ischar(Name)
    error(message('SLControllib:modelpack:errArgumentType','Name','string'))
end

%Look for full port names that contain the search string
if Exact
    idx = strcmp(PortList.getFullName,Name);
else
    idx = ~cellfun('isempty',strfind(PortList.getFullName,Name));
end
Port = PortList(idx);
end  % findPort

function [Value,Dim,Locations] = getParameterData(this,TunedBlock,ParamID)
% GETPARAMETERDATA  private method to retrieve the parameter data from the SISOTOOL
% data object.
%
% Input
%   TunedBlock - the block with the parameter
%   ParamID    - index to the tuned parameter in this block

found = false;
Value = [];
Locations = {TunedBlock.getIdentifier};
%Check if parameter is part of PZGroup
ZPKSpecs = TunedBlock.getZPKParameterSpec;
if ~isempty(ZPKSpecs)
    if ZPKSpecs.GainSpec.getID.isSame(ParamID)
        %Gain parameter
        found = true;
        Value = TunedBlock.getFormattedGain;
    end
    if ~found && ~isempty(ZPKSpecs.PZGroupSpec)
        pID = ZPKSpecs.PZGroupSpec.getID;
        if iscell(pID), pID = [pID{:}]; end
        idx = 1;
        while ~found && idx <= numel(pID)
            found = ParamID.isSame(pID(idx));
            if ~found, idx = idx + 1; end
        end
        if found
            %PZGroup parameter
            Format = ZPKSpecs.PZGroupSpec(idx).Format;
            Value  = TunedBlock.PZGroup(idx).getValue(Format);
            found  = true;
        end
    end
end
%Check if parameter is part of tuned mask
if ~found && ~isempty(TunedBlock.MaskParamSpec)
    Parameters = getParameters(TunedBlock);
    idx = strcmp({Parameters.Name},ParamID.getName);
    if any(idx)
        Value = Parameters(idx).Value;
        found = true;
    end
end

if found
    %Set dimension
    Dim = size(Value);
    %Check for other locations of parameter
    Responses = getResponses(this.Model);
    if ~isempty(Responses)
        Loops = Responses(isLoopTransfer(Responses) & issiso(Responses));
        for ct = 1:numel(Loops)
            Compensators = getTunedFactors(Loops(ct));
            if ismember(TunedBlock, Compensators)
                Locations{end+1} = sprintf('%s/%s',getIdentifier(Loops(ct)),getIdentifier(TunedBlock)); %#ok<AGROW>
            end
        end
    end
else
    error(message('SLControllib:modelpack:errParameterNotFound',ParamID.getFullName));
end

end  % getParameterData

function [Ts,Dim] = getStateData(this,Name,Path)
% GETSTATEDATA  private method to retrieve the state data from the SISOTOOL
% data object.
%
% Input:
%    Name - a string with the state name
%    Path - a string with the state path

Ts  = [];
Dim = [];
if isempty(Path)
    Model = this.Model;
    %All SISOTOOL states are at the root level and so have empty paths, now
    %check whether we want Response, or TunedBlock states.
    R = getResponses(Model);
    if ~isempty(R)
        idx = strcmp(getIdentifier(R),Name);
        if any(idx)
            %Is a tuned loop
            sys = getModel(R(idx));
            Ts = sys.Ts;
            Dim = localGetStateDim(sys);
        end
    end
    TB = getTunableBlocks(Model);
    if ~isempty(TB)
        idx = strcmp(getIdentifier(TB),Name);
        if any(idx)
            %Is a tuned block
            sys = zpk(TB(idx));
            Ts  = sys.Ts;
            Dim = localGetStateDim(sys);
        end
    end
end
end

function n = localGetStateDim(sys)
%Helper function to determine the state dimension of an lti object
if isa(sys,'ltipack.frddata') || isa(sys,'frd')||isa(sys,'genfrd')
    n = [0,1];
else
    n = [order(sys),1];
end
end
