#####################################################################
# To test sequence of actions on loader clamp:
# close, clamp, unclamp, open
# Tested on loader proto at LPSC in May 2017
#####################################################################
#import CCS Scripting Implementation                                                                                          
from org.lsst.ccs.scripting import CCS
#import the definition for a Java Exception                                                                                   
import java.time
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)

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

#getting timeouts
loaderConfigFuture = loaderStandalone.sendAsynchCommand("getConfigurationInfo")
print "getting loader configuration info"
loaderConfig = loaderConfigFuture.get()
timeoutHandoff = java.time.Duration.ofMillis(int(loaderConfig.getCurrentValueForParameter("carrier//timeoutForGoingToHandOff")))
timeoutStorage = java.time.Duration.ofMillis(int(loaderConfig.getCurrentValueForParameter("carrier//timeoutForGoingToStorage")))
timeoutToOpen = java.time.Duration.ofMillis(int(loaderConfig.getCurrentValueForParameter("clamp//timeoutForOpeningHooks")))
timeoutFiveMillis = java.time.Duration.ofMillis(5)

###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.sendSynchCommand(timeoutFiveMillis, "getStoragePosition")))
print "STORAGE position=", storagePosition

handoffPosition=int((carrier.sendSynchCommand(timeoutFiveMillis, "getHandoffPosition")))
print "HANDOFF position=", handoffPosition

CCS.setThrowExceptions(True)

result = tcpProxy.sendSynchCommand(timeoutFiveMillis, "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.sendSynchCommand(timeoutFiveMillis, "isInitialized"):
    print "plutoGateway.isInitialized()=", result.getResult()
else:
    raise Exception("plutoGateway not Ready")

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

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

#carrier is at HANDOFF
if carrier.sendSynchCommand(timeoutFiveMillis, "isAtHandoffPosition"):
    print "loader carrier is at HANDOFF position: OK"
else:
    raise Exception("loader carrier is NOT at HANDOFF position")

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

#pince should be ouverte
if not clamp.sendSynchCommand(timeoutFiveMillis, "isOpened"):
    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.sendSynchCommand(timeoutFiveMillis, "updateStateWithSensors")
    while not autochanger.sendSynchCommand(timeoutFiveMillis, "isHoldingFilter"):
        print "Waiting for autochanger hold filter (AP2 & AF3) ..."
        sleep(waitTime)
        loaderStandalone.sendSynchCommand(timeoutFiveMillis, "updateStateWithSensors")
    else:
        print "autochanger is holding filter"

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

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

def sequenceCloseClampUnclampOpen():
    napTime = 2

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

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

    ###wait for autochanger not holding filter
    waitForAutochangerNOTHoldingFilter()

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

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

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

    waitForAutochangerHoldingFilter()

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

################################################################################
# Main Sequence
################################################################################s
waitForAutochangerHoldingFilter()
iter = 5
print "Iterations number = ", iter

for i in range(iter):
  
    print "begin ITERATION ", i
    sequenceCloseClampUnclampOpen()
    print "end ITERATION ", i
    
print "END LOOP"    






