"""
Autochanger sequence
====================

"""
from org.lsst.ccs.scripting import CCS
from org.lsst.ccs.command import *
from java.time import Duration
from java.lang import Exception
from time import sleep, time

CCS.setThrowExceptions(True)

class AutochangerSequence(object):

    def __init__(self, subsystem_name, n_sequences, wait_time):
        self.name = subsystem_name
        self.n_seq = n_sequences
        self.waitTime = wait_time

        subsystem = CCS.attachSubsystem(subsystem_name)
        print("Connected to %s" % subsystem_name)
        print(subsystem.sendSynchCommand("getCCSVersions"))

        self.config = subsystem.sendAsynchCommand("getConfigurationInfo").get()

        self.initialiseTimeouts()
        self.checkControllers()

        self.autochangerTrucks = CCS.attachSubsystem(self.name + "/autochangerTrucks")
        self.masterCtl = CCS.attachSubsystem(self.name + "/linearRailMasterController")
        self.onlineClamps = CCS.attachSubsystem(self.name + "/onlineClamps")

        self.checkInitialConditions()

    def initialiseTimeouts(self):
        timeoutClamps = int(self.config.getCurrentValueForParameter("onlineClamps/timeoutForLockingClamps"))
        timeoutTrucks = int(self.config.getCurrentValueForParameter("autochangerTrucks/timeoutForTrucksMotion"))
        timeouts = {
            "lockingClamps": Duration.ofMillis(timeoutClamps),
            "trucksMotion": Duration.ofMillis(timeoutTrucks),
        }
        self.timeouts = timeouts

    def checkControllers(self):
        tcpProxy = CCS.attachSubsystem(self.name + "/tcpProxy")
        slaveCtl = CCS.attachSubsystem(self.name + "/linearRailSlaveController")
        plutoGateway = CCS.attachSubsystem(self.name + "/acSensorsGateway")

        if not tcpProxy.sendSynchCommand("allDevicesBooted").getResult():
            print("not all hardware boooted")

        if not plutoGateway.sendSynchCommand("isInitialized"):
            raise Exception("plutoGateway not Ready")

        if not self.masterCtl.sendSynchCommand("isInitialized"):
            raise Exception("linearRailMasterController not initialized")

        masterMode = self.masterCtl.sendSynchCommand("readMode")
        if str(masterMode.getResult()) != 'PROFILE_POSITION' :
            self.masterCtl.sendSynchCommand("changeMode", "PROFILE_POSITION")
        print("Master Controller Mode =", masterMode.getResult())

        if not slaveCtl.sendSynchCommand("isInitialized"):
            raise Exception("linearRailSlaveController not initialized")

        slaveMode = slaveCtl.sendSynchCommand("readMode")
        if str(slaveMode.getResult()) != 'MASTER_ENCODER' :
            slaveCtl.sendSynchCommand("changeMode", "MASTER_ENCODER")
        print("Slave Controller Mode =", slaveMode.getResult())

    def checkInitialConditions(self):
        """Check that initial conditions are OK
        
        Autochanger not empty, latches locked, onlineClamps clamped
        """
        autochanger = CCS.attachSubsystem(self.name + "/autochanger")
        latches = CCS.attachSubsystem(self.name + "/latches")

        #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 self.onlineClamps.sendSynchCommand("isLocked"):
            print("autochanger onlineClamps LOCKED : OK")
        else:
            raise Exception("autochanger online clamps not LOCKED : can't execute autochangerSequence1")

    def readPosition(self):
        return int(self.masterCtl.sendSynchCommand("readPosition"))

    def littleNap(self):
        print("Sleeping %ds..." % self.waitTime)
        sleep(self.waitTime)

    def initialisePosition(self):
        """Put autochanger in online position"""

        print("")
        print("Initialisation")
        print("--------------")
        print("INITIAL POSITION Position = %d" % self.readPosition())
        if self.autochangerTrucks.sendSynchCommand("isAtOnline"):
            print("autochanger is at ONLINE")
        else:
            print("Going to INITIAL position ONLINE")
            self.autochangerTrucks.sendSynchCommand(self.timeouts["trucksMotion"], "moveAndClampFilterOnline")
            print("Position = %d" % self.readPosition())
            self.littleNap()

    def classicSequence(self, idx=1, nidx=1):
        """Move to each socket and come back"""
        print("")
        print("Iteration %d/%d" % (idx, nidx))
        print("-------------")
        t0 = time()
        
        print("Unlocking and opening ONLINE clamps ...")
        self.onlineClamps.sendSynchCommand(timeouts["lockingClamps"], "unlockAndOpenClamps")
        
        self.littleNap()

        print("moving To Absolute TargetPosition 900001 ...")
        self.autochangerTrucks.sendSynchCommand(self.timeouts["trucksMotion"], "moveToAbsoluteTargetPosition", "900001")
        
        print("Position = %d" % self.readPosition())
        
        print("Executing command moveAndClampFilterOnline ..."    )
        self.autochangerTrucks.sendSynchCommand(self.timeouts["trucksMotion"], "moveAndClampFilterOnline")

        print("Position = %d" % self.readPosition())
        elapsed_sec = time() - t0
        minutes, sec = int(elapsed_sec // 60), int(elapsed_sec % 60)
        print("")
        print("time elapsed: %d'%d" % (minutes, sec))

    def run(self):
        """Execute the sequence"""
        print(__doc__)

        self.initialisePosition()
    
        for i in range(self.n_seq):
            self.classicSequence(idx=i+1, nidx=self.n_seq)
    

if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser()
    parser.add_argument(
        "-s", "--subsystem", 
        type=str, choices=["fcs", "autochanger-standalone"], default="fcs", 
        help="Name of the running subsystem: 'fcs' or 'autochanger-standalone'")
    parser.add_argument(
        "-n", "--n_sequences", 
        type=int, default=1, 
        help="Number of full sequences executed (default 1)")
    parser.add_argument(
        "-w", "--wait_time", 
        type=int, default=2, 
        help="Sleeping time in secs between moves (default 2s)")

    args = parser.parse_args()

    AutochangerSequence(args.subsystem, args.n_sequences, args.wait_time).run()
