import os
import sys
import time

try:
    import libximc.highlevel as ximc
except ImportError:
    cur_dir = os.path.abspath(os.path.dirname(__file__))
    ximc_dir = os.path.join(cur_dir, "..", "..", "..", "ximc")
    ximc_package_dir = os.path.join(ximc_dir, "crossplatform", "wrappers", "python")
    sys.path.append(ximc_package_dir)
    import libximc.highlevel as ximc


def test_status(axis: ximc.Axis) -> None:
    print("\nStatus:")
    status = axis.get_status()
    print(" rpm: {},".format(status.CurSpeed), end="")
    print(" pos: {},".format(status.CurPosition), end="")
    print(" upwr: {},".format(status.Upwr), end="")
    print(" ipwr: {},".format(status.Ipwr), end="")
    print(" flags: {},".format(status.Flags), end="")
    print(" mvsts: {}".format(status.MvCmdSts))


def test_get_position(axis: ximc.Axis) -> 'tuple':
    print("\nRead position")
    pos = axis.get_position()
    print("Position: {} steps, {} microsteps".format(pos.Position, pos.uPosition))
    return pos.Position, pos.uPosition


def test_left(axis: ximc.Axis) -> None:
    print("\nMoving left for 3 seconds...")
    axis.command_left()
    time.sleep(3)


def test_move(axis: ximc.Axis, distance: int, udistance: int) -> None:
    print("\nGoing to {} steps, {} microsteps".format(distance, udistance))
    axis.command_move(distance, udistance)


def test_wait_for_stop(axis: ximc.Axis, interval: int) -> None:
    print("\nWaiting for stop...")
    axis.command_wait_for_stop(interval)


def test_get_speed(axis: ximc.Axis) -> int:
    print("\nGet speed")
    move_settings = axis.get_move_settings()
    print("Speed: {}".format(move_settings.Speed))
    return move_settings.Speed


def test_set_speed(axis: ximc.Axis, speed: int) -> None:
    print("\nSet new speed {}".format(speed))
    move_settings = axis.get_move_settings()
    move_settings.Speed = speed
    axis.set_move_settings(move_settings)


def test_set_microstep_mode_256(axis: ximc.Axis) -> None:
    print("\nSet microstep mode to 256")
    engine_settings = axis.get_engine_settings()

    # Change MicrostepMode parameter to MICROSTEP_MODE_FRAC_256
    # (use MICROSTEP_MODE_FRAC_128, MICROSTEP_MODE_FRAC_64 ... for other microstep modes)
    engine_settings.MicrostepMode = ximc.MicrostepMode.MICROSTEP_MODE_FRAC_256

    axis.set_engine_settings(engine_settings)


def choose_device() -> str:
    devenum = search_devices()
    if len(devenum) < 1:
        print("The real controller is not found or busy with another app.\nThe virtual controller is opening to check "
              "the operation of the library...")
        device_name = create_virtual_device_name()
    else:
        device_name = get_device_name_from_list(devenum)
    return device_name


def create_virtual_device_name() -> str:
    path = os.path.join(os.path.expanduser("~"), "virt_controller.bin")
    device_name = "xi-emu:///" + path
    print("Virtual controller file path: '{}'".format(path))
    return device_name


def get_device_name_from_list(devenum) -> str:
    while True:
        print_device_list(devenum)
        print("When using an Ethernet (TCP protocol) connection, multi-user interaction with the device is possible. "
              "In this case, the same device may be controlled simultaneously from multiple sources, which can "
              "potentially lead to control conflicts.\nEnter device number from the list:")
        try:
            number = int(input())
        except Exception:
            continue

        if 1 <= number <= len(devenum):
            device_name = devenum[number - 1]["uri"]
            print("Selected device: '{}'".format(device_name))
            return device_name

        if number == len(devenum) + 1:
            return create_virtual_device_name()


def print_device_list(devenum) -> None:
    print("\nFound devices list:")
    for i, device in enumerate(devenum, start=1):
        print(" #{}. {}".format(i, device["uri"]))
    print(" #{}. virtual".format(len(devenum) + 1))


def search_devices():
    print("\nSearching for devices...")
    probe_flags = ximc.EnumerateFlags.ENUMERATE_PROBE | ximc.EnumerateFlags.ENUMERATE_NETWORK
    enumerate_hints = "addr="
    devenum = ximc.enumerate_devices(probe_flags, enumerate_hints)
    if not devenum:
        print("No devices found")
    else:
        print("Found {} devices".format(len(devenum) + 1))
    return devenum


def warn_user_about_modifications() -> bool:
    while True:
        print("\nThe script will modify the following controller settings:\n"
              " engine settings\n"
              " move settings\nContinue? [y/n]:")
        answer = input()
        if answer.lower() == "y":
            return False

        if answer.lower() == "n":
            return True


def main() -> None:
    print("This is a ximc test program\nlibximc version " + ximc.ximc_version())

    if warn_user_about_modifications():
        return

    device_name = choose_device()
    print("\nOpening device...", end="")
    try:
        axis = ximc.Axis(device_name)
        axis.open_device()
    except Exception:
        print("\nError opening device\nPress Enter to exit...")
        return
    print(" Done")


    # ******************************************* #
    #                    Tests                    #
    # ******************************************* #
    test_status(axis)
    test_set_microstep_mode_256(axis)
    start_pos, ustart_pos = test_get_position(axis)
    # First move
    test_left(axis)
    test_get_position(axis)
    # Second move
    current_speed = test_get_speed(axis)
    test_set_speed(axis, current_speed // 2)
    test_move(axis, start_pos, ustart_pos)
    test_wait_for_stop(axis, 100)
    test_status(axis)

    print("\nClosing...", end=" ")
    axis.close_device()
    print("Done")
    input("Press Enter to close the script...")


if __name__ == "__main__":
    main()
