
#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 Motor Test 2 for device " + DEVICE + " with the following parameters:\n" + str(params))
            middle      = float(test.getParam("midPoint"))
            loopTimes   = int(test.getParam("repeatTimes"))
            span        = float(test.getParam("spanFromMidPoint"))
            delayS      = int(test.getParam("delayS"))
            if(delayS<1): delayS=1
        except:
            ret = 'Could not retrieve testing parameters - ' + traceback.format_exc()
            success = False
            test.sendFeedback( ret, success)
            return

        #scan = ManualScan(['idX'], ['idMotorStatus', 'idLogicalPosition', 'idDiameter', 'idMotorPosition', 'idPotiRaw', 'idEncoderPosition', 'idBtvsRaw', 'idBtvsProc', 'idDiff01', 'idDiff02'] , [ 0.0], [ 3000.0], [20])
        scan = ManualScan(['idX'], ['idMotorStatus', 'idMotorPosition', 'idEncoderPosition', 'idError'])
        scan.setPlotTitle(plotName + " parameters")
        scan.start()
        
        #coloured plot (one colour per scan)
        p1 = plot(None,name="Run0", context = plotName + " error")[0] 

        #Creating channels: dimension 1
        try:
            idInkr = Channel(DEVICE+':MOTOR.VAL', type = 'd')
            idMotorStatus = Channel(DEVICE+':MOTOR.MSTA', type = 'd')
            idMotorPosition = Channel(DEVICE+':MOTOR.RBV', type = 'd')
            idEncoderPosition = Channel(DEVICE+':ENCODER', type = 'd')
            idEndSwitchL = Channel(DEVICE+':MOTOR.LLS', type = 'd')
            idEndSwitchH = Channel(DEVICE+':MOTOR.HLS', type = 'd')
            idLimitH = Channel(DEVICE+':MOTOR.HLM', type = 'd')
            idLimitL = Channel(DEVICE+':MOTOR.LLM', type = 'd')
        except:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            test.sendFeedback( ret, success)
            return

        #remove limits    
        idLimitH.put(999999.9, timeout=None)
        idLimitL.put(-999999.9, timeout=None)
        
        
        min = 9999999999
        minPos = 0
        max = 0
        maxPos = 0
        minDev = []
        minDevPos = []
        maxDev = []
        maxDevPos = []       

        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) > 1 : # 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
            test.log(  'Pausing ' + str(delayS) + 's' )
            sleep( delayS ) # Settling time
            p1.addSeries(LinePlotSeries("Run"+str(count))) 
            maxDev.append(0)
            maxDevPos.append(0)
            minDev.append(9999999999)
            minDevPos.append(0)
            #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
                    test.sendFeedback( ret, success)
                    return
                #Detector idMotorStatus
                detector1 = idMotorStatus.get()
                detector4 = idMotorPosition.get()
                detector6 = idEncoderPosition.get()
                endH = idEndSwitchH.get()
                endL = idEndSwitchL.get()
                #Manipulation idDiff01
                #Variable Mappings
                a =  detector4
                b =  detector6
                idDiff01 = a-b
                countSteps = countSteps + 1
                scan.append ([countSteps], [countSteps], [detector1, detector4, detector6, idDiff01])
                p1.getSeries(count).appendData(setpoint2, idDiff01) 
                #compute min and max differences
                if abs(idDiff01) > max:
                    max = abs(idDiff01)
                    maxPos = setpoint2
                if abs(idDiff01) < min:
                    min = abs(idDiff01)
                    minPos = setpoint2                
                #compute min and max deviation
                if abs(idDiff01) > maxDev[count-1]:
                    maxDev[count-1] = abs(idDiff01)
                    maxDevPos[count-1] = setpoint2
                if abs(idDiff01) < minDev[count-1]:
                    minDev[count-1] = abs(idDiff01)
                    minDevPos[count-1] = setpoint2                
                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
                    test.log(  '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
                    test.log(  'End of span (' + str(setpoint2) + '), changing direction to ' + str(direction) )
                    break
        #show differences in the plot
        import java.awt.Color
        p1.addMarker(maxPos, None, "Max=" + str(max), java.awt.Color.LIGHT_GRAY)
        p1.addMarker(minPos, None, "Min=" + str(min), java.awt.Color.LIGHT_GRAY)
        #show deviations in the plot
        absMaxDev = 0
        absMinDev = 9999999999
        absMaxPos = 0
        absMinPos = 0
        for index in range(len(maxDev)):
            if absMaxDev < maxDev[index]:
                absMaxDev = maxDev[index]
                absMaxPos = maxDevpos[index]
            if absMinDev > minDev[index]:
                absMinDev = minDev[index]
                absMinPos = minDevPos[index]
        p1.addMarker(absMaxPos, None, "MaxDev=" + str(absMaxDev), java.awt.Color.LIGHT_GRAY)
        p1.addMarker(absMinPos, None, "MinDev=" + str(absMinDev), java.awt.Color.LIGHT_GRAY)
            
        
        #set limits back
        idLimitH.put(145.0, timeout=None)
        idLimitL.put(0.0, timeout=None)

        #Closing channels
        idInkr.close()
        idMotorStatus.close()
        idMotorPosition.close()
        idEncoderPosition.close()
        idLimitH.close()
        idLimitL.close()
        scan.end()
        ret = 'Slide moved back and forth (' + str(count) + ' runs)'
        success = True

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

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