#####################################################################
# To test sequence of actions on loader clamp:
# close, clamp, 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 loaderSequenceCloseClampUnclampOpen.py
#####################################################################
#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()
for i in range(5):
  
    print("ITERATION number", i)
    sequenceCloseClampUnclampOpen()
    
    
print("END LOOP"    )






