# caget original alignment slider values
caget H1:SUS-ITMX_M0_OPTICALIGN_P_OFFSET; caget H1:SUS-ITMX_M0_OPTICALIGN_Y_OFFSET;
H1:SUS-ITMX_M0_OPTICALIGN_P_OFFSET -167.307
H1:SUS-ITMX_M0_OPTICALIGN_Y_OFFSET 105.256

# center optical lever

# gather "as found" coil balance 
caget H1:SUS-ITMX_L2_COILOUTF_UL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_UR_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LR_GAIN;
H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 0.909
H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -0.965
H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1.035
H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 1.099

# re-write that as an install command
caput H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 0.909; 
caput H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -0.965; 
caput H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1.035; 
caput H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 1.099;

# set up oscialltory excitation path and drive matrix
caput H1:SUS-ITMX_L2_LKIN2OSEM_1_1 1.0; caput H1:SUS-ITMX_L2_LKIN2OSEM_2_1 -1.0; caput H1:SUS-ITMX_L2_LKIN2OSEM_3_1 -1.0; caput H1:SUS-ITMX_L2_LKIN2OSEM_4_1 1.0;

caput H1:SUS-ITMX_LKIN_Y_OSC_FREQ 3.9; caput H1:SUS-ITMX_LKIN_Y_OSC_SINGAIN 10000; caput H1:SUS-ITMX_LKIN_Y_OSC_COSGAIN 10000; caput H1:SUS-ITMX_LKIN_P_OSC_FREQ 3.9; caput H1:SUS-ITMX_LKIN_P_OSC_SINGAIN 10000; caput H1:SUS-ITMX_LKIN_P_OSC_COSGAIN 10000;

caput H1:SUS-ITMX_LKIN_P_DEMOD_SIG_GAIN 1; caput H1:SUS-ITMX_LKIN_P_DEMOD_I_GAIN 1; caput H1:SUS-ITMX_LKIN_P_DEMOD_Q_GAIN 1; caput H1:SUS-ITMX_LKIN_Y_DEMOD_SIG_GAIN 1; caput H1:SUS-ITMX_LKIN_Y_DEMOD_I_GAIN 1; caput H1:SUS-ITMX_LKIN_Y_DEMOD_Q_GAIN 1;

cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_I FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_I FM1 ON;
cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_Q FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_Q FM1 ON;
cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_SIG FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_P_DEMOD_SIG FM1 ON;

cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_I FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_I FM1 ON;
cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_Q FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_Q FM1 ON;
cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_SIG FMALL OFF; cdsutils switch H1:SUS-ITMX_LKIN_Y_DEMOD_SIG FM1 ON;

# set up COILOUTF to get the most drive
caput H1:SUS-ITMX_BIO_L2_UL_STATEREQ -2; caput H1:SUS-ITMX_BIO_L2_UR_STATEREQ -2; caput H1:SUS-ITMX_BIO_L2_LL_STATEREQ -2; caput H1:SUS-ITMX_BIO_L2_LR_STATEREQ -2;

cdsutils switch H1:SUS-ITMX_L2_COILOUTF_UL FMALL OFF; cdsutils switch H1:SUS-ITMX_L2_COILOUTF_LL FMALL OFF; cdsutils switch H1:SUS-ITMX_L2_COILOUTF_UR FMALL OFF; cdsutils switch H1:SUS-ITMX_L2_COILOUTF_LR FMALL OFF;

# open diagnotistic tools and strip tool
StripTool /ligo/home/jeffrey.kissel/Templates/StripTool/CoilBalancing_ITMX_L2_P.stp &
StripTool /ligo/home/jeffrey.kissel/Templates/StripTool/CoilBalancing_ITMX_L2_Y.stp &

diaggui 2018-09-05_H1SUSITMX_L2_CoilBalancing_ASDs.xml &

# turn on oscillator
caput H1:SUS-ITMX_LKIN_P_OSC_CLKGAIN 110000; caput H1:SUS-ITMX_LKIN_Y_OSC_CLKGAIN 110000; caput H1:SUS-ITMX_L2_LKIN_EXC_SW 1;

# restore unbalanced results to make sure phase of LKINs is OK
caput H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 1.0; caput H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -1.0; caput H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1; caput H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 1.0;

# phase is bad
(was P = 259, Y = 256)

# Tune phase by-hand
caget H1:SUS-ITMX_LKIN_P_DEMOD_PHASE; caget H1:SUS-ITMX_LKIN_Y_DEMOD_PHASE;
H1:SUS-ITMX_LKIN_P_DEMOD_PHASE 191
H1:SUS-ITMX_LKIN_Y_DEMOD_PHASE 231

# Now check for phase insensitivity of Q by adding a large imbalance.
# First gather results with no imbalance
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16

H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 3.6061557138 0.0140386039431
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.203807335475 0.0119561991026
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -8.72759542942 0.0117307043989
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.172466085978 0.0114102312527

# then perturn the balance
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT -0.05
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.05

Note -- 
- Pitch was the wrong way
+ Yaw was the right way

cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 -0.814135293812 0.014008480246
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.0523973254568 0.0138189376538
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -14.3954840803 0.0124917080746
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.145635257831 0.0112111653999

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

dPhi_P = 180/pi * atan( (0.203807335475 - 0.0523973254568) / (3.6061557138 - -0.814135293812) )
dPhi_P = 1.9618
dPhi_Y = 180/pi * atan( (-0.172466085978 - -0.145635257831) / (-8.72759542942 - -14.3954840803) )
dPhi_Y =-0.27123

Adding 2 deg to P, leaving Y alone
caget H1:SUS-ITMX_LKIN_P_DEMOD_PHASE; caget H1:SUS-ITMX_LKIN_Y_DEMOD_PHASE;
H1:SUS-ITMX_LKIN_P_DEMOD_PHASE 193
H1:SUS-ITMX_LKIN_Y_DEMOD_PHASE 231


# restore to unbalanced
caput H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 1.0; caput H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -1.0; caput H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1; caput H1:SUS-ITMX_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 ITMX L2 PIT +0.05
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT +0.01
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT +0.015

/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.03
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.01
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.01

# Balance looks good.
# New Balance is 
caget H1:SUS-ITMX_L2_COILOUTF_UL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_UR_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LR_GAIN;
H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 1.13098
H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -0.973363
H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1.02334
H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 0.880719

# Measure DEMOD output to confirm
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 0.170980390987 0.0173459436437
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.075668032628 0.0196293924306
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -0.202383961352 0.0135352991968
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 0.0185476530163 0.0133833696628

# 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?)
# Nope. 
# P DEMOD's I phase is high, so keep going with +Y
# Y DEMOD's I phase is low, so keep going with +P
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT +0.001
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.001

# Check again
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 0.0612140417635 0.0146764077405
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.0478253706294 0.0139960596638
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -0.13863884909 0.0115213282664
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.0441312226822 0.00837834092288
# no, but closer. Try one more 0.001 nudge
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT +0.001
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 YAW +0.001

# Check again
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 0.00725626343628 0.0138861768523
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.113939210332 0.0158941041369
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 0.0271456346044 0.0133086655688
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.0103702577188 0.0119928569442
# P DEMOD is within +/-std of 0, so yaw is good.
# Y DEMOD has now overshot by a bit, so going back by half in pitch,
/ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT -0.0005

# Check again
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 -0.0127908072178 0.0179001946678
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.0470961165114 0.0141858662881
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -0.129469864185 0.0123245870995
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.00611569899134 0.0153389949632

# Hrmm ... P DEMOD still OK, but Y DEMOD is now too low again. 
# Measuring again just to confirm.
cdsutils avg -s 200 H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 -0.0251822975687 0.0139139497842
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.153502975239 0.0148007957609
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -0.115228760501 0.0149973821375
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.00411851199868 0.0139461529921
# Yup, still too low. nudging pitch back in positive
 /ligo/svncommon/SusSVN/sus/trunk/Common/PythonTools/perturbcoilbalance_fourosem.py H1 ITMX L2 PIT +0.0005

# Check again
H1:SUS-ITMX_LKIN_P_DEMOD_I_OUT16 -0.0333551795967 0.01612997567
H1:SUS-ITMX_LKIN_P_DEMOD_Q_OUT16 0.0340521535231 0.0155561082614
H1:SUS-ITMX_LKIN_Y_DEMOD_I_OUT16 -0.0309002276603 0.0122683108456
H1:SUS-ITMX_LKIN_Y_DEMOD_Q_OUT16 -0.0350294934234 0.0120241908064
# Yeah.... alright. I'm getting bored. Let's call it good.

# Gather new "awesomeness" balance awesomeness,
caget H1:SUS-ITMX_L2_COILOUTF_UL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LL_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_UR_GAIN; caget H1:SUS-ITMX_L2_COILOUTF_LR_GAIN;
H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 1.13552
H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -0.973361
H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1.02334
H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 0.877201

# Turn it into a command
caput H1:SUS-ITMX_L2_COILOUTF_UL_GAIN 1.13552; 
caput H1:SUS-ITMX_L2_COILOUTF_LL_GAIN -0.973361; 
caput H1:SUS-ITMX_L2_COILOUTF_UR_GAIN -1.02334; 
caput H1:SUS-ITMX_L2_COILOUTF_LR_GAIN 0.877201;