% Set parameters for the actuator TF fit, so that can be done separately.

AorB = 'A';
% srate = 64;

saveplots = 0; % Should we print the plots?  
savefolder = './';
%%

WienerCodeLocation = '/ligo/svncommon/40mSVN/WienerBaseCode/';
addpath(WienerCodeLocation)
clear WienerCodeLocation

%SRCLff  % get data, make TFs
load SRCL_FF_Filter.mat
ffTot = [freq, ffFilt]; % concatenate freq in.
freqidx1 = find(freq > 10 & freq < 13);
freqidx2 = find(freq > 15 & freq < 66);
freqidx3 = find(freq > 67 & freq < 141);
freqidx4 = find(freq > 145 & freq < 215);

newSrate = 1/(freq(2) - freq(1));

freqidx = [freqidx1.', freqidx2.', freqidx3.', freqidx4.'];
ffTot = ffTot(freqidx,:);
%% Filters for prefiltering witness data
% eg. HEPI->DARM TF

useActuatorPreweight = 0;

useCoherenceAbove = 0.95;
NactuatorFit = 5;  %Fit order for vectfit, for actuator TF



%%

WienerFFparams.useActuatorPreweight = useActuatorPreweight; clear useActuatorPreweight
% WienerFFparams.ActuatorDataFolderA  = ActuatorDataFolderA;  clear ActuatorDataFolderA
% WienerFFparams.ActuatorDataFolderB  = ActuatorDataFolderB;  clear ActuatorDataFolderB
WienerFFparams.CohCutoff            = useCoherenceAbove;    clear useCoherenceAbove
WienerFFparams.ActuatorFitOrder     = NactuatorFit;         clear NactuatorFit

WienerFFparams.srate                = newSrate;                clear srate
WienerFFparams.SavePlots            = saveplots;            clear saveplots
WienerFFparams.savefolder           = savefolder;           clear savefolder
%%


%% 
FittedActuator = FitActuatorTFs(WienerFFparams, ffTot, ones(size(ffTot(:,1))),AorB); 

%%
A = full(FittedSS.A);
B = FittedSS.B;
C = FittedSS.C;
D = FittedSS.D;
E = FittedSS.E;

[z,p,k] = zpkdata(ss(A,B,C,D),'v');
z = round(z*1e6)/1e6;  
p = round(p*1e6)/1e6;
% [m0,p0] = bode(zpk(z,p,k),2*pi*0.1); %get parameters at 0.1 Hz

figure(1)
mag = squeeze(freqresp(zpk(z, p, k), logspace(0,4,1000),'Hz'));
subplot(211)
loglog(logspace(0,4,1000), abs(mag), freq(freqidx), abs(ffFilt(freqidx)))
ylabel('Mag')
subplot(212)
semilogx(logspace(0,4,1000), angle(mag)*180/pi, freq(freqidx), angle(ffFilt(freqidx))*180/pi)
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')
legend('Fitted TF', 'Meas TF')

suptitle('SRCL FF filter')
%%

z = cplxpair(z);
p = cplxpair(p);

[z_fq] = GetFreqQ(z);
[p_fq] = GetFreqQ(p);

disp(' ')
disp('SRCL FF Filter')
disp('   Freq       Q')
z_fq
p_fq
k


%% Plot fitted actuator
% 
% figure(502)
% bode(zpk(z, p, k))