function determineDemodPhase(doplot, resultDir, p)
% Determine the optimal demodulation phase

%% Default options
try
    doplot;
catch
    doplot = 0;
end

try
    resultDir;
catch
    resultDir = 0;
end

%% Units constants
nm=1e-9;
pm=1e-12;

%% Construct a model
[opt,n,l, pr] = modelLCGT(p);

%% ************** Find Optimal Demodulation Phases  *************
f=p.ftickle; % Tickle Frequency
dpPhaseCell={};
% Tickle
[fDC, sigDC, sigAC]=tickle(opt,[],f);

%% ASPO_1I for DARM
[p.dpASPO1, signal, offset] = optimalDemodPhase({sigDC,sigAC},pr.ASPO_1I, pr.ASPO_1Q, ...
                          getDriveNumbers(opt,{'ETMX','ETMY'}),[1,-1],'I','signal','max');
dpPhaseCell=[dpPhaseCell, ['p.dpASPO1 = ',num2str(p.dpASPO1)]];

%% REFL_2I for CARM
[p.dpREFL2, signal, offset] = optimalDemodPhase({sigDC,sigAC},pr.REFL_2I, pr.REFL_2Q, ...
                          getDriveNumbers(opt,{'ETMX','ETMY'}),[1,1],'I','signal','max');
dpPhaseCell=[dpPhaseCell, ['p.dpREFL2 = ',num2str(p.dpREFL2)]];

%% REFL_1Q for MICH (Minimize CARM)
[p.dpREFL1, signal, offset] = optimalDemodPhase({sigDC,sigAC},pr.REFL_1I, pr.REFL_1Q, ...
                          getDriveNumbers(opt,{'ETMX','ETMY'}),[1,1], ...
                          'Q','signal','min');
dpPhaseCell=[dpPhaseCell, ['p.dpREFL1 = ',num2str(p.dpREFL1)]];

%% POP1Q for MICH (minimizing CARM)
[p.dpPOP1, signal, offset] = optimalDemodPhase({sigDC,sigAC},pr.POP_1I, pr.POP_1Q, ...
                          getDriveNumbers(opt,{'ETMX','ETMY'}),[1,1], ...
                          'Q','signal','min');
dpPhaseCell=[dpPhaseCell, ['p.dpPOP1 = ',num2str(p.dpPOP1)]];

%% POP2I for PRC
[p.dpPOP2, signal, offset] = optimalDemodPhase({sigDC,sigAC},pr.POP_2I, pr.POP_2Q, ...
                          getDriveNumbers(opt,{'PRM'}),[1], ...
                          'I','signal','max');
dpPhaseCell=[dpPhaseCell, ['p.dpPOP2 = ',num2str(p.dpPOP2)]];

%% REFL_2DI for PRC

 [p.dpREFL2Dp, signalP, offsetP] = optimalDemodPhase({sigDC,sigAC},pr.REFL_2DpI, pr.REFL_2DpQ, ...
                          getDriveNumbers(opt,{'PRM'}),[1],'I','signal','max');
               
 [p.dpREFL2Dm, signalM, offsetM] = optimalDemodPhase({sigDC,sigAC},pr.REFL_2DmI, pr.REFL_2DmQ, ...
                          getDriveNumbers(opt,{'PRM'}),[1],'I','signal','max');

dpPhaseCell=[dpPhaseCell, ['p.dpREFL2Dp = ',num2str(p.dpREFL2Dp)]];
dpPhaseCell=[dpPhaseCell, ['p.dpREFL2Dm = ',num2str(p.dpREFL2Dm)]];

%% REFL_1DQ for MICH by minimizing SRC signal

 [p.dpREFL1Dp, signalP, offsetP] = optimalDemodPhase({sigDC,sigAC},pr.REFL_1DpI, pr.REFL_1DpQ, ...
                          getDriveNumbers(opt,{'SRM'}),[1],'Q','signal','min');
               
 [p.dpREFL1Dm, signalM, offsetM] = optimalDemodPhase({sigDC,sigAC},pr.REFL_1DmI, pr.REFL_1DmQ, ...
                          getDriveNumbers(opt,{'SRM'}),[1],'Q','signal','min');

dpPhaseCell=[dpPhaseCell, ['p.dpREFL1Dp = ',num2str(p.dpREFL1Dp)]];
dpPhaseCell=[dpPhaseCell, ['p.dpREFL1Dm = ',num2str(p.dpREFL1Dm)]];


%% REFL_1ThQ for MICH by minimizing SRCL

 [p.dpREFL1Th, signalP, offsetP] = optimalDemodPhase({sigDC,sigAC},pr.REFL_1ThI, pr.REFL_1ThQ, ...
                          getDriveNumbers(opt,{'SRM'}),[1],'Q','signal','min');
               
dpPhaseCell=[dpPhaseCell, ['p.dpREFL1Th = ',num2str(p.dpREFL1Th)]];

%% REFL_2ThI for PRCL

 [p.dpREFL2Th, signalP, offsetP] = optimalDemodPhase({sigDC,sigAC},pr.REFL_2ThI, pr.REFL_2ThQ, ...
                          getDriveNumbers(opt,{'PRM'}),[1],'I','signal','max');
               
dpPhaseCell=[dpPhaseCell, ['p.dpREFL2Th = ',num2str(p.dpREFL2Th)]];

%% POP_1ThI for PRCL

 [p.dpPOP1Th, signalP, offsetP] = optimalDemodPhase({sigDC,sigAC},pr.POP_1ThI, pr.POP_1ThQ, ...
                          getDriveNumbers(opt,{'PRM'}),[1],'I','signal','max');
               
dpPhaseCell=[dpPhaseCell, ['p.dpPOP1Th = ',num2str(p.dpPOP1Th)]];

%% Write results in a file
fname='demodPhaseLCGT.m';
fid=fopen(fname,'w');
for ii=1:length(dpPhaseCell)
    fprintf(fid,'%s;\n',dpPhaseCell{ii});
end
fclose(fid);

if resultDir
    fname=[resultDir, '/demodPhases.txt'];
    fid=fopen(fname,'w');
    for ii=1:length(dpPhaseCell)
        fprintf(fid,'%s;\n',dpPhaseCell{ii});
    end
    fclose(fid);
end

%% Tickle with new demod phases
if doplot
    [opt,n,l, pr] = modelLCGT(p);
    [fDC, sigDC, sigAC]=tickle(opt,[],f);
end

%% Drive names and driving matrix
if doplot
    driveNames = {'ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM'};
    cDrvNames = {'DARM','CARM','MICH','PRC','SRC'};
    mDrv = [1,-1,0,0,0,0,0; %DARM
            1,1,0,0,0,0,0;  %CARM
            1,-1,-1,1,0,0,0;%MICH
            0,0,0,0,0,1,0;  %PRC
            0,0,0,0,0,0,1]; %SRC
    if p.MICHBS
        mDrv = [1,-1,0,0,0,0,0; %DARM
                1,1,0,0,0,0,0;  %CARM
                0,0,0,0,1,-1/sqrt(2),1/sqrt(2);%MICH
                0,0,0,0,0,1,0;  %PRC
                0,0,0,0,0,0,1]; %SRC
    end
end
%% Sensing matrix for ASPO1
if doplot
    close all
    probeNames = {'ASPO_1I', 'ASPO_1Q'};
    cPrbNames = {'ASPO_1I', 'ASPO_1Q'};
    mPrb = [1,0;
            0,1];
    
    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for ASPO_1','FontSize',16);
                  
    h2=figure(2);
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames)
    title('Vector Sensing Matrix for ASPO_1','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_ASPO1.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_ASPO1.pdf']);
    end
end
%% Sensing matrix for REFL2
if doplot
    close all

    probeNames = {'REFL_2I', 'REFL_2Q'};
    cPrbNames = {'REFL_2I', 'REFL_2Q'};
    mPrb = [1,0;
            0,1];
    
    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for REFL_2','FontSize',16);
                  
    h2=figure(2);
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames)
    title('Vector Sensing Matrix for REFL_2','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_REFL2.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_REFL2.pdf']);
    end
end

%% Sensing Matrix for REFL_1
if doplot
    close all
    
    probeNames = {'REFL_1I', 'REFL_1Q'};
    cPrbNames = {'REFL_1I', 'REFL_1Q'};
    mPrb = [1,0;
            0,1];

    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for REFL_1','FontSize',16);
    h2=figure(2);
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Vector Sensing Matrix for REFL_1','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_REFL1.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_REFL1.pdf']);
    end
end
%% Sensing Matrix for POP1
if doplot
    close all

    probeNames = {'POP_1I', 'POP_1Q'};
    cPrbNames = {'POP_1I', 'POP_1Q'};
    mPrb = [1,0;
            0,1];

    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for POP_1','FontSize',16);
    h2=figure(2);
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Vector Sensing Matrix for POP_1','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_POP1.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_POP1.pdf']);
    end
end
%% Sensing Matrix for POP2
if doplot
    close all
   
    probeNames = {'POP_2I', 'POP_2Q'};
    cPrbNames = {'POP_2I', 'POP_2Q'};
    mPrb = [1,0;
            0,1];

    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for POP_2','FontSize',16);              
    h2=figure(2);
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Vector Sensing Matrix for POP_2','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_POP2.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_POP2.pdf']);
    end              
end
%% Sensing matrix for REFL_2D
if doplot
    close all
    
    probeNames = {'REFL_2DpI', 'REFL_2DpQ','REFL_2DmI', 'REFL_2DmQ'};
    cPrbNames = {'REFL_2DpI', 'REFL_2DpQ','REFL_2DmI', 'REFL_2DmQ'};
    mPrb = [1,0,0,0;
            0,1,0,0;
            0,0,1,0;
            0,0,0,1];
    
    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for REFL_2D','FontSize',16);
    h2=figure(2);
    set(h2,'Position',[1,1,900,800]);  
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    %title('Vector Sensing Matrix for REFL_2D','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_REFL2D.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_REFL2D.pdf']);
    end  
end
%% Sensing matrix for REFL_1D
if doplot
    close all

    probeNames = {'REFL_1DpI', 'REFL_1DpQ','REFL_1DmI', 'REFL_1DmQ'};
    cPrbNames = {'REFL_1DpI', 'REFL_1DpQ','REFL_1DmI', 'REFL_1DmQ'};
    mPrb = [1,0,0,0;
            0,1,0,0;
            0,0,1,0;
            0,0,0,1];
    
    h1=figure(1);
    plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    title('Sensing Matrix for REFL_1D','FontSize',16);                  
    h2=figure(2);
    set(h2,'Position',[1,1,900,800]); 
    plotVectorSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
    %title('Vector Sensing Matrix for REFL_1D','FontSize',16);
    if resultDir
       print(h1, '-dpdf', [resultDir, 'SensingMatrix_REFL1D.pdf']);
       print(h2, '-dpdf', [resultDir, 'SensingMatrixVector_REFL1D.pdf']);
    end  
end
close all