% DARM Open Loop Simulator

%paramters to fit
GW_K=-0.232;
al_gain=1.2;


f = logspace(-2,4,1000);
w = 2*pi*f;

Ts=1/2^14;

%[z,p,k] = cheby1(2,2,2*pi*30,'s');
%FM1 = zpk(p,-2*pi*[0 0],1.99078);
FM1=zpk([-75.757+i*153.31;-75.757-i*153.31],[0;0],1.99078);


%[z,p,k] = butter(2,2*pi*1000,'s');
%FM2 = zpk([-2*pi*10 -2*pi*100],p,1000);
%[z,p,k] = cheby1(2,3,2*pi*1000,'s');
%FM2 = zpk(-2*pi*[10 100],p,1000/pi*10^(6.83/20));
FM2=zpk([-62.8318;-628.241],[-4389.6-i*4389.6;-4389.6+i*4389.6],976.28);

%[z,p,k] = butter(2,2*pi*2000,'s');
%FM3 = zpk(z,p,k);
FM3=zpk([],[-8485+i*8485;-8485-i*8485],1.43991e+08);

%FM4 = zpk(-2*pi*20,-2*pi*1,1);
FM4=zpk([-125.663],[-6.28319],0.996375);

%tw0 = resgain(6.25,30,0.1);
%tw1 = resgain(12.25,50,0.05);
%tw2 = resgain(29.75,50,0.1);
%FM6 = tw0 * tw1 * tw2;



%[b,z,k] = cheby1(2,2.5,2*pi*30,'s');
%[b,p,k] = cheby1(2,2.5,2*pi*5,'s');
%FM7 = zpk(z,p,1);

FM8 = zpk([-6207.83;-6207.84],...
	  [-25045.8+i*0.00338783;-25045.8-i*0.00338783],16.2775);

%[z,p1,k] = cheby1(2,2,2*pi*5,'s');
%[z,p2,k] = cheby1(2,2,2*pi*50,'s');
%FM9 = zpk(p2,p1,1); 
FM9=zpk([-126.26+i*255.513;-126.26-i*255.513],...
	[-12.6263+i*25.552;-12.6263-i*25.552],1.00005);


DARMDF= FM1 * FM2 * FM3 * FM8 * FM9;
%DARMDF = c2d(DARMDF,Ts,'tustin');

%ETM filters
violin = zpk([0+i*2152.47;0-i*2152.47;0+i*2156.2;0-i*2156.2], ...
    [-10.7623+i*2152.44;-10.7623-i*2152.44;-10.781+i*2156.18;- ...
     10.781-i*2156.18],1);


TestMass1 =    zpk([-0.286381-i*42459.4;-0.286381+i*42459.4; ...
		    0.445078+i*42878.3;0.445078-i*42878.3;-0.59542+i*43367; ...
    -0.59542-i*43367;0.616056-i*43747.2;0.616056+i*43747.2], ...
    [-13087.9+i*37719;-13087.9-i*37719;-2587.62-i*40832.8;-2587.62+ ...
     i*40832.8;-2840.14-i*44817.4; ...
    -2840.14+i*44817.4;-14915-i*42984.7;-14915+i*42984.7], ...
		   0.894608);

TestMass2=zpk([0.102758-i*42236.9;0.102758+i*42236.9;-0.282105-i* ...
	       43936.2;-0.282105+i*43936.2], ...
    [-536.533-i*41352.3;-536.533+i*41352.3;-578.742-i*44605.7;- ...
     578.742+i*44605.7],1.00821);


%ETMDF=violin * TestMass1 * TestMass2;
ETMDF=violin;

%ETMDF=c2d(ETMDF,Ts,'tustin');

% The Total Digital Compensator
%DF =  DARMDF * ETMDF;

% Input Matrix
asq2darm = 0.014;

% Gain Knob
%GW_K = -0.187838;
%GW_K = -0.25;

% Output Matrix
darm2lm = -0.33;

% Snubber
snubber = 0.63; %fudge factor to fit DARM gain of 1.72 nm/count

%Pendulum
Q = 10;
w0 = 2*pi*0.76; % Angular frequency

% Pendulum TF (factor 2 to account for Ly-Lx)
pendTF = 2 * tf(1,[1 w0/Q w0^2]); 

WhiteGain = 18;
WhiteGain = 10^(WhiteGain/20);

wfb = 1;
WhiteFilt = zpk(-2*pi*[15 15],-2*pi*[150 150],(150/15)^2);
AntiWhiteFilt = zpk(-2*pi*[150 150],-2*pi*[15 15],(15/150)^2);

% Arm Cavity
fc = 87.3; % Hz
armTF = 2*pi*fc * tf(1,[1 2*pi*fc]);

opgain = 0.47e7; %Optical Gain in Amps / Meter
opgain=opgain*al_gain;

% LSC PD TF
Zrf = 400;
Grf = 10;
%pdTF = Grf*Zrf*tf(2*pi*2.5e6,[1 2*pi*2.5e6]);
pdTF = Zrf*Grf;


% Anti-Aliasing Filter
[z,p,k] = ellip(8,0.035,80,2*pi*7570,'s');
aaf = zpk(z,p,k);

% Anti-Imaging Filter
[z,p,k] = ellip(4,4,60,2*pi*7570,'s');
aif = zpk(z,p,k*10^(4/20));

% ADC Time Delay
adc_delay = 50e-6;

% DAC Time Delay
dac_delay = 75e-6;

time_delay = adc_delay + dac_delay;

% Makes the State-Space object from the .mdl
%[A,B,C,D] = linmod('darm_09');
%warning off
%darmsys = ss(A,B,C,D);