%% Cucumber control system model for lentickle 
%
%   Being Develiped for DRMI ....
%
%          25 Jan 2012, KK
%
%
%

%%
function cucumber = C1cucumber(opt,pos)    
    if nargin<2
        pos = [];
    end
    
    %% First Create probeSens

    
                    %  probes     sensors
    probeSensPairs = {'REFL 3I1', 'REFL_3I1'
                      'REFL 3Q1', 'REFL_3Q1'
                      'AS I2',   'AS_I2'
                      'AS Q2',   'AS_Q2'
                      'AS DC',   'AS_DC'
                      'AS 3I2',  'AS_3I2'
                      'AS 3Q2',  'AS_3Q2'
                      'AS DC',    'AS_DC'};
                  
    % now we can use this to create our matrix.
    
    Nsens = size(probeSensPairs,1);
    probeSens = sparse(Nsens,opt.Nprobe);
    
    for jSens = 1:Nsens
                  %sensor index, probe index = 1
        probeSens(jSens,getProbeNum(opt,probeSensPairs{jSens,1})) = 1; %#ok<SPRIX>
    end
                        
    % We will also need the list of sensor names
    
    sensNames = probeSensPairs(:,2).';
    
    %% The other boring matrix is mirrDrive
    
                    %  mirrors  drives driveType
    mirrDrivePairs = {'IX',    'IX',   1
                      'IY',    'IY',   1
                      'BS',    'BS',   1
                      'PR',    'PR',   1
                      'SR',    'SR',   1
                      'AM',    'AM',   1
                      'PM',    'PM',   1
                      'OSC_AM','Mod1', 'amp'
                      'OSC_PM','Mod1', 'phase'};
                  
    % now we can use this to create our matrix.
    
    Nmirr = size(mirrDrivePairs,1);
    mirrDrive = sparse(opt.Ndrive,Nmirr);
    
    mirrNames = mirrDrivePairs(:,1).';   
    
    for jMirr = 1:Nmirr
                  %drive index, mirror index = 1
        mirrDrive(getDriveNum(opt, mirrDrivePairs{jMirr,2}, mirrDrivePairs{jMirr,3}), jMirr) = 1; %#ok<SPRIX>
    end
                        
    %% Control System Model
    % what we've done so far is just to reduce the number of inputs and
    % outputs to/from the Optickle model to make everything more resonable.
    % Now it's time to actually build the control system model.
    
    %% Input Matrix (sensDof)
    % we will just construct the input matrix we want. For out michelson,
    % let's just define a simple control system of two degrees of freedom,
    % the common mode arm mirror motion, COMM, and the differential mode,
    % DIFF. The ordering of the rows and columns are important, the first
    % sensor definded in probeSens is the fist sensor here.
    
              % REFL3I1 REFL3Q1 ASI2 ASQ2 ASDC AS3I2 AS3Q2 AS DC
    sensDof = [     0     0      0    1    0     0     0     0     % MICH
                    1     0      0    0    0     0     0     0     % PRCL
                    0     0      0    0    0     0     1     0 ];  % SRCL
    
     
    % Now that we've defined our DOFs, let's store the names we will use to
    % refer to them.
    
    dofNames = { 'MICH', 'PRCL','SRCL'};
    
    %% Control Filters (ctrlFilt)
    % These are the feedback filters.
    
               % MICH                    PRCL                   SRCL
    ctrlFilt = [ filtZPK([5,10],[1,1000],5),...
                     filtZPK([10,50],[1,1000],50), ...
                     filtZPK([10,50],[1,1000],50)]; %#ok<*NBRAK>
                 
                 
    % here we should also store the desired UGF of the loops
                % MICH PRCL SRCL
    setUgfDof = [ 100 100 100];
    
    %% Output Matrix (dofMirr)
    % remember, order matters.
    
              % MICH  PRCL SRCL
    dofMirr = [    1    0   0        % IX
                  -1    0   0        % IY
                   0    0   0        % BS
                   0    1   0        % PR
                   0    0   1        % SR
                   0    0   0        % AM
                   0    0   0        % PM
                   0    0   0        % OSC AM
                   0    0   0];      % OSC PM

    %% Pendulum compensation (mirrFilt)
    % We'll just do something really dumb for pendulum compensation: 2
    % zeros at 1Hz and a few poles at 5kHz.
    
    unityFilt = filtZPK([],[],1); % just a flat TF for non-mirrors
    compFilt = filtZPK([],[],1);
    %filtZPK([1,1],[5000,5000,5000],1); % dumb compensation
    
               % IX       IY       BS       PR       SR       AM        PM        OSCAM     OSCPM
    mirrFilt = [ compFilt compFilt compFilt compFilt compFilt unityFilt unityFilt unityFilt unityFilt ];
    
    %% Mechanical Response (pendFilt)
    % The mechanical response of the mirrors is defined in the Optickle
    % Model, we should be able to just get the filters from there. Optickle
    % stores them as zpk objects, we need to convert them to mf (mevans
    % filter) objects.
    %
    % There may be cases where the optical mechanical response will not be
    % the same as the actuator mechanical response. In that case, you'll
    % want to create these filters manually, rather than taking them from
    % the Optickle model.
    
    for jMirr = 1:Nmirr
        optic = getOptic(opt,mirrDrivePairs(jMirr,2));
        if numel(optic.mechTF) ~= 0
            pendFilt(jMirr) = zpk2mf(optic.mechTF); %#ok<AGROW>
        else
            % mechTF isn't set, so put in the indentity TF.
            pendFilt(jMirr) = unityFilt; %#ok<AGROW>
        end
    end
    
    %% Store all the needed variables in the cucumber
    cucumber.opt       = opt;
    cucumber.probeSens = probeSens;
    cucumber.sensNames = sensNames;
    cucumber.mirrDrive = mirrDrive;
    cucumber.mirrNames = mirrNames;
    cucumber.sensDof   = sensDof;
    cucumber.dofNames  = dofNames;
    cucumber.ctrlFilt  = ctrlFilt;
    cucumber.setUgfDof = setUgfDof;
    cucumber.dofMirr   = dofMirr;
    cucumber.mirrFilt  = mirrFilt;
    cucumber.pendFilt  = pendFilt;
    
    %% Choosing RF demod phases
    % One step we skipped when making the Optickle model was choosing the
    % demod phases of our RF sensors. Because Lentickle knows something
    % about the control system, we can use it to modify the Optickle model
    % to make certain sensors have maximum response to certain degrees of
    % freedom. (Example, you want DARM to be mostly coupled to AS_Q not
    % AS_I.)
    
    % Let's prepare the arguments for the phasing function.
    
phaseMICH = struct( 'Iname', 'AS_I2', ... % name of the I sensor
                      'Qname', 'AS_Q2', ... % name of the Q sensor
                      'IorQ',  'Q',    ... % maximize I or Q?
                      'DOF',   'MICH');    % maximize which DOF?

phasePRCL = struct( 'Iname', 'REFL_3I1', ...
                        'Qname', 'REFL_3Q1', ...
                        'IorQ',  'I',      ...
                        'DOF',   'PRCL');
                    
 phaseSRCL = struct( 'Iname', 'AS_3I2', ...
                        'Qname', 'AS_3Q2', ...
                        'IorQ',  'Q',      ...
                        'DOF',   'SRCL');
                    
                    
    f0 = 150; %choose a frequency (Hz) at which to do the maximization
    
    cucumber = lenticklePhase(cucumber,pos,f0,phaseMICH,phasePRCL, phaseSRCL); % this changes the opt inside cucumber
    
end