print("modbus test")

# Настройки клиента Modbus
MODBUS_SERVER_HOST = "192.168.70.55"  # IP-адрес Modbus-сервера
MODBUS_SERVER_PORT = 502
MODBUS_SLAVE_ID = 11

indent = 21100
s = 800
k = 2

from pymodbus.client import ModbusTcpClient
from pymodbus.constants import Endian
from pymodbus.payload import BinaryPayloadDecoder
import time
import struct


def two_byte_convert(result):
    # print(result)
    v0 = result.registers[0]
    value = (v0 << 16) + result.registers[1]
    if result.registers[0] > (65535 * 0.5):
        value = value - (1 << 32)
    return value


def bulb(addr):
    try:
        coils = client.read_coils(addr, 1, MODBUS_SLAVE_ID)
        print("читаем начальное состояние", coils.bits)

        new_bits = coils.bits
        new_bits[0] = not new_bits[0]
        print("пытаемся записать", new_bits, coils.slave_id)
        result = client.write_coil(addr, new_bits[0], coils.slave_id)
        print(result)

        print(
            "читаем после попытки",
            client.read_coils(0, len(coils.bits), coils.slave_id).bits,
        )
    except Exception as e:
        raise e


def get_coordinates():
    try:
        data = [
            ("j1", 2268),
            ("j2", 2270),
            ("j3", 2272),
            ("j4", 2274),
            ("j5", 2276),
            ("j6", 2278),
            ("x", 2332),
            ("y", 2334),
            ("z", 2336),
            ("u", 2338),
            ("v", 2340),
            ("w", 2342),
        ]
        i = 2
        for d in data:
            result = client.read_holding_registers(d[1], 2, MODBUS_SLAVE_ID)
            value = two_byte_convert(result)
            adjusted_value = value / 1000.0
            i *= 2
            print(d[0], result.registers, adjusted_value)
    except Exception as e:
        raise e


def get_or_set_speed(number=None):
    result = client.read_holding_registers(20200, 1, MODBUS_SLAVE_ID)
    print("сейчас скорость", result.registers[0] * 0.1)
    if number and number > 0 and number <= 1000:
        client.write_register(20200, number * 10, MODBUS_SLAVE_ID)
        result = client.read_holding_registers(20200, 1, MODBUS_SLAVE_ID)
        print("установлена скорость", result.registers[0] * 0.1)


def set_user_register(n, values):
    client.write_registers(indent + s + k * n, values, MODBUS_SLAVE_ID)


def to_double(integr):
    return [(integr >> 16) & 0xFFFF, integr & 0xFFFF]


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

# bulb(0)
# get_coordinates()
# get_or_set_speed(10)
try:
    data = [
            (0, to_double(0)),
            (1, to_double(0)),
            (2, to_double(0)),
            (3, [0, 0]),
            (4, [0, 0]),
            (5, [0, 0]),
        ]
    for item in data:
        d, values = item
        set_user_register(d, values)
    # старт в авторежиме single loop
    client.write_register(20002, 1, MODBUS_SLAVE_ID)

    total = 0
    state = None
    step = 0
    counter = 0
    while True:
        time.sleep(1)
        total += 1
        green_light = client.read_coils(0, 1, MODBUS_SLAVE_ID).bits[0]
        print(f"total {total}, state {state}, green light {green_light}")
        
        if state == green_light:
            continue

        state = green_light
        
        if state == True:
            continue

        step += 1
        # собираем предварительные данные
        # x = two_byte_convert(client.read_holding_registers(2332, 2, MODBUS_SLAVE_ID))
        # y = two_byte_convert(client.read_holding_registers(2334, 2, MODBUS_SLAVE_ID))
        # z = two_byte_convert(client.read_holding_registers(2336, 2, MODBUS_SLAVE_ID))
        # u = two_byte_convert(client.read_holding_registers(2338, 2, MODBUS_SLAVE_ID))
        # v = two_byte_convert(client.read_holding_registers(2340, 2, MODBUS_SLAVE_ID))
        # w = two_byte_convert(client.read_holding_registers(2342, 2, MODBUS_SLAVE_ID))
        # print(
        #     f"Предварительные данные X {x * 0.001}, Y {y * 0.001}, Z {z*0.001}, U {u*0.001}, V {v*0.001}, W {w*0.001}"
        # )
        
        # работаем с REL относительными координатами
        x = 0
        y = 0
        z = 0
        
        w = 100
        h = 100
        if step == 1:
            x -= w * 1000
        elif step == 2:
            y -= h * 1000
        elif step == 3:
            x += w * 1000
        elif step == 4:
            y += h * 1000
        elif step == 5:
            z += h * 1000
        elif step == 6:
            z -= h * 1000
        else:
            counter += 1
            step = 0
            state = None

        data = [
            (0, to_double(x)),
            (1, to_double(y)),
            (2, to_double(z)),
            (3, [0, 0]),
            (4, [0, 0]),
            (5, [0, 0]),
        ]
        for item in data:
            d, values = item
            set_user_register(d, values)
            result = client.read_holding_registers(
                indent + s + k * d, k, MODBUS_SLAVE_ID
            )
            print(s + d, "===", two_byte_convert(result) * 0.001)

        bulb(0)


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

client.close()