# Script Motor Test 2 for production system
# 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

###### 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 with the following parameters:")
            test.log(params)
            middle = float(test.getParam("midPoint"))
            loopTimes = int(test.getParam("repeatTimes"))
            span = float(test.getParam("spanFromMidPoint"))
            settlingTime = 0.2 #seconds
        except:
            ret = 'Could not retrieve testing parameters - ' + traceback.format_exc()
            success = False
            test.sendFeedback(ret, success)
            return

        scan = ManualScan(['time [1/'+ str(1/settlingTime) + ' s]'], ['Motor Pos (IST3:2)', 'Poti Position (IST2:1)',
                                    'Motor Status (STA:1)', 'Inkr (INKR:2)', 'InkrRb (INKRRB:2)',
                                    'Diameter (DIAM:2)', 'Logical Position (IST:2)',
                                    'Motor Pos Raw (IST3:1)','Poti Pos From Beam (IST1:2)',
                                    'Poti Raw (POSA:1)',  'Poti Ref1 Position (REF1:1)', 'Poti Ref2 Position (REF2:1)'])
        scan.setPlotTitle(plotName)
        scan.start()
        # coloured plot (one colour per scan)
        p1 = plot(None, name="Poti-Increment difference", context=plotName + " difference")[0]
        p2 = plot(None, name="Poti-MotorPosition difference", context=plotName + " difference")[0]

        # 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]
            idMotorPositionRaw  = 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]
            idMotorPosition     = Channel(DEVICE + ':IST3:2'    , type = 'd')  # current position as from motor step counter [mm]
            idPotiPosFromBeam   = 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

        direction = 1.0
        startDefault = middle - span
        endDefault = middle + span
        end = endDefault + 1
        # find position: it will be the middle point of the test
        test.log('Moving to middle point  ' + str(middle))
        idInkr.put(middle, timeout=None)  # TODO: Set appropriate timeout
        readback2 = idInkr.get()
        if abs(readback2 - middle) > 5:  # TODO: Check accuracy
            ret = 'Actor idInkr could not be set to the value ' + str(middle) + ' (current value: ' + str(
                readback2) + ')'
            success = False
            test.sendFeedback(ret, success)
            return
        start = readback2 + direction
        countSteps = 0
        count = 0
        test.log('Moving around middle point (+-' + str(span) + ')')
        for setpoint1 in range(0, (loopTimes * 2)):
            count = count + 1
            sleep(5)  # Settling time
            p1.addSeries(LinePlotSeries("Run" + str(count)))
            # RegionPositioner idInkr
            for setpoint2 in frange(start, end, direction):
                readback1 = setpoint1
                idInkr.put(setpoint2, timeout=None)  # TODO: Set appropriate timeout
                sleep(settlingTime)  # Settling time
                readback2 = idInkr.get()

                MotorStatus         = idMotorStatus.get()
                LogicalPosition     = idLogicalPosition.get()
                PotiRaw             = idPotiRaw.get()
                MotorPositionRaw    = idMotorPositionRaw.get()
                MotorPosition       = idMotorPosition.get()
                PotiPosFromBeam     = idPotiPosFromBeam.get()
                PotiPosition        = idPotiPosition.get()
                PotiRef1Position    = idPotiRef1Position.get()
                PotiRef2Position    = idPotiRef2Position.get()
                Diameter            = idDiameter.get()
                Inkr                = idInkr.get()
                InkrRb              = idInkrRb.get()

                idDiff01 = PotiPosition - MotorPosition
                countSteps = countSteps + 1
                scan.append([setpoint2], [setpoint2],
                            [MotorPosition, PotiPosition, MotorStatus, Inkr, InkrRb, Diameter, LogicalPosition,
                             MotorPositionRaw, PotiPosFromBeam, PotiRaw, PotiRef1Position, PotiRef2Position])
                p1.getSeries(count).appendData(setpoint2, idDiff01)
                if (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
                    test.log('End of span (' + str(setpoint2) + '), changing direction to ' + str(direction))
                    break
                if (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
                    test.log('End of span (' + str(setpoint2) + '), changing direction to ' + str(direction))
                    break


                #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 so, invert direction
                if endH:
                    # invert direction and swap start with end of translation
                    end = startDefault - 1
                    start = setpoint2 - direction
                    direction = -1.0
                    test.log('Reached R2 switch, changing direction to ' + str(direction))
                    break
                elif endL:
                    # invert direction and swap start with end of translation
                    end = endDefault + 1
                    start = setpoint2 - direction
                    direction = 1.0
                    test.log('Reached R1 switch, changing direction to ' + str(direction))
                    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()
        idMotorPositionRaw.close()
        idMotorPosition.close()
        idPotiPosFromBeam.close()
        idPotiPosition.close()
        idPotiRef1Position.close()
        idPotiRef2Position.close()
        idDiameter.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)
