---

import math
import re

class CNCPlannerX:
    def __init__(self, accel=150.0, min_speed=2.0, junction_deviation=0.02):
        self.accel = accel                  # Системное ускорение (мм/сек^2)
        self.min_speed = min_speed          # Стартовая скорость осей (мм/сек)
        self.junction_deviation = junction_deviation  # Константа жесткости станины (мм)
        
    def parse_and_plan(self, gcode_lines):
        # 1. Сборка модальных координат и линеаризация очереди
        segments = []
        curr_x, curr_y, curr_z = 0.0, 0.0, 0.0
        curr_f = 600.0 # Подача по умолчанию (мм/мин)
        
        for idx, line in enumerate(gcode_lines):
            line = line.strip().split(';')[0].upper() # Убираем комментарии G-кода
            if not line:
                continue
                
            # Извлекаем параметры регулярными выражениями
            g_match = re.search(r'G([0-1])', line)
            m_match = re.search(r'M([0-9]+)', line)
            
            x_match = re.search(r'X([-+]?[0-9]*\.?[0-9]+)', line)
            y_match = re.search(r'Y([-+]?[0-9]*\.?[0-9]+)', line)
            z_match = re.search(r'Z([-+]?[0-9]*\.?[0-9]+)', line)
            f_match = re.search(r'F([0-9]*\.?[0-9]+)', line)
            
            # Подстановка модальных значений
            cmd_type = 1 if g_match else (3 if m_match else 1)
            line_num = idx + 1
            
            if f_match: curr_f = float(f_match.group(1))
            target_x = float(x_match.group(1)) if x_match else curr_x
            target_y = float(y_match.group(1)) if y_match else curr_y
            target_z = float(z_match.group(1)) if z_match else curr_z
            
            # Вычисляем дельты перемещения для текущего кадра
            dx = target_x - curr_x
            dy = target_y - curr_y
            dz = target_z - curr_z
            length = math.sqrt(dx*dx + dy*dy + dz*dz)
            
            segments.append({
                'type': cmd_type, 'line': line_num,
                'x': target_x, 'y': target_y, 'z': target_z,
                'f_min': curr_f, 'f_sec': curr_f / 60.0, # переводим подачу в мм/сек
                'dx': dx, 'dy': dy, 'dz': dz, 'length': length,
                'v_max': curr_f / 60.0, # Ограничение скорости сверху подачей кадра
                'v_start': self.min_speed, # Будет пересчитано Look-Ahead ниже
                'v_end': self.min_speed     # Будет пересчитано Look-Ahead ниже
            })
            
            # Сдвигаем текущую точку
            curr_x, curr_y, curr_z = target_x, target_y, target_z

        # ====================================================================
        # 2. АЛГОРИТМ LOOK-AHEAD: РАСЧЕТ СКОРОСТЕЙ НА СТЫКАХ УГЛОВ (Junction Deviation)
        # ====================================================================
        num_segs = len(segments)
        for i in range(num_segs - 1):
            seg_curr = segments[i]
            seg_next = segments[i+1]
            
            # Если один из смежных отрезков пустой, скорость стыка равна минимальной
            if seg_curr['length'] < 0.001 or seg_next['length'] < 0.001:
                v_junction = self.min_speed
            else:
                # Находим косинус угла излома траектории между векторами
                cos_theta = (seg_curr['dx']*seg_next['dx'] + 
                             seg_curr['dy']*seg_next['dy'] + 
                             seg_curr['dz']*seg_next['dz']) / (seg_curr['length'] * seg_next['length'])
                
                # Защита от вычислительной погрешности float в Python
                cos_theta = max(-1.0, min(1.0, cos_theta))
                
                if cos_theta > 0.99999:
                    # Линия абсолютно прямая — тормозить на стыке векторов не нужно
                    v_junction = min(seg_curr['v_max'], seg_next['v_max'])
                elif cos_theta < -0.99999:
                    # Полный реверс (разворот на 180 градусов) — гасим скорость в полный ноль
                    v_junction = self.min_speed
                else:
                    # Формула Junction Deviation (вписывание виртуального радиуса поворота R)
                    sin_theta_d2 = math.sqrt((1.0 - cos_theta) / 2.0)
                    cos_theta_d2 = math.sqrt((1.0 + cos_theta) / 2.0)
                    
                    R = self.junction_deviation * (cos_theta_d2 / (1.0 - cos_theta_d2))
                    
                    # Безопасная скорость на угле по центростремительному лимиту
                    v_junction = math.sqrt(self.accel * R)
                    # Ограничиваем скорость стыка сверху рабочими подачами смежных кадров
                    v_junction = min(v_junction, seg_curr['v_max'], seg_next['v_max'])
                    v_junction = max(v_junction, self.min_speed)
            
            # Присваиваем скорости сопряжения стыка двум кадрам на границе
            seg_curr['v_end'] = v_junction
            seg_next['v_start'] = v_junction

        # Самый первый и самый последний кадры программы принудительно зануляем/тормозим в min_speed
        if num_segs > 0:
            segments[0]['v_start'] = self.min_speed
            segments[-1]['v_end'] = self.min_speed

        # ====================================================================
        # 3. ФОРМИРОВАНИЕ ПАКЕТОВ B: ДЛЯ ОТПРАВКИ ПО WI-FI (СТРОГО 7 РАЗДЕЛИТЕЛЕЙ)
        # ====================================================================
        packet_stream = []
        for seg in segments:
            # Формат пакета: B:TYPE;LINE_NUM;X;Y;Z;F_MAX;V_START;V_END
            # Все координаты и скорости округляем до 3-х знаков после запятой
            packet = (f"B:{seg['type']};{seg['line']};"
                      f"{seg['x']:.3f};{seg['y']:.3f};{seg['z']:.3f};"
                      f"{seg['f_min']:.3f};{seg['v_start']:.3f};{seg['v_end']:.3f}\n")
            packet_stream.append(packet)
            
        return packet_stream