#!/usr/bin/python

import numpy as np
import ezca as ez
import time
ezca = ez.Ezca()


#pitPR3toPR2=-9.58; #9.2 from pr2spotmove times 1.0412 from theory
#yawPR3toPR2=+9.58; #9.2 from pr2spotmove times 1.0412 from theory

# set 2
gppr3=0.086
gpprm=-13.07
gpim4=-305
gpsr2=-2.73
gpsrm=4.29
gpex=-0.7
gpey=-0.7
gpix=-0.45
gpiy=-0.35

# set 1
gppr3=0.07
gpprm=-13.2
gpim4=-325
gpsr2=-2.39
gpsrm=3.2
gpex=-0.7
gpey=-0.7
gpix=-0.45
gpiy=-0.35

# set 1 yaw
gypr3=0.028
gyprm=11.07
gyim4=61.22
gysr2=2.22
gysrm=2.74
gyex=-0.5
gyey=+0.5
gyix=+0.32
gyiy=-0.31


ppr3=ezca['SUS-PR3_M1_OPTICALIGN_P_OFFSET'];
pprm=ezca['SUS-PRM_M1_OPTICALIGN_P_OFFSET'];
pim4=ezca['SUS-IM4_M1_OPTICALIGN_P_OFFSET'];
psr2=ezca['SUS-SR2_M1_OPTICALIGN_P_OFFSET'];
psrm=ezca['SUS-SRM_M1_OPTICALIGN_P_OFFSET'];
pex =ezca['SUS-ETMX_M0_OPTICALIGN_P_OFFSET'];
pey =ezca['SUS-ETMY_M0_OPTICALIGN_P_OFFSET'];
pix =ezca['SUS-ITMX_M0_OPTICALIGN_P_OFFSET'];
piy =ezca['SUS-ITMY_M0_OPTICALIGN_P_OFFSET'];

ypr3=ezca['SUS-PR3_M1_OPTICALIGN_Y_OFFSET'];
yprm=ezca['SUS-PRM_M1_OPTICALIGN_Y_OFFSET'];
yim4=ezca['SUS-IM4_M1_OPTICALIGN_Y_OFFSET'];
ysr2=ezca['SUS-SR2_M1_OPTICALIGN_Y_OFFSET'];
ysrm=ezca['SUS-SRM_M1_OPTICALIGN_Y_OFFSET'];
yex =ezca['SUS-ETMX_M0_OPTICALIGN_Y_OFFSET'];
yey =ezca['SUS-ETMY_M0_OPTICALIGN_Y_OFFSET'];
yix =ezca['SUS-ITMX_M0_OPTICALIGN_Y_OFFSET'];
yiy =ezca['SUS-ITMY_M0_OPTICALIGN_Y_OFFSET'];


print "Start:"
print 'SUS-PR3_M1_OPTICALIGN_P_OFFSET',ppr3
print 'SUS-PRM_M1_OPTICALIGN_P_OFFSET',pprm
print 'SUS-IM4_M1_OPTICALIGN_P_OFFSET',pim4
print 'SUS-SR2_M1_OPTICALIGN_P_OFFSET',psr2
print 'SUS-SRM_M1_OPTICALIGN_P_OFFSET',psrm
print 'SUS-ETMX_M0_OPTICALIGN_P_OFFSET',pex
print 'SUS-ETMY_M0_OPTICALIGN_P_OFFSET',pey
print 'SUS-ITMX_M0_OPTICALIGN_P_OFFSET',pix
print 'SUS-ITMY_M0_OPTICALIGN_P_OFFSET',piy

print 'SUS-PR3_M1_OPTICALIGN_Y_OFFSET',ypr3
print 'SUS-PRM_M1_OPTICALIGN_Y_OFFSET',yprm
print 'SUS-IM4_M1_OPTICALIGN_Y_OFFSET',yim4
print 'SUS-SR2_M1_OPTICALIGN_Y_OFFSET',ysr2
print 'SUS-SRM_M1_OPTICALIGN_Y_OFFSET',ysrm
print 'SUS-ETMX_M0_OPTICALIGN_Y_OFFSET',yex
print 'SUS-ETMY_M0_OPTICALIGN_Y_OFFSET',yey
print 'SUS-ITMX_M0_OPTICALIGN_Y_OFFSET',yix
print 'SUS-ITMY_M0_OPTICALIGN_Y_OFFSET',yiy


while True:
  rpprm=ezca['SUS-PRM_M1_OPTICALIGN_P_OFFSET'];
  gp=1.0/gpprm*(rpprm-pprm)
  ezca['SUS-PR3_M1_OPTICALIGN_P_OFFSET']  = ppr3 + gppr3*gp
  #ezca['SUS-PRM_M1_OPTICALIGN_P_OFFSET']  = pprm + gpprm*gp
  ezca['SUS-IM4_M1_OPTICALIGN_P_OFFSET']  = pim4 + gpim4*gp
  ezca['SUS-SR2_M1_OPTICALIGN_P_OFFSET']  = psr2 + gpsr2*gp
  ezca['SUS-SRM_M1_OPTICALIGN_P_OFFSET']  = psrm + gpsrm*gp
  ezca['SUS-ETMX_M0_OPTICALIGN_P_OFFSET'] = pex  + gpex *gp
  ezca['SUS-ETMY_M0_OPTICALIGN_P_OFFSET'] = pey  + gpey *gp
  ezca['SUS-ITMX_M0_OPTICALIGN_P_OFFSET'] = pix  + gpix *gp
  ezca['SUS-ITMY_M0_OPTICALIGN_P_OFFSET'] = piy  + gpiy *gp

  ryprm=ezca['SUS-PRM_M1_OPTICALIGN_Y_OFFSET'];
  gy=1.0/gyprm*(ryprm-yprm)
  ezca['SUS-PR3_M1_OPTICALIGN_Y_OFFSET']  = ypr3 + gypr3*gy
  #ezca['SUS-PRM_M1_OPTICALIGN_Y_OFFSET']  = yprm + gyprm*gy
  ezca['SUS-IM4_M1_OPTICALIGN_Y_OFFSET']  = yim4 + gyim4*gy
  ezca['SUS-SR2_M1_OPTICALIGN_Y_OFFSET']  = ysr2 + gysr2*gy
  ezca['SUS-SRM_M1_OPTICALIGN_Y_OFFSET']  = ysrm + gysrm*gy
  ezca['SUS-ETMX_M0_OPTICALIGN_Y_OFFSET'] = yex  + gyex *gy
  ezca['SUS-ETMY_M0_OPTICALIGN_Y_OFFSET'] = yey  + gyey *gy
  ezca['SUS-ITMX_M0_OPTICALIGN_Y_OFFSET'] = yix  + gyix *gy
  ezca['SUS-ITMY_M0_OPTICALIGN_Y_OFFSET'] = yiy  + gyiy *gy