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

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





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






