import serial
import csv
import time
import struct

# Константы
SERIAL_PORT = '/dev/ttyS0'  # UART порт на MangoPi
BAUD_RATE = 230400          # скорость передачи LDS02RR
DURATION = 5                # секунд записи

# Заголовок пакета LDS02RR
HEADER = 0xFA
PACKET_SIZE = 22            # размер пакета в байтах

def parse_packet(data):
    """Парсить один пакет данных лидара"""
    if len(data) < PACKET_SIZE:
        return None
    
    if data[0] != HEADER:
        return None

    # Индекс угла (0xA0 = 0°, 0xF9 = 359°)
    index = data[1] - 0xA0
    if index < 0 or index > 89:
        return None

    # Скорость вращения
    speed = struct.unpack_from('<H', data, 2)[0] / 64.0

    points = []
    for i in range(4):
        offset = 4 + i * 4
        distance_raw = struct.unpack_from('<H', data, offset)[0]
        quality = struct.unpack_from('<H', data, offset + 2)[0] >> 8

        distance = distance_raw / 4.0  # в мм
        angle = index * 4 + i          # угол в градусах

        points.append((angle, distance, quality))

    return points

def save_to_csv(filename, duration_seconds):
    """Читать данные лидара и записывать в CSV"""
    try:
        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
        print(f"Подключено к {SERIAL_PORT}")
    except Exception as e:
        print(f"Ошибка подключения: {e}")
        return

    with open(filename, 'w', newline='') as f:
        writer = csv.writer(f)
        writer.writerow(['timestamp', 'angle', 'distance_mm', 'quality'])

        start_time = time.time()
        buffer = bytearray()

        while time.time() - start_time < duration_seconds:
            data = ser.read(PACKET_SIZE)
            if not data:
                continue

            buffer.extend(data)

            # Ищем заголовок пакета
            while len(buffer) >= PACKET_SIZE:
                if buffer[0] == HEADER:
                    packet = bytes(buffer[:PACKET_SIZE])
                    points = parse_packet(packet)

                    if points:
                        t = round(time.time() - start_time, 3)
                        for angle, distance, quality in points:
                            writer.writerow([t, angle, 
                                           round(distance, 1), 
                                           quality])
                    buffer = buffer[PACKET_SIZE:]
                else:
                    buffer.pop(0)

    ser.close()
    print("Готово!")

# Запуск
save_to_csv('lidar.csv', DURATION)