function [tfNewFunction,rMassGain,rDampingGain,rSpringGain]...
    = get2Zeros(tfOldFunction)
%
%   [tfNewFunction,rMassGain,rDampingGain,rSpringGain]...
%           = get2Zeros(zOldFunction)
%
%   This function removes two zeros from a transfer function, favouring
%   complex zeros.  It returns the transfer function with the zeros removed
%   and a mass gain, damping gain and spring gain such that:
%
%   (rMassGain*s^2 + rDampingGain*s + rSpringGain)*tfNewFunction =
                                                       tfOldFunction
%   Project: IMPACT
%   Title of Code: get2Zeros
%   Relevant additional documentation: N/A
%   Developer: Carlos Gonzalez
%   External Dependencies: NA
%
%   Disclaimer
%   CAUTION: It is strongly recommended that changes to this code
%   should be avoided without first consulting with SgurrControl
%   and acquiring permission. SgurrControl is not liable for any
%   damage caused as a result of unauthorised changes to this
%   document.


% Set up
[arZeros,arPoles,arGain]=zpkdata(tfOldFunction,'v');
iCntZeros = 1;

if isempty(arZeros)
    tfNewFunction = tfOldFunction;
    rMassGain = 0;
    rDampingGain = 0;
    rSpringGain = 1;
else
    % If not all the zeros are real then find the first non-real ones
    if ~isreal(arZeros)
        
        while isreal(arZeros(iCntZeros))
            iCntZeros = iCntZeros + 1;
        end
        
        % Pre-allocate
        arIndex = 1:numel(arZeros);
        arIndex(iCntZeros)=[];
        
        % Find the relative conjugate pair and remove them, separating them
        % into the separate gains
        [~, minIndex] = min(abs(abs(arZeros(iCntZeros))*...
            exp(-1i*angle(arZeros(iCntZeros))) - arZeros(arIndex)));
        
        arNewZeros = arZeros;
        arNewZeros([iCntZeros,arIndex(minIndex)])=[];
        
        tfNewFunction = zpk(arNewZeros,arPoles,arGain);
        rMassGain = 1;
        rDampingGain = -2*real(arZeros(iCntZeros));
        rSpringGain = abs(arZeros(iCntZeros))^2;
        
    else
        
        % remove the first two zeros and separate them out
        arNewZeros = arZeros;
        arNewZeros([1,2])=[];
        tfNewFunction = zpk(arNewZeros,arPoles,arGain);
        rMassGain = 1;
        rDampingGain = -2*(arZeros(1) + arZeros(2));
        rSpringGain = arZeros(1)*arZeros(2);
        
    end
end
