#caget original alignment slider values
H1:SUS-ITMY_M0_OPTICALIGN_P_OFFSET 18.3369
H1:SUS-ITMY_M0_OPTICALIGN_Y_OFFSET -151.24

#center optical lever

#Gather "as found" coil balance
caget H1:SUS-ITMY_L2_COILOUTF_UL_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_LL_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_UR_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_LR_GAIN;
H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 0.973282
H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.01098
H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -0.98898
H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.02728

# Rewrite that as an install command
caput H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 0.973282;
caput H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.01098;
caput H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -0.98898;
caput H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.02728;

#Set up oscillator excitation path and drive matrix
caput H1:SUS-ITMY_L2_LKIN2OSEM_1_1 1.0; caput H1:SUS-ITMY_L2_LKIN2OSEM_2_1 -1.0; caput H1:SUS-ITMY_L2_LKIN2OSEM_3_1 -1.0; caput H1:SUS-ITMY_L2_LKIN2OSEM_4_1 1.0;

caput H1:SUS-ITMY_LKIN_Y_OSC_FREQ 4.0; caput H1:SUS-ITMY_LKIN_Y_OSC_SINGAIN 10000; caput H1:SUS-ITMY_LKIN_Y_OSC_COSGAIN 10000; caput H1:SUS-ITMY_LKIN_P_OSC_FREQ 4.0; caput H1:SUS-ITMY_LKIN_P_OSC_SINGAIN 10000; caput H1:SUS-ITMY_LKIN_P_OSC_COSGAIN 10000;

caput H1:SUS-ITMY_LKIN_P_DEMOD_SIG_GAIN 1; caput H1:SUS-ITMY_LKIN_P_DEMOD_I_GAIN 1; caput H1:SUS-ITMY_LKIN_P_DEMOD_Q_GAIN 1; caput H1:SUS-ITMY_LKIN_Y_DEMOD_SIG_GAIN 1; caput H1:SUS-ITMY_LKIN_Y_DEMOD_I_GAIN 1; caput H1:SUS-ITMY_LKIN_Y_DEMOD_Q_GAIN 1;

cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_I FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_I FM1 ON;
cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_Q FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_Q FM1 ON;
cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_SIG FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_P_DEMOD_SIG FM1 ON;

cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_I FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_I FM1 ON;
cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_Q FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_Q FM1 ON;
cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_SIG FMALL OFF; cdsutils switch H1:SUS-ITMY_LKIN_Y_DEMOD_SIG FM1 ON;

#Set up COILOUTF to get the most drive
caput H1:SUS-ITMY_BIO_L2_UL_STATEREQ -2; caput H1:SUS-ITMY_BIO_L2_UR_STATEREQ -2; caput H1:SUS-ITMY_BIO_L2_LL_STATEREQ -2; caput H1:SUS-ITMY_BIO_L2_LR_STATEREQ -2;

cdsutils switch H1:SUS-ITMY_L2_COILOUTF_UL FMALL OFF; cdsutils switch H1:SUS-ITMY_L2_COILOUTF_LL FMALL OFF; cdsutils switch H1:SUS-ITMY_L2_COILOUTF_UR FMALL OFF; cdsutils switch H1:SUS-ITMY_L2_COILOUTF_LR FMALL OFF;


#Turn on oscillator
caput H1:SUS-ITMY_LKIN_P_OSC_CLKGAIN 115000; caput H1:SUS-ITMY_LKIN_Y_OSC_CLKGAIN 115000; caput H1:SUS-ITMY_L2_LKIN_EXC_SW 1;

#Restore unbalanced results to make sure phase of LKINs is OK
caput H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 1.0; caput H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.0; caput H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -1; caput H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.0;

#Seems like phase is pretty good...
# double checking
# check the goodness of tuning.
# measure DEMOD output with balance as is...
cdsutils avg -s 200 H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16

H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 0.245519852825 0.00532318561209
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 -0.00546512561472 0.00448975979976
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 0.664444108307 0.00509813294914
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.0242881870529 0.00511762350243

# Then perturb the balance...
# BE MINDFUL OF SATURATIONS

/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.05
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 YAW +0.05

# measure DEMOD output again
H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 1.29283845603 0.00635217765211
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 -0.0163386390879 0.0052213411666
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 -0.971686122715 0.00669284642674
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.0484747687698 0.00635410884795

dPhi = 180/pi * atan( (Q_DC^{before} - Q_DC^{after}) / (I_DC^{before} - I_DC^{after}) )

dPhi_P = 180/pi * atan( (-0.00546512561472 - -0.0163386390879) / (0.245519852825 - 1.29283845603) )
dPhi_P =
     -0.59484

dPhi_Y = 180/pi * atan( (0.0242881870529 - 0.0484747687698) / (0.664444108307 - -0.971686122715) )
dPhi_Y =
     -0.84693

# Both phase changes are small enough, won't bother changing.

# Restore unbalanced values
caput H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 1.0; caput H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.0; caput H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -1; caput H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.0;

# Begin balancing!!
# Minimize the I phase
# Remember PIT imbalance shows up in YAW modulator, YAW imbalance shows up in PIT modulator
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.01
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.007
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.001
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.001
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 PIT -0.001

/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 YAW -0.005
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 YAW -0.005

# Balance looks good.
# New Balance is 
H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 0.970347
H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.00995
H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -0.989949
H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.03035

# Measure DEMOD
H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 -0.0245710772189 0.00704130185547
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 0.0425274949505 0.00789090886104
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 -0.0347399575432 0.00756534398619
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.0565340150328 0.00681254638218
# Look at the I phase for P and Y. Is it 0 +/- within the std deviation? (i.e. is the result statistically consistent with zero wihin a 68% confidence interval?)
# mehh... it's not, but the StripTool looks pretty close to zero.
# Measure again...
H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 -0.0214784849784 0.0130807254945
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 0.0018164542201 0.0138811259195
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 -0.0129665806607 0.0133191915537
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.00218462433666 0.012451518395
# mehh ... closer, but the uncertainty's larger in the measurement.
# one more check...
H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 -0.019837905595 0.0049311213555
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 0.0136711541365 0.00731385076477
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 0.000703754775313 0.00566601700725
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.0417053521553 0.00809617374349

# OK. Let's leave PIT imbalance (the YAW DEMOD) alone, but adjust YAW imbalance (the PIT DEMOD) a bit.
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMY L2 YAW +0.001
H1:SUS-ITMY_LKIN_P_DEMOD_I_OUT16 0.00103501160011 0.00243373127489
H1:SUS-ITMY_LKIN_P_DEMOD_Q_OUT16 0.00735979117977 0.00270060003333
H1:SUS-ITMY_LKIN_Y_DEMOD_I_OUT16 -0.0149198963342 0.00306063767139
H1:SUS-ITMY_LKIN_Y_DEMOD_Q_OUT16 0.0436143444093 0.00305133448988

#Winner winner Chicken dinner.
caget H1:SUS-ITMY_L2_COILOUTF_UL_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_LL_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_UR_GAIN; caget H1:SUS-ITMY_L2_COILOUTF_LR_GAIN;
H1:SUS-ITMY_L2_COILOUTF_UL_GAIN 0.971803
H1:SUS-ITMY_L2_COILOUTF_LL_GAIN -1.01146
H1:SUS-ITMY_L2_COILOUTF_UR_GAIN -0.988465
H1:SUS-ITMY_L2_COILOUTF_LR_GAIN 1.02881

Now restore nominal configuration (well, at least high drive)

#Turn off oscillator
caput H1:SUS-ITMY_LKIN_P_OSC_CLKGAIN 0; caput H1:SUS-ITMY_LKIN_Y_OSC_CLKGAIN 0; caput H1:SUS-ITMY_L2_LKIN_EXC_SW 0;
#Restore COILOUTF compensation
caput H1:SUS-ITMY_BIO_L2_UL_STATEREQ 2; caput H1:SUS-ITMY_BIO_L2_UR_STATEREQ 2; caput H1:SUS-ITMY_BIO_L2_LL_STATEREQ 2; caput H1:SUS-ITMY_BIO_L2_LR_STATEREQ 2;
# Zero out LKIN Matrix
caput H1:SUS-ITMY_L2_LKIN2OSEM_1_1 0; caput H1:SUS-ITMY_L2_LKIN2OSEM_2_1 0; caput H1:SUS-ITMY_L2_LKIN2OSEM_3_1 0; caput H1:SUS-ITMY_L2_LKIN2OSEM_4_1 0;

# ACCEPT NEW BALANCE VALUES INTO SDF SYSTEM.