import subprocess
import time
import traceback

running = False
counts = {}

def countHS(motor):
  global counts
  try:
    time.sleep(1.0)
    counts[motor]=0   
    def on_change(value):
        global counts
        print "HomeSW changed on" , motor, "to", value
        if int(value) == 1:
            counts[motor]=counts[motor]+1            

    with Channel(motor+".ATHM", callback=on_change, monitored=True, name="test") as ch:
        if int(ch.read()) == 1:
            counts[motor]=1
        while(running==True):
            time.sleep(0.1)
    print "Home Switch motor "+motor +" reached "+ str(counts[motor]) + " times\n"
    return counts[motor]
  except:
    print "Exception counting "+str(motor) 
    traceback.print_exc()
    raise

def init():
  try:
    global running
    running = True

    print "startint CMU initilization"
    x = caget('X11MA-OP-CM:ox')
    y = caget('X11MA-OP-CM:oy')
    z = caget('X11MA-OP-CM:oz')
    Rx= caget('X11MA-OP-CM:oRx')
    Ry= caget('X11MA-OP-CM:oRy')
    Rz= caget('X11MA-OP-CM:oRz')
    baf=caget('X11MA-OP2-CM:TRB.RBV')

    print "Old motor positions: x = " +str(x)+ \
                             ", y = " +str(y)+ \
                             ", z = " +str(z)+ \
                             ", Rx = " +str(Rx)+ \
                             ", Ry = " +str(Ry)+ \
                             ", Rz = " +str(Rz)+ \
                             ", baffle = " +str(baf)

    subprocess.call("X_X11MA_init-CMU.sh", shell=True)
    print "Positions after initialisation: x = " +str(caget('X11MA-OP-CM:ox'))+ \
                                        ", y = " +str(caget('X11MA-OP-CM:oy'))+ \
                                        ", z = " +str(caget('X11MA-OP-CM:oz'))+ \
                                        ", Rx = " +str(caget('X11MA-OP-CM:oRx'))+ \
                                        ", Ry = " +str(caget('X11MA-OP-CM:oRy'))+ \
                                        ", Rz = " +str(caget('X11MA-OP-CM:oRz'))+ \
                                        ", baffle = " +str(caget('X11MA-OP2-CM:TRB.RBV'))

    print "Restoring old motor positions"
    caput('X11MA-OP-CM:y',y)
    caput('X11MA-OP-CM:z',z)
    caput('X11MA-OP-CM:Rx',Rx)
    caput('X11MA-OP-CM:Ry',Ry)
    caput('X11MA-OP-CM:Rz',Rz)

#check restored positions after restore
    print "Restored positions: x = " +str(caget('X11MA-OP-CM:ox'))+ \
                        ", y = " +str(caget('X11MA-OP-CM:oy'))+ \
                        ", z = " +str(caget('X11MA-OP-CM:oz'))+ \
                        ", Rx = " +str(caget('X11MA-OP-CM:oRx'))+ \
                        ", Ry = " +str(caget('X11MA-OP-CM:oRy'))+ \
                        ", Rz = " +str(caget('X11MA-OP-CM:oRz'))+ \
                        ", baffle = " +str(caget('X11MA-OP2-CM:TRB.RBV'))
    if(abs(caget('X11MA-OP-CM:ox')-x)>0.001):
        print "X position not ok"
    elif(abs(caget('X11MA-OP-CM:oy')-y)>0.001):
        print "Y position not ok"
    elif(abs(caget('X11MA-OP-CM:oz')-z)>0.001):
        print "Z position not ok"
    elif(abs(caget('X11MA-OP-CM:oRx')-Rx)>0.001):
        print "Rx position not ok"
    elif(abs(caget('X11MA-OP-CM:oRy')-Ry)>0.001):
        print "Ry position not ok"
    elif(abs(caget('X11MA-OP-CM:oRz')-Rz)>0.001):
        print "Rz position not ok"
    elif(abs(caget('X11MA-OP2-CM:TRB.RBV')-baf)>0.001):
        print "baffle position not ok"
    else:
        print "All positions OK"
    
    running = False
    return 0
  except:
    print "Exception in init"
    traceback.print_exc()
    raise    
    
#Fork and join
futures = fork(init,(countHS,("X11MA-OP2-CM:TRY",)), \
                    (countHS,("X11MA-OP2-CM:TRZ",)), \
                    (countHS,("X11MA-OP2-CM:ROX",)), \
                    (countHS,("X11MA-OP2-CM:ROY",)), \
                    (countHS,("X11MA-OP2-CM:ROZ",)), \
                    (countHS,("X11MA-OP2-CM:TRB",))  )
ret = join(futures)

#check home switch status after init
homesw=True    
for i in range(1,5):
    if(ret[i]!=1):
        homesw=False
print "Home switch status for all motors: %s" %("OK" if homesw == True else "Not OK")

#final status report        
if(homesw==True and ret[0]==0):
    print "init-CMU: ==> done. OK!"
else:
    print "init-CMU: ==> not done. NOT OK!"