#Script Motor Test 2
#Go to absolute position A, then move +B steps, then -2B steps, then +2Bsteps (ie oscillate round centre position, logging after each movement); repeat N times 

import traceback

#by default, failed
ret = 'Test failed'
status = False

#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 = idInkr.get()+direction
countSteps = 0
for setpoint1 in range(0, 3):
    sleep( 2 ) # Settling time
    #RegionPositioner idInkr
    for setpoint2 in frange(start, end, direction):
        readback1 = setpoint1
        sleep( 0.1 ) # Settling time
        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
        count =  setpoint1
        idDiff02 = a-b
        #Manipulation idDiff01
        #Variable Mappings
        a =  detector4
        b =  detector6
        count =  setpoint1
        idDiff01 = a-b
        countSteps = countSteps + 1
        scan.append ([countSteps], [countSteps], [detector1, detector2, detector3, detector4, detector5, detector6, detector7, detector8, idDiff02, idDiff01])
        if endH>0.0 :
            #invert direction and swap start with end of translation
            end = startDefault
            start = readback2 - direction
            direction = -1.0
            print 'End H switch, changing direction to ' + str(direction)
            break
        if endL>0.0 :
            #invert direction and swap start with end of translation
            end = endDefault
            start = readback2 - direction
            direction = 1.0
            print 'End L switch, changing direction to ' + str(direction)
            break


		
#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 = 'Test done'
status = True