function dummy = importdata(LoopData,P,H,F,C)
%IMPORTDATA  Imports plant and compensator model data.
%
%   The plant P, sensor H, prefilter F, and compensator C are 
%   specified as structures with fields Name (model name) and
%   Model (model data). To skip a particular component, set its
%   value to [].
% 
%   Note: This function assumes that the data 

%   Author(s): P. Gahinet
%   Copyright 1986-2000 The MathWorks, Inc.
%   $Revision: 1.14 $  $Date: 2000/08/16 20:03:50 $

% REVISIT
dummy = [];

% Check that data is valid (may throw an error)
% REVISIT: should return [P,H,F,C]=checkdata(...
ImportData = checkdata(LoopData,{P,H,F,C});
[P,H,F,C] = deal(ImportData{:});

% Create @MODELDATA object for first import
if isempty(LoopData.Plant)
    LoopData.Plant = sisodata.modeldata;
    LoopData.Sensor = sisodata.modeldata;
    LoopData.PreFilter = sisodata.modeldata;
end

% Import data (error free)
LocalImportModel('plant',LoopData.Plant,P);
LocalImportModel('sensor',LoopData.Sensor,H);
LocalImportModel('prefilter',LoopData.PreFilter,F);
LocalImportCompensator(LoopData.Compensator,C);
   

%----------------- Local functions -----------------


%%%%%%%%%%%%%%%%%%%%
% LocalImportModel %
%%%%%%%%%%%%%%%%%%%%
function LocalImportModel(Component,ModelData,ImportData)
% Formats model data for plant, sensor, or prefilter

if isempty(ImportData)
    return
end

ModelData.Name = ImportData.Name;
ModelData.Model = ImportData.Model;

% REVISIT: hack around failure below
%[ModelData.Model,ModelData.Zero,ModelData.Pole,ModelData.Gain] = ...
%    LocalCheckModelData(Component,ImportData.Model);

[z,p,k] = zpkdata(ImportData.Model,'v');
ModelData.Zero = z;
ModelData.Pole = p;
ModelData.Gain = k;


%%%%%%%%%%%%%%%%%%%%%%%%%%
% LocalImportCompensator %
%%%%%%%%%%%%%%%%%%%%%%%%%%
function LocalImportCompensator(CompData,ImportComp)
% Formats model data for plant, sensor, or prefilter

% RE: Two compensator representations are used
%                                 prod(s-zi)
%  1) C(s) = sign_r * K_r * s^m * ----------  when FORMAT = 'rlocus'
%                                 prod(s-pj)
%
%                                 prod(1-s/zi)
%  2) C(s) = sign_b * K_b * s^m * ------------  when FORMAT = 'bode'
%                                 prod(1-s/pj)
%
%                                         prod(1-(z-1)/(zi-1))
%     (or C(z) = sign_b * K_b * (z-1)^m * --------------------
%                                         prod(1-(z-1)/(pi-1))
%
%  The fields GainSign and GainMag store sign_* and K_*

% ZPK data
[z,p,k,Ts] = zpkdata(ImportComp.Model,'v');

% Detect notch components
[z,p,zn,pn] = LocalFindNotch(z,p,Ts);

% Real poles and zeros
pr = p(~imag(p),:);
zr = z(~imag(z),:);

% Complex poles and zeros
pc = p(imag(p)>0,:);
zc = z(imag(z)>0,:);

% Adjust length of PZ group list (reuse existing groups)
Nc = length(CompData.PZGroup);
Nf = length(pr)+length(zr)+length(pc)+length(zc)+size(zn,2);
% Delete extra groups
if Nc>Nf,
    delete(CompData.PZGroup(Nf+1:Nc));
    CompData.PZGroup = CompData.PZGroup(1:Nf,:);
end
% Add new groups
for ct=1:Nf-Nc,
    CompData.PZGroup = [CompData.PZGroup ; sisodata.pzgroup];
end

% Update PZ groups
N = 0;
for ct=1:length(zr)
    set(CompData.PZGroup(N+ct),'Type','Real','Zero',zr(ct),'Pole',zeros(0,1));
end
N = N + length(zr);
for ct=1:length(pr)
    set(CompData.PZGroup(N+ct),'Type','Real','Zero',zeros(0,1),'Pole',pr(ct));
end
N = N + length(pr);
for ct=1:length(zc)
    set(CompData.PZGroup(N+ct),'Type','Complex',...
        'Zero',[zc(ct);conj(zc(ct))],'Pole',zeros(0,1));
end
N = N + length(zc);
for ct=1:length(pc)
    set(CompData.PZGroup(N+ct),'Type','Complex',...
        'Zero',zeros(0,1),'Pole',[pc(ct);conj(pc(ct))]);
end
N = N + length(pc);
for ct=1:size(zn,2)
    set(CompData.PZGroup(N+ct),'Type','Notch',...
        'Zero',zn(:,ct),'Pole',pn(:,ct));
end

% Compensator data
CompData.Name = ImportComp.Name;
CompData.Gain = struct('Sign',sign(k)+(~k),'Magnitude',abs(k));
CompData.Ts = Ts;

% Adjust gain data for current format
CompData.Gain = convertgain(CompData,'ZeroPoleGain',CompData.Format);


%%%%%%%%%%%%%%%%%%
% LocalFindNotch %
%%%%%%%%%%%%%%%%%%
function [z,p,zn,pn] = LocalFindNotch(z,p,Ts)
% Detects notch filters in imported compensator
NearTol = sqrt(eps);
nz = length(z);
np = length(p);

% Get natural freq. and damping
[wz,zetaz] = damp(z,Ts);
[wp,zetap] = damp(p,Ts);

% Sort Wn
zeta = [zetaz;zetap];
idx = [1:nz,nz+1:nz+np]';
[wn,is] = sort([wz;wp]);
idx = idx(is,:);
zeta = zeta(is,:);

% Find isolated groups of four roots with same wn
nr = nz+np;
delta = [(abs(wn(2:nr,:)-wn(1:nr-1,:))<NearTol*wn(1:nr-1,:));0];
isNotchSeed = ...
    [delta(1:nr-3,:) & delta(2:nr-2,:) & delta(3:nr-1,:) & ~delta(4:nr,:) ; zeros(3,1)];

% Such groups with two poles and two zeros qualify as notches
isZero = (idx<=nz);
isPole = (idx>nz);
isPZPair = (filter([1 1 1 1],[1 0 0 0],isZero)==2 & ...
    filter([1 1 1 1],[1 0 0 0],isPole)==2);
isNotchSeed = isNotchSeed & [isPZPair(4:nr,:);zeros(3,1)];

% Check compatibility of damping (|zetaz|<|zetap|)
for k=find(isNotchSeed)',
    zetaz = zeta(k-1+find(isZero(k:k+3)));
    zetap = zeta(k-1+find(isPole(k:k+3)));
    isNotchSeed(k) = (abs(zetaz(1))<=abs(zetap(1)));
end

% Extract notches
idxn = find(isNotchSeed);
zn = zeros(2,length(idxn));
pn = zeros(2,length(idxn));
for ct=1:length(idxn),
    % Position of notch poles and zeros in Z and P
    k = idxn(ct);
    idxz = idx(k-1+find(isZero(k:k+3)));
    idxp = idx(k-1+find(isPole(k:k+3)))-nz;
    % Extract notch
    zk = z(idxz(1));
    pk = p(idxp(1));
    zn(:,ct) = real(zk) + [1i;-1i] * abs(imag(zk));
    pn(:,ct) = real(pk) + [1i;-1i] * abs(imag(pk));
end

% Delete notch roots from Z and P
isNotch = zeros(nr,1);
isNotch([idxn;idxn+1;idxn+2;idxn+3],:) = 1;
z(idx(isZero & isNotch),:) = [];
p(idx(isPole & isNotch)-nz,:) = [];
    
