function [A,B,RAB,Hfit,iter] = sti(w,H,nA,nB,maxiter,tol,W)

% STI   Frequency domain transfer function reduction
%    [A,B] = STI(W,H,NA,NB) fits to an experimental transfer function H(W),
%    W in rad/s, a rational function of s B(s)/A(s) with degrees (NB-1)/NA.
%    Multi-output transfer functions are represented by a multi-column H.
%    [A,B,RAB,HFIT,ITER] = STI(W,H,NA,NB,MAXITER,TOL,WG) specifies the maximum number
%    of iterations, the convergence tolerance and a weighting vector.
%    RAB returns the covariance matrix of [A(2:NA+1) B(1,:) B(2,:) ...] and
%    HFIT the fitted transfer function.
%    Note: STI uses POLYVAL giving the coefficient in the descending order, e.g.
%    choosing NA = 1 and NB = 1 gives a transfer function as
%    H(s) = B[1]/(A[2]+s*A[1])
%    equivalent to
%    H(W) = B[1]/(A[2]+j*W*A[1])
%
% [+GenLib General Purpose Library+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.

% default arguments and argument size
if nargin < 5, maxiter = 20; end
if nargin < 6, tol = 1e-4; end
if nargin < 7, W = 1; end
[nw,nH] = size(H);
if length(w) ~= nw, error('Frequency and transfer function size mismatch'), end
n = max([nA nB-1 1]); nth = nA + nB*nH;
if nth > 10, ktrace = 1:nA; else, ktrace = 1:nth; end

% normalise w
wnorm = max(w(:)); w = w/wnorm;
z = repmat(j*w(:),[1,n]).^repmat(1:n,[nw,1]);

% initial A guess and preallocation
A = [zeros(1,nA) 1]; th = zeros(nth,1);
phi = zeros(nH*nw,nth); phy = zeros(nH*nw,1);

% iterate
G = Inf; iter = 0;
while iter < maxiter & G > tol
 iter = iter + 1;

 % fitting
 Aw = W ./ polyval(A,z(:,1)); % weights
 for kH = 1:nH
  kk = (kH-1)*nw+1:kH*nw;
  phi(kk,nA+(1+(kH-1)*nB:kH*nB)) = repmat(Aw,[1,nB]) .* [-z(:,nB-1:-1:1) -ones(nw,1)];
  if nA
   phi(kk,1:nA) = repmat(Aw,[1,nA]) .* [repmat(H(:,kH),[1,nA-1]).*z(:,nA-1:-1:1) H(:,kH)];
   phy(kk) = -Aw .* H(:,kH) .* z(:,nA);
  else
   phy(kk) = -Aw .* H(:,kH);
  end
 end
 thnew = real(phi'*phi) \ real(phi'*phy); % \ to use Cholesky
 G = norm(th - thnew);

 % trace
 clc
 disp('iteration = '), disp(iter)
 disp('norm of difference = '), disp(G)
 disp('prev    new'), disp([th(ktrace) thnew(ktrace)])

 % new A coefficient
 th = thnew; A = [1;th(1:nA)]';
 
end

% B coefficients dispatching and fitted H
B = zeros(nH,nB); Hfit = zeros(nw,nH);
Aw = 1. ./ polyval(A,z(:,1));
for kH = 1:nH
 B(kH,:) = th(nA+(1+(kH-1)*nB:kH*nB))';
 Hfit(:,kH) = polyval(B(kH,:),z(:,1)) .* Aw;
end

% covariance
phy = phi*th - phy;
RAB = real(phy'*phy)/nw/nH * (real(phi'*phi) \ eye(nth)); % \ to use Cholesky

% denormalise
A = A .* wnorm.^(0:nA); B = B .* repmat(wnorm.^(nA-nB+1:nA),[nH,1]);
Aw = 1:nA;
for kH = 1:nH, Aw = [Aw nA-nB+1:nA]; end
Aw = wnorm.^Aw;
RAB = RAB .* repmat(Aw,[nth,1]) .* repmat(Aw',[1,nth]); 
