#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 


###### DO NOT MODIFY THE CODE BELOW ######
import sys, inspect, os, traceback, time
global print_log, sendFeedback
def startTest(testName, DEVICE, params):
    try:
        import traceback
        #get the path of this script
        testPath = inspect.getfile(inspect.currentframe()) 
        #by default, failed 
        ret = 'Test failed'
        success = False   
        #plot name to be given to the scan. Use: scan.setPlotTitle(plotName)
        plotName = DEVICE + ' - ' + testName    
####### WRITE YOUR CODE HERE BELOW #######  

        #get parameters from the calling interface
        try:
            print_log(testName, DEVICE, "Running test Motor Test 2 for device " + DEVICE + " with the following parameters:\n" + str(params))
            middle = float(params["midPoint"]["value"])
            loopTimes = int(params["repeatTimes"]["value"])
            span = float(params["spanFromMidPoint"]["value"])
        except:
            ret = 'Could not retrieve testing parameters - ' + traceback.format_exc()
            success = False
            sendFeedback(testPath, testName, DEVICE, ret, success)
            return

        #scan = ManualScan(['idX'], ['idMotorStatus', 'idLogicalPosition', 'idDiameter', 'idMotorPosition', 'idPotiRaw', 'idPotiProc', 'idBtvsRaw', 'idBtvsProc', 'idDiff01', 'idDiff02'] , [ 0.0], [ 3000.0], [20])
        scan = ManualScan(['idX'], ['idMotorStatus', 'idLogicalPosition', 'idDiameter', 'idMotorPosition', 'idPotiRaw', 'idPotiProc', 'idBtvsRaw', 'idBtvsProc', 'idDiff01', 'idDiff02'])
        scan.setPlotTitle(plotName)
        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:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            sendFeedback(testPath, testName, DEVICE, ret, success)
            return

        #remove limits    
        idLimitH.put(999999.9, timeout=None)
        idLimitL.put(-999999.9, timeout=None)

        direction = 1.0
        startDefault = middle - span 
        endDefault = middle + span
        end = endDefault+1
        #find position: it will be the middle point of the test
        print_log(testName, DEVICE,  'Moving to middle point  ' + str(middle)  )
        idInkr.put(middle, timeout=None) # TODO: Set appropriate timeout
        readback2 = idInkr.get()
        if abs(readback2 - middle) > 1 : # TODO: Check accuracy
            ret = 'Actor idInkr could not be set to the value ' + str(middle) + ' (current value: ' + str(readback2) + ')'
            success = False
            sendFeedback(testPath, testName, DEVICE, ret, success)
            return    
        start = readback2+direction
        countSteps = 0
        count = 0
        print_log(testName, DEVICE,  'Moving around middle point (+-' + str(span) + ')' )
        for setpoint1 in range(0, loopTimes*2):
            count = count + 1
            sleep( 2 ) # Settling time
            #RegionPositioner idInkr
            for setpoint2 in frange(start, end, direction):
                readback1 = setpoint1
                idInkr.put(setpoint2, timeout=None) # TODO: Set appropriate timeout
                sleep( 0.2 ) # Settling time
                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
                    sendFeedback(testPath, testName, DEVICE, ret, success)
                    return
                #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
                idDiff01 = a-b
                countSteps = countSteps + 1
                scan.append ([countSteps], [countSteps], [detector1, detector2, detector3, detector4, detector5, detector6, detector7, detector8, idDiff02, idDiff01])
                if endH>0.0 or (direction > 0.0 and setpoint2 >= end -1):
                    #invert direction and swap start with end of translation
                    end = startDefault-1
                    start = setpoint2 - direction
                    direction = -1.0
                    print_log(testName, DEVICE,  'End of span (' + str(setpoint2) + '), changing direction to ' + str(direction) )
                    break
                if endL>0.0  or ( direction < 0.0 and setpoint2 <= end +1):
                    #invert direction and swap start with end of translation
                    end = endDefault+1
                    start = setpoint2 - direction
                    direction = 1.0
                    print_log(testName, DEVICE,  'End of span (' + str(setpoint2) + '), 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 = 'Slide moved back and forth (' + str(count) + ' runs)'
        success = True

############# END OF YOUR CODE ###########
###### DO NOT MODIFY THE CODE BELOW ######        
        sendFeedback(testPath, testName, DEVICE, ret, success)
    except:
        ret = traceback.format_exc()
        success = False
        sendFeedback(testPath, testName, DEVICE, ret, success)
        return

#prepare and send feedback to calling tool   
def sendFeedback(testPath, testName, DEVICE, returnString, testPassed):
    print_log(testName, DEVICE, 'End of test. Result:')
    print_log(testName, DEVICE,  'Test path: ' + testPath)
    print_log(testName, DEVICE,  'Test name: ' + testName)
    print_log(testName, DEVICE,  'Device: ' + DEVICE)
    print_log(testName, DEVICE,  'Test passed: ' + str(testPassed))
    print_log(testName, DEVICE,  'Return string: ' + returnString)
    ret = [testPath, DEVICE, returnString, testPassed]
    set_return(ret)
 
def print_log(testName, DEVICE, text):
    time.ctime()
    now = time.strftime('%Y.%m.%d %H:%M:%S') 
    print now + ' ' + DEVICE + ' - ' + testName + ': ' + str(text)
    
#launch the test
#from pshellTestGeneral import testUtils
#testUtil = testUtils()
#testUtil.print_log(test, device, "CLASSE!!!")
startTest(test, device, parameters)
