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






