%% function plotVectorSensingMatrix(opt,sigAC,probeNames, mPrb, cPrbNames,
%                                       driveNames, mDrv, cDrvNames)
%
% Plot a sensing matrix of specified probes and drives.
% To generate sigAC, please tickle the model at very very low frequency
% so that sigAC is almost real valued. This function assume this.
% f = 1e-9 or something is recommended.
%
% === Inputs ===
%
% opt: Optickle instance.
%
% sigAC: Output of tickle. tickle has to be called with only one frequency
%        point. [Nprb*Ndrv]
%
% probeNames: A cell array of probe names.
%
% mPrb: A matrix of size [2 x length(probeNames)].
%       This matrix is used to convert from normal probes to compound
%       probes.
%
% cPrbNames: A cell array of compound probe names [2n].
%
% driveNames: A cell array of drive names.
%
% mDrv: A matrix of size [length(driveNames)*length(cDrvNames)].
%       This matrix is used to convert from normal drives to compound
%       drives.
%
% cDrvNames: A cell array of compound drive names.
%

function plotVectorSensingMatrix(opt,sigAC,probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames)




%%
test=0;
if test
    probeNames={'REFL_1I','REFL_1Q','REFL_2I','REFL_2Q'};
    mPrb=[1,0,0,0;0,1,1,0];
    cPrbNames={'REFL_1I','Some shit'};
    driveNames={'PRM','BS','ETMX','ETMY'};
    mDrv=[1,0,0,0;0,1,0,0;0,0,1,1;0,0,1,-1];
    cDrvNames={'PRM','BS','CARM','DARM'};
end
%%

try
    probeNames; 
catch
    probeNames=getProbeName(opt);
    mPrb = eye(length(probeNames));
    cPrbNames=probeNames;
end
try
    driveNames; 
catch
    driveNames=getDriveNames(opt);
    mDrv = eye(length(driveNames));
    cDrvNames=driveNames;
end
try, f; catch, f=0.1; end
%% Complex sensing matrix
iPrb=cellfun(@(x)getProbeNum(opt,x),probeNames);
iDrv=getDriveNumbers(opt,driveNames);

sigACr=sigAC(iPrb,iDrv);
mSens=mPrb*sigACr*mDrv';

%% Real valued sensing matrix

%mSens = real(mSens*conj(mSens(1,1)))/abs(mSens(1,1));
mSens = real(mSens);
%% Plot
Nprb=length(cPrbNames);
Ndrv=length(cDrvNames);

%Number of I-Q phase group
Ngrp = Nprb/2;

for kk=1:Ngrp
    ax=subplot(Nprb,1,kk);  

    %Make grid
    for ii=0:Ngrp
        line([0,Ndrv],[ii,ii]);
    end
    for ii=0:Ndrv
        line([ii,ii],[0,1]);
    end
    set(ax, 'XLim',[0,Ndrv],'YLim',[0,1]);
    set(ax,'XTick',[0.5:Ndrv-0.5], 'YTick', [0.5]);
    set(ax,'XTickLabel',cDrvNames);
    xlabel(cPrbNames{2*kk-1});
    ylabel(cPrbNames{2*kk});
    
    for jj=1:Ndrv
        %Make axes
        line([jj-0.9,jj-0.1],[0.5, 0.5],'Color','k');
        line([jj-0.5,jj-0.5],[0.1, 0.9],'Color','k');

        sigI = abs(mSens(1,jj));
        sigQ = abs(mSens(2,jj));
        A = sqrt(sigI^2+sigQ^2);
        sigI = sigI/A/2.5;
        sigQ = sigQ/A/2.5;

        line([jj-0.5,jj-0.5+sigI],[0.5,0.5+sigQ]);
        text(jj-0.5,0.6,num2str(A, '%5.1e'), 'HorizontalAlignment','center');
        text(jj-0.5,0.4,num2str(atan2(sigQ, sigI)*180/pi, '%5.2e'), 'HorizontalAlignment','center');
    end
end