"""
Loader test sequence
====================

The goal of this script is to test the repeatability of 
loader actions over a given a number of cycles.

Depending on the presence of the autochanger, an input from
the user might be needed to mock for the local protection 
system the release of the filter by the autochanger.

Initialisation
--------------
If the loader is empty:
- homing
- moveEmptyToStorage

If the filter is not clamped:
- moveFilterToHandoff
- open hooks
- moveEmptyToStorage

If the filter is clamped:
- moveFilterToEngaged
- unclamp
- moveFilterToHandoff
- openClamps
- moveEmptyToStorage


Cycles
------
In a given cycle, the following actions are performed:
- moveEmptyToHandoff
- close hooks
- moveFilterToEngaged
- clamp
- moveFilterToStorage
- moveFilterToEngaged
- unclamp 
- moveFilterToHandoff
- open hooks
- moveEmptyToStorage

"""
from time import time, sleep
from java.time import Duration
from java.lang import Exception

from org.lsst.ccs.scripting import CCS

CCS.setThrowExceptions(True)

#####################################################################
# ==============================
# Parameters that may be changed
# ==============================

# NUMBER OF CYCLES / nombre d'iterations a changer si besoin est
N_ITER = 1

# wait duration for switch change (seconds)
waitTime = 15

# nap duration (seconds)
napTime = 2

#####################################################################

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

# Timeouts
loaderConfigFuture = loaderStandalone.sendAsynchCommand("getConfigurationInfo")
print("getting loader configuration info")
loaderConfig = loaderConfigFuture.get()
timeoutHandoff = Duration.ofMillis(
    int(loaderConfig.getCurrentValueForParameter("carrier/timeoutForGoingToHandOff"))
)
timeoutStorage = Duration.ofMillis(
    int(loaderConfig.getCurrentValueForParameter("carrier/timeoutForGoingToStorage"))
)
timeoutEngaged = Duration.ofMillis(
    int(loaderConfig.getCurrentValueForParameter("carrier/timeoutForGoingToEngaged"))
)
timeoutToOpen = Duration.ofMillis(
    int(loaderConfig.getCurrentValueForParameter("clamp/timeoutForOpeningHooks"))
)

# Attach subsystems
loader = CCS.attachSubsystem(loaderSubsName + "/loader")
carrier = CCS.attachSubsystem(loaderSubsName + "/carrier")
clamp = CCS.attachSubsystem(loaderSubsName + "/clamp")
hooksController = CCS.attachSubsystem(loaderSubsName + "/hooksController")
plutoGateway = CCS.attachSubsystem(loaderSubsName + "/loaderPlutoGateway")
acAP2 = CCS.attachSubsystem(loaderSubsName + "/acAP2")
acAF0 = CCS.attachSubsystem(loaderSubsName + "/acAF0")
tcpProxy = CCS.attachSubsystem(loaderSubsName + "/loaderTcpProxy")
# and fake autochanger
autochanger = CCS.attachSubsystem(loaderSubsName + "/autochanger")

storagePosition = int(carrier.sendSynchCommand("getStoragePosition"))
print("STORAGE position=", storagePosition)

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

# Test readiness of devices, controllers and pluto
result = tcpProxy.sendSynchCommand("allDevicesBooted")
if result:
    print("allDevicesBooted=", result)
else:
    raise Exception("CANopen Hardware not ready")

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

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

# Test readiness of loader
if loader.sendSynchCommand("isConnectedOnCamera"):
    print("loader carrier is connected on camera: OK")
else:
    raise Exception("loader carrier is not connected on camera")

if acAP2.sendSynchCommand("isOn"):
    print("AP2 is ON: OK")
else:
    raise Exception("AP2 has to be ON to continue")

##############
# Autochanger
##############
autochangerSubsName = "autochanger-standalone_AC1"
autochangerStandalone = CCS.attachSubsystem(autochangerSubsName)
print("Connected to autochanger-standalone_AC1 ", autochangerStandalone)

# acPlutoGateway
acPlutoGateway = CCS.attachSubsystem(autochangerSubsName + "/acSensorsGateway")
autochangerRunning = acPlutoGateway.sendSynchCommand("isInitialized")
if autochangerRunning:
    print("autochanger is running and acPlutoGateway isInitialized")
else:
    print("autochanger is NOT running or acPlutoGateway is NOT initialized")

# latches
latches = CCS.attachSubsystem(autochangerSubsName + "/latches")

autochangerConfigFuture = autochangerStandalone.sendAsynchCommand("getConfigurationInfo")
autochangerConfig = autochangerConfigFuture.get()
timeoutAcLatches = Duration.ofMillis(
    int(autochangerConfig.getCurrentValueForParameter("latches/timeoutForOpening"))
)


########################
# Autochanger interface
########################
def updateStateWithSensors():
    loaderStandalone.sendSynchCommand("updateStateWithSensors")
    sleep(1)


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


def waitForAutochangerNOTHoldingFilter():
    updateStateWithSensors()
    while not acAF0.sendSynchCommand("isOn"):
        print("Waiting for autochanger release filter (AP2 & AF0 & AF1) ...")
        sleep(waitTime)
        updateStateWithSensors()
    else:
        print("autochanger is NOT holding filter")


###########################
# Main sequences of action
###########################

def littleNap():
    "a little nap to let time to see what's hapenning"
    print("zZzZzZzZz...(%d sec)" % napTime )
    sleep(napTime)

def timed(func):
    def timer(*args):
        t0 = time()
        func(*args)
        t1 = time()
        print("Elapsed time for action %s: %.3f" % (func.__name__, t1 - t0) )
    return timer

@timed
def autochangerOpenLatches():
    if autochangerRunning:
        print("Autochanger is opening latches....")
        latches.sendSynchCommand(timeoutAcLatches, "open")
        autochangerStandalone.sendSynchCommand("updateStateWithSensors")
        waitForAutochangerNOTHoldingFilter()
    else:
        waitForAutochangerNOTHoldingFilter()

@timed
def autochangerCloseLatches():
    if autochangerRunning:
        print("Autochanger is closing latches....")
        latches.sendSynchCommand(timeoutAcLatches, "close")
        autochangerStandalone.sendSynchCommand("updateStateWithSensors")
        waitForAutochangerHoldingFilter()
    else:
        waitForAutochangerHoldingFilter()        

@timed
def sequenceMoveEmptyToHandoffAndCloseClamp():
    print("Executing loader command : moveEmptyToHandoffAndClose")
    loader.sendSynchCommand(timeoutHandoff, "moveEmptyToHandoffAndClose")

@timed
def sequenceMoveFilterFromHandoffToStorage():
    print("Executing loader command : moveFilterToStorage")
    loader.sendSynchCommand(timeoutStorage, "moveFilterToStorage")
    littleNap()

@timed
def sequenceMoveFilterFromStorageToHandoff():
    print("Executing loader command : moveFilterToHandoff")
    loader.sendSynchCommand(timeoutHandoff, "moveFilterToHandoff")

@timed
def sequenceOpenClampAndMoveEmptyToStorage():
    print("Executing loader command : openClampAndMoveEmptyToStorage")
    loader.sendSynchCommand(timeoutHandoff, "openClampAndMoveEmptyToStorage")


#####################
# Initial conditions
#####################
def goToInitialConditions():
    print("BEGIN goToInitialConditions")
    print("Initial conditions: put loader empty at STORAGE position...")
    # no filter is in the loader
    if loader.sendSynchCommand("isEmpty"):
        print("loader is NOT holding a filter: OK")
        goToInitialConditionsEmpty()
    # a filter is in the loader 
    else:
        print("a filter is on the loader")
        goToInitialConditionsLoaded()
    print("END goToInitialConditions")

def goToInitialConditionsEmpty():
    # homing is done through the open clamp command
    if not clamp.sendSynchCommand("isHomingDone"):
        print("homing has to be done : please wait while opening clamp... ")
        clamp.sendSynchCommand(timeoutToOpen, "open")
    # clamp should be opened
    if not clamp.sendSynchCommand("isOpened"):
        print("clamp has to be opened : opening clamp...")
        clamp.sendSynchCommand(timeoutToOpen, "open")
    # move to storage
    if carrier.sendSynchCommand("isAtStorage"):
        print("loader carrier is already at STORAGE")
    else:
        print("loader going to STORAGE without filter...")
        carrier.sendSynchCommand(timeoutStorage, "goToStorage")        
            
def goToInitialConditionsLoaded():
    # clamp should be clamped
    if clamp.sendSynchCommand("isClamped"):
        print("clamp is clamped on filter: OK")
    else:
        print("clamping....")
        clamp.sendSynchCommand(timeoutToOpen, "clamp")
    # clamp is now clamped so carrier can be moved with filter
    sequenceMoveFilterFromStorageToHandoff()
    autochangerCloseLatches()
    sequenceOpenClampAndMoveEmptyToStorage()
    

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

def main(n_iter):
    goToInitialConditions()

    for i in range(n_iter):
        print("begin ITERATION  ", i)
        sequenceMoveEmptyToHandoffAndCloseClamp()
        autochangerOpenLatches()
        sequenceMoveFilterFromHandoffToStorage()
        sequenceMoveFilterFromStorageToHandoff()
        autochangerCloseLatches()
        sequenceOpenClampAndMoveEmptyToStorage()
        print("end ITERATION  ", i)
        
    print("END LOOP")

if __name__ == "__main__":
    import sys
    try:
        n_iterations = int(sys.argv[1])
    except IndexError:
        n_iterations = N_ITER

    main(n_iterations)
