clear all

cycleT = 60e-6;

% AI, AA shapes from http://nodus.ligo.caltech.edu:8080/40m/8555
[z,p,k] = ellip(4,4,60,2*pi*7570,'s');
AI = zpk(z,p,k*10^(4/20)) * zpk([],-2*pi*13e3,2*pi*13e3);
AI.OutputDelay = 1*cycleT;

[z,p,k] = ellip(8,0.001,80,2*pi*7570,'s');
AA = zpk(z,p,k*10^(0.001/20)) * zpk([],-2*pi*32768,2*pi*32768);
AA.InputDelay = 1*cycleT;

% Mirror Pendulum
q=5;
w0=2*pi*.97;
pend=zpk([], -(w0/(2*q))*[1+sqrt(1-4*(q^2)),1-sqrt(1-4*(q^2))],1);
pend=pend /abs(freqresp(pend,0)) * db2mag(90);

% Digital FMs
fm1=zpk(-2*pi*[0.357143+1i*0.349927;0.357143-1i*0.349927],...
    -2*pi*[0.000714286+1i*0.000699854;0.000714286-1i*0.000699854],1)*250003;
fm2=zpk(-2*pi*[1.25+1i*4.84123;1.25-1i*4.84123;5],...
    -2*pi*[0.5+1i*2.95804;0.5-1i*2.95804;1],1)*(13.8898);
fm3=zpk(-2*pi*[17.8571+1i*17.4964;17.8571-1i*17.4964],...
    [0.16+1i*3.19599;0.16-1i*3.19599],1)*(60.9508);
fm4=zpk(-2*pi*[8.75+1i*33.8886;8.75-1i*33.8886],-2*pi*[0.5+1i*2.95804;0.5-1i*2.95804],1)*...
    zpk(-2*pi*[3000],-2*pi*[1],1)*...
    zpk(-2*pi*[10;350],-2*pi*[2121.32+1i*2121.32;2121.32-1i*2121.32;700],1)*1360.115;
fm5=zpk(-2*pi*[1],-2*pi*[3000],1);
fm6=zpk(-2*pi*[1.25+1i*4.84123;1.25-1i*4.84123;7.5+1i*29.0474;7.5-1i*29.0474],...
    -2*pi*[2.5+1i*9.68246;2.5-1i*9.68246;0.166+1i*16.5992;0.166-1i*16.5992],1)*(0.833168);
dGain = -0.005;

dCtrl = fm1*fm2*fm3*fm4*fm5*fm6*dGain;

% CM servo shape - Assume Flat
CM = 1;

% CM single arm compensation
comp = zpk(-2*pi*1.6e3,-2*pi*79,1);
comp = comp /abs(freqresp(comp,0));

% CM servo boosts 
BOOST = zpk(-2*pi*4000, -2*pi*40, 1);
BOOST1 = zpk(-2*pi*20000, -2*pi*1000, 1);
BOOST2 = zpk(-2*pi*20000, -2*pi*1000, 1);
BOOST3 = zpk(-2*pi*4500, -2*pi*300, 1);

comm = CM;

fastLoop = comm * comp;
fastLoop = fastLoop/abs(freqresp(fastLoop,2*pi*200));
slowLoop = comm * pend * AA * dCtrl * AI;
slowLoop = slowLoop/abs(freqresp(slowLoop,2*pi*200));
bode(fastLoop,slowLoop,{2*pi*.1,2*pi*10000});
grid on
%%
figure()
bode(slowLoop,slowLoop-fastLoop,slowLoop-5*fastLoop,{2*pi*10,2*pi*10000});
title('Opposite Signs');
legend('CM Slow','CM Slow - AO','CM Slow - 5xAO')
grid on
figure()
bode(slowLoop,slowLoop+fastLoop,slowLoop+5*fastLoop,{2*pi*10,2*pi*10000});
title('Same Signs');
legend('CM Slow','CM Slow + AO','CM Slow + 5xAO')
grid on