# OUTPUT_MODE=LIVE

import serial
import struct
import time
import signal
import glob

BAUD = 115200
MSP_SET_MOTOR = 214

running = True


def find_port():
    ports = glob.glob("/dev/ttyACM*")

    if not ports:
        raise RuntimeError("Полетник не найден")

    return ports[0]


def send_motors(s, value):
    values = [
        value,
        value,
        value,
        value,
        1000,
        1000,
        1000,
        1000
    ]

    payload = struct.pack("<8H", *values)

    size = len(payload)
    checksum = size ^ MSP_SET_MOTOR

    for b in payload:
        checksum ^= b

    packet = (
        b"$M<"
        + bytes([size, MSP_SET_MOTOR])
        + payload
        + bytes([checksum])
    )

    s.write(packet)
    s.flush()


def stop_handler(signum, frame):
    global running
    running = False


signal.signal(signal.SIGTERM, stop_handler)
signal.signal(signal.SIGINT, stop_handler)


PORT = find_port()

print("Полетник: " + PORT, flush=True)

s = serial.Serial(
    PORT,
    BAUD,
    timeout=0.1
)

time.sleep(0.5)

try:

    while running:

        send_motors(s, 1050)

        print(
            "Моторы: ПУСК | 1050",
            flush=True
        )

        time.sleep(0.1)

finally:

    # При нажатии СТОП веб-интерфейс посылает SIGTERM.
    # Здесь гарантированно отправляем 1000.
    for i in range(10):

        try:
            send_motors(s, 1000)
            time.sleep(0.05)

        except:
            pass

    s.close()

    print(
        "Моторы: СТОП | 1000",
        flush=True
    )