"""
Discover which filters are in the carousel
==========================================

"""
from java.time import Duration
from org.lsst.ccs.scripting import CCS


CCS.setThrowExceptions(True)

TIMEOUT = Duration.ofSeconds(30)

class FilterDiscovery(object):
    def __init__(self, main_subsystem):
        self.fcs = CCS.attachSubsystem(main_subsystem)
        self.autochanger = CCS.attachSubsystem("%s/%s" % (main_subsystem, "autochanger"))
        self.carousel = CCS.attachSubsystem("%s/%s" % (main_subsystem, "carousel"))

    def move_to_socket(self, socket_id):
        if not self.autochanger.sendSynchCommand("isAtHandoff"):
            self.autochanger.sendSynchCommand(TIMEOUT, "goToHandoff")
        self.carousel.sendSynchCommand("rotateSocketToStandby", "socket%d" % socket_id)

    def discover_filter_on_socket(self):
        self.autochanger.sendSynchCommand(TIMEOUT, "moveToApproachStandbyPositionWithHighVelocity")
        self.autochanger.sendSynchCommand(TIMEOUT, "moveToStandbyWithLowVelocity")
        filter_id = self.autochanger.sendSynchCommand("getFilterID")
        filter_name = self.autochanger.sendSynchCommand("getFilterOnTrucksName")
        self.autochanger.sendSynchCommand(TIMEOUT, "moveToApproachStandbyPositionWithLowVelocity")
        self.autochanger.sendSynchCommand(TIMEOUT, "moveToHandoffWithHighVelocity")
        return (filter_name, filter_id)

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

        # Make sure there the autochanger is empty first
        if self.autochanger.sendSynchCommand("isHoldingFilter"):
            self.fcs.sendSynchCommand("storeFilterOnCarousel")
        # Make sure the autochanger trucks are aligned
        self.autochanger.sendSynchCommand("alignFollower")
        for socket_id in range(1, 6):
            self.move_to_socket(socket_id)
            if not self.carousel.sendSynchCommand("isHoldingFilterAtStandby"):
                print("Socket %i contains no filter" % (socket_id))
                continue
            name, filter_id = self.discover_filter_on_socket()
            print("Socket %i contains filter %s with ID %i" % (socket_id, name, filter_id))

        print("\Discovery done.")


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser("FCS filter discovery")
    parser.add_argument(
        "-s", "--subsystem", 
        type=str, choices=["fcs", "fcs-PROTO"], default="fcs", 
        help="Name of the running subsystem: 'fcs' or 'fcs-PROTO'")
    args = parser.parse_args()

    FilterDiscovery(main_subsystem=args.subsystem).run()
