load('system.mat');

f = logspace(0, 6, 1e3);

kp = logspace(-4, 4, 100);
ki = [0 logspace(-4, 0, 9)];

[Kp, Ki] = meshgrid(kp, ki);

gains = [Kp(:) Ki(:)];

C = pid(gains(:, 1), gains(:, 2));

loopinfo = loopsens(C * G, 1);

S = loopinfo.Si;
T = loopinfo.Ti;

S_pwr = squeeze(abs(freqresp(S, f, 'Hz')).^2);
T_pwr = squeeze(abs(freqresp(T, f, 'Hz')).^2);
H_mlo_pwr = squeeze(abs(freqresp(H_mlo, f, 'Hz')).^2);
H_mo_pwr = squeeze(abs(freqresp(H_mo, f, 'Hz')).^2);

%% Plots
figure;
semilogx(f, pow2db(H_mlo_pwr));
grid on; title('MLO Noise (Approximation)');

figure;
semilogx(f, pow2db(H_mo_pwr));
grid on; title('MO Noise (Approximation)');

figure;
semilogx(f, pow2db(S_pwr));
grid on; title('Sensitivity Function Power');

figure;
semilogx(f, pow2db(T_pwr));
grid on; title('Complementary Function Power');

figure;
bode(S);
grid on;

figure;
bode(T);
grid on;

jitter = S_pwr * H_mlo_pwr + T_pwr * H_mo_pwr;