function tfModel = fitTf(arFrequencies,arRealComponent,...
    arImaginaryComponent,iOrderNumerator,iOrderDenominator,varargin)

% function tfModel = fitTF(arFrequencies,arRealComponent,...
% arImaginaryComponent,iOrderNumerator,iOrderDenominator)
%   
%   This function fits the complex data with real and imaginary components
%   arRealComponent and arImaginaryComponent(R-real,I-imaginary) at the 
%   frequencies arFrequencies to a transfer function with numerator order
%   iOrderNumerator and denominator order iOrderDenominator.
%   The poles and zeros of the output transfer function (Q) are on the left
%   hand side plane.
%
%   SgurrControl 2016
%   
%   Project: IMPACT
%   
%   Title of Code: fitTf
%   
%   Relevant additional documentation: The method followed is that
%   described in https://www.researchgate.net/publication/228628601 The A,
%   B, C, and D matrices refer to those in said document, as do the e and g
%   subvectors.  The counters a, b, c, and l have also been kept with their
%   names from the document.
%   
%   Developer: Carlos Gonzalez
%   
%   External Dependencies: NA
%
%   SgurrControl 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.

iNumFreq = numel(arFrequencies);
rWeights = ones(1,iNumFreq);
arSolution = zeros(iOrderNumerator+1+iOrderDenominator,1);
iCntLoop = 1;
rError = inf;
if nargin>5
    bOnlyLHPZeros = varargin{1};
else
    bOnlyLHPZeros = 1;
end

warning('off','MATLAB:nearlySingularMatrix');

while rError > 1e-4 && iCntLoop < 100
    
    M = zeros(iOrderNumerator + iOrderDenominator + 1,...
        iOrderNumerator + iOrderDenominator + 1);
    
    V = zeros(iOrderNumerator + iOrderDenominator + 1,1);
    
    % A submatrix
    for l = 0:iOrderNumerator
        
        for c = 0:iOrderNumerator
            
            M(l + 1, c + 1) = sum((-real((1j*arFrequencies).^l).*real((1j*arFrequencies).^c)...
                - imag((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c)).*rWeights);
        
        end
    end
    
    % B submatrix
    for l = 0:iOrderNumerator
        
        for c = 1:iOrderDenominator
            
            M(l + 1, iOrderNumerator + c + 1) = sum((real((1j*arFrequencies).^l).*real((1j*arFrequencies).^c).*arRealComponent...
                + imag((1j*arFrequencies).^l).*real((1j*arFrequencies).^c).*arImaginaryComponent...
                - real((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c).*arImaginaryComponent...
                + imag((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c).*arRealComponent).*rWeights);
        
        end
    end
    
    % C submatrix
    for l = 1:iOrderDenominator
        
        for c = 0:iOrderNumerator
            
            M(iOrderNumerator + l + 1, c + 1) =...
                sum((-real((1j*arFrequencies).^l).*real((1j*arFrequencies).^c).*arRealComponent...
                + imag((1j*arFrequencies).^l).*real((1j*arFrequencies).^c).*arImaginaryComponent...
                - real((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c).*arImaginaryComponent...
                - imag((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c).*arRealComponent).*rWeights);
            
        end
    end
    
    % D submatrix
    for l = 1:iOrderDenominator
        for c = 1:iOrderDenominator
            
            M(l + iOrderNumerator + 1, c + iOrderNumerator + 1) =...
                sum((real((1j*arFrequencies).^l).*real((1j*arFrequencies).^c)...
                + imag((1j*arFrequencies).^l).*imag((1j*arFrequencies).^c)).*(arRealComponent.^2 + arImaginaryComponent.^2).*rWeights);
        
        end
    end
    
    % e subvector
    for l = 0:iOrderNumerator
        
        V(l + 1,1) = -sum((real((1j*arFrequencies).^l).*arRealComponent+imag((1j*arFrequencies).^l).*arImaginaryComponent).*rWeights);
        
    end
    
    
    % g subvector
    for l = 1:iOrderDenominator
        
        V(l + 1 + iOrderNumerator,1) = -sum(real((1j*arFrequencies).^l).*(arRealComponent.^2 + arImaginaryComponent.^2).*rWeights);
    
    end
       
    arPreviousSolution = arSolution;
    arSolution = M\V;
    
    % Change unstable poles
    arPoles = roots([arSolution(end:-1:iOrderNumerator + 2)',1]);
    
    for iCntPoles = 1:numel(arPoles)
        
        if (real(arPoles(iCntPoles)) > 0)
            
            arPoles(iCntPoles) = -real(arPoles(iCntPoles))...
                + imag(arPoles(iCntPoles))*1j;
        
        end
        
    end
    [arNumerator, arDenominator] = tfdata(zpk([],arPoles,1/abs(arSolution(end))),'v');
    arSolution(end:-1:iOrderNumerator+2) = arDenominator(1:end-1)/arNumerator(end);
    
    % Change positive zeros
    if bOnlyLHPZeros
        arZeros = roots(arSolution(iOrderNumerator + 1:-1:1));
        if(~isempty(arZeros))
            
            for iCntZeros = 1:numel(arZeros)
                
                if (real(arZeros(iCntZeros)) > 0)
                    
                    arZeros(iCntZeros)=-real(arZeros(iCntZeros))...
                        + imag(arZeros(iCntZeros))*1j;
                    
                end
            end
            
            aiIndNoZero = find(arSolution(iOrderNumerator + 1:-1:1)~=0);
            arSolutionNum = arSolution(iOrderNumerator + 1:-1:1);
            rGain = 1/abs(arSolutionNum(min(aiIndNoZero)));
            [arNumerator, arDenominator] = tfdata(zpk([],arZeros,rGain),'v');
            
            aiSolNum = iOrderNumerator + 1:-1:1;
            arSolution(aiSolNum(min(aiIndNoZero)):-1:1) = arDenominator/arNumerator(end);
        end
    end

    rError = sum((arPreviousSolution - arSolution).^2);
    tfModel = tf(1,[arSolution(end:-1:iOrderNumerator + 2)',1]);
    
    [rMagnitude,~,~] = bode(tfModel,arFrequencies);
    rWeights = (reshape(rMagnitude,1,iNumFreq)).^2;
    iCntLoop = iCntLoop + 1;
end
warning('on','MATLAB:nearlySingularMatrix');
tfModel = minreal(tf(arSolution(iOrderNumerator + 1:-1:1)',...
    [arSolution(end:-1:iOrderNumerator + 2)',1]));


