###############################################################
# Script to test the real hardware of autochanger
# on autochanger prototype in standalone mode with the dummy filter.
# Initial Conditions : 
#   autochanger loaded with a filter, latches closed, trucks at ONLINE position
# Sequence of actions:
# - unlockAndOpenClamps
# - moveToAbsoluteTargetPosition 900000
# - moveToAbsolutePosition 2600
# - closeAndLockClamps
###############################################################
from org.lsst.ccs.scripting import CCS

import java.time
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
iter = int(10)
#####################################################################
waitTime = int(0)

CCS.setThrowExceptions(True)

timeoutFiveMillis = java.time.Duration.ofMillis(5)


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

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

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

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

result = tcpProxy.sendSynchCommand(timeoutFiveMillis, "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 = java.time.Duration.ofMillis(int(autochangerConfig.getCurrentValueForParameter("onlineClamps//timeoutForLockingClamps")))
timeoutForTrucksMotion = java.time.Duration.ofMillis(int(autochangerConfig.getCurrentValueForParameter("autochangerTrucks//timeoutForTrucksMotion")))
timeoutTenSec = java.time.Duration.ofSeconds(10)

####getting welknown positions

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

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

onlinePosition = int((autochangerTrucks.sendSynchCommand(timeoutFiveMillis, "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(timeoutFiveMillis, "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(timeoutFiveMillis, "readMode")
print "Slave Controller Mode =", slaveMode.getResult()

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

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

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

#autochanger latches are CLOSED.
if latches.sendSynchCommand(timeoutFiveMillis, "isClosed"):
    print "autochanger latches CLOSED : OK"
else:
    raise Exception("autochanger not closed : can't execute autochangerSequence1")
    
#autochanger onlineClamps are LOCKED.
if onlineClamps.sendSynchCommand(timeoutFiveMillis, "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(timeoutFiveMillis, "readPosition")))
print "INITIAL POSITION Position =", position
if autochangerTrucks.sendSynchCommand(timeoutFiveMillis, "isAtOnline"):
    print "autochanger is at ONLINE"
else:
    print "Going to INITIAL position ONLINE"
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveAndClampFilterOnline")
    position = int((masterCtl.sendSynchCommand(timeoutFiveMillis, "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(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 900000 ..."
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveToAbsoluteTargetPosition 900001")
    position = int((masterCtl.sendSynchCommand(timeoutFiveMillis, "readPosition")))
    print "Position =", position
    
    print "Executing command moveToAbsolutePosition 2600 ..."    
    autochangerTrucks.sendSynchCommand(timeoutTenSec,"moveToAbsoluteTargetPosition 2600")
    position = int((masterCtl.sendSynchCommand(timeoutFiveMillis, "readPosition")))
    print "Position =", position
    print "Executing command closeAndLockClamps ..."  
    onlineClamps.sendSynchCommand(timeoutForLockingClamps, "closeAndLockClamps")
    print "Sleeping", waitTime, "s....."
    sleep(waitTime)
    print "==> end ITERATION number", i

print "END LOOP"

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






