#Script Motor Test 3
#Moves from CCW to CW as a series of discrete translations (C times) logs after each translation. When end switch is encountered change direction. Repeat N times

###### Init - DO NOT MODIFY THE CODE BELOW ######
global sys, inspect, os, traceback
import sys, inspect, os, traceback

def startTest(testName, DEVICE, params):
    #by default, assume the test failed
    ret = 'Test failed'
    success = False
    #plot name to be given to the scan. Use: scan.setPlotTitle(plotName)
    plotName = DEVICE + ' - ' + testName
    #put the whole custom code under try/catch
    try:
        #get the path of this script
        testPath = inspect.getfile(inspect.currentframe())
        #init the testing tool class. It can be sued in the following ways:
        test = TestingTool(testName, testPath, DEVICE, params)

######### WRITE YOUR CODE HERE BELOW #############
        #get parameters from the calling interface
        try:
            test.log("Running test Motor Test 3 with the following parameters:")
            test.log(params)
            loopTimes = int(test.getParam("repeatTimes")) # C times
            translation = int(test.getParam("translation")) # Increment mm
            samplePeriod = 0.2 #seconds
        except:
            test.log("Could not retrieve testing parameters: ", sys.exc_info()[0])
            ret = 'Could not retrieve testing parameters - ' + traceback.format_exc()
            success = False
            test.sendFeedback(ret, success)
            return
            
        scan = ManualScan(['time [1/'+ str(1/samplePeriod) + ' s]'], [
                                    'Btvs Poti Position (IST3:2)',
                                    'Mcs Poti Position (IST2:1)',
                                    'Motor Status (STA:1)',
                                    'Inkr (INKR:2)',
                                    'InkrRb (INKRRB:2)',
                                    'Diameter (DIAM:2)',
                                    'Logical Position (IST:2)',
                                    'Btvs Raw (IST3:1)',
                                    'Motor Position (IST1:2)',
                                    'Poti Raw (POSA:1)',
                                    'Poti Ref1 Position (REF1:1)',
                                    'Poti Ref2 Position (REF2:1)'])
        scan.setPlotTitle(plotName)
        scan.start()
        p1 = plot(None, name = "Motor Position - Btvs Poti Position", context = plotName + " Motor-Poti difference")[0]
        # Creating channels
        try:
            idMotorStatus       = Channel(DEVICE + ':STA:1'     , type = 'd')  # DSP device_status reg
            idLogicalPosition   = Channel(DEVICE + ':IST:2'     , type = 'd')  # Shows current position in logical units as calculated from motor step counter [1..n]
            idPotiRaw           = Channel(DEVICE + ':POSA:1'    , type = 'd')  # poti raw data [ADC units]
            idBtvsRaw           = Channel(DEVICE + ':IST3:1'    , type = 'd')  # shows current position in steps as as obtained from motor step counter [steps]
            idInkr              = Channel(DEVICE + ':INKR:2'    , type = 'd')  # move relative distance (positive means towards R2) [mm]
            idInkrRb            = Channel(DEVICE + ':INKRRB:2'  , type = 'd')  # readback of move relative distance (positive means towards R2) [mm]
            idDiameter          = Channel(DEVICE + ':DIAM:2'    , type = 'd')  # collimator diameter [mm]
            idBtvs              = Channel(DEVICE + ':IST3:2'    , type = 'd')  # current position as from motor step counter [mm]
            idMotorPosition     = Channel(DEVICE + ':IST1:2'    , type = 'd')  # current position from beam as from potentiometer [mm]
            idPotiPosition      = Channel(DEVICE + ':IST2:1'    , type = 'd')  # current position as from potentiometer [mm]
            idPotiRef1Position  = Channel(DEVICE + ':REF1:1'    , type = 'd')  # R1 position as from potentiometer [mm]
            idPotiRef2Position  = Channel(DEVICE + ':REF2:1'    , type = 'd')  # R2 position as from potentiometer [mm]
            idCom               = Channel(DEVICE + ':COM:2'     , type = 'd')  # motor commands [0=Stop; 1=Calibrate; 2=gotoR1; 3=gotoR2]
            
        except:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            test.sendFeedback( ret, success)
            return

        
        #go to position at Low end switch: it will be the starting point of the test
        test.log('Homing')
        idCom.put(2, timeout=10) # go to R1
        # wait for motor to get to Ref1
        for setpoint1 in range(0, 100):
            sleep( 2 ) # Settling time
            MotorStatus = idMotorStatus.get()
            if bool(int(MotorStatus) & 4): # arrived at Ref1 (low  limit)
                break
        if not bool(int(MotorStatus) & 4): # not arrived at Ref1 (low  limit)
            ret = 'Could not reach Ref1'
            success = False
            test.sendFeedback( ret, success)
            return
        
        direction = 1
        countSteps = 0
        test.log('Starting testing sequence')
        count = 0
        for setpoint1 in range(0, (loopTimes*2)):
            count = count + 1
            sleep( 2 ) # Settling time
            # add multi cuve plot
            p1.addSeries(LinePlotSeries("Run"+str(count)))
            
            for setpoint2 in frange(0, 1000000):
                
                idInkr.put(translation*direction, timeout=10)
                sleep(samplePeriod) # Settling time
                currentTime = float(java.lang.System.currentTimeMillis())

                MotorStatus         = idMotorStatus.get()
                LogicalPosition     = idLogicalPosition.get()
                PotiRaw             = idPotiRaw.get()
                BtvsRaw             = idBtvsRaw.get()
                Btvs                = idBtvs.get()
                MotorPosition       = idMotorPosition.get()
                PotiPosition        = idPotiPosition.get()
                PotiRef1Position    = idPotiRef1Position.get()
                PotiRef2Position    = idPotiRef2Position.get()
                Diameter            = idDiameter.get()
                Inkr                = idInkr.get()
                InkrRb              = idInkrRb.get()

                idDiff01 = MotorPosition-Btvs
                countSteps = countSteps + (translation*direction)
                
                # multi plot
                scan.append([currentTime], [currentTime],[
                             Btvs,
                             PotiPosition,
                             MotorStatus,
                             Inkr,
                             InkrRb,
                             Diameter,
                             LogicalPosition,
                             BtvsRaw,
                             MotorPosition,
                             PotiRaw,
                             PotiRef1Position,
                             PotiRef2Position])
                # draw diff against steps
                p1.getSeries(count).appendData(countSteps, idDiff01)

                #extract Status bits
                endH = bool(int(MotorStatus) & 8) #Ref2 (high limit)
                endL = bool(int(MotorStatus) & 4) #Ref1 (low  limit)

                #check if arrived to R1 or R2
                if  endH:
                    #invert direction and swap start with end of translation
                    direction = -1
                    test.log('Reached R2 switch, changing target to R1')
                    break
                elif endL:
                    #invert direction and swap start with end of translation
                    direction = 1
                    test.log('Reached R1 switch, changing target to R2')
                    break

                #check if any error bit is raised
                if  bool(int(MotorStatus) & int('10000',2)): #error: abort test
                    test.sendFeedback('Motor switched off (bit#4)', False)
                    return
                if  bool(int(MotorStatus) & int('100000',2)): #error: abort test
                    test.sendFeedback('No motor link (bit#5)', False)
                    return
                if  bool(int(MotorStatus) & int('1000000',2)): #error: abort test
                    test.sendFeedback('No poti link (bit#6)', False)
                    return
                if  bool(int(MotorStatus) & int('10000000',2)): #error: abort test
                    test.sendFeedback('Calibration error (bit#7)', False)
                    return
                if  bool(int(MotorStatus) & int('100000000',2)): #error: abort test
                    test.sendFeedback('Cannot get to R1 (bit#8)', False)
                    return
                if  bool(int(MotorStatus) & int('1000000000',2)): #error: abort test
                    test.sendFeedback('Cannot get to R2 (bit#9)', False)
                    return
                if  bool(int(MotorStatus) & int('10000000000',2)): #error: abort test
                    test.sendFeedback('Position measurement mismatch (bit#10)', False)
                    return
                if  bool(int(MotorStatus) & int('100000000000',2)): #error: abort test
                    test.sendFeedback('Movement timeout (bit#11)', False)
                    return

        #Closing channels
        idMotorStatus.close()
        idLogicalPosition.close()
        idPotiRaw.close()
        idBtvsRaw.close()
        idBtvs.close()
        idMotorPosition.close()
        idPotiPosition.close()
        idPotiRef1Position.close()
        idPotiRef2Position.close()
        idDiameter.close()
        idInkr.close()
        idInkrRb.close()
        idCom.close()

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

################ END OF YOUR CODE ################
###### Final - DO NOT MODIFY THE CODE BELOW ######

        #just in case the feedback was forgotten
        test.sendFeedback(ret, success)
    except:
        #generic error handler
        ret = traceback.format_exc()
        success = False
        test.sendFeedback(ret, success)

#launch the test
startTest(test, device, parameters)