function fitTFbypeaks();
% fit TFs by finding peaks(zeros/poles).
% use this for obtaining zpk for mechanical TFs for Optickle model
% needs (1) parambLCGT.m            for acquiring suspension data file names
% Author: Yuta Michimura

dof={'pitch','yaw'};

for dd=1:2
    p=parambLCGT('positive',dof{dd});
    data={p.TM_Hact_TFdata
          p.BS_Hact_TFdata
        p.RC_Hact_TFdata};
    for kk=1:length(data)
        data{kk}
        [ZQ,PQ,g]=fitTFbypeaks0(data{kk});
        title(data{kk},'interpreter','none');
        ZQ(:,1)
        PQ(:,1)
        g
    end
end

function [ZQ,PQ,g]=fitTFbypeaks0(TFdata);

data=importdata(TFdata);
H=squeeze(data(:,2).*exp(i.*data(:,3)/180*pi));
freq=data(:,1);
absH=abs(H);
iZ=[];
iP=[];
prevkk=1;
thiskk=1;
for kk=1:length(freq)-2  
    if absH(kk)>absH(kk+1) && absH(kk+1)<absH(kk+2)  % find zeros
        iZ=[iZ;kk+1];
        thiskk=kk+1;
    end
    if absH(kk)<absH(kk+1) && absH(kk+1)>absH(kk+2)  % find poles
        iP=[iP;kk+1];
        thiskk=kk+1;
    end
    if prevkk ~= thiskk
        prevkk=thiskk;
        if length(iZ)*length(iP) ~=0
            if abs(freq(iP(end))/freq(iZ(end))-1)<0.01 || abs(absH(iP(end))/absH(iZ(end))-1)<0.5  % ignore minor peaks
                iZ=iZ(1:end-1);
                iP=iP(1:end-1);
            end
        end
    end
end
ZQ=[];
PQ=[];
for kk=1:length(iZ);
    if freq(iZ(kk)) < 0.04
        ZQ=[ZQ;freq(iZ(kk)) 0.5e1];  % Q for peaks below 0.04 Hz
    else
        ZQ=[ZQ;freq(iZ(kk)) 4e2];  % Q for all the other peaks
    end
end
for kk=1:length(iP);
    if freq(iP(kk)) < 0.04
        PQ=[PQ;freq(iP(kk)) 0.5e1];  % Q for peaks below 0.04 Hz
    else
        PQ=[PQ;freq(iP(kk)) 4e2];  % Q for all the other peaks
    end
end

Hfit=filterdesign(freq,ZQ,PQ,1);
g=abs(H(end))/abs(Hfit(end));        % fit gain at freq(end)
if abs(g*Hfit(1))/abs(H(1))<0.1      % if # of poles are smaller than # of zeros, fitting doesn't go correctly
    ZQ=ZQ(2:end,:);                  % so, just ignore the lowest zero
    Hfit=filterdesign(freq,ZQ,PQ,1);
    g=abs(H(end))/abs(Hfit(end));
end

figure();
plotbode(freq,H,g*Hfit);
legend('data','fit')