from pyps2000b import PS2000B
import globals as g
import pandas
import time


def set_devices():  # creates device objects for all PSUs
    g.XY_DEVICE = PS2000B.PS2000B(g.XY_PORT)
    g.Z_DEVICE = PS2000B.PS2000B(g.Z_PORT)
    g.X_AXIS = (g.XY_DEVICE, 0, g.RELAY_PINS[0], 0)  # (device, channel, arduino pin, axis index)
    g.Y_AXIS = (g.XY_DEVICE, 1, g.RELAY_PINS[1], 1)
    g.Z_AXIS = (g.Z_DEVICE, 0, g.RELAY_PINS[2], 2)


def activate_all():  # enables remote control and output on all PSUs and channels
    g.XY_DEVICE.enable_all()
    g.Z_DEVICE.enable_all()


def deactivate_all():  # disables remote control and output on all PSUs and channels
    g.XY_DEVICE.disable_all()
    g.Z_DEVICE.disable_all()


def setup_arduino():
    for pin in g.RELAY_PINS:
        g.ARDUINO.pinMode(pin, "Output")
        g.ARDUINO.digitalWrite(pin, "LOW")


def safe_arduino():  # sets output pins to low and closes serial connection
    for pin in g.RELAY_PINS:
        g.ARDUINO.digitalWrite(pin, "LOW")


def print_status(axis):  # axis = axis control variable, stored in globals.py
    device = axis[0]  # PSU
    channel = axis[1]  # output channel on the PSU
    print("%s, %0.2f V, %0.2f A"
          % (device.get_device_status_information(channel), device.get_voltage(channel), device.get_current(channel)))


def print_status_3():
    print("X-Axis:")
    print_status(g.X_AXIS)
    print("Y-Axis:")
    print_status(g.Y_AXIS)
    print("Z-Axis:")
    print_status(g.Z_AXIS)


def set_to_zero(device):  # sets voltages and currents to 0
    device.voltage1 = 0
    device.current1 = 0
    device.voltage2 = 0
    device.current2 = 0


def power_down():  # temporary, set all outputs to 0 but keep connections enabled
    set_to_zero(g.XY_DEVICE)
    set_to_zero(g.Z_DEVICE)
    safe_arduino()


def shut_down():  # shutdown at program end, set outputs to 0 and disable connections
    set_to_zero(g.XY_DEVICE)
    set_to_zero(g.Z_DEVICE)
    deactivate_all()
    safe_arduino()
    g.ARDUINO.close()


def set_field_simple(vector):  # forms magnetic field as specified by vector, w/o cancelling ambient field
    i_vec = vector/g.COIL_CONST
    set_current_vec(i_vec)


def set_field(vector):  # forms magnetic field as specified by vector, corrected for ambient field
    field = vector - g.AMBIENT_FIELD
    i_vec = field/g.COIL_CONST
    set_current_vec(i_vec)


def set_current_vec(i_vec):  # sets needed currents on each axis for given vector
    set_axis_current(g.X_AXIS, i_vec[0])
    set_axis_current(g.Y_AXIS, i_vec[1])
    set_axis_current(g.Z_AXIS, i_vec[2])


def set_axis_current(axis, value):  # sets current with correct polarity on one axis
    device = axis[0]
    channel = axis[1]
    ardPin = axis[2]
    axisIndex = axis[3]
    if abs(value) > g.MAX_AMPS[axisIndex]:  # prevent excessive currents
        set_to_zero(device)  # set currents and voltages to 0
        device.disable_all()  # disable outputs on PSU
        safe_arduino()  # set arduino pins to low and close serial link
        raise ValueError("Invalid current value. Tried %0.2fA, max. %0.2fA allowed" % (value, g.MAX_AMPS[axisIndex]))
    elif value >= 0:  # switch polarity as needed
        g.ARDUINO.digitalWrite(ardPin, "LOW")
    elif value < 0:
        g.ARDUINO.digitalWrite(ardPin, "HIGH")
    else:
        raise Exception("This should be impossible.")
    device.set_current(abs(value), channel)
    maxVoltage = min(max(1.1 * g.MAX_AMPS[axisIndex] * g.RESISTANCES[axisIndex], 12), g.MAX_VOLTS)  # limit voltage
    device.set_voltage(maxVoltage)


def execute_csv(filepath, printing=0):  # runs through csv file containing times and desired field vectors
    # csv format: time (s); xField (T); yField (T); zField (T)
    # decimal commas
    print("Reading File:", filepath)
    file = pandas.read_csv(filepath, sep=';', decimal=',', header=0)  # read csv file
    array = file.to_numpy()  # convert csv to array
    t_zero = time.time()
    t_ref = t_zero
    i = 0
    print("Starting Execution...")
    while i < len(array):
        t = time.time() - t_zero
        if t >= array[i, 0]:
            field_vec = array[i, 1:4]
            print("t = %0.2f s, target field vector = " % (array[i, 0]), field_vec)
            set_field(field_vec)
            i = i + 1
        if t - t_ref >= 1 and printing == 1:  # print status every second
            print_status_3()
            t_ref = t
    print("File executed, powering down channels.")
    power_down()  # set currents and voltages to 0, set arduino pins to low