#####################################################################
# To test sequence of actions on loader :
# goToHandoff, close, goToEngaged, clamp, goToStorage, goToEngaged, unclamp, goToHandoff, open
#####################################################################
### 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


#
CCS.setThrowExceptions(True)

#time to wait for switch change
waitTime = int(15)

#time to do a nap
napTime = int(2)

#####################################################################
# launch loaderProto-standalone subsystem and initialize
#####################################################################
loaderSubsName = "loaderProto-standalone"
loaderStandalone = CCS.attachSubsystem(loaderSubsName)
print "Connected to loaderProto-standalone ", loaderStandalone

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

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

###AP2
acAP2 = CCS.attachSubsystem(loaderSubsName + "/acAP2")

###AF0
acAF0 = CCS.attachSubsystem(loaderSubsName + "/acAF0")

##Proxy
tcpProxy = CCS.attachSubsystem(loaderSubsName + "/loaderTcpProxy")

storagePosition=int((carrier.synchCommandLine(5, "getStoragePosition")).getResult())
print "STORAGE position=", storagePosition

handoffPosition=int((carrier.synchCommandLine(5, "getHandoffPosition")).getResult())
print "HANDOFF position=", handoffPosition

CCS.setThrowExceptions(True)

result = tcpProxy.synchCommandLine(5, "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.synchCommandLine(5, "isInitialized").getResult():
    print "plutoGateway.isInitialized()=", result.getResult()
else:
    raise Exception("plutoGateway not Ready")

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

############################################################################
# Check initial conditions
############################################################################
#loader is connected on camera
if loader.synchCommandLine(5, "isConnectedOnCamera").getResult():
    print "loader carrier is connected on camera: OK"
else:
    raise Exception("loader carrier is not connected on camera")

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

############################################################################
# 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.synchCommandLine(5, "isEmpty").getResult():
        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():
    #homing has to be done
    if not clamp.synchCommandLine(5, "isHomingDone").getResult():
        print "homing has to be done : please wait while opening clamp... "
        clamp.synchCommandLine(int(timeoutToOpen), "open")
    #clamp should be opened
    if not clamp.synchCommandLine(5, "isOpened").getResult():
        print "clamp has to be opened : opening clamp..."
        clamp.synchCommandLine(int(timeoutToOpen), "open")
    #carrier is at STORAGE : nothing to do
    if carrier.synchCommandLine(5, "isAtStorage").getResult():
        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.synchCommandLine(int(timeoutStorage), "goToStorage")        
            
############################################################################
# def go to initial conditions when a filter is in the loader
############################################################################   
def goToInitialConditionsLoaded():
    #clamp should be clamped
    if clamp.synchCommandLine(5, "isClamped").getResult():
        print "clamp is clamped on filter: OK"
    else:
        print "clamping...."
        clamp.synchCommandLine(int(timeoutToOpen), "clamp")
    #clamp is now clamped so carrier can be moved with filter
    sequenceMoveFilterFromStorageToHandoffAndOpen()
    sequenceGoToStorageWithoutFilter()
        
    

############################################################################
# def to wait for autochanger holding filter
############################################################################

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

############################################################################
# def to wait for autochanger NOT holding filter
############################################################################
def waitForAutochangerNOTHoldingFilter():
    loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
#    while not autochanger.synchCommandLine(5, "isNotHoldingFilter").getResult():
    while not acAF0.synchCommandLine(5, "isOn").getResult():
        print "Waiting for autochanger release filter (AP2 & AF0 & AF1) ..."
        sleep(waitTime)
        loaderStandalone.synchCommandLine(5, "updateStateWithSensors")
        sleep(2)
    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 sequenceMoveEmptyToHandoffAndCloseClamp():
###############################################################################
    print "=> sequenceMoveEmptyToHandoffAndCloseClamp"
    ###goToHandoff
    print "Going to handoff without filter...."
    loader.synchCommandLine(int(timeoutHandoff), "moveEmptyToHandoffAndClose")
   
    waitForAutochangerHoldingFilter()



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

    littleNap()

###############################################################################
def sequenceMoveFilterFromStorageToHandoff():
###############################################################################
    print "=>   sequenceMoveFilterFromStorageToHandoff"
    ###moveFilterToHandoff
    #carrier.raiseProfileVelocity();
    #    carrier.goToEngaged();
    #    clamp.unclamp();
    #    carrier.slowProfileVelocity();
    #   carrier.goToHandOff();
    loader.synchCommandLine(int(timeoutHandoff), "moveFilterToHandoff")

    waitForAutochangerHoldingFilter()

###############################################################################
def sequenceOpenClampAndMoveEmptyToStorage():
###############################################################################
    print "=> sequenceOpenClampAndMoveEmptyToStorage"
    
    ###go to STORAGE
    print "Loader opens hooks and go to STORAGE without filter ...."
    loader.synchCommandLine(60, "openClampAndMoveEmptyToStorage")


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






