import time
from org.lsst.ccs.scripting import CCS


CURRENT_TO_UNCLAMP_XPLUS = -2500
CURRENT_TO_UNCLAMP_XMINUS = -3200

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 "Coming next: %s\n" % text
        print "Press any key to proceed..."
        return raw_input()

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

    def move_trucks(self, position, dangerous=False):
        position = int(position)
        print "trucks: moveToAbsoluteTargetPosition %d" % position
        if dangerous:
            self.trucks.sendSynchCommand("moveToAbsoluteTargetPositionDangerous", position)
        else:
            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 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)
        time.sleep(0.1)

    def unclamping_sequence(self):
        self.send_current_to_clamp("Xplus", -100)
        self.send_current_to_clamp("Xminus", -100)
        self.send_current_to_clamp("Xplus", CURRENT_TO_UNCLAMP_XPLUS)
        self.send_current_to_clamp("Xminus", CURRENT_TO_UNCLAMP_XMINUS)

        self.wait_for_user("VERIFY CAROUSEL CLAMPS ARE UNCLAMPED")
        self.move_trucks(997000)
        # Move AC to approach position
        self.wait_for_user("moving AC to 970 000 : DANGEROUS")
        self.move_trucks(970000, dangerous=True)

        self.wait_for_user("moving AC to  76 600")
        self.move_trucks(76600)
        # Release voltage in clamps
        self.disable_clamps()

    
if __name__ == "__main__":
    CarouselAC().unclamping_sequence()
