import serial
import re
import time

SERIAL_PORT = "/dev/ttyACM0"
BAUD_RATE = 115200
start_calib = 55
stop_calib = 530

def open_serial():
    try:
        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
        time.sleep(1)
        return ser
    except serial.SerialException as e:
        print(f"Ошибка открытия порта: {e}")
        return None

def send_command(ser, command):
    if ser is None:
        print("Ошибка: порт не открыт")
        return []
    
    ser.reset_input_buffer()
    ser.write((command + "\n").encode())
    time.sleep(0.01)

    response = []
    start_time = time.time()
    while time.time() - start_time < 1: 
        if ser.in_waiting > 0:
            line = ser.readline().decode().strip()
            if line:
                response.append(line)
    
    return response

def set_servo_angle(ser, num_servo, angle):
    response = send_command(ser, f"0xAB{num_servo}gg{angle}0xFF")
    if response:
        pass
    else:
        print("Нет ответа от платы на установку угла")

def read_analog_values(ser, num_servo):
    response = send_command(ser, f"0xAB{num_servo}rr0xFF")
    if response:
       pass
    else:
        print("Not data")
    return response


def chew_angle(num_servo, angle):
    ser = open_serial()
    if ser:
        set_servo_angle(ser,str(num_servo), str(angle))
        time.sleep(0.1)
        
        data = read_analog_values(ser, num_servo)
        if isinstance(data, list):
            data = " ".join(data)
        

        numbers = list(map(int, re.findall(r"\d+", data)))
        print("Извлечённые числа:", numbers)
        
        if numbers:
            avg = sum(numbers) / len(numbers)
            # При 0° -> 55, при 180° -> 585, т.е. масштаб: (585-55)/180 = 530/180
            computed_angle = (avg - start_calib) * (180 / stop_calib)
            if abs(angle - computed_angle) < 1:
                pass
            else:
                print("Угол не совпадает")
                if abs(angle - computed_angle) > 1:
                    ang = computed_angle - 30
                    if ang < 20:
                        set_servo_angle(ser, str(num_servo), str(0))
                    else:
                        set_servo_angle(ser, str(num_servo), str(ang))
                        chew_angle(num_servo, angle)
        else:
            print("Не удалось извлечь числа из данных")
        ser.close()

num_servo = 1
angle = 20
time.sleep(3)
chew_angle(num_servo, angle)
angle = 160
time.sleep(3)
chew_angle(num_servo, angle)