classdef (Hidden = true) piddataP2 < ltipack.piddata2 & ltipack.parallelPID
    % Class definition for @piddataP2 (pid2 object data)
    
    %   Author(s): B. Singh
    %   Copyright 2009-2015 The MathWorks, Inc.
    
    % Formula:
    %                            1                    s
    %   u = Kp * (b*r-y) + Ki * --- * (r-y) + Kd * -------- * (c*r-y)
    %                            s                  Tf*s+1
    % TF representation:
    %   u = [(Kp*b + Ki/s + Kd*c*s/(Tf*s+1)) -(Kp + Ki/s + Kd*s/(Tf*s+1))]*[r;y]
    %
    % Discretization means replace integrators above with discretizers
    %   for 'ForwardEuler':     1/s --> Ts/(z-1)
    %   for 'BackwardEuler':    1/s --> Ts*z/(z-1)
    %   for 'Trapezoidal':      1/s --> Ts*(z+1)/2/(z-1)
    % Delay structure is initialized as 0 and remains 0
    
    properties
    end
    
    %% Public methods
    methods
        
        % constructor
        function this = piddataP2(Kp,Ki,Kd,Tf,b,c,Ts)
            if nargin == 7
                % parameters
                this.Kp = Kp;
                this.Ki = Ki;
                this.Kd = Kd;
                this.Tf = Tf;
                this.b = b;
                this.c = c;
                this.Ts = Ts;
                this.Delay = ltipack.utDelayStruct(1,2,false);
            end
        end
                       
        function PID2 = pid2(PID2,Options)
            % 2dof-Parallel to 2dof-parallel conversion.
            % When specified, OPTIONS is a struct/object with fields IFormula and DFormula.
            % Changing IFormula/DFormula in 1-dof PID only affect Kp and Tf
            % For 2-dof PID case, therefore, Kp, Tf and b are affected. No change in c
            Ts = PID2.Ts;
            if nargin>1 && Ts~=0
                [IF,DF,newForm] = ltipack.piddata.getTargetFormulas(PID2.IFormula,PID2.DFormula,Options);
                if newForm
                    Kp = PID2.Kp;
                    Ki = PID2.Ki;
                    Kd = PID2.Kd;
                    Tf = PID2.Tf;
                    IFormula = PID2.IFormula;
                    DFormula = PID2.DFormula;
                    B = PID2.b;
                    C = PID2.c;
                    
                    [Kp1,~,~,~] = PID2.utChangeIFDF(Kp*B,Ki,Kd*C,Tf,Ts,IFormula,DFormula,IF,DF);
                    [Kp2,Ki2,Kd2,Tf2] = PID2.utChangeIFDF(Kp,Ki,Kd,Tf,Ts,IFormula,DFormula,IF,DF);
                    
                    PID2.Kp = Kp2;
                    PID2.Ki = Ki2;
                    PID2.Kd = Kd2;
                    PID2.Tf = Tf2;
                    if Kp2==0
                        PID2.b = 1;
                    else
                        PID2.b = Kp1/Kp2;
                    end
                    
                    PID2.IFormula = IF;
                    PID2.DFormula = DF;
                end
            end
        end
        
        function PIDS2 = pidstd2(PID2,varargin)
            % 2dof-Parallel to 2dof-Standard conversion
            % When specified, OPTIONS is a struct/object containing the fields IFormula and DFormula.
            % In 2-dof PID parallel -> standard conversion does not affect b,c
            Ts = PID2.Ts;
            % convert to PID2 with desired formulas
            PID2 = pid2(PID2,varargin{:});
            % transform to PIDSTD coefficients, Paralle <--> Standard conversion does not affect b,c
            [Kp,Ti,Td,N] = ltipack.piddata.convertPIDF('Parallel','Standard',PID2.Kp,PID2.Ki,PID2.Kd,PID2.Tf); %#ok<*PROP>
            % return piddataS2
            PIDS2 = ltipack.piddataS2(Kp,Ti,Td,N,PID2.b,PID2.c,Ts);
            if Ts~=0
                PIDS2.IFormula = PID2.IFormula;
                PIDS2.DFormula = PID2.DFormula;
            end
        end
        
        function PID = make1DOF(PID2)
            % 2-dof -> 1-dof conversion by removing b,c
            Ts = PID2.Ts;
            % return piddataP
            PID = ltipack.piddataP(PID2.Kp,PID2.Ki,PID2.Kd,PID2.Tf,Ts);
            if Ts~=0
                PID.IFormula = PID2.IFormula;
                PID.DFormula = PID2.DFormula;
            end
        end
        
        function [c2,cf,F] = get2DOFComponents(PID2)
            % Split 2-DOF PID controller into two parts based on different
            % loop structures
            
            Ts = PID2.Ts;
            Kp = PID2.Kp;
            Ki = PID2.Ki;
            Kd = PID2.Kd;
            Tf = PID2.Tf;
            IF = PID2.IFormula;
            DF = PID2.DFormula;
            B = PID2.b;
            C = PID2.c;
            
            % c2 is the 1-dof PID controller (b*Kp + Ki/s + c*Kd*D(s))
            c2 = ltipack.piddataP(Kp*B,Ki,Kd*C,Tf,Ts);
            
            % cf is the feedforward controller (Kp*(b-1) + Kd*(c-1)*D(s))
            cf = ltipack.piddataP(Kp*(B-1),0,Kd*(C-1),Tf,Ts);
            
            if Ts~=0
                c2.IFormula = IF;
                c2.DFormula = DF;
                cf.IFormula = IF;
                cf.DFormula = DF;
            end
            
            c2 = zpk(c2); cf = zpk(cf);
            
            % F is the pre-filter (Kp*b + Ki/s + Kd*c*D(s))/(Kp + Ki/s + Kd*D(s))
            % Compute in zpk form to explicitly cancel poles, zeros
            C2 = zpk(PID2);
            
            zNum = [C2.z{1};C2.p{2}];
            zDen = [C2.z{2};C2.p{1}];
            kNum = C2.k(1);
            kDen = -C2.k(2);
            
            commonRoots = intersect(zNum,zDen);
            zNum = setdiff(zNum,commonRoots,'rows');
            zDen = setdiff(zDen,commonRoots,'rows');
            F = ltipack.zpkdata({zNum},{zDen},kNum/kDen,Ts);
            
        end

        function D = scaleTime(D,sf)
            % Time vector scaling tnew = sf * told
            
            % scale Delay and Ts
            D = scaleTime@ltipack.ltidata(D,sf);
            % scale controller coefficients
            D = scaleTime@ltipack.parallelPID(D,sf);
        end
        
        function [P,I,D,T,b,c] = utGetInternalParams(this)
            % Return internal parameters P,I,D,T,b,c
            [P,I,D,T] = utGetInternalParams@ltipack.parallelPID(this);
            b = this.b;
            c = this.c;
        end
    end
    
    methods(Static)
        function D = array(size)
            % Create a pid2 array of a given size
            D = ltipack.piddataP2.newarray(size);
        end
        
    end
    
end