
###### DO NOT MODIFY THE CODE BELOW ######
global print_log, sendFeedback, inspect, log, sys, inspect, os, traceback
import sys, inspect, os, traceback

def print_log(testName, DEVICE, text):
    time.ctime()
    now = time.strftime('%Y.%m.%d %H:%M:%S') 
    textToLog = now + ' ' + DEVICE + ' - ' + testName + ': ' + str(text)
    print textToLog
    log ( textToLog )

#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,  'Device:        ' + DEVICE)
    print_log(testName, DEVICE,  'Test name:     ' + testName)
    print_log(testName, DEVICE,  'Test path:     ' + testPath)
    print_log(testName, DEVICE,  'Test passed:   ' + str(testPassed))
    print_log(testName, DEVICE,  'Return string: ' + returnString)
    ret = [testPath, DEVICE, returnString, testPassed]
    set_return(ret)

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

        #Pre-actions
        # try:
            # caput(DEVICE+':INIT.PROC', '1')
        # except:
            # ret = 'Unable to create channel - ' + traceback.format_exc()
            # success = False
            # sendFeedback(testPath, testName, DEVICE, ret, success)
            # return      

            
        #get parameters from the calling interface
        try:
            print_log(testName, DEVICE,  "Running test Initialise with the following parameters:")
            print_log(testName, DEVICE,  params )
            loopTimes = int(params["repeatTimes"]["value"])
            delaySeconds = int(params["delayS"]["value"])
        except:
            ret = 'Could not retrieve testing parameters - ' + traceback.format_exc()
            success = False
            sendFeedback(testPath, testName, DEVICE, ret, success)
            return
            
        scan = ManualScan(['idX'], ['Motor Position (RBV)', 'Encoder Position (ENCODER)', 'Diff Motor - Encoder'] )
        scan.setPlotTitle(plotName)
        scan.start()
        #Creating channels: dimension 1
        try:
            idInit = Channel(DEVICE+':INIT.PROC', type = 'l')
            idReady = Channel(DEVICE+':RDY', type = 'l')
            idInterlock = Channel(DEVICE+':ILK', type = 'l')
            idMotorPosition = Channel(DEVICE+':MOTOR.RBV', type = 'd')
            idEncoderPosition = Channel(DEVICE+':ENCODER', type = 'd')
            #idBtvsRaw = Channel(DEVICE+':IST3:1', type = 'd')
            idEndSwitchL = Channel(DEVICE+':MOTOR.LLS', type = 'd')
            #idBtvsProc = Channel(DEVICE+':IST3:2', type = 'd')
            idEndSwitchH = Channel(DEVICE+':MOTOR.HLS', type = 'd')
        except:
            ret = 'Unable to create channel - ' + traceback.format_exc()
            success = False
            sendFeedback(testPath, testName, DEVICE, ret, success)
            return

        count = 0
        timeout = 90000 #timeout in ms
        for count in range(1, loopTimes+1):
            print_log(testName, DEVICE,  'Initialisation #' + str(count) + '/' + str(loopTimes))
            idInit.put(1, timeout=None) # TODO: Set appropriate timeout
            timeStampStart = float(java.lang.System.currentTimeMillis())
            sleep(0.1)
            ready = 0
            interlock = idInterlock.get()
            timeElapsed = 0 #in ms
            while (ready == 0) and timeElapsed<timeout:
                #Detector time
                timeStamp = float(java.lang.System.currentTimeMillis())
                timeElapsed = timeStamp - timeStampStart
                ready = idReady.get()
                sleep( 0.1 )
                detector4 = idMotorPosition.get()
                detector6 = idEncoderPosition.get()
                ready = idReady.get()
                interlock = idInterlock.get()
                #end switches
                endH = idEndSwitchH.get()
                endL = idEndSwitchL.get()
                #Manipulation idDiff01
                a =  detector4
                b =  detector6
                idError = a-b
                scan.append ([timeStamp],[timeStamp], [detector4, detector6, idError])
            if ready == 1 and interlock == 1:
                print_log(testName, DEVICE,  'Initialisation #' + str(count) + '/' + str(loopTimes) + ' successful')            
                ret = 'Drive initialised ' + str(count) + ' times'
                success = True
            else:   
                ret = 'The RDY and ILK signals indicate the drive was NOT ready at the expected time (after ' + str(timeout/1000) + 's).'
                success = False
                break
            if(count < loopTimes):
                print_log(testName, DEVICE,  'Next initialisation starting in ' + str(delaySeconds) + 's')
                sleep( delaySeconds ) # pause between two init
               
        idInit.close()
        idReady.close()
        idMotorPosition.close()
        idEncoderPosition.close()
        scan.end()     

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

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