#Script Motor Test 1
#moves to CCW switch; then for M times moves N times to CW switch then CCW switch; between each M pauses for delay; log at CCW and CW 

import traceback

#by default, failed
ret = 'Test failed'
status = False
#DEVICE = 'PO2DV-NCS-LS'

#TODO: Set the diplay names of positioners and detectors
#scan = ManualScan(['idX', 'idInkr'], ['idMotorStatus', 'idLogicalPosition', 'idDiameter', 'idMotorPosition', 'idPotiRaw', 'idPotiProc', 'idBtvsRaw', 'idBtvsProc', 'idDiff01', 'idDiff02'] , [-0.5, 0.0], [4.0, 3000.0], [3000, 20])
scan = ManualScan(['idX'], ['idMotorStatus', 'idLogicalPosition', 'idDiameter', 'idMotorPosition', 'idPotiRaw', 'idPotiProc', 'idBtvsRaw', 'idBtvsProc', 'idDiff01', 'idDiff02'] , [ 0.0], [ 3000.0], [20])
scan.start()

#Creating channels: dimension 1
try:
	#RegionPositioner idInkr
	#idInkr = Channel(DEVICE+':INKR:2', type = 'd')
	idInkr = Channel(DEVICE+':MOTOR.VAL', type = 'd')
	#ScalarDetector idMotorStatus
	#idMotorStatus = Channel(DEVICE+':STA:1', type = 'd')
	idMotorStatus = Channel(DEVICE+':MOTOR.MSTA', type = 'd')
	#ScalarDetector idLogicalPosition
	#idLogicalPosition = Channel(DEVICE+':IST:2', type = 'd')
	idLogicalPosition = Channel(DEVICE+':MOTOR.RVAL', type = 'd')
	#ScalarDetector idDiameter
	#idDiameter = Channel(DEVICE+':DIAM:2', type = 'd')
	idDiameter = Channel(DEVICE+':ENCODERoff', type = 'd')
	#ScalarDetector idMotorPosition
	#idMotorPosition = Channel(DEVICE+':IST1:2', type = 'd')
	idMotorPosition = Channel(DEVICE+':MOTOR.RBV', type = 'd')
	#ScalarDetector idPotiRaw
	#idPotiRaw = Channel(DEVICE+':POSA:1', type = 'd')
	idPotiRaw = Channel(DEVICE+':ENCODERraw', type = 'd')
	#ScalarDetector idPotiProc
	#idPotiProc = Channel(DEVICE+':POSA:2', type = 'd')
	idPotiProc = Channel(DEVICE+':ENCODER', type = 'd')
	#ScalarDetector idBtvsRaw
	#idBtvsRaw = Channel(DEVICE+':IST3:1', type = 'd')
	idBtvsRaw = Channel(DEVICE+':MOTOR.LLS', type = 'd')
	#ScalarDetector idBtvsProc
	#idBtvsProc = Channel(DEVICE+':IST3:2', type = 'd')
	idBtvsProc = Channel(DEVICE+':MOTOR.HLS', type = 'd')
	#ScalarDetector idEndSwitchL
	#idBtvsRaw = Channel(DEVICE+':IST3:1', type = 'd')
	idEndSwitchL = Channel(DEVICE+':MOTOR.LLS', type = 'd')
	#ScalarDetector idEndSwitchH
	#idBtvsProc = Channel(DEVICE+':IST3:2', type = 'd')
	idEndSwitchH = Channel(DEVICE+':MOTOR.HLS', type = 'd')
	#high position limit
	idLimitH = Channel(DEVICE+':MOTOR.HLM', type = 'd')
	#low position limit
	idLimitL = Channel(DEVICE+':MOTOR.LLM', type = 'd')
except:
	print "Unexpected error:", sys.exc_info()[0]
	ret = 'Unable to create channel - ' + traceback.format_exc()
	success = False
	raise
	sys.exit()

#remove limits	
idLimitH.put(999999.9, timeout=None)
idLimitL.put(-999999.9, timeout=None)
	
#Dimension 1
direction = 1.0;
startDefault = -100.0
endDefault = 1000.0
end = endDefault
#find position at Low end switch: it will be the starting point of the test
idInkr.put(-100.0, timeout=None) # TODO: Set appropriate timeout
start = startDefault #idInkr.get()+direction
setpoint2 = end
count = 0
for setpoint1 in range(0, 3):
    sleep( 2 ) # Settling time
    #RegionPositioner idInkr
    idInkr.put(setpoint2, timeout=None) # TODO: Set appropriate timeout
    readback2 = idInkr.get()
    #if abs(readback2 - setpoint2) > 1 : # TODO: Check accuracy
    #    ret = 'Actor idInkr could not be set to the value ' + str(setpoint2) + ' (current value: ' + str(readback2) + ')'
    #    success = False
    #    raise Exception(ret)
    #Detector idMotorStatus
    detector1 = idMotorStatus.get()
    #Detector idLogicalPosition
    detector2 = idLogicalPosition.get()
    #Detector idDiameter
    detector3 = idDiameter.get()
    #Detector idMotorPosition
    detector4 = idMotorPosition.get()
    #Detector idPotiRaw
    detector5 = idPotiRaw.get()
    #Detector idPotiProc
    detector6 = idPotiProc.get()
    #Detector idBtvsRaw
    detector7 = idBtvsRaw.get()
    #Detector idBtvsProc
    detector8 = idBtvsProc.get()
    #end switches
    endH = idEndSwitchH.get()
    endL = idEndSwitchL.get()
    #Manipulation idDiff02	
    #Variable Mappings
    a =  detector4
    b =  detector8
    idDiff02 = a-b
    #Manipulation idDiff01
    #Variable Mappings
    a =  detector4
    b =  detector6
    count = count + 1
    idDiff01 = a-b
    if endH>0.0 :
        #invert direction and swap start with end of translation
        setpoint2 = start
        print 'End H switch, changing target to ' + str(setpoint2)
        scan.append ([setpoint2], [readback2], [detector1, detector2, detector3, detector4, detector5, detector6, detector7, detector8, idDiff02, idDiff01])
    if endL>0.0 :
        #invert direction and swap start with end of translation
        setpoint2 = end
        print 'End L switch, changing target to ' + str(setpoint2)
        scan.append ([setpoint2], [readback2], [detector1, detector2, detector3, detector4, detector5, detector6, detector7, detector8, idDiff02, idDiff01])
		
#set limits back
idLimitH.put(145.0, timeout=None)
idLimitL.put(0.0, timeout=None)

#Closing channels
idInkr.close()
idMotorStatus.close()
idLogicalPosition.close()
idDiameter.close()
idMotorPosition.close()
idPotiRaw.close()
idPotiProc.close()
idBtvsRaw.close()
idBtvsProc.close()

scan.end()
ret = 'Slide moved back and forth (' + str(count) + ' runs)'
status = True