function runbKAGRAp1_ASC()
% Sweep plots and sensing matrix for bKAGRA Phase 1 ASC
% uses Optickle 2 and Tools directory in KAGRAOptickle SVN
% needs (1) parambKAGRAp1.m          for parameter setting
%       (2) optbKAGRAp1.m            for constructing Optickle model
%       (3) plotSweep.m              for sweep plots (lives in Tools directory)
%       (4) showSensingMatrix.m      for showing sensing matrix (lives in Tools directory)
% Initiated by: Yuta Michimura
% Edited by: Yutaro Enomoto

%% Clear everything
close all
clear all
%cd('C:\Users\yuta\Lab\LCGT\IFOmodel\KAGRAOptickle\bKAGRAPhase1')  % CHANGE THIS TO YOUR DIRECTORY
cd('/Users/yenomoto/Desktop/KAGRAOptickle/mif/KAGRAOptickle/bKAGRAPhase1')

%% Set parameters and save directory
% set parameters
p = parambKAGRAp1();

% set results directory
saveDir=['./results/',p.modelName,'-',datestr(now,'yyyy-mm-dd'),'/'];
mkdir(saveDir);

%% Construct optickle model and set some parameters, lists
opt = optbKAGRAp1(p);
opt2dot(opt, [saveDir,p.modelName,'.pdf']); % GraphViz needs to be installed

% some parameters
lambda = opt.lambda(1);

% DOF definitions
probeNames={'REFLA_I', 'REFLA_Q','REFLB_I', 'REFLB_Q', 'ASA_I', 'ASA_Q', 'ASB_I', 'ASB_Q'...
    , 'POPA_DC', 'POPB_DC', 'TRXA_DC', 'TRXB_DC', 'TRYA_DC', 'TRYB_DC'};  % probes of interest for ASC
driveNames = {'ETMX','ETMY','BS','PRM','PR2','PR3'};  % drives of interest
dofNames = {'COMM','DIFF','BS','PRM','PR2''','PR3'''};   % DOF of interest
mDrv = [1,1,0,0,0,0; % COMM  Conversion matrix for optic motion to DOF motion
        1,-1,0,0,0,0; % DIFF
        0,0,1,0,0,0; % BS
        0,0,0,1,0,0; % PRM
        %0,0,0,0,1,0; %PR2
        %0,0,0,0,0,1]; %PR3
        0,0,0,0,0.99,-0.122;
        0,0,0,0,0.122,0.99];
    
%% Check power at each field, each probe
[fDC, sigDC] = opt.tickle([], []);
fprintf('DC fields (fDC matrix):\n');
showfDC(opt, fDC);
fprintf('\nProbes (sigDC matrix):\n');
showsigDC(opt, sigDC);

%% Check stability of PRC
if abs(p.criteria) > 1
    warning('PRMI is unstable!');
end

%p.criteria

%% Sensing Matrix for ASC
% Pitch
[sigAC] = opt.tickle01([], p.ftickle01);
diary on
fprintf('COMM = ETMX + ETMY\n');
fprintf('DIFF = ETMX - ETMY\n');
fprintf('PR2'' = 0.99*PR2 - 0.122*PR3\n');
fprintf('PR3'' = 0.122*PR2 + 0.99*PR3\n');
fprintf('\nASC Sensing Matrix for %s:\n',p.modelName);
diary([saveDir,'/ASCPITSensingMatrix.txt'])
showSensingMatrix(opt,sigAC,probeNames,driveNames,dofNames,mDrv);
showSensingMatrix(opt,sigAC,{'REFLA_I', 'REFLB_I', 'ASA_Q', 'POPA_DC', 'TRXA_DC', 'TRYA_DC'},driveNames,dofNames,mDrv);
%showSensingMatrix(opt,sigAC,probeNames,driveNames)
%showSensingMatrix(opt,sigAC,{opt.probe.name},driveNames)
%showSensingMatrix(opt,sigAC)
% 
% readoutNames = {'TRCOMMA_DC', 'TRDIFFA_DC','ASA_Q-k*TRDIFFA_DC'};
% mPrb = [1,1,0;
%         1,-1,0;
%         -4.07,4.07,1];
% fprintf('\nASC Sensing Matrix for %s (COMM, BS, DIFF):\n',p.modelName);
% showSensingMatrix(opt,sigAC,{'TRXA_DC', 'TRYA_DC', 'ASA_Q'},driveNames,dofNames,mDrv,readoutNames,mPrb);
% 
% fprintf('\nASC Sensing Matrix for %s (PRM):\n',p.modelName);
% showSensingMatrix(opt,sigAC,{'REFLB_I'},driveNames,dofNames,mDrv);
% 
% readoutNames = {'REFLA_I-COMM-BS ','POPA_DC-PRM'};
% mPrb = [10.78 + 11.11,10.78-11.11,1,0,0;
%         0            ,0          ,0,18.45,1];
% fprintf('\nASC Sensing Matrix for %s (PR2'', PR3''):\n',p.modelName);
% showSensingMatrix(opt,sigAC,{'TRXA_DC', 'TRYA_DC', 'REFLA_I', 'REFLB_I', 'POPA_DC'},driveNames,dofNames,mDrv,readoutNames,mPrb);

readoutNames = {'a','b','c','d','e','f'};
mPrb = [1,0,0,0,0,0;
        0,1,0,0,0,0;
        0,0,1,0,0,0;
        0,0,0,1,0,0;
        0,0,0,18,1,0;
        0,0,0,-2.9,0,1]...
     * [1,0,0,0,0,0;
        0,0,1,0,0,0;
        0,0,0,0,1,-1;
        0,1,0.015,0,0,0;
        0,0,0,1,0,0;
        1,0,0,0,16,0];
showSensingMatrix(opt,sigAC,{'REFLA_I', 'REFLB_I', 'ASA_Q', 'POPA_DC', 'TRXA_DC', 'TRYA_DC'},driveNames,dofNames,mDrv,readoutNames,mPrb);
diary off

%mPrb
%p.difcriteria
%p.z
%p.z0
%sqrt(lambda*(p.z0+p.z^2/p.z0)/pi)