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

        sockets = [1, 2, 3, 4, 5]
        current_socket_id = self.carousel.sendSynchCommand("getSocketAtStandbyID")
        current_sockets = sockets[current_socket_id-1:] + sockets[:current_socket_id-1]

        # Make sure there the autochanger is empty first
        if self.autochanger.sendSynchCommand("isHoldingFilter"):
            filter_id = self.autochanger.sendSynchCommand("getFilterID")
            filter_name = self.autochanger.sendSynchCommand("getFilterOnTrucksName")
            current_sockets.remove(current_socket_id)
            print("Socket %i holds filter %s with ID %i" % (current_socket_id, filter_name, filter_id))
            self.fcs.sendSynchCommand(TIMEOUT, "storeFilterOnCarousel")
        
        # Make sure the autochanger trucks are aligned
        self.autochanger.sendSynchCommand("alignFollower")

        for socket_id in current_sockets:
            self.move_to_socket(socket_id)
            if self.carousel.sendSynchCommand("isHoldingFilterAtStandby"):
                name, filter_id = self.discover_filter_on_socket()
                print("Socket %i holds filter %s with ID %i" % (socket_id, name, filter_id))
            else:
                print("Socket %i holds no filter" % (socket_id))
            if not self.carousel.sendSynchCommand("isSocketAtStandbyAvailable"):
                print("    and is currently NOT AVAILABLE")

        print("\nDiscovery 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()
