import RPi.GPIO as GPIO
import serial
import time
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import keyboard

# Настройки GPIO
IN1 = 23
IN2 = 24
IN3 = 17
IN4 = 27
ENA = 26
ENB = 19

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(IN1, GPIO.OUT)
GPIO.setup(IN2, GPIO.OUT)
GPIO.setup(IN3, GPIO.OUT)
GPIO.setup(IN4, GPIO.OUT)
GPIO.setup(ENA, GPIO.OUT)
GPIO.setup(ENB, GPIO.OUT)

# Параметры для лидара
lidar_port = '/dev/ttyUSB1'  # Убедитесь, что это правильный порт
baud_rate = 115200

# Настройка серийного соединения
ser = serial.Serial(lidar_port, baud_rate, timeout=1)

# Параметры скорости
speed = 10  # Регулируйте скорость по необходимости
GPIO.output(ENA, speed)
GPIO.output(ENB, speed)

# Функции управления движением
def move_forward():
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    print("Робот движется вперед")

def move_backward():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    print("Робот движется назад")

def stop():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    print("Робот остановлен")

def read_lidar_data():
    try:
        line = ser.readline().decode('utf-8', errors='ignore').strip()
        if line:
            print(f"Считанные данные от лидара: {line}")  # Отладка
            return line  # Вернем считанные данные
        else:
            print("Нет данных")
            return None
    except Exception as e:
        print(f"Ошибка чтения данных от лидара: {e}")
        return None

# Визуализация карты препятствий
def update_plot(frame):
    data = read_lidar_data()  # Получаем данные от лидара
    if data is not None:
        # Логика для визуализации данных
        # Вставьте здесь ваш код для построения карты препятствий
        pass  # Замените на свою логику

# Основной цикл
def main():
    print("Нажмите 'W' для запуска автономного движения и ' ' (пробел) для остановки.")
    while True:
        if keyboard.is_pressed('w'):
            move_forward()
            print("Автономное движение включено")
        if keyboard.is_pressed('s'):
            move_backward()
        if keyboard.is_pressed(' '):
            stop()
            print("Автономное движение остановлено")
        # Добавьте паузу, чтобы избежать высокой загрузки CPU
        time.sleep(0.1)

# Запуск программы
if __name__ == "__main__":
    main()