import u3

# Initializes the labjacks
d1 = u3.U3(autoOpen=False)
d1.open(handleOnly=True, serial=320094269)

d2 = u3.U3(autoOpen=False)
d2.open(handleOnly=True, serial=320095365)


# Changes the DAC output of the labjack
def SetDac(d, dac_num, voltage):
    if voltage < 0:
        print('output voltage must be >= 0')
        return
    if voltage > 5:
        print('output voltage must be <= 5')
        return

    DAC_VALUE = d.voltageToDACBits(voltage, dacNumber=0, is16Bits=False)
    if dac_num == 0:
        d.getFeedback(u3.DAC0_8(DAC_VALUE))
    else:
        d.getFeedback(u3.DAC1_8(DAC_VALUE))


# Switches the DAC value depending on the desired magnet direction
def SwitchMagnet(magnet_num, direction):
    val = direction * 5

    if magnet_num == 1:
        SetDac(d1, 0, val)
    elif magnet_num == 2:
        SetDac(d1, 1, val)
    elif magnet_num == 3:
        SetDac(d2, 0, val)
    elif magnet_num == 4:
        SetDac(d2, 1, val)


def SwitchAllMagnetsOn():
    SwitchMagnet(1, 0)
    SwitchMagnet(2, 0)
    SwitchMagnet(3, 0)
    SwitchMagnet(4, 0)


def SwitchAllMagnetsOff():
    SwitchMagnet(1, 1)
    SwitchMagnet(2, 1)
    SwitchMagnet(3, 1)
    SwitchMagnet(4, 1)


# Detects the output voltage of the hall sensor, which should be binary
# depending on whether the train is over the sensor
def GetIfOverHallSensor(hall_num):
    return d1.getAIN(hall_num - 1) < 1


# Simple test. Moves train from one end of the track to the other end.
# The sensor closest to the edge (with sensor value of 1) is not used
# in our example
def PropelTrain():
    SwitchAllMagnetsOn()

    while True:
        if GetIfOverHallSensor(2):
            SwitchMagnet(1, 0)
            SwitchMagnet(2, 1)
        if GetIfOverHallSensor(3):
            SwitchMagnet(2, 0)
            SwitchMagnet(3, 1)
        if GetIfOverHallSensor(4):
            SwitchMagnet(3, 0)
            SwitchMagnet(4, 1)
        if GetIfOverHallSensor(5):
            SwitchMagnet(4, 0)