print("modbus test")
from func import *

# from func import set_user_reg_800
from pymodbus.client import ModbusTcpClient
import time
import glob

client = ModbusTcpClient(
    host=MODBUS_SERVER_HOST,
    port=MODBUS_SERVER_PORT,
)
client.connect()
# максимальное количество coils = 286

# bulb(2)
# get_coordinates()
# get_or_set_speed(10)

all_files = glob.glob("data/*.NC.result")
file = all_files[0]
data = []
with open(file, "r") as fp:
    lines = fp.readlines()
    for l in lines:
        r = l.strip().split(",")
        r[1] = int(float(r[1]) * 1000)
        r[2] = int(float(r[2]) * 1000)
        r[3] = int(float(r[3]) * 1000)

        data.append(r)


# print(data)
def multiply_1000(n):
    return int(n * 1000)


GOTO_Y = 5
try:
    total = 0
    state = None
    step = 0
    counter = 0

    paths = data
    paths = [
        ("line", 0, 0, 277.8, 0, 5, 0),
        # ("line", 30, 30, 0, 0, 0, 0),
        # ("line", 30, -30, 0, 0, 0, 0),
        # ("line", 0, 0, 0, 0, 0, 5),
        # ("line", -30, -30, 0, 0, 0, 0),
        # ("line", -30, 30, 0, 0, 0, 0),
        # ("line", 0, 0, 0, 0, -5, -5),
        # ("line", 0, 0, 30, 0, 0, 0),
    ]

    bulb(GOTO_Y, False, client)
    get_or_set_speed(10, client)
    
    # ставим пользовательские переменные в ноль
    set_user_reg_800(
        [
            (0, to_double(0)),
            (1, to_double(0)),
            (2, to_double(0)),
            (3, [0, 0]),
            (4, [0, 0]),
            (5, [0, 0]),
        ],
        client,
    )
    # _, _, _, u_target, v_target, w_target = collect_coordinates(client)

    # старт в авторежиме single loop
    start_in_auto(client)

    while True:
        time.sleep(0.01)
        total += 1
        green_light = client.read_coils(GOTO_Y, 1, MODBUS_SLAVE_ID).bits[0]

        if state == green_light:
            continue

        state = green_light

        if state == True:
            continue

        x, y, z, u, v, w = collect_coordinates(client)

        if step >= len(paths):
            # counter += 1
            # step = 0
            stop_immediately(client)
            exit()

        current_step = paths[step]
        line_type, *coord = current_step
        print(f"{line_type} {step} of {len(paths)} {current_step}")

        if line_type == "line":
            set_x, set_y, set_z, set_u, set_v, set_w = coord

            set_user_reg_800(
                [
                    (0, to_double(multiply_1000(set_x))),
                    (1, to_double(multiply_1000(set_y))),
                    (2, to_double(multiply_1000(set_z))),
                    (3, to_double(multiply_1000(set_u))),
                    (4, to_double(multiply_1000(set_v))),
                    (5, to_double(multiply_1000(set_w))),
                ],
                client,
            )
            # bulb(4, True, client)

        if not x == y == z == u == v == w == 0:
            bulb(GOTO_Y, True, client)
        step += 1


except Exception as e:
    print("error", e)

client.close()