#####################################################################
# To test sequence of actions on loader :
# goToHandoff, close, clamp, goToStorage, goToHandoff, unclamp, open
# Tested on loader proto at LPSC in May 2017
# In scripts directory (org-lsst-ccs-subsystem-fcs/main/scripts)
# this script's name is loaderClampCarrierTest.py
#####################################################################
### change iteration number / nombre d'iterations a changer si besoin est
iter = 2
#####################################################################

#import CCS Scripting Implementation                                                                                          
from org.lsst.ccs.scripting import CCS
#import the definition for a Java Exception                                                                                   
from java.lang import Exception
from org.lsst.ccs.command import CommandInvocationException
from time import sleep

#
CCS.setThrowExceptions(True)

#time to wait for switch change
waitTime = int(5)

#time to do a nap
napTime = int(2)

#####################################################################
# launch loader-standalone subsystem and initialize
#####################################################################
loaderStandalone = CCS.attachSubsystem("loader-standalone")
print "Connected to loader-standalone ", loaderStandalone

#getting timeouts
loaderConfigFuture = loaderStandalone.asynchCommandLine("getConfigurationInfo")
print "getting loader configuration info"
loaderConfig = loaderConfigFuture.get()
timeoutHandoff = loaderConfig.getCurrentValueForParameter("carrier//timeoutForGoingToHandOff")
timeoutStorage = loaderConfig.getCurrentValueForParameter("carrier//timeoutForGoingToStorage")
timeoutToOpen = loaderConfig.getCurrentValueForParameter("clamp//timeoutForOpeningHooks")

###loader
loader = CCS.attachSubsystem("loader-standalone/loader")

###carrier
carrier = CCS.attachSubsystem("loader-standalone/carrier")

##Clamp
clamp = CCS.attachSubsystem("loader-standalone/clamp")
hooksController = CCS.attachSubsystem("loader-standalone/hooksController")

###plutoGateway
plutoGateway = CCS.attachSubsystem("loader-standalone/loaderPlutoGateway")

###fake autochanger
autochanger = CCS.attachSubsystem("loader-standalone/autochanger")


##Proxy
tcpProxy = CCS.attachSubsystem("loader-standalone/loaderTcpProxy")

storagePosition=int((carrier.synchCommandLine(5, "getStoragePosition")).getResult())
print "STORAGE position=", storagePosition

handoffPosition=int((carrier.synchCommandLine(5, "getHandoffPosition")).getResult())
print "HANDOFF position=", handoffPosition

CCS.setThrowExceptions(True)

result = tcpProxy.synchCommandLine(5, "isCANDevicesReady")
if result.getResult():
    print "isCANDevicesReady=", result.getResult()
else:
    raise Exception("CANopen Hardware not ready")


############################################################################
# Check controllers and plutoGateway are READY
############################################################################
#hardware is READY 
#print "Check that controllers and plutoGateway are initialized...."
if plutoGateway.synchCommandLine(5, "isInitialized").getResult():
    print "plutoGateway.isInitialized()=", result.getResult()
else:
    raise Exception("plutoGateway not Ready")

if hooksController.synchCommandLine(5, "isInitialized").getResult():
    print "hooksController.isInitialized()=", result.getResult()
else:
    raise Exception("hooksController not Ready")

############################################################################
# Check initial conditions
############################################################################
#loader is connected on camera
if loader.synchCommandLine(5, "isConnectedOnCamera").getResult():
    print "loader carrier is connected on camera: OK"
else:
    raise Exception("loader carrier is connected on camera")

#carrier is at STORAGE
if carrier.synchCommandLine(5, "isAtStoragePosition").getResult():
    print "loader carrier is at STORAGE position: OK"
else:
    raise Exception("loader carrier is NOT at STORAGE position")

#no filter is in the loader
if loader.synchCommandLine(5, "isEmpty").getResult():
    print "loader is NOT holding a filter: OK"
else:    
    raise Exception("loader is NOT empty. this sequence has to be executed with no filter in the loader.")

    
#clamp should be opened
if not clamp.synchCommandLine(5, "isOpened").getResult():
    raise Exception("loader clamp is not opened. this sequence has to be executed with clamp opened at the beginning")
else:
    print "loader clamp is opened: OK"

############################################################################
# def to wait for autochanger holding filter
############################################################################

def waitForAutochangerHoldingFilter():
    loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
    while not autochanger.synchCommandLine(5, "isHoldingFilter").getResult():
        print "Waiting for autochanger hold filter (AP2 & AF3) ..."
        sleep(waitTime)
        loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
    else:
        print "autochanger is holding filter"

############################################################################
# def to wait for autochanger NOT holding filter
############################################################################
def waitForAutochangerNOTHoldingFilter():
    loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
    while autochanger.synchCommandLine(5, "isHoldingFilter").getResult():
        print "Waiting for autochanger release filter (AP2 & AF0 & AF1) ..."
        sleep(waitTime)
        loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
        sleep(1)
    else:
        print "autochanger is NOT holding filter"


############################################################################
# def to do a little nap
############################################################################
def littleNap():
    ### a little nap to let time to see what's hapenning
    print "a little nap to let time to see what's hapenning"
    sleep(napTime)

################################################################################
# Sequence :
# - goToHandoff,
# - close,
# - wait for autochanger to release filter,
# - goToStorage
# - goToHandoff
# - unclamp,
# - open
################################################################################
CCS.setThrowExceptions(True)

###
def sequenceGoToHandoffWithoutFilterAndCloseClamp():

    print "=> sequenceGoToHandoffWithoutFilterAndCloseClamp"
    ###goToHandoff
    print "Going to handoff without filter...."
    carrier.synchCommandLine(int(timeoutHandoff), "goToHandoff")

    waitForAutochangerHoldingFilter()

    ###closing hooks
    print "Closing hooks at HANDOFF...."
    clamp.synchCommandLine(int(timeoutToOpen), "close")

###
def sequenceClampAndGoToStorageWithFilter():

    print "=> sequenceClampAndGoToStorageWithFilter"
    ###wait for autochanger not holding filter
    waitForAutochangerNOTHoldingFilter()

    ###clamping hooks
    print "Clamping hooks at HANDOFF...."
    clamp.synchCommandLine(int(timeoutToOpen), "clamp")

    littleNap()

    ###go to STORAGE
    print "Loader going to STORAGE with filter ...."
    carrier.synchCommandLine(int(timeoutStorage), "goToStorage")

    littleNap()

###
def sequenceGoToHandoffWithFilterUnclampAndOpen():

    print "=>   sequenceGoToHandoffWithFilterUnclampAndOpen"
    ###goToHandoff
    print "Going to handoff with filter...."
    carrier.synchCommandLine(int(timeoutHandoff), "goToHandoff")     

    ###unclamping hooks
    print "Unclamping hooks at HANDOFF ...."
    clamp.synchCommandLine(int(timeoutToOpen), "unclamp")

    waitForAutochangerHoldingFilter()

    ###opening hooks
    print "Opening hooks at HANDOFF...."
    clamp.synchCommandLine(int(timeoutToOpen), "open")

###
def sequenceGoToStorageWithoutFilter():

    print "=> sequenceGoToStorageWithoutFilter"
    
    ###go to STORAGE
    print "Loader going to STORAGE without filter ...."
    carrier.synchCommandLine(int(timeoutStorage), "goToStorage")


################################################################################
# Main Sequence
################################################################################s

for i in range(iter):
  
    print "begin ITERATION  ", i
    sequenceGoToHandoffWithoutFilterAndCloseClamp()
    sequenceClampAndGoToStorageWithFilter()
    sequenceGoToHandoffWithFilterUnclampAndOpen()
    sequenceGoToStorageWithoutFilter()
    print "end ITERATION  ", i
    
print "END LOOP"    






