###############################################################
# 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 ONLINE position
# Sequence of actions:
# - unlockAndOpenClamps
# - moveToAbsoluteTargetPosition 900000
# - moveAndClampFilterOnline
###############################################################
from org.lsst.ccs.scripting import CCS
from java.time import Duration
from java.lang import Exception
from org.lsst.ccs.command import *
from time import sleep
#import datetime


#####################################################################
### change iteration number / nombre d'iterations a changer si besoin est
n_iter = 2
#####################################################################
waitTime = 0

CCS.setThrowExceptions(True)


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

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

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

#tcpProxy
tcpProxy = CCS.attachSubsystem(acSubsName + "/tcpProxy")

result = tcpProxy.sendSynchCommand("allDevicesBooted")
if result.getResult():
    print "allDevicesBooted", result.getResult()
else:
    print "not all hardware boooted"
#    raise Exception("CANopen Hardware not ready")

###plutoGateway
plutoGateway = CCS.attachSubsystem(acSubsName + "/acSensorsGateway")

###autochanger
autochanger = CCS.attachSubsystem(acSubsName + "/autochanger")

###autochangerTrucks
autochangerTrucks = CCS.attachSubsystem(acSubsName + "/autochangerTrucks")

###onlineClamps
onlineClamps = CCS.attachSubsystem(acSubsName + "/onlineClamps")

###latches
latches = CCS.attachSubsystem(acSubsName + "/latches")

####getting timeouts
autochangerConfigFuture = autochangerStandalone.sendAsynchCommand("getConfigurationInfo")
print "getting loader configuration info"
autochangerConfig = autochangerConfigFuture.get()

timeoutForLockingClamps = Duration.ofMillis(
    int(autochangerConfig.getCurrentValueForParameter("onlineClamps/timeoutForLockingClamps"))
)
timeoutForTrucksMotion = Duration.ofMillis(
    int(autochangerConfig.getCurrentValueForParameter("autochangerTrucks/timeoutForTrucksMotion"))
)
timeoutTenSec = Duration.ofSeconds(10)

####getting welknown positions

standbyPosition = int(autochangerTrucks.sendSynchCommand("getStandbyPosition"))
print "STANDBY position=", standbyPosition

handoffPosition = int(autochangerTrucks.sendSynchCommand("getHandoffPosition"))
print "HANDOFF position = ", handoffPosition

onlinePosition = int(autochangerTrucks.sendSynchCommand("getOnlinePosition"))
print "ONLINE position =", onlinePosition

############################################################################
# Check controllers and plutoGateway are READY
############################################################################
#hardware is READY 
#print "Check that controllers and plutoGateway are initialized...."
if plutoGateway.sendSynchCommand("isInitialized"):
    print "plutoGateway.isInitialized()=", result.getResult()
else:
    raise Exception("plutoGateway not Ready")


#Read of the linearRailSlaveController mode
slaveCtl = CCS.attachSubsystem(acSubsName + "/linearRailSlaveController")
slaveMode = slaveCtl.sendSynchCommand("readMode")
print "Slave Controller Mode =", slaveMode.getResult()

#Read of the linearRailMasterController mode
masterCtl = CCS.attachSubsystem(acSubsName + "/linearRailMasterController")
masterMode = masterCtl.sendSynchCommand("readMode")
print "Master Controller Mode =", masterMode.getResult()

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

if masterCtl.sendSynchCommand("isInitialized"):
    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.sendSynchCommand("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.sendSynchCommand(timeoutFiveMillis,"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.sendSynchCommand("isEmpty"):
    raise Exception("autochanger empty : can't execute autochangerSequence1")
else:
    print "autochanger loaded with a filter : OK"

#autochanger latches are CLOSED.
if latches.sendSynchCommand("isClosed"):
    print "autochanger latches CLOSED : OK"
else:
    raise Exception("autochanger not closed : can't execute autochangerSequence1")
    
#autochanger onlineClamps are LOCKED.
if onlineClamps.sendSynchCommand("isLocked"):
    print "autochanger onlineClamps LOCKED : OK"
else:
    raise Exception("autochanger online clamps not LOCKED : can't execute autochangerSequence1")

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

position = int((masterCtl.sendSynchCommand("readPosition")))
print "INITIAL POSITION Position =", position
if autochangerTrucks.sendSynchCommand("isAtOnline"):
    print "autochanger is at ONLINE"
else:
    print "Going to INITIAL position ONLINE"
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveAndClampFilterOnline")
    position = int((masterCtl.sendSynchCommand("readPosition")))
    print "Position=", position
    sleep(waitTime)
    print "END OF GOING TO INITIAL POSITION"

#####################################################################
# loop to move the trucks from ONLINE to 900000, and back
#####################################################################

for i in range(n_iter):
    print "==> ITERATION number", i
    print "Unlocking and opening ONLINE clamps ..."
    onlineClamps.sendSynchCommand(timeoutForLockingClamps, "unlockAndOpenClamps")
    print "Sleeping", waitTime, "....."
    sleep(waitTime)
    print "moving To Absolute TargetPosition 900001 ..."
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveToAbsoluteTargetPosition 900001")
    position = int((masterCtl.sendSynchCommand("readPosition")))
    print "Position =", position
    
    print "Executing command moveAndClampFilterOnline ..."    
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveAndClampFilterOnline")
    position = int((masterCtl.sendSynchCommand("readPosition")))
    print "Position =", position
    print "Sleeping", waitTime, "s....."
    sleep(waitTime)
    print "==> end ITERATION number", i

print "END LOOP"

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






