#####################################################################
# To test sequence of actions on loader with AP2, AF0, AF3 signals simulated with USB_IO board:
# goToHandoff, close, goToEngaged, clamp, goToStorage, goToEngaged, unclamp, goToHandoff, open
# !!!! ATTENTION : a py4j server has to be launched before running this script !!!!
#####################################################################
### 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 time import sleep
import java.time

#to use USB_IO board
from DIO96_Driver import *
from py4j.java_gateway import JavaGateway, GatewayParameters
from py4j.protocol import Py4JError

gwparms = GatewayParameters(address="134.158.154.52", port=25333)
gateway = JavaGateway(gateway_parameters=gwparms)
CCS = gateway.entry_point

DIO = DIO96_Driver("LOADER")


 


#
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
#####################################################################
loaderSubsName = "loaderProto-standalone"
loaderStandalone = CCS.attachSubsystem(loaderSubsName)
print("Connected to loaderProto-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")))
timeoutEngaged = java.time.Duration.ofMillis(int(loaderConfig.getCurrentValueForParameter("carrier//timeoutForGoingToEngaged")))
timeoutToOpen = java.time.Duration.ofMillis(int(loaderConfig.getCurrentValueForParameter("clamp//timeoutForOpeningHooks")))
timeoutFiveMillis = java.time.Duration.ofMillis(5)

###loader
loader = CCS.attachSubsystem(loaderSubsName + "/loader")

###carrier
carrier = CCS.attachSubsystem(loaderSubsName + "/carrier")

##Clamp
clamp = CCS.attachSubsystem(loaderSubsName + "/clamp")
hooksController = CCS.attachSubsystem(loaderSubsName + "/hooksController")

###plutoGateway
plutoGateway = CCS.attachSubsystem(loaderSubsName + "/loaderPlutoGateway")

###fake autochanger
autochanger = CCS.attachSubsystem(loaderSubsName + "/autochanger")


##Proxy
tcpProxy = CCS.attachSubsystem(loaderSubsName + "/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, "allDevicesBooted")
if result.getResult():
    print("allDevicesBooted=", 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 not connected on camera")

############################################################################
# def go to initial conditions
############################################################################
def goToInitialConditions():
    print("BEGIN goToInitialConditions")
    print("Initial conditions : empty at STORAGE position...")
    #no filter is in the loader
    if loader.sendSynchCommand(timeoutFiveMillis, "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 go to initial conditions when empty
############################################################################        
def goToInitialConditionsEmpty():
    #clamp should be opened
    if not clamp.sendSynchCommand(timeoutFiveMillis, "isOpened"):
        print("clamp has to be open : opening clamp...")
        clamp.sendSynchCommand(timeoutToOpen, "open")
    #carrier is at STORAGE : nothing to do
    if carrier.sendSynchCommand(timeoutFiveMillis, "isAtStoragePosition"):
        print("loader carrier is at STORAGE position: OK")
    #carrier is NOT at STORAGE : have to move carrier to STORAGE
    else:
        ###go to STORAGE
        print("Loader going to STORAGE without filter ....")
        carrier.sendSynchCommand(timeoutStorage, "goToStorage")        
            
############################################################################
# def go to initial conditions when a filter is in the loader
############################################################################   
def goToInitialConditionsLoaded():
    #clamp should be clamped
    if clamp.sendSynchCommand(timeoutFiveMillis, "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
    sequenceMoveFilterFromStorageToHandoffAndOpen()
    sequenceGoToStorageWithoutFilter()
        
    

############################################################################
# def to wait for autochanger holding filter : simulate by USB_IO board that autochanger is holding filter (AP2 & AF3)
############################################################################

def waitForAutochangerHoldingFilter():
    # mettre ici DIO.loaderForceProtectionSystemCondition
    # ou DIO.dual
    dualSensor = ['DUAL_AP2', 'DUAL_AF0b', 'DUAL_AF1b', 'DUAL_APF3', 'DUAL_PERMIT'] 
    DIO.resetBuf()
    DIO.setADCDualSensorValue2Buf(dualSensor)
    DIO.write2AllDIO()
    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")


############################################################################
# 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,
# - goToEngaged (slowly)
# - clamp
# - goToStorage (normal speed)
# - goToEngaged (normal speed)
# - unclamp
# - goToHandoff (slowly)
# - open
################################################################################
CCS.setThrowExceptions(True)

###
def sequenceGoToHandoffWithoutFilterAndCloseClamp():

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

    waitForAutochangerHoldingFilter()

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

###
def sequenceMoveFilterFromHandoffToStorage():

    print("=> sequenceMoveFilterFromHandoffToStorage")
    ###wait for autochanger not holding filter
    waitForAutochangerNOTHoldingFilter()
    
    ###
    print("Executing command : moveFilterToStorage")
    loader.sendSynchCommand(timeoutHandoff, "moveFilterToStorage")

    littleNap()

###
def sequenceMoveFilterFromStorageToHandoffAndOpen():

    print("=>   sequenceMoveFilterFromStorageToHandoffAndOpen")
    ###moveFilterToHandoff
    loader.sendSynchCommand(timeoutHandoff, "moveFilterToHandoff")

    waitForAutochangerHoldingFilter()

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

###
def sequenceGoToStorageWithoutFilter():

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


################################################################################
# Main Sequence
################################################################################s
goToInitialConditions()
for i in range(iter):
  
    print("begin ITERATION  ", i)
    sequenceGoToHandoffWithoutFilterAndCloseClamp()
    sequenceMoveFilterFromHandoffToStorage()
    sequenceMoveFilterFromStorageToHandoffAndOpen()
    sequenceGoToStorageWithoutFilter()
    print("end ITERATION  ", i)
    
print("END LOOP"    )






