###############################################################
# Script to test the real hardware of autochanger
# on autochanger prototype in standalone mode
# Initial Conditions : 
#   autochanger loaded with a filter, latches closed, trucks at STANDBY position
# Sequence of actions:
# - goToOnline
# - closeAndLockOnlineClamps
# - unlockAndOpenOnlineClamps
# - goToStandby
###############################################################
from org.lsst.ccs.scripting import CCS
from java.lang import Exception
from java.lang import RuntimeException
from org.lsst.ccs.command import *
from time import sleep
#import datetime


#####################################################################
### change iteration number / nombre d'iterations a changer si besoin est
iter = int(2)
#####################################################################
waitTime = int(0)

CCS.setThrowExceptions(True)

#####################################################################
# launch autochanger-standalone subsystem and initialize
#####################################################################
autochangerStandalone = CCS.attachSubsystem("autochanger-standalone")

print "Connected to autochanger-standalone=> ", autochangerStandalone

ccsVersion= autochangerStandalone.synchCommandLine(5,"getCCSVersions")
print "TESTS of autochanger sequence1 with CCS version number"
print ccsVersion.getResult()

#print "Date=", date()

#tcpProxy
tcpProxy = CCS.attachSubsystem("autochanger-standalone/tcpProxy")

result = tcpProxy.synchCommandLine(5, "isCANDevicesReady")
if result.getResult():
    print "isCANDevicesReady=", result.getResult()
else:
    raise Exception("CANopen Hardware not ready")

###plutoGateway
plutoGateway = CCS.attachSubsystem("autochanger-standalone/acSensorsGateway")

###autochanger
autochanger = CCS.attachSubsystem("autochanger-standalone/autochanger")

###autochangerTrucks
autochangerTrucks = CCS.attachSubsystem("autochanger-standalone/autochangerTrucks")

###onlineClamps
onlineClamps = CCS.attachSubsystem("autochanger-standalone/onlineClamps")

###latches
latches = CCS.attachSubsystem("autochanger-standalone/latches")

####getting timeouts
autochangerConfigFuture = autochangerStandalone.asynchCommandLine("getConfigurationInfo")
print "getting loader configuration info"
autochangerConfig = autochangerConfigFuture.get()
timeoutForLockingClamps = autochangerConfig.getCurrentValueForParameter("onlineClamps//timeoutForLockingClamps")
timeoutForTrucksMotion = autochangerConfig.getCurrentValueForParameter("autochangerTrucks//timeoutForTrucksMotion")

####getting welknown positions

standbyPosition = int((autochangerTrucks.synchCommandLine(5, "getStandbyPosition")).getResult())
print "STANDBY position=", standbyPosition

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

onlinePosition = int((autochangerTrucks.synchCommandLine(5, "getOnlinePosition")).getResult())
print "ONLINE position =", onlinePosition

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


#Read of the linearRailSlaveController mode
slaveCtl = CCS.attachSubsystem("autochanger-standalone/linearRailSlaveController")
slaveMode = slaveCtl.synchCommandLine(5, "readMode")
print "Slave Controller Mode =", slaveMode.getResult()

#Read of the linearRailMasterController mode
masterCtl = CCS.attachSubsystem("autochanger-standalone/linearRailMasterController")
masterMode = masterCtl.synchCommandLine(5, "readMode")
print "Master Controller Mode =", masterMode.getResult()

if slaveCtl.synchCommandLine(5, "isInitialized").getResult():
    print "linearRailSlaveController.isInitialized()=", result.getResult()
else:
    raise Exception("linearRailSlaveController not initialized")

if masterCtl.synchCommandLine(5, "isInitialized").getResult():
    print "linearRailMasterController.isInitialized()=", result.getResult()
else:
    raise Exception("linearRailMasterController not initialized")

if str(masterMode.getResult()) != 'PROFILE_POSITION' :
   #  raise Exception("ERROR on master Controller mode")
   print "ERROR on master Controller mode - have to change it."
   result = masterCtl.synchCommandLine(5, "changeMode PROFILE_POSITION")
else:
   print "master Controller is in mode", masterMode.getResult()

if str(slaveMode.getResult()) != 'MASTER_ENCODER' :
   #  raise Exception("ERROR on slave Controller mode")
   print "ERROR on slave Controller mode - have to change it."
   result = slaveCtl.synchCommandLine(5,"changeMode MASTER_ENCODER")
else:
   print "slave Controller is in mode", slaveMode.getResult()


######################################################################
# check that initial conditions are OK
######################################################################
#not empty, latches locked, onlineClamps clamped

#autochanger is loaded with a filter
if autochanger.synchCommandLine(5, "isEmpty").getResult():
    raise Exception("autochanger empty : can't execute autochangerSequence1")
else:
    print "autochanger loaded with a filter : OK"

#autochanger latches are CLOSED.
if latches.synchCommandLine(5, "isLocked").getResult():
    print "autochanger latches CLOSED : OK"
else:
    raise Exception("autochanger not closed : can't execute autochangerSequence1")
    
#autochanger onlineClamps are OPENED.
if onlineClamps.synchCommandLine(5, "isOpened").getResult():
    print "autochanger onlineClamps OPENED : OK"
else:
    raise Exception("autochanger online clamps not OPENED : can't execute autochangerSequence1")

######################################################################
# Move Trucks to STANDBY position if trucks are not already at STANDBY
######################################################################

position = int((masterCtl.synchCommandLine(5, "readPosition")).getResult())
print "INITIAL POSITION Position =", position
if position < standbyPosition - 100:
   print "Going to INITIAL position STANDBY"
   resultMvt=autochangerTrucks.synchCommandLine(10000,"goToStandby")
   print "resultMvt=", str(resultMvt)
   position = int((masterCtl.synchCommandLine(5, "readPosition")).getResult())
   print "Position=", position
   sleep(waitTime)
   print "END OF GOING TO INITIAL POSITION"

#####################################################################
# loop to move the trucks from STANDBY to ONLINE
#####################################################################

for i in range(iter):
   print "ITERATION number", i
   print "Going to ONLINE ..."
   autochangerTrucks.synchCommandLine(int(timeoutForTrucksMotion), "goToOnline")
   position = int((masterCtl.synchCommandLine(5, "readPosition")).getResult())
   print "Position =", position
   print "Sleeping", waitTime, "....."
   sleep(waitTime)
   
   print "Locking ONLINE clamps ...."
   onlineClamps.synchCommandLine(int(timeoutForLockingClamps),"closeAndLockClamps")
   
   print "Opening ONLINE clamps ...."
   onlineClamps.synchCommandLine(int(timeoutForLockingClamps),"unlockAndOpenClamps")
   
   print "Going to STANDBY"
   resultMvt = autochangerTrucks.synchCommandLine(int(timeoutForTrucksMotion), "goToStandby")
   position = int((masterCtl.synchCommandLine(5, "readPosition")).getResult())
   print "Position =", position
   print "Sleeping", waitTime, "s....."
   sleep(waitTime)
   print "end ITERATION number", i

print "END LOOP"

##################################END##################################






