# OUTPUT_MODE=LIVE

from pymavlink import mavutil
import time
import math

PORT = "/dev/ttyACM0"

print("Подключение к Cube...", flush=True)

m = mavutil.mavlink_connection(PORT)
m.wait_heartbeat()

print("Cube подключён. Читаю датчики...", flush=True)

gps = None
pressure = None
attitude = None
hud = None
imu = None

while True:

    start = time.time()

    # Собираем сообщения от Cube
    while time.time() - start < 0.2:

        msg = m.recv_match(blocking=True, timeout=0.05)

        if msg is None:
            continue

        msg_type = msg.get_type()

        if msg_type == "GPS2_RAW":
            gps = msg

        elif msg_type == "SCALED_PRESSURE":
            pressure = msg

        elif msg_type == "ATTITUDE":
            attitude = msg

        elif msg_type == "VFR_HUD":
            hud = msg

        elif msg_type == "RAW_IMU":
            imu = msg

    parts = []

    # GPS
    if gps is not None:

        lat = gps.lat / 10000000.0
        lon = gps.lon / 10000000.0
        alt = gps.alt / 1000.0

        parts.append(
            "GPS: %.7f, %.7f | Высота: %.2f м | Спутники: %d | HDOP: %.2f | Hacc: %.2f м"
            % (
                lat,
                lon,
                alt,
                gps.satellites_visible,
                gps.eph / 100.0,
                gps.h_acc / 1000.0
            )
        )

    else:
        parts.append("GPS: нет данных")

    # Ориентация Cube
    if attitude is not None:

        roll = math.degrees(attitude.roll)
        pitch = math.degrees(attitude.pitch)
        yaw = math.degrees(attitude.yaw)

        parts.append(
            "Roll: %.2f° | Pitch: %.2f° | Yaw: %.2f°"
            % (
                roll,
                pitch,
                yaw
            )
        )

    # Курс и высота
    if hud is not None:

        parts.append(
            "Курс: %d° | Высота Cube: %.2f м"
            % (
                hud.heading,
                hud.alt
            )
        )

    # Барометр
    if pressure is not None:

        parts.append(
            "Барометр: %.2f hPa | T: %.2f°C"
            % (
                pressure.press_abs,
                pressure.temperature / 100.0
            )
        )

    # Магнитометр
    if imu is not None:

        parts.append(
            "MAG: X=%d Y=%d Z=%d"
            % (
                imu.xmag,
                imu.ymag,
                imu.zmag
            )
        )

    # ОДНА строка = одно полное обновление поля "Результат"
    print(" || ".join(parts), flush=True)

    time.sleep(0.2)