#!/usr/bin/python

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

# This script monitors PR3 OPITCALIGN channels, and uses the input alignment drive matrix to make pr2 and im4 follow in such a way that the spot only moves on pr2, but not im4 or pr3

pitPR3toPR2=-9.2;
yawPR3toPR2=+9.2;
pitPR3toIM4=56;
yawPR3toIM4=11;
pitPR3toPRM=1.5;
yawPR3toPRM=2.2;

p3r=ezca['SUS-PR3_M1_OPTICALIGN_P_OFFSET'];
y3r=ezca['SUS-PR3_M1_OPTICALIGN_Y_OFFSET'];
p2r=ezca['SUS-PR2_M1_OPTICALIGN_P_OFFSET'];
y2r=ezca['SUS-PR2_M1_OPTICALIGN_Y_OFFSET'];
p4r=ezca['SUS-IM4_M1_OPTICALIGN_P_OFFSET'];
y4r=ezca['SUS-IM4_M1_OPTICALIGN_Y_OFFSET'];
pMr=ezca['SUS-PRM_M1_OPTICALIGN_P_OFFSET'];
yMr=ezca['SUS-PRM_M1_OPTICALIGN_Y_OFFSET'];

print "Start:"
print "SUS-PR3_M1_OPTICALIGN_P_OFFSET",p3r
print "SUS-PR3_M1_OPTICALIGN_Y_OFFSET",y3r
print "SUS-PR2_M1_OPTICALIGN_P_OFFSET",p2r
print "SUS-PR2_M1_OPTICALIGN_Y_OFFSET",y2r
print "SUS-IM4_M1_OPTICALIGN_P_OFFSET",p4r
print "SUS-IM4_M1_OPTICALIGN_Y_OFFSET",y4r
print "SUS-PRM_M1_OPTICALIGN_P_OFFSET",p2r
print "SUS-PRM_M1_OPTICALIGN_Y_OFFSET",y2r

while True:
  p3=ezca['SUS-PR3_M1_OPTICALIGN_P_OFFSET'];
  y3=ezca['SUS-PR3_M1_OPTICALIGN_Y_OFFSET'];
  ezca['SUS-PR2_M1_OPTICALIGN_P_OFFSET'] = p2r + pitPR3toPR2*(p3-p3r)
  ezca['SUS-PR2_M1_OPTICALIGN_Y_OFFSET'] = y2r + yawPR3toPR2*(y3-y3r)
  ezca['SUS-IM4_M1_OPTICALIGN_P_OFFSET'] = p4r + pitPR3toIM4*(p3-p3r)
  ezca['SUS-IM4_M1_OPTICALIGN_Y_OFFSET'] = y4r + yawPR3toIM4*(y3-y3r)
  ezca['SUS-PRM_M1_OPTICALIGN_P_OFFSET'] = pMr + pitPR3toPRM*(p3-p3r)
  ezca['SUS-PRM_M1_OPTICALIGN_Y_OFFSET'] = yMr + yawPR3toPRM*(y3-y3r)