%analyze data from charge measurement for ETMY

%% import the data

%pre factor is because I frogt to apply calibration in DTT
mag=(4.92035e-16/1.29353e-6)*[1.24195e-7,... ref 2 drive bias bias -400 V
    5.88469e-8,... ref5 drive bias (gain 0.5) bias -200V
    7.82342e-9,...ref 8 drive bias bias 0V
    3.28606e-8,...ref 11 drive signal bias 0
    2.08399e-7,...ref 14 drive signal signal offset 50,000 cnts DA L2L
    1.29353e-6];%ref 17drive signal bias -400V

phase=[-80.97,...
    -79.2897,...
    72.56,...
    -146.168,...
    34.924,...
    -144.835];

TF_cmplx=mag.*exp(2*pi.*1i*phase/360);

TF_bias_drive=TF_cmplx(1:3);
TF_signal_drive=TF_cmplx(4:6);

%% calibrate 

%clibrate bias drive into volts applied to ESD
DAC_volts_per_cnts=20/2^18; %volts per count out of DAC
HV_gain=40; %volts out of HV driver per Volts out of DAC

%bias low pass from page 10 of D1500016
RC1=30e3*3e-6;
RC2=10e3*10e-9;
bias_filter=zpk([],[1/RC1,1/RC1, 1/RC2],(RC1*RC1*RC2)^-1);

excitation_freq=12;  %Hz
bias_filt_frsp=evalfr(bias_filter, 1j*2*pi*excitation_freq);  %get the frequency response of the filter at the excitation frequency

TF_bias_drive_m_per_V=TF_bias_drive./(DAC_volts_per_cnts*HV_gain*bias_filt_frsp);  %meters of DARM per volt applied to bias electrode

%calibrate signal drive into volts applied to ESD
EUL2ESD=0.25; %drive was measured at DRIVEALIGN_L_OUT_DQ
% %ESD was in LOW voltage mode with low pass on, DC gain of 2 (page 7 of
% %D1500016)
% %this low pass is compensated in the OUTF, don't need this model!
% signal_LP_filt=zpk(2*pi*[50,50],2*pi*[2.2,2.2],(2.2/50)^2);
% signal_filt_frsp=evalfr(signal_LP_filt, 1j*2*pi*excitation_freq);  %get the frequency response of the filter at the excitation frequency
% LV_gain=2; %gain of 2 in LV driver
%use ESD driver in HV mode
TF_signal_drive_m_per_V=TF_signal_drive./(EUL2ESD*DAC_volts_per_cnts*HV_gain);

signal_offset=-1*50000*EUL2ESD*DAC_volts_per_cnts*HV_gain;  %in votls

P=bodeoptions;
P.MagUnits= 'abs';
P.MagScale='log';
P.PhaseWrapping='On';
P.FreqUnits='Hz';
figure(1)
bodeplot(bias_filter, signal_LP_filt,P)

%use sus model to convert DARM meters to Newtons on test mass
addpath('/ligo/svncommon/SusSVN/sus/trunk/QUAD/Common/FilterDesign/MatFiles/')
addpath('/ligo/svncommon/SusSVN/sus/trunk/Common/MatlabTools/')
load('dampingfilters_QUAD_2014-11-10_LLO_model.mat','susModel', 'freq')
tstL2tstL_mpN = squeeze(susModel.dampedf(susModel.dampedout.tst.disp.L, susModel.dampedin.tst.drive.L,:));
%find excitation freq
m_per_Newton=tstL2tstL_mpN(find(freq>excitation_freq,1));

signal_N_per_V=abs(TF_signal_drive_m_per_V./m_per_Newton).*sign(cos(angle(TF_signal_drive_m_per_V./m_per_Newton))); %am I screwing up signs? probably
bias_N_per_V=abs(TF_bias_drive_m_per_V./m_per_Newton).*sign(cos(angle(TF_bias_drive_m_per_V./m_per_Newton)));

%% get parameters out  (Equation 2 of G1600699)

bias_applied=40*9.5*[-1,-0.5,0];
bias_applied_signal_drive=[0,signal_offset,-9.5*40];

figure(2)
plot(bias_applied, abs(bias_N_per_V),'o',...
    bias_applied_signal_drive, abs(signal_N_per_V), 'o')
ylabel('Newtons per volt')
legend('drive bias', 'drive signal electrodes', 'Location', 'NorthWest')

%beta+beta2= drive bias no DC bias or signal  beta-beta2= drive signal no
%DC bias or signal

beta=(bias_N_per_V(3)+signal_N_per_V(1))/2
beta2=(bias_N_per_V(3)-signal_N_per_V(1))/2

%alpha+gamma= 2*slope of bias_N_per_V with Vb
alpha_plus_gamma1=(bias_N_per_V(1)-bias_N_per_V(3))/(2*bias_applied(1));  %since I forgot to turn of the linearization this meas is not good
alpha_plus_gamma2=(signal_N_per_V(3)-signal_N_per_V(1))/(2*bias_applied_signal_drive(3));

%gamma-alpha=
gamma_minus_alpha=(signal_N_per_V(2)-signal_N_per_V(1))/(2*bias_applied_signal_drive(2));

gamma=(gamma_minus_alpha+alpha_plus_gamma2)/2
alpha=(alpha_plus_gamma2-gamma_minus_alpha)/2

Veff=(beta-beta2)/(2*(alpha-gamma))