import time
import traceback

running = False
counts = {}
encoder_raw = {}


def crossZero(encoder):
  global counts
  global encoder_raw
  try:
    time.sleep(1.0)
    counts[encoder]=0
    if caget(encoder+".RVAL") > 0:
        encoder_raw[encoder]=1
    elif caget(encoder+".RVAL") < 0:
        encoder_raw[encoder]=-1   
    def on_change(value):
        global counts
        global encoder_raw 
        if (encoder_raw[encoder]==1 and value < 0):
            counts[encoder]=counts[encoder]+1 
            encoder_raw[encoder]=-1
            print "Zero on" , encoder, "crossed"
        elif(encoder_raw[encoder]==-1 and value > 0):
            counts[encoder]=counts[encoder]+1 
            encoder_raw[encoder]=1
            print "Zero on" , encoder, "crossed"    
              
        #if int(value) == 0:
        #    print "Raw vlaue on" , encoder, "crossed", value
        #    counts[encoder]=counts[encoder]+1            

    with Channel(encoder+".RVAL", callback=on_change, monitored=True, name="test") as ch:
        while(running==True):
            time.sleep(0.1)
    print "Encoder "+encoder +" crossed 0 "+ str(counts[encoder]) + " times"
    return counts[encoder]
  except:
    print "Exception counting "+str(encoder) 
    traceback.print_exc()
    raise
    
def init():
  try:
    global running
    running = True

    print "startint FE slit initilization"
    cenV = caget('X11MA-FE-SV:center')
    cenH = caget('X11MA-FE-SH:center')
    offV = caget('X11MA-FE-DSVER.A')
    offH = caget('X11MA-FE-DSHOR.A')

    print "Old motor positions: centerV = " +str(cenV)+ \
                             ", centerH = " +str(cenH)+ \
                             ", offset V = " +str(offV)+ \
                             ", offset H = " +str(offH)
                             
    print "Initializing UP motor."
    caput('X11MA-FE-DSUP:II.PROC',1)
    time.sleep(1.0)
    cawait('X11MA-FE-SV1:TR1.DMOV', 1)
    print "Initializing DOWN motor."
    caput('X11MA-FE-DSDW:II.PROC',1)
    time.sleep(1.0)
    cawait('X11MA-FE-SV1:TR2.DMOV', 1)
    print "Initializing RING motor."
    caput('X11MA-FE-DSRI:II.PROC',1)
    time.sleep(1.0)
    cawait('X11MA-FE-SH1:TR1.DMOV', 1)
    print "Initializing WALL motor."
    caput('X11MA-FE-DSWA:II.PROC',1)
    time.sleep(1.0)
    cawait('X11MA-FE-SH1:TR2.DMOV', 1)

    print "Setting offH and offV to 0."
    caput('X11MA-FE-DSVER.A',0)
    caput('X11MA-FE-DSHOR.A',0)

    print "Checking encoder offset values."
    if(abs(caget('X11MA-FE-SV1:EC1Off')+42683)>10):
        print "Up position not ok"
    elif(abs(caget('X11MA-FE-SV1:EC2Off')+41362)>10):
        print "Down position not ok"
    elif(abs(caget('X11MA-FE-SH1:EC1Off')+40782)>10):
        print "Ring position not ok"
    elif(abs(caget('X11MA-FE-SH1:EC2Off')+39548)>10):
        print "Wall position not ok"
    else:
        print "All positions OK"

    print "restoring Center and off values"
    caput('X11MA-FE-SV:center',cenV)
    caput('X11MA-FE-SH:center',cenH)
    caput('X11MA-FE-DSVER.A',offV)
    caput('X11MA-FE-DSHOR.A',offH)
    
    running = False
    return 0
  except:
    print "Exception in init"
    traceback.print_exc()
    raise    

#Fork and join
futures = fork(init,(crossZero,("X11MA-FE-SV1:EC1",)), \
                    (crossZero,("X11MA-FE-SV1:EC2",)), \
                    (crossZero,("X11MA-FE-SH1:EC1",)), \
                    (crossZero,("X11MA-FE-SH1:EC2",))  )
ret = join(futures)

#check zero crossing status after init
zerocr=True    
for i in range(1,4):
    if(ret[i] != 2):
        zerocr=False
#final status report  
print "Zero crossing status for all motors: %s" %("OK" if zerocr == True else "Not OK")
print "init FE-slits: ==> done. %s" % ("OK" if zerocr == True else "Not OK")