import sys
import time
from org.lsst.ccs.scripting import CCS
from java.lang import Exception


AUTOCHANGER_CLAMPING_POSITION = 1005500
HANDOFF_POSITION = 76600


class CarouselAC(object):

    def __init__(self):
        self.autochanger = CCS.attachSubsystem("fcs/autochanger")
        self.carousel = CCS.attachSubsystem("fcs/carousel")
        self.latchXminus = CCS.attachSubsystem("fcs/tcpProxy/latchXminusController")
        self.latchXplus = CCS.attachSubsystem("fcs/tcpProxy/latchXplusController")
        self.trucks = CCS.attachSubsystem("fcs/autochanger/autochangerTrucks")
        self.clampXminus = CCS.attachSubsystem("fcs/tcpProxy/clampXminusController")
        self.clampXplus = CCS.attachSubsystem("fcs/tcpProxy/clampXplusController")

    def wait_for_user(self, text):
        print "%s\n" % text
        print "Press any key to proceed..."
        return raw_input()

    def get_autochanger_position(self):
        pos = self.trucks.sendSynchCommand("getPosition")
        return int(pos)

    def align_slave(self):
        self.trucks.sendSynchCommand("alignSlave")
        time.sleep(1)

    def handoff(self):
        self.trucks.sendSynchCommand("goToHandOff")

    def safety_checks(self):
        acHoldsFilter = self.carousel.sendSynchCommand("isAutochangerHoldingFilter")
        return acHoldsFilter

    def move_trucks(self, position):
        position = int(position)
        print "trucks: moveToAbsoluteTargetPosition %d" % position
        self.trucks.sendSynchCommand("moveToAbsoluteTargetPosition", position)
        time.sleep(1)

    def disable_clamps(self):
        print "carousel clamps: disableVoltage"
        self.clampXminus.sendSynchCommand("disableVoltage")
        self.clampXplus.sendSynchCommand("disableVoltage")

    def rotate_carousel(self, socket_id):
        print "carousel: rotateSocketToStandby %d" % socket_id
        self.carousel.sendSynchCommand("rotateSocketToStandby", "socket%d" % socket_id)

    def send_current_to_clamp(self, clamp, current):
        assert clamp in ["Xminus", "Xplus"]
        if clamp == "Xminus":
            self.clampXminus.sendSynchCommand("enableAndWriteCurrent", current)
        else:
            self.clampXplus.sendSynchCommand("enableAndWriteCurrent", current)

    def _send_current_to_latches(self, current):
        self.latchXminus.sendSynchCommand("enableAndWriteCurrent", current)
        self.latchXplus.sendSynchCommand("enableAndWriteCurrent", current)
        time.sleep(1)
        self.latchXminus.sendSynchCommand("enableAndWriteCurrent", 0)
        self.latchXplus.sendSynchCommand("enableAndWriteCurrent", 0)
        time.sleep(1)

    def open_latches(self):
        _send_current_to_latches(300)
    
    def close_latches(self):
        _send_current_to_latches(-300)

    def clamping_sequence(self, socket_id):
        self.safety_checks()

        # Move AC to safe position
        if not self.trucks.sendSynchCommand("isAtHandoff"):
            self.move_trucks(HANDOFF_POSITION)

        # Align slave
        self.align_slave()

        # Check conditions for rotation
        self.carousel.sendSynchCommand("checkConditionsForRotation")

        # Rotate to socket
        try:
            self.rotate_carousel(socket_id)
        except Exception as e:
            print "Carousel already at socket %d" % socket_id

        # Release voltage in clamps
        self.disable_clamps()
        # Move AC to approach position
        self.wait_for_user("Verify clamps at socket %d are ready to clamp" % socket_id)

        self.move_trucks(AUTOCHANGER_CLAMPING_POSITION)

    
if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser()
    parser.add_argument(
        "-s", "--socket_id", 
        type=int, choices=[1, 2, 3, 4, 5],
        help="Socket ID where to clamp the filter")
    args = parser.parse_args()

    CarouselAC().clamping_sequence(args.socket_id)

