%[optDemodPhase, signal, offset] = optimalDemodPhase(model,IPrb, QPrb, iDrv,
%                                                        vDrv, quad, quant, mode,f) 
%
% Find an optimal demodulation phase.
% The behavior of the optimization is controlled by quad, quant and mode. 
%quad='Q', quant='signal', mode='min' means "minimize the Q-phase signal".
% === Inputs ===
%
% model: An optickle instance or a cell array of {sigDC, sigAC}
%
% IPrb: Probe index for I-phase probe.
%
% QPrb: Probe index for Q-phase probe.
%
% iDrv: Driving indices array. This is an one dimensional array of drive
%       numbers. The listed drives will be used for signal injection. A
%       compound actuator will be formed by a linear combination of thoes
%       drives. 
%
% vDrv: Drive combination vector. This vector represents how the drives are
%       combined to form a compound actuator. Each element of the vector is
%       a coefficient for the corresponding drive when taking a linear
%       combination. 
%
% quad: Quadrature of interest. 'I' or 'Q'
%
% quant: Quantity of interest. 'signal' or 'offset'
%
% mode: 'min' or 'max'
%
% f: frequency at which the AC response is evaluated.
%
% === Outputs ===
%
% optDemodPhase: Optimal demodulation phase in degrees.
%
% signal: Signal at the optimal demodulation phase. [2*1] vector.
%
% offset: DC offset at the optimal demodulation phase. [2*1] vector.
%

function [optDemodPhase, signal, offset] = optimalDemodPhase(model, IPrb, QPrb, iDrv, vDrv, quad, quant, mode, f)

%% Test
test=0;
if test
   model=opt;
   p.dpPOP1 = 0;
   f=0.1; 
   IPrb=pr.POP_1I;
   QPrb=pr.POP_1Q;
   iDrv=getDriveNumbers(opt,{'ETMX','ETMY'});
   vDrv=[1,1];
   quad = 'I';
   quant = 'signal';
   mode = 'max';
end

%% Parse arguments

if isa(model,'Optickle')
    doTickle=1;
    opt=model;
else
    doTickle=0;
    sigDC=model{1};
    sigAC=model{2}; 
end
    
    
try, quad; catch, quad = 'Q'; end
try, quant; catch, quant = 'signal'; end
try, mode; catch, mode = 'min'; end
try, f; catch, f=0.1; end

%% Tickle
% if doTickle
%     [fDC, sigDC,sigAC] = tickle(opt, [], f);
% end
%%
iPrb=[IPrb;QPrb];  % Probe vector
vDrv=vDrv(:);  % Column vector

% Reduced signal arrays
sigACr=sigAC(iPrb,iDrv);

errorSigACI=[1,0]*sigACr*vDrv;
errorSigACQ=[0,1]*sigACr*vDrv;

offsetDCI = sigDC(IPrb);
offsetDCQ = sigDC(QPrb);

%%
if quant == 'signal'
    a = angle(errorSigACI); 
    x = real(errorSigACI*exp(-i*a));
    y = real(errorSigACQ*exp(-i*a));
    demdPhase = atan2(y,x)*180/pi; 
else
    demdPhase = atan2(offsetDCQ, offsetDCI)*180/pi;
end
if (strcmp(quad,'I') && strcmp(mode,'max')) || (strcmp(quad,'Q') && strcmp(mode,'min'))
    optDemodPhase=demdPhase;
    theta=-optDemodPhase*pi/180;
    rotM=[cos(theta),-sin(theta);sin(theta),cos(theta)];
    signal=rotM*[errorSigACI;errorSigACQ];
    offset=rotM*[offsetDCI;offsetDCQ];
else
    optDemodPhase=demdPhase+90;
    theta=-optDemodPhase*pi/180;
    rotM=[cos(theta),-sin(theta);sin(theta),cos(theta)];
    signal=rotM*[errorSigACI;errorSigACQ];
    offset=rotM*[offsetDCI;offsetDCQ];
end