% This file calculates frequency responses of the LSC lm
% servo. It assumes that 'mich' has been run, such that the 
% state-space model of the servo is in the variable 'lmsys'



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% README %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%% IMPORTANT NOTE: If you run the Simulink model simulation, and then 
%%%%% subsequently run this script, darmsys will be altered and produce 
%%%%% incorrect results due to the addition of invSensFunc3 and ActFunc 
%%%%% to the model.  We treat Rana's original model as a black box, and
%%%%% therefore must compile his model without our blocks attached.

%%%%% TO GET CORRECT RESULTS: First, open 'darm_02.mdl' in Simulink and
%%%%% check that Rana Adhikari's model DOES NOT have invSensFunc3 and
%%%%% ActFunc connected to it.

%%%%% Second, run this script, doDARM.

%%%%% Third, add in two LTI blocks in Simulink, name one invSensFunc3 and
%%%%% the other ActFunc, and run the Error Signal (Output 2) through
%%%%% invSensFunc3 and Control Signal (Output 3) through ActFunc.  

%%%%% Fourth: Sum the outputs of the two blocks by subtracting the ActFunc 
%%%%% block's output from the invSensFunc3 block's output.  
%%%%% This reconstructs our strain.

%%%%% Finally, add a sine wave input to GW in (Input 1) at amplitude below
%%%%% 1E-7 and run the simulation.  If you scope the output of the sum
%%%%% block, you should see a sine wave output that is similar to the input
%%%%% sine wave, with a small delay at the beginning and the same amplitude 
%%%%% at low frequency.  Look at plot 555 to see amplitude reconstruction.

%%%%% Craig Cahillane - 8/23/13
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


makeDARMsys;

% set up frequencies
fmin = 1;
fmax = 1e4;
f = logspace(log10(fmin), log10(fmax), 1e3);

% open loop gain
clear clg olg;
clg = darmsys(4,7);
olg = -darmsys(2,7) / clg;

% Actuation Function (from DARM_OUT to mirror motion)
clear ActFunc;
ActFunc = darmsys(5,7) / darmsys(3,7);

% Sensing Function (from GW in to DARM_IN1)
clear SensFunc;
SensFunc = darmsys(2,1) / darmsys(1,1);

olgxz = squeeze(freqresp(olg, 2*pi*f));

UGF = 150;   % desired UGF
[m,p] = bode(olg, 2*pi*UGF);

huvt1 = darmsys(2,1) / SensFunc;
huvt2 = darmsys(3,1) * ActFunc;

huvt = huvt1 - huvt2;
%huvt = minreal(huvt);

%%%%% Craig Cahillane 7/25/13-8/23/13
%poleFreq = ; % [Hz]
clear invSensFunc;
zeroFreq = 8000; % [Hz]
% ^ Future Step: Upsample, add poles at higher frequency, 
% then decimate - Craig Cahillane 8/23/13

% Find number of poles we need to add to invSensFunc
[n,a] = size(zero(SensFunc));
[m,b] = size(pole(SensFunc));

% Find SensFunc's real positive zeros: These will become invSensFunc's real 
% positive poles which create exponential growth rather than decay.
zeros = zero(SensFunc);
count = 0;
for i=1:size(zeros)
    if real(zeros(i)) > 0.0001   
        count = count + 1;
    end
end

numZero2Add = count + m - n;  % Number of poles we must add to invSensFunc
Zero2Add = [];
for i=1:numZero2Add
    Zero2Add = [Zero2Add zeroFreq];
end

addZero = zpk(-2*pi.*Zero2Add,[],2*pi*zeroFreq);
invSensFunc = 1/(SensFunc*addZero);

% figure(12345)
% bode(invSensFunc,2*pi*f)
% title('invSensFunc');
% grid on;

%%% Normalize the invSensFunc at 0 dB for low frequency [f(52) = 1.6003]
%%% f(860) is around the peak of the invSensFunc's magnitude
[mag,phase] = bode(invSensFunc,2*pi*f);
scaler = 1/mag(52);
invSensFunc = scaler*invSensFunc;

figure(54321)   % Plot scaled invSensFunc with added poles
bode(invSensFunc,2*pi*f)
title('Normalized invSensFunc with added poles (0 dB for low frequency)');
grid on;

CheckFunc2 = darmsys(6,1) / darmsys(1,1);
CheckFunc1 = darmsys(7,1) / darmsys(1,1);

%%% Create invSensFunc2, which has all positive real poles removed
unstable = [];  
poles = pole(invSensFunc);
for i=1:size(poles)   % Locate the postive real poles for elimination
    if real(poles(i)) > 0.0001;
        unstable = [unstable poles(i)];
    end
end

clear invSensFunc2;
unstableZero = zpk(unstable,[],1);
invSensFuncInfo = unstableZero*invSensFunc;

% minreal() is actually creating new positive real poles depending on the
% gain in invSensFunc.  Why??
% minreal() is supposed to simplify systems with the same poles and zeros,
% and it does this.  But it has a response that depends on gain for some
% reason.  Try increasing or decreasing the gain on invSensFuncInfo and
% looking at the plots of invSensFunc2 if you don't believe me.  In other
% words, minreal() is a strangely fragile function and care must be taken
% when messing around with gain here.  If it works the way it should (or at
% least the way it works for me), there should be output that says '55
% states removed' in the command window after running doDARM.

invSensFunc2 = minreal(invSensFuncInfo);

figure(67890)  % Plot of invSensFunc with Positive Real Poles Removed
bode(invSensFunc2,2*pi*f)
title('invSensFunc2 (Positive Real Poles Removed from invSensFunc)');
grid on;

figure(7890)
bode(invSensFunc2*darmsys(2,1),2*pi*f)
title('invSensFunc2 times Error Signal');
grid on;

% Align invSensFunc2*Error_Signal's freq response with Rana's Error_Signal/SensFunc
[mag2,phase2] = bode(invSensFunc2*darmsys(2,1),2*pi*f);
[mag3,phase3] = bode(huvt1,2*pi*f);
[~,sizef] = size(f);
for i=1:sizef
    if f(i) > 210.0   % If the frequency is above 210, use that scaling factor to align invSensFunc3 and huvt1
        scaler2 = mag3(i)/mag2(i);
        break; 
    end
end

invSensFunc3 = scaler2*invSensFunc2;

figure(99999)
bode(invSensFunc3,2*pi*f)
title('invSensFunc3 with Gain');
grid on;

% figure(9999)
% bode(invSensFunc3*darmsys(2,1),2*pi*f)
% title('invSensFunc3 with Gain times Error Signal');
% grid on;


% Reconstruct the strain frequency response for Simulink compatible model
strain = invSensFunc3*darmsys(2,1) - ActFunc*darmsys(3,1);

figure(555)
bode(invSensFunc3*darmsys(2,1),ActFunc*darmsys(3,1),strain,2*pi*f)
[mag4,phase4] = bode(strain,2*pi*f);
mag4 = squeeze(mag4);
title('Error, Control, Strain: Reconstruction of figure 77112');
grid on;

% strain = minreal(strain);
% 
% figure(5555)
% bode(invSensFunc3*darmsys(2,1),ActFunc*darmsys(3,1),strain,2*pi*f)
% title('Error, Control, minreal(Strain): Reconstruction of figure 77112');
% grid on;

figure(77)
bode(1/SensFunc,invSensFunc3,2*pi*f)
title('Comparision of invSensFunc3 and 1/SensFunc');
grid on;

figure(777)
bode(huvt1,invSensFunc3*darmsys(2,1),2*pi*f)
title('Comparison of original Error Signal and modified Error Signal');
grid on;


%%%%% End Craig Cahillane modifications 8/23/13

%figure(77)
%margin(olg)
%xlim([1 7e3])

%% Make the plots
if 1==1
    
    figure(87090)

    top = subplot('Position',[0.13,0.52,0.82,0.4]);
    semilogx(f, 20*log10(abs(olgxz)), 'b');
    grid
    set(top,'XTickLabel',[]);
    axis tight
    set(gca,'YTick',[-120:20:140])
    ylim([-50 100])
    ylabel('Magnitude [dB]')
    title(' DARM Open Loop Gain')
    %legend('Measurement','Model','Difference')

    bottom = subplot('Position',[0.13,0.1,0.82,0.4]);
    semilogx(f, 180/pi*(angle(olgxz)), 'b');
    grid
    axis tight
    set(gca,'YTick',[-360:45:360])
    xlabel('Frequency [Hz]')
    ylabel('Phase [deg]')


    figure(77112)
    bode(huvt1, huvt2, huvt, 2*pi*f)
    [mag5,phase5] = bode(huvt, 2*pi*f);
    mag5 = squeeze(mag5);
    grid on;
    
    % Craig Cahillane 9/25/13
    figure(101010)
    loglog(f,mag5)
    hold on
    loglog(f,mag4,'Color','red')
    grid on;
    xlabel('Frequency (Hz)');
    ylabel('Magnitude');
    title('Comparison between Strain Reproductions');
    legend('MATLAB Strain Reconstruction','Simulink Strain Reconstruction');
end
