classdef (CaseInsensitiveProperties = true, TruncatedProperties = true) ...
        ControllerPoles < TuningGoal.Generic
    % Constraint on the controller dynamics.
    %
    %   R = TuningGoal.ControllerPoles(BLOCKID,MINDECAY,MINDAMPING,MAXFREQ)
    %   creates a tuning requirement R for the controller dynamics. The string
    %   BLOCKID designates one of the tuned blocks making up the controller
    %   (see tunableBlock). The scalars MINDECAY, MINDAMPING, and MAXFREQ
    %   specify the minimum decay rate, minimum damping, and maximum natural
    %   frequency for the poles of the designated block. You can use this
    %   requirement to ensure that the controller is stable (MINDECAY>=0)
    %   and free of fast or resonant dynamics.
    %
    %   Example: If the "Compensator" block is parameterized as a second-order
    %   transfer function using tunableTF, the requirement
    %      R = TuningGoal.ControllerPoles('Compensator',0.1,0,30)
    %   restricts its poles to the region:
    %      Re(s) < -0.1,    |s| < 30
    %
    %   Use VIEWSPEC(R) to visualize this requirement and use SYSTUNE and
    %   related commands to tune the control system parameters subject to
    %   this and other requirements.
    %
    %   Note: When MINDECAY>=0, this requirement enforces stability of the
    %   specified tuned block. You can set MINDECAY<0 to allow for unstable
    %   controllers. The MINDAMPING setting is ignored in such case.
    %
    %   See also pole, evalGoal, viewGoal, TuningGoal.Poles, TuningGoal.Margins,
    %   systune, looptune, slTuner.

    %   Author: P. Gahinet
    %   Copyright 2009-2016 The MathWorks, Inc.

    properties
        % Block name (string).
        %
        % This property specifies the name of the control element to which
        % this requirement applies.
        Block

        % Minimum decay rate of controller poles (default = 0).
        %
        % Constrains all poles of the control element to satisfy
        %       Re(s) < -alpha       (continuous)
        %    log(|z|) < -alpha*Ts    (discrete)
        % where alpha is the specified value. Use a nonnegative value to
        % ensure that the controller dynamics are stable.
        MinDecay = 0;

        % Minimum damping of controller poles (default = 0).
        %
        % The closed-loop poles must satisfy
        %    Re(s) < -zeta * |s|
        % where zeta is the specified minimum damping ratio (a value between
        % 0 and 1). In discrete time, the damping ratio is computed using
        % s=log(z)/Ts. This setting is ignored when the controller is allowed
        % to be unstable (MinDecay<0).
        MinDamping = 0;

        % Maximum natural frequency of controller poles (default = Inf).
        %
        % Constrains all poles of the tuned element to satisfy
        %         |s| < rho       (continuous)
        %    |log(z)| < rho*Ts    (discrete)
        % where rho>0 is the specified value. This is useful to prevent fast
        % controller dynamics.
        MaxFrequency = Inf;
    end

    methods

        % Constructor
        function this = ControllerPoles(BlockName,varargin)
            narginchk(1,4)
            ni = nargin;
            try
                this.Block = BlockName;
                if ni>1
                    this.MinDecay = varargin{1};
                end
                if ni>2
                    this.MinDamping = varargin{2};
                end
                if ni>3
                    this.MaxFrequency = varargin{3};
                end
            catch ME
                throw(ME)
            end
        end

        function this = set.Block(this,Value)
            % SET function for Block
            if ~(ischar(Value) && isrow(Value))
                error(message('Control:tuning:ControllerPoleReq2'))
            end
            this.Block = Value;
        end

        function this = set.MinDecay(this,Value)
            % SET function for MinDecay
            try
                checkMinDecay(this,Value)
            catch ME
                throw(ME)
            end
            this.MinDecay = double(Value);
        end

        function this = set.MinDamping(this,Value)
            % SET function for MinDamping
            if ~(isnumeric(Value) && isscalar(Value) && ...
                    isreal(Value) && isfinite(Value) && Value>=0 && Value<=1)
                error(message('Control:tuning:PoleReq5'))
            end
            this.MinDamping = double(Value);
        end

        function this = set.MaxFrequency(this,Value)
            % SET function for MaxFrequency
            if ~(isnumeric(Value) && isscalar(Value) && ...
                    isreal(Value) && Value>0)
                error(message('Control:tuning:PoleReq2'))
            end
            this.MaxFrequency = double(Value);
        end

    end

    methods (Access = protected)

        function [H,fObj] = evalSpec_(this,CL)
            % Evaluates requirement for given closed-loop model
            H = getBlockValue(CL,this.Block);
            if nargout>1
                E = pole(H);
                Ts = abs(H.Ts);
                if Ts>0
                    E = log(E(E~=0))/Ts;
                end
                % Ignore integrators to produce consistent results for PIDs
                E = E(abs(E)>1e-7);
                if isempty(E)
                    fObj = 0;
                else
                    MinDecay = this.MinDecay;
                    fSpec = zeros(1,3);
                    if MinDecay<0
                        fSpec(1) = max(real(E))/(-MinDecay);
                    else
                        if MinDecay>0
                            fSpec(1) = NSOptUtil.SpectralPenalty(...
                                min(-real(E))/MinDecay);
                        end
                        if this.MinDamping>0
                            fSpec(1) = NSOptUtil.SpectralPenalty(...
                                min(-real(E)./abs(E))/this.MinDamping);
                        end
                    end
                    fSpec(3) = max(abs(E))/this.MaxFrequency;
                    fObj = max(fSpec);
                end
            end
        end

        function delta = getViewExtent_(this)
            % Compute extent for view
            delta = 8;
            if this.MaxFrequency<Inf
                delta = max(delta,1.1*this.MaxFrequency);
            end
            if this.MinDecay>0
                delta = max(delta,this.MinDecay);
            end
        end

    end

    methods (Hidden) %% Tuning goal API

        % Compute data for Goal wave forms
        function [Ts,TU] = viewSpecGoalData(~,CL)
            if isequal(CL,[])
                Ts = 0;
                TU = 'seconds';
            else
                Ts = CL.Ts;
                TU = CL.TimeUnit;
            end
        end

        % Compute data for Design wave forms
        function CPoles = viewSpecDesignData(this,CL)
            H = evalSpec_(this,CL);
            p = pole(H);
            CPoles = zpk([],p,1,'Ts',CL.Ts,'TimeUnit',CL.TimeUnit);
            CPoles.Name = getString(message('Control:systunegui:TGPlotPoles'));
        end

        % Validate if data is compatible for plotting
        function b = isCompatible(this,CL,CurrentDesignWaveforms)
            % Check for consistent size and sample time
            CPoles = viewSpecDesignData(this,CL);
            NewValue = CPoles;
            if controllib.chart.internal.utils.isChartResponse(CurrentDesignWaveforms.CPoles)
                OldValue = CurrentDesignWaveforms.CPoles.Model;
            else
                OldValue = CurrentDesignWaveforms.CPoles.DataSrc.Model;
            end
            b = isequal(size(NewValue),size(OldValue)) && isequal(NewValue.Ts,OldValue.Ts);
        end

        % Update limits
        function updateLimits(~,Ts,plotHandle,XLimitsFocus,YLimitsFocus)
            % Pick square Y limits that will show constraints if nearly active
            if controllib.chart.internal.utils.isChart(plotHandle)
                view = qeGetView(plotHandle);
                axGrid = qeGetAxesGrid(view);
                Xlim = XLimitsFocus{1};
                Ylim = YLimitsFocus{1};
                if Ts==0
                    xc = (Xlim(1)+Xlim(2))/2;
                    delta = max(Xlim(2)-Xlim(1),Ylim(2)-Ylim(1));
                    Xlim = [xc-delta,xc+delta];
                    Ylim = [-delta,delta];
                else
                    xc = 0;
                    delta = max([2 Xlim(2)-Xlim(1) Ylim(2)-Ylim(1)]);
                    Xlim = [xc-0.6*delta,xc+0.6*delta];
                    Ylim = 0.6*[-delta,delta];
                end
                axGrid.XLimitsFocus = {Xlim};
                axGrid.YLimitsFocus = {Ylim};
            else
                ax = plotHandle;
                XLimMode = XLimitsFocus;
                YLimMode = YLimitsFocus;
                if strcmp(XLimMode,'auto') || strcmp(YLimMode,'auto')
                    Xlim = get(ax,'Xlim');
                    Ylim = get(ax,'Ylim');
                    if Ts==0
                        xc = (Xlim(1)+Xlim(2))/2;
                        delta = max(Xlim(2)-Xlim(1),Ylim(2)-Ylim(1));
                        set(ax,'XLim',[xc-delta,xc+delta],'YLim',[-delta,delta])
                    else
                        xc = 0;
                        delta = max([2 Xlim(2)-Xlim(1) Ylim(2)-Ylim(1)]);
                        set(ax,'XLim',[xc-0.6*delta,xc+0.6*delta],'YLim',0.6*[-delta,delta])
                    end
                end
            end
        end

        function [h, GoalWaveforms, CurrentDesignWaveforms,Ts] = createPlot(this,CL,ax)
            % Create plot
            [Ts,TU] = viewSpecGoalData(this,CL);
                GoalWaveforms = struct('Bounds',[]);
            if isequal(CL,[])
                % Just show bounds and target shape
                if controllibutils.CSTCustomSettings.getCSTPlotsVersion == 2
                    h = controllib.chart.PZPlot(Axes=ax,Options=pzoptions('cstprefs'));
                    delta = getViewExtent_(this);
                    xc = -0.75*delta;
                    h.XLimits = {[xc-delta,xc+delta]};
                    h.YLimits = {[-delta,delta]};
                else
                    h = ltiplot(ax,'pzmap',[],[],pzoptions('cstprefs'),cstprefs.tbxprefs);
                    ltiplotmenu(h,'pzmap');
                    delta = getViewExtent_(this);
                    xc = -0.75*delta;
                    set(getaxes(h.AxesGrid),'XLim',[xc-delta,xc+delta],'YLim',[-delta,delta])
                    h.Visible = 'on';  % new plot created with Visible='off'
                end
                CurrentDesignWaveforms = struct('CPoles',[]);
            else
                CPoles = viewSpecDesignData(this,CL);
                h = pzplot(ax,CPoles,'b');
                if controllib.chart.internal.utils.isChart(h)
                    CurrentDesignWaveforms = struct('CPoles',h.Systems(1));
                    h.Systems(1).Style.SemanticColor = controllib.plot.internal.utils.GraphicsColor(1).SemanticName;
                else
                    CurrentDesignWaveforms = struct('CPoles',h.Response(1));
                    h.Response(1).setstyle(SemanticColor=controllib.plot.internal.utils.GraphicsColor(1).SemanticName);
                end
            end
            % Thicker lines
            if controllib.chart.internal.utils.isChart(h)
                for ct=1:numel(h.Systems)
                    h.Systems(ct).Style.LineWidth = 1.5;
                end
            else
                for ct=1:numel(h.Response)
                    h.Response(ct).setstyle('LineWidth',1.5)
                end
            end
            % Title
            if controllib.chart.internal.utils.isChart(h)
                h.Title.String = getString(message('Control:tuning:strStableBlock1',this.Name,this.Block));
                h.Title.Interpreter = 'none';
                h.Grid = 'on';
            else
                h.AxesGrid.Title = getString(message('Control:tuning:strStableBlock1',this.Name,this.Block));
                h.AxesGrid.TitleStyle.Interpreter = 'none';
                h.AxesGrid.Grid = 'on';
            end
            % Plot bounds
            if controllib.chart.internal.utils.isChart(h)
                tau = funitconv('rad/TimeUnit','rad/s',TU);
                h.addBoundSystem(MinDecay=tau*this.MinDecay,MinDamping=this.MinDamping,...
                    MaxFrequency=tau*this.MaxFrequency,Ts=Ts,...
                    SystemName=getString(message('Control:systunegui:TGPlotBounds')));
                h.Systems(end).Style.SemanticFaceColor = controllib.plot.internal.utils.GraphicsColor(8).SemanticName;
                h.Systems(end).Style.SemanticEdgeColor = controllib.plot.internal.utils.GraphicsColor(8).SemanticName;
                h.Systems(end).Style.FaceAlpha = 0.3;
                h.Systems(end).Style.EdgeAlpha = 0.3;
                GoalWaveforms.Bounds = h.Systems(end);
            else
                tau = funitconv('rad/TimeUnit','rad/s',TU);
                r = h.addPoleBound(tau*this.MinDecay,this.MinDamping,tau*this.MaxFrequency,Ts);
                r.Name = getString(message('Control:systunegui:TGPlotBounds'));
                r.setstyle(SemanticColor=controllib.plot.internal.utils.GraphicsColor(8,"tertiary").SemanticName);
                % Set Goal wave forms for TBound
                GoalWaveforms.Bounds = r;
            end
        end

        % Update Goal waveforms
        function [Ts,TU] = updateGoal(this,CL,hPlot,GoalWaveforms)
            % Compute data for goals
            [Ts,TU] = viewSpecGoalData(this,CL);
            h = getPlotHandle(hPlot);
            % Goal Focus
            tau = funitconv('rad/TimeUnit','rad/s',TU);
            % Update plotted goals
            if controllib.chart.internal.utils.isChart(h)
                GoalWaveforms.Bounds.MaxFrequency = tau*this.MaxFrequency;
                GoalWaveforms.Bounds.MinDecay = tau*this.MinDecay;
                GoalWaveforms.Bounds.MinDamping = this.MinDamping;
            else
                GoalWaveforms.Bounds.Data.MaxFrequency = tau*this.MaxFrequency;
                GoalWaveforms.Bounds.Data.MinDecay = tau*this.MinDecay;
                GoalWaveforms.Bounds.Data.MinDamping = this.MinDamping;
                GoalWaveforms.Bounds.draw
            end
            % Set Title for the plot
            setTitle(hPlot,getString(message('Control:tuning:strStableBlock1',this.Name,this.Block)));
        end

        % Update Design waveforms
        function updateDesign(this,DesignWaveforms,CL)
            % Compute data for design
            CPoles = viewSpecDesignData(this,CL);
            % Update design
            if controllib.chart.internal.utils.isChartResponse(DesignWaveforms.CPoles)
                DesignWaveforms.CPoles.Model = CPoles;
            else
                DesignWaveforms.CPoles.DataSrc.Model = CPoles;
            end
        end

        % Add design
        function DesignWaveforms = addDesign(this,PlotHandle,Design,Name,Color)
            % Compute data for design
            CPoles = viewSpecDesignData(this,Design);
            % Add response
            if controllib.chart.internal.utils.isChart(PlotHandle)
                addSystem(PlotHandle,CPoles,SystemName=sprintf('%s: %s',getString(message('Control:systunegui:TGPlotPoles')),Name));
                PlotHandle.Systems(end).Style.SemanticColor = Color;
                %PlotHandle.Systems(end).Style.LineWidth = 1.75;
                DesignWaveforms = struct('CPoles',PlotHandle.Systems(end));
            else
                src = resppack.ltisource(CPoles, 'Name', sprintf('%s: %s',getString(message('Control:systunegui:TGPlotPoles')),Name));
                rX = PlotHandle.addresponse(src);
                % Define characteristics
                chars = src.getCharacteristics('pzmap');
                rX.setCharacteristics(chars);
                rX.DataFcn = {'pzmap' src rX};
                rX.setstyle('Color',Color)
                rX.draw;
                % DesignWaveforms
                DesignWaveforms = struct('CPoles',rX);
            end
        end

        function Type = getComparisonStyleType(~)
            Type = 'Color';
        end

    end

    methods (Hidden)

        function validateGoal(this,~)
            if this.MinDecay>=this.MaxFrequency
                error(message('Control:systunegui:PoleMinMax'))
            end
        end

        function SPEC = getSpecData(this,SPEC,bNames)
            % Builds standardized requirement description for SYSTUNE.
            MinDecay = this.MinDecay; %#ok<*PROPLC>
            if MinDecay>=this.MaxFrequency
                error(message('Control:tuning:PoleReq3',getID(this)))
            end
            SPEC.Type = 0;
            SPEC.Band = [0,Inf];
            SPEC.Stabilize = (MinDecay>=0);
            SPEC.Spectral = struct('MinDecay',MinDecay,...
                'MinDamping',this.MinDamping,'MaxFrequency',this.MaxFrequency);
            % Identify block
            iB = localResolveName(this.Block,bNames);
            if ~isscalar(iB)
                error(message('Control:tuning:PoleReq4',getID(this),this.Block))
            end
            SPEC.Model = iB;
            SPEC.Config = 0;
        end

    end

    methods (Access = protected)

        function checkMinDecay(~,Value)
            % Overloaded by subclass
            if ~(isnumeric(Value) && isscalar(Value) && ...
                    isreal(Value) && Value<Inf)
                error(message('Control:tuning:ControllerPoleReq1'))
            end
        end

    end


end


function iB = localResolveName(BlockID,BlockNames)
% Resolve block identifier against list of block names

% Simulink identifiers may contain /, blanks, etc. Turn such identifiers
% into valid block names in the same way this is done in slTuner.
BlockID = ltipack.createVarName(BlockID);

% Now look for exact match
iB = find(ltipack.strcmpEnd(BlockID,BlockNames));
end

