%% function plotSensingMatrix(opt,sigAC,probeNames, mPrb, cPrbNames,
%                                       driveNames, mDrv, cDrvNames, noiseAC)
%
%
% Plot a sensing matrix of specified probes and drives.
%
% === 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 [length(probeNames)*length(cPrbNames)].
%       This matrix is used to convert from normal probes to compound
%       probes.
%
% cPrbNames: A cell array of compound probe names.
%
% 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.
%
% noiseAC (optional): noise matrix ( Nprb x Naf )


%function mSens = plotSensingMatrixASC_bLCGT(opt,sigAC,probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames, noiseAC)

load('tickle01_Ilcgt_110226');


%%
test=1;
if test
    probeNames={'REFL_A I1'
        'REFL_A Q1'
        'REFL_B I1'
        'REFL_B Q1'
        'REFL_A I2'
        'REFL_A Q2'
        'REFL_B I2'
        'REFL_B Q2'
        'AS_A I1'
        'AS_A Q1'
        'AS_B I1'
        'AS_B Q1'
        'POP_A I1'
        'POP_A Q1'
        'POP_B I1'
        'POP_B Q1'
        'POP_A I2'
        'POP_A Q2'
        'POP_B I2'
        'POP_B Q2'};
%        'POX_A I1'
%        'POX_A Q1'
%        'POX_B I1'
%        'POX_B Q1'
%        'POX_A I2'
%        'POX_A Q2'
%        'POX_B I2'
%        'POX_B Q2'
%        'POY_A I1'
%        'POY_A Q1'
%        'POY_B I1'
%        'POY_B Q1'
%        'POY_A I2'
%        'POY_A Q2'
%        'POY_B I2'
%        'POY_B Q2'};
    
    %mPrb=[1,0,0,0;0,1,1,0];
    mPrb = eye(length(probeNames));
    %cPrbNames={'REFL_1I','Some shit'};
    cPrbNames=probeNames;
%    driveNames={'EX','EY','IX','IY','PR2','PR3','SR2','SR3','BS'};
    driveNames={'EX','EY','IX','IY','BS'};
    %mDrv=[1,0,0,0;0,1,0,0;0,0,1,1;0,0,1,-1];
%    cDrvNames = {'CS', 'CH', 'DS', 'DH', 'PR2', 'PR3', 'SR2', 'SR3', 'BS'};
    cDrvNames = {'CS', 'CH', 'DS', 'DH', 'BS'};
    
        % Drive matrix
r = 0.808; %(in case LCGT)
%mDrv = [1  r  1  r  0  0  0  0  0 
%        1  r -1 -r  0  0  0  0  0 
%        r -1  r -1  0  0  0  0  0 
%        r -1 -r  1  0  0  0  0  0 
%        0  0  0  0  1  0  0  0  0 
%        0  0  0  0  0  1  0  0  0 
%        0  0  0  0  0  0  1  0  0 
%        0  0  0  0  0  0  0  1  0 
%        0  0  0  0  0  0  0  0  1];
    
mDrv = [1  r  1  r  0 
        1  r -1 -r  0 
        r -1  r -1  0 
        r -1 -r  1  0 
        0  0  0  0  1];
    
end

Nprb=length(cPrbNames);
Ndrv=length(cDrvNames);
mSens = zeros(Nprb, Ndrv);

%% Complex sensing matrix
iPrb=cellfun(@(x)getProbeNum(opt,x),probeNames);
iDrv=getDriveNumbers(opt,driveNames);
sigACr=sigAC(iPrb, iDrv, 572); % @10Hz
%sigACr=sign(real(sigAC(iPrb, iDrv, 572))).*abs(sigAC(iPrb, iDrv, 572));


%normNoise = 0;
%if normNoise
%   noiseInv = diag(1./noiseAC(iPrb));
%   sigACr = noiseInv * sigACr;
%end

for i=1:Nprb
mDrAC = mDrv \ sigACr(i,:)';
mSens(i,:) = mDrAC';
end

%% Find largest signal

mLrgst=zeros(Nprb,Ndrv);
for ii=1:Nprb
    [m,maxjj]=max(abs(mSens(ii,:)));
    mLrgst(ii,maxjj)=1;
end
mScnd=zeros(Nprb,Ndrv);
for ii=1:Nprb
    [m,secondjj]=max(abs(mSens(ii,:).*(1-mLrgst(ii,:))));
    mScnd(ii,secondjj)=1;
end

%% Plot
ax=axes();

%Make grid
for ii=0:Nprb
    line([0,Ndrv],[ii,ii],'Color','k');
end
for ii=0:Ndrv
    line([ii,ii],[0,Nprb],'Color','k');
end
set(ax, 'XLim',[0,Ndrv],'YLim',[0,Nprb]);
set(ax,'XTick',[0.5:Ndrv-0.5], 'YTick', [0.5:Nprb-0.5]);
set(ax,'XTickLabel',cDrvNames,'YTickLabel',cPrbNames);

for ii=1:Nprb
    for jj=1:Ndrv
        text(jj-0.5,ii-0.5,num2str(sign(real(mSens(ii,jj)))*abs(mSens(ii,jj)), 3), 'HorizontalAlignment','center','color',[mLrgst(ii,jj)+0.5*mScnd(ii,jj),0,0],'FontSize',12);
        %text(jj-0.5,ii-0.5,num2str(mSens(ii,jj), 3),'HorizontalAlignment','center','color',[mLrgst(ii,jj)+0.5*mScnd(ii,jj),0,0],'FontSize',10);
    end
end
%if normNoise
%    title('Noise Normalized Sensing Matrix [1/m/sqrt(Hz)]','FontSize',16);
%else
    title('Angle Sensing Matrix [W/rad]','FontSize',12);
%end

%%for text file
format('short','g')
mx = zeros(Nprb,Ndrv);
for ii=1:Nprb
    for jj=1:Ndrv
mx(Nprb-ii+1,jj) = sign(real(mSens(ii,jj)))*abs(mSens(ii,jj));
    end
end