# 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


###### 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 #############

        # DEVICE = 'PO2DV-NCS-LS'
        # get parameters from the calling interface
        try:
            test.log("Running test with the following parameters:")
            test.log(params)
            loopTimes = int(test.getParam("repeatTimes"))
            delaySeconds = int(test.getParam("delayS"))
            samplePeriod = 0.05 #seconds
        except:
            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)'])
                            
        p1 = plot(None, name = "Motor Position - Btvs Poti Position", context = plotName + " pos difference")[0]
        scan.setPlotTitle(plotName)
        scan.start()
        # Creating channels: dimension 1
        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]
            idCom               = Channel(DEVICE + ':COM:2'     , type = 'd')  # motor commands [0=Stop; 1=Calibrate; 2=gotoR1; 3=gotoR2]
            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]
        except:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            test.sendFeedback(ret, success)
            return
        
        end = 4.0 #go to R2
        start =  3.0 #go to R1
        setpoint2 = end
        count = 0
        test.log('Starting test sequence')
        for setpoint1 in range(0, (loopTimes * 2)):
            p1.addSeries(LinePlotSeries("Run"+str(count)))
            sleep(delaySeconds)  # Settling time between runs
            # RegionPositioner idInkr
            idCom.put(setpoint2, timeout=None)  # TODO: Set appropriate timeout
            count = count + 1
            # scan for changes
            for scanTimes in range(0, 100000):
                sleep(samplePeriod)
                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            

                scan.append([currentTime], [currentTime],
                            [Btvs, PotiPosition, MotorStatus, Inkr, InkrRb, Diameter, LogicalPosition,
                             BtvsRaw, MotorPosition, PotiRaw, PotiRef1Position, PotiRef2Position])

                p1.getSeries(count).appendData(currentTime, 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
                    setpoint2 = start
                    test.log('Reached R2 switch, changing target to R1')
                    break
                elif endL:
                    # invert direction and swap start with end of translation
                    setpoint2 = end
                    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()
        idCom.close()
        idInkr.close()
        idInkrRb.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)
