function [varargout] = LSCmodel(varargin)
% LSCmodel   -------------------------------------------------------%%%
% Evaluates a model of one of the interferometers LSC feedback loops %%
% Usage:                                                             %%
%        [openloopgain, sensing, actuation, response]                %%
%        = LSCmodel(IFOin,'darm',f,ugf)                              %%
%                                                                    %%
%                                                                    %%
%      Returns the open loop gain (OLG, unitless),                   %%
%      Sensing function (SENSE, in counts/meter), etc.               %%
%       each in a 2 column array, where                              %%
%         the first column is the frequency vector (real) and        %%
%         the second column is the response vector (complex)         %%
%                                                                    %% 
%     the response function R is in strain/count                     %%
%        i.e.  h(f) = AS_Q(f) * R(f)                                 %%
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - -
%  v 1.1.0   Aug  4th, 2003                   
%  v 1.1.1   Aug 14th, 2003  - Some comments added
%  v 1.2.2   Aug 17th, 2003  - Added SUS LSC filters 
%  v 1.2.3   Aug 25th, 2003  - added ugf as 4th varargin
%                              + fixed x2 bug in 'hd' calculation
%-------------------------------------------------------------------%%%

global lsc


%----- Parse input arguments -----------------------------%%%%
if nargin < 1
  error('Not enough inputs: specify IFO params!')

elseif nargin > 0
  IFOin = varargin{1};
  loop_name = 'darm';       % default loop is DARM
  lsc = eval(strcat('IFOin.',loop_name));
  ff = logspace(log10(lsc.fl),log10(lsc.fu),lsc.npt);
  
  if nargin > 1
    loop_name = varargin{2};
  
    if nargin > 2
      ff = varargin{3};
      fdim = size(ff);
      if fdim(1) > 1 & fdim(2) == 1
        ff = ff';
      end
      
        if nargin > 3                % 4th argument is UGF
          lsc.ugf = varargin{4};
        end
    end
  end
  
else
  error('%s','Invalid Number of Input Arguments')
end
%---------------------------------------------------------%%%%%



switch upper(loop_name)
 case 'CM'
  module_name = 'LSC';
 otherwise
  module_name = loop_name;
  mcld = zeros(size(ff))';
  aod = zeros(size(ff))';
end

n = 1:length(ff);

if max(ff) < lsc.ugf
   warning('Specified frequencies are below the UGF')
   ff_low = log10(max(ff)+1);
   ff_higher = log10(lsc.ugf*3);
   ff_plus = logspace(ff_low,ff_higher,301);
   ff = [ff ff_plus];
   quack = 1;
end


  
ugf = lsc.ugf;
iugf = min(find(ff>ugf));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%     Analog portion of loop               %%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lscpend = pendulum(lsc);

fc = lsc.cavpole;          % cavity pole
cavpole = zpk([], -2*pi*fc, 2*pi*fc);

aa = IFOin.misc.aa;
ai = IFOin.misc.ai;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% Time Delay  %%%%%%%%%%
[num,den] = pade(lsc.tdelay,4);
tdelay = tf(num,den);

%%%%%%%%%% DAC sample-and-hold  %%%%%%%%%%%%%%%%%%%%
fs = lsc.fs;
d2a = (sin(pi*ff/fs)./(pi*ff/fs)).*exp(-i*pi*ff/fs);

%%%%%%%%%% Coil Driver Snubber Circuit  %%%%%%%%%%%%%%%%%%%
snub = snubber(lsc);

%%%%%%%%%%%%  Sensing function   %%%%%%%%%%%%%%%%%%%
sense = cavpole * lsc.hflowpass * aa * lsc.electronics_gain;

%%%%%%%%%%% Actuation function   %%%%%%%%%%%%%%%%%%%
actuation = lscpend * ai * tdelay * snub;

%%%%%%%%%  compute freq response vectors  %%%%%%%%%%
hsense = squeeze(freqresp(sense,2*pi*ff));
hact = d2a' .* squeeze(freqresp(actuation,2*pi*ff));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%             Digital portion of loop                %%%
%                                                          %%
% First filter file is for the LSC loop (e.g. DARM)        %%
% The 2nd & 3rd are for the SUS modules (e.g. ETMX & ETMY) %%
% It still works even there's just one mass (e.g. RM)      %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
filtnums = lsc.digitalfilters;
file = lsc.filterfile;
[sos,dgain,filtname] = ...
    readfilters(file,upper(module_name),filtnums);
clear h
for ii = 1:size(sos,1),
    [b,a] = sos2tf(sos(ii,:));
    h(ii,:) = freqz(b,a,ff,fs);
end
hd = prod(h,1);
hd = shiftdim(hd,1);
hd = dgain * lsc.lsc_gain * lsc.itmtrx * hd;

if ischar(lsc.susfilterfile1)
  filtnums = lsc.susdigitalfilters1;
  file = lsc.susfilterfile1;
  [sos,dgain,filtname] = ...
    readfilters(file,upper('LSC'),filtnums);
  clear h
  for ii = 1:size(sos,1),
    [b,a] = sos2tf(sos(ii,:));
    h(ii,:) = freqz(b,a,ff,fs);
  end
  hd1 = prod(h,1);
  hd1 = shiftdim(hd1,1);
  if ~ischar(lsc.susfilterfile2)
    hact = hact .* hd1;
  end
end

if ischar(lsc.susfilterfile2)
  filtnums = lsc.susdigitalfilters2;
  file = lsc.susfilterfile2;
  [sos,dgain,filtname] = ...
    readfilters(file,upper('LSC'),filtnums);
  clear h
  for ii = 1:size(sos,1),
    [b,a] = sos2tf(sos(ii,:));
    h(ii,:) = freqz(b,a,ff,fs);
  end
  hd2 = prod(h,1);
  hd2 = shiftdim(hd2,1);
  if ischar(lsc.susfilterfile1)
    hact = hact .* (hd1 + hd2)/2;
  else
    hact = hact .* hd2;
  end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%       Total loop gain                    %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lscolg = hsense .* hact .* hd;
gtemp = interp1(ff([iugf-2:iugf+2]),abs(lscolg([iugf-2:iugf+2])),lsc.ugf);
lscolg = lscolg / gtemp;                   % Sensing gain fudged to
hsense = hsense / gtemp;                   % get the UGF right
response = 1./hsense .* (1 + lscolg);

%%%%%%%%%%%%%%%%% Put returned data into arrays %%%%%%%%%%%
openloopgain = [ff(n)' lscolg(n)];
sensing = [ff(n)' hsense(n)];
actuating = [ff(n)' hact(n)];
response = [ff(n)' response(n)];
%%%%%%  optional AO loop gain for CM servo %%%%%%%%%%%%%%
switch upper(loop_name)
 case 'CM'
  mcl2f = squeeze(freqresp(lsc.mcl2f,2*pi*ff));
  mcld = lscolg .* mcl2f;
  hact = hact .* mcl2f;
  
  ao = zpk(cavpole * lsc.hflowpass * lsc.ao_c);
  aod = squeeze(freqresp(ao,2*pi*ff));
  nn = min(find(ff>ugf));
  ao_fudge = abs(aod(nn));                 % Fudges AO to set the UGF
  aod = aod / ao_fudge;

  nn = min(find(ff>lsc.xover));
  mcl_fudge = abs(mcld(nn)/aod(nn));      % Fudges MCL to set xover freq
  mcld = mcld / mcl_fudge;  
   
  hsense = hsense / mcl_fudge;
  
  lscolg = mcld + aod;
  
  response = 1./hsense .* (1 + lscolg);
  %%%%%%%%%% Put returned data into arrays %%%%%
  openloopgain = [ff(n)' lscolg(n) mcld(n) aod(n)];
  sensing = [ff(n)' hsense(n)];
  actuating = [ff(n)' hact(n)];
  response = [ff(n)' response(n)];

end
%%%%%%%%%%%%%%%%% Output Argument Assignments %%%%%%%%%%%
if nargout < 1
    mybodeplot(openloopgain)
elseif nargout > 0
  varargout(1) = {openloopgain};
  if nargout > 1
    varargout(2) = {sensing};
    if nargout > 2
      varargout(3) = {actuating};
      if nargout == 4
        varargout(4) = {response};
      end
    end
  end 
elseif nargout > 4
  error('Too Many Output Arguments')
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ------------------------------------------------------
% Sub-functions of LSCmodel  -----------------------
% \/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function lscpend = pendulum(lsc)

wp = 2 * pi * lsc.pendf0;
Qp = lsc.pendQ;
sys = tf(wp^2, [1 wp/Qp wp^2]);
DCcal = lsc.DCcal;
lscpend = DCcal * sys;

return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function snub = snubber(lsc)

R_ser = lsc.R_ser;
R_snub = lsc.R_snub;
C_snub = lsc.C_snub;
C_cabl = lsc.C_cabl;
R_coil = lsc.R_coil;
H_coil = lsc.H_coil;

A_snub = R_snub * C_snub;
den_s3 = A_snub*C_cabl*H_coil*R_ser;
den_s2 = R_ser*A_snub*R_coil*C_cabl+R_ser*C_cabl*H_coil+ ...
              R_ser*C_snub*H_coil+A_snub*H_coil;
den_s1 = R_ser*A_snub+C_cabl*R_coil+C_snub*R_coil+...
              A_snub*R_coil+H_coil;
den_s0 = R_coil+R_ser;
TF_gain= R_coil + R_ser;
snub = TF_gain * tf([A_snub 1],[den_s3 den_s2 den_s1 den_s0]);
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [sos,gain,name] = readfilters(file,module,varargin)

ret = 1;
fm = [varargin{:}];
fid = fopen(file);
firstflag = 1;
mlen = length(module);

while 1
  
  tline = fgetl(fid);
  
  if ~ischar(tline), break, end
  
  if strncmp(tline,module,mlen)
    
    arr = strread(tline,'%s','delimiter',' ');
    rfm = str2double(arr(2));
    
    if ismember(rfm,fm)
      if firstflag        
        name = arr(7);
        gain = str2double(arr(8));
        coef = str2double([arr(9) arr(10) arr(11) arr(12)]);
	firstflag = 0;
      else
        name = strcat(name,'/',arr(7));
        gain = gain*str2double(arr(8));
        coef = [coef str2double([arr(9) arr(10) arr(11) arr(12)])];
      end
      
      nsos = str2double(arr(4));
      
      if nsos > 1
        for ksos=1:nsos-1
	       tline = fgetl(fid);
	       arr = strread(tline,'%s','delimiter',' ');
	       coef = [coef str2double([arr(1) arr(2) arr(3) arr(4)])];
	    end;
      end
    end;
  end
end
fclose(fid);
g = coef;
dim = length(g);
n2b = dim/4;
soscoef = [];

for i = 1:n2b,
   a = [1, g(1+(i-1)*4), g(2+(i-1)*4)];
   b = [1, g(3+(i-1)*4), g(4+(i-1)*4)];
   soscoef = [soscoef; b(1) b(2) b(3) a(1) a(2) a(3)];
end
sos = soscoef;
return
%%%%%%% ***************************************************************