
%% QUAD TRANSFER FUNCTION MODELS

addpath /ligo/svncommon/SusSVN/sus/trunk/QUAD/Common/MatlabTools/QuadModel_Production

% build the quad model
fr = logspace(-1,1,1000);
quadmodel = generate_QUAD_Model_Production(fr, 'fiber' , [] , 0 , 1 ,'/opt/rtcds/lho/h1/chans/H1SUSETMY.txt');
ss = quadmodel.dampedss;

% extract the TF we need (PIT)

% torque on PUM to test mass angle
[tp_z, tp_p, tp_k] = ss2zp(ss.A, ss.B, ss.C, ss.D, quadmodel.dampedin.pum.drive.P);
tp_z = tp_z(:,quadmodel.dampedout.tst.disp.P);
tp_k = tp_k(quadmodel.dampedout.tst.disp.P);
tp_z = tp_z(~isinf(tp_z));
pit_pum2tst = zpk(tp_z, tp_p, tp_k);
% torque on test mass to test mass angle
[tp_z, tp_p, tp_k] = ss2zp(ss.A, ss.B, ss.C, ss.D, quadmodel.dampedin.tst.drive.P);
tp_z = tp_z(:,quadmodel.dampedout.tst.disp.P);
tp_k = tp_k(quadmodel.dampedout.tst.disp.P);
tp_z = tp_z(~isinf(tp_z));
pit_tst2tst = zpk(tp_z, tp_p, tp_k);

% extract the TF we need (YAW)

% torque on PUM to test mass angle
[ty_z, ty_p, ty_k] = ss2zp(ss.A, ss.B, ss.C, ss.D, quadmodel.dampedin.pum.drive.Y);
ty_z = ty_z(:,quadmodel.dampedout.tst.disp.Y);
ty_k = ty_k(quadmodel.dampedout.tst.disp.Y);
ty_z = ty_z(~isinf(ty_z));
yaw_pum2tst = zpk(ty_z, ty_p, ty_k);
% torque on test mass to test mass angle
[ty_z, ty_p, ty_k] = ss2zp(ss.A, ss.B, ss.C, ss.D, quadmodel.dampedin.tst.drive.Y);
ty_z = ty_z(:,quadmodel.dampedout.tst.disp.Y);
ty_k = ty_k(quadmodel.dampedout.tst.disp.Y);
ty_z = ty_z(~isinf(ty_z));
yaw_tst2tst = zpk(ty_z, ty_p, ty_k);

%% Radiation pressure stiffness matrix
Pin = 2.8;  % input power
grec = 29;  % recycling gain

Pcav = Pin * grec * 0.5 * 283;  % power into the cavity (input x recycling gain x BS x arm roundtrips)
L = 3994.5;                   % arm length
R1 = 1934;                    % ITM Roc
R2 = 2245;                    % ETM Roc
c = 299792458;                % speed of light
% optical stiffness matrix
K = 2*Pcav / (c * (R1 + R2 - L)) * [ R1*(L-R2), R1*R2; R1*R2 , R2*(L-R1) ];

% diagonalize the matrix
[V,D] = eig(K);

% compute the radiation pressure modified transfer functions
nfr = length(fr);
P_tst2tst = fresp(pit_tst2tst, fr);
Y_tst2tst = fresp(yaw_tst2tst, fr);
P_pum2tst = fresp(pit_pum2tst, fr);
Y_pum2tst = fresp(yaw_pum2tst, fr);

P = zeros(2,2,nfr);
Y = zeros(2,2,nfr);
Pd = zeros(2,2,nfr);
Yd = zeros(2,2,nfr);
for i=1:nfr
    % transfer functions from PUM to TST in mirror basis
    P(:,:,i) = inv(eye(2) + [P_tst2tst(i), 0; 0, P_tst2tst(i)] * K) * [P_pum2tst(i), 0; 0, P_pum2tst(i)];
    Y(:,:,i) = inv(eye(2) + [Y_tst2tst(i), 0; 0, Y_tst2tst(i)] * K) * [Y_pum2tst(i), 0; 0, Y_pum2tst(i)];
    % transfer functions from PUM to TST in radiation pressure basis
    Pd(:,:,i) = inv(eye(2) + inv(V)*[P_tst2tst(i), 0; 0, P_tst2tst(i)] * V * D) * inv(V) * [P_pum2tst(i), 0; 0, P_pum2tst(i)] * V;
    Yd(:,:,i) = inv(eye(2) + inv(V)*[Y_tst2tst(i), 0; 0, Y_tst2tst(i)] * V * D) * inv(V) * [Y_pum2tst(i), 0; 0, Y_pum2tst(i)] * V;
end

%% Plot results in mirror basis for ETM
figure()
ax(1) = subplot(221);
loglog(fr, abs(P_pum2tst), 'r', fr, abs(squeeze(P(2,2,:))), 'b', 'LineWidth', 2)
title('ETM - Pitch')
ylabel('Abs [rad / (Nm)]')
grid
legend('No RP', sprintf('%.1f W', Pin))
ax(2) = subplot(222);
loglog(fr, abs(Y_pum2tst), 'r', fr, abs(squeeze(Y(2,2,:))), 'b', 'LineWidth', 2)
title('ETM - Yaw')
ylabel('Abs [rad / (Nm)]')
legend('No RP', sprintf('%.1f W', Pin))
grid
ax(3) = subplot(223);
semilogx(fr, angle(P_pum2tst), 'r', fr, angle(squeeze(P(2,2,:))), 'b', 'LineWidth', 2)
ylabel('Phase [rad]')
xlabel('Frequency [Hz]')
grid
ax(4) = subplot(224);
semilogx(fr, angle(Y_pum2tst), 'r', fr, angle(squeeze(Y(2,2,:))), 'b', 'LineWidth', 2)
ylabel('Phase [rad]')
xlabel('Frequency [Hz]')
grid
linkaxes(ax, 'x');

%% Plot results in RP basis
figure()
ax(1) = subplot(221);
loglog(fr, abs(P_pum2tst), 'r', fr, abs(squeeze(Pd(1,1,:))), 'b', fr, abs(squeeze(Pd(2,2,:))), 'g', 'LineWidth', 2)
title('RP basis - Pitch')
ylabel('Abs [rad / (Nm)]')
grid
legend('No RP', sprintf('Soft %.1f W', Pin), sprintf('Hard %.1f W', Pin))
ax(2) = subplot(222);
loglog(fr, abs(Y_pum2tst), 'r', fr, abs(squeeze(Yd(1,1,:))), 'b', fr, abs(squeeze(Yd(2,2,:))), 'g', 'LineWidth', 2)
title('RP basis - Yaw')
ylabel('Abs [rad / (Nm)]')
legend('No RP', sprintf('Soft %.1f W', Pin), sprintf('Hard %.1f W', Pin))
grid
ax(3) = subplot(223);
semilogx(fr, angle(P_pum2tst), 'r', fr, angle(squeeze(Pd(1,1,:))), 'b', fr, angle(squeeze(Pd(2,2,:))), 'g', 'LineWidth', 2)
ylabel('Phase [rad]')
xlabel('Frequency [Hz]')
grid
ax(4) = subplot(224);
semilogx(fr, angle(Y_pum2tst), 'r', fr, angle(squeeze(Yd(1,1,:))), 'b', fr, angle(squeeze(Yd(2,2,:))), 'g', 'LineWidth', 2)
ylabel('Phase [rad]')
xlabel('Frequency [Hz]')
grid
linkaxes(ax, 'x');

