0xFF

import numpy as np
import matplotlib.pyplot as plt

def s_curve_motion_profile(distance, max_jerk, max_acceleration, max_velocity):
    """
    Рассчитывает профили движения с S-образной кривой скорости
    
    Профиль создает движение с ограниченным рывком (jerk), ускорением и скоростью,
    обеспечивая плавное ускорение и торможение при движении к цели.
    
    Args:
        distance: целевое расстояние перемещения
        max_jerk: максимальный рывок (производная ускорения)
        max_acceleration: максимальное ускорение
        max_velocity: максимальная скорость
    
    Returns:
        t_values: массив временных точек
        s_values: массив значений пройденного пути
        v_values: массив значений скорости
        a_values: массив значений уск
    # Минимальный интервал времени для достижения максимального ускорения
    t_j = max_acceleration / max_jerk
    
    # Минимальный интервал времени для достижения максимальной скорости при максимальном ускорении
    t_a = max_velocity / max_acceleration
    
    # Проверяем, достигается ли максимальная скорость
    if t_a <= 2 * t_j:
        # Максимальная скорость не достигается
        # Корректируем максимальное ускорение
        t_a = 2 * t_j
        max_acceleration = max_velocity / t_a
    
    # Вычисляем скорость в конце фазы разгона
    v_peak = max_jerk * (t_j**2) + max_acceleration * (t_a - t_j)
    
    # Расстояние, пройденное во время фазы разгона
    s_accel = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t_a - 2 * t_j)**2 / 2
    
    # Проверяем, достигается ли максимальная скорость
    has_cruise_phase = True
    t_v = 0  # Время фазы постоянной скорости

    # Рассчитываем полный профиль движения
    if 2 * s_accel > distance:
        # Цель слишком близка, нужно корректировать профиль
        has_cruise_phase = False
        
        # Для случая без фазы постоянной скорости рассчитываем время и скорость
        # Используем более надежный алгоритм
        v_max_reachable = np.sqrt(max_acceleration * distance / 2)
        if v_max_reachable < max_velocity:
            max_velocity = v_max_reachable
            
        t_a = max_velocity / max_acceleration
        if t_a <= 2 * t_j:
            t_a = 2 * t_j
            max_acceleration = max_velocity / t_a
            
        # Пересчитываем пройденное расстояние для проверки
        s_accel = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t_a - 2 * t_j)**2 / 2
    else:
        # Есть фаза постоянной скорости
        t_v = (distance - 2 * s_accel) / max_velocity
    
    # Общее время движения
    total_time = 2 * t_a + t_v
    
    # Создаем временную сетку с высоким разрешением
    dt = 0.01
    t_values = np.arange(0, total_time + dt, dt)
    s_values = np.zeros_like(t_values)
    v_values = np.zeros_like(t_values)
    a_values = np.zeros_like(t_values)
    j_values = np.zeros_like(t_values)
    
    # Фаза 1: Увеличение ускорения (положительный рывок)
    phase1_mask = t_values <= t_j
    j_values[phase1_mask] = max_jerk
    a_values[phase1_mask] = max_jerk * t_values[phase1_mask]
    v_values[phase1_mask] = (max_jerk * t_values[phase1_mask]**2) / 2
    s_values[phase1_mask] = (max_jerk * t_values[phase1_mask]**3) / 6
    
    # Фаза 2: Постоянное ускорение (нулевой рывок)
    phase2_mask = (t_values > t_j) & (t_values <= t_a - t_j)
    j_values[phase2_mask] = 0
    a_values[phase2_mask] = max_acceleration
    
    phase2_indices = np.where(phase2_mask)[0]
    if len(phase2_indices) > 0:
        for i in phase2_indices:
            t = t_values[i]
            v_values[i] = (max_jerk * t_j**2) / 2 + max_acceleration * (t - t_j)
            s_values[i] = (max_jerk * t_j**3) / 6 + (max_acceleration * t_j**2) / 2 + max_acceleration * (t - t_j)**2 / 2 - (max_acceleration * t_j**2) / 2 + max_acceleration * t_j * (t - t_j)
    
    # Фаза 3: Уменьшение ускорения (отрицательный рывок)
    phase3_mask = (t_values > t_a - t_j) & (t_values <= t_a)
    j_values[phase3_mask] = -max_jerk
    
    phase2_indices = np.where(phase2_mask)[0]
    phase3_indices = np.where(phase3_mask)[0]
    
    if len(phase3_indices) > 0:
        for i in phase3_indices:
            t = t_values[i]
            delta_t = t - (t_a - t_j)
            a_values[i] = max_acceleration - max_jerk * delta_t
            
            # Если есть предыдущая фаза, используем ее значения, иначе вычисляем
            if len(phase2_indices) > 0:
                last_phase2_idx = phase2_indices[-1]
                v_values[i] = v_values[last_phase2_idx] + max_acceleration * delta_t - (max_jerk * delta_t**2) / 2
                s_values[i] = s_values[last_phase2_idx] + v_values[last_phase2_idx] * delta_t + (max_acceleration * delta_t**2) / 2 - (max_jerk * delta_t**3) / 6
            else:
                # Вычисляем с нуля, если нет предыдущей фазы
                v_1 = (max_jerk * t_j**2) / 2  # скорость в конце фазы 1
                v_values[i] = v_1 + max_acceleration * delta_t - (max_jerk * delta_t**2) / 2
                
                s_1 = (max_jerk * t_j**3) / 6  # позиция в конце фазы 1
                s_values[i] = s_1 + v_1 * delta_t + (max_acceleration * delta_t**2) / 2 - (max_jerk * delta_t**3) / 6
    
    phase3_indices = np.where(phase3_mask)[0]
    last_phase3_idx = phase3_indices[-1] if len(phase3_indices) > 0 else 0
    
    if has_cruise_phase:
        # Фаза 4: Постоянная скорость (движение с макс. скоростью)
        phase4_mask = (t_values > t_a) & (t_values <= t_a + t_v)
        j_values[phase4_mask] = 0
        a_values[phase4_mask] = 0
        v_values[phase4_mask] = max_velocity
        
        phase4_indices = np.where(phase4_mask)[0]
        if len(phase4_indices) > 0:
            for i in phase4_indices:
                t = t_values[i]
                delta_t = t - t_a
                s_values[i] = s_values[last_phase3_idx] + max_velocity * delta_t
            
            last_phase4_idx = phase4_indices[-1]
        else:
            last_phase4_idx = last_phase3_idx
        
        # Фаза 5: Начало торможения (отрицательный рывок)
        phase5_mask = (t_values > t_a + t_v) & (t_values <= t_a + t_v + t_j)
        j_values[phase5_mask] = -max_jerk
        
        phase5_indices = np.where(phase5_mask)[0]
        if len(phase5_indices) > 0:
            for i in phase5_indices:
                t = t_values[i]
                delta_t = t - (t_a + t_v)
                a_values[i] = -max_jerk * delta_t
                v_values[i] = max_velocity - (max_jerk * delta_t**2) / 2
                s_values[i] = s_values[last_phase4_idx] + max_velocity * delta_t - (max_jerk * delta_t**3) / 6
            
            last_phase5_idx = phase5_indices[-1]
        else:
            last_phase5_idx = last_phase4_idx
        
        # Фаза 6: Постоянное отрицательное ускорение
        phase6_mask = (t_values > t_a + t_v + t_j) & (t_values <= t_a + t_v + t_a - t_j)
        j_values[phase6_mask] = 0
        a_values[phase6_mask] = -max_acceleration
        
        phase6_indices = np.where(phase6_mask)[0]
        if len(phase6_indices) > 0:
            for i in phase6_indices:
                t = t_values[i]
                delta_t = t - (t_a + t_v + t_j)
                v_values[i] = v_values[last_phase5_idx] - max_acceleration * delta_t
                s_values[i] = s_values[last_phase5_idx] + v_values[last_phase5_idx] * delta_t - (max_acceleration * delta_t**2) / 2
            
            last_phase6_idx = phase6_indices[-1]
        else:
            last_phase6_idx = last_phase5_idx
        
        # Фаза 7: Уменьшение торможения до полной остановки
        phase7_mask = (t_values > t_a + t_v + t_a - t_j)
        j_values[phase7_mask] = max_jerk
        
        phase7_indices = np.where(phase7_mask)[0]
        if len(phase7_indices) > 0:
            for i in phase7_indices:
                t = t_values[i]
                delta_t = t - (t_a + t_v + t_a - t_j)
                a_values[i] = -max_acceleration + max_jerk * delta_t
                v_values[i] = v_values[last_phase6_idx] - max_acceleration * delta_t + (max_jerk * delta_t**2) / 2
                s_values[i] = s_values[last_phase6_idx] + v_values[last_phase6_idx] * delta_t - (max_acceleration * delta_t**2) / 2 + (max_jerk * delta_t**3) / 6
    else:
        # Без фазы постоянной скорости - сразу к торможению
        # Фаза 4: Начало торможения (отрицательный рывок)
        phase4_mask = (t_values > t_a) & (t_values <= t_a + t_j)
        j_values[phase4_mask] = -max_jerk
        
        phase4_indices = np.where(phase4_mask)[0]
        if len(phase4_indices) > 0:
            for i in phase4_indices:
                t = t_values[i]
                delta_t = t - t_a
                a_values[i] = -max_jerk * delta_t
                v_values[i] = v_values[last_phase3_idx] - (max_jerk * delta_t**2) / 2
                s_values[i] = s_values[last_phase3_idx] + v_values[last_phase3_idx] * delta_t - (max_jerk * delta_t**3) / 6
            
            last_phase4_idx = phase4_indices[-1]
        else:
            last_phase4_idx = last_phase3_idx
        
        # Фаза 5: Постоянное отрицательное ускорение
        phase5_mask = (t_values > t_a + t_j) & (t_values <= t_a + (total_time - t_a - t_j))
        j_values[phase5_mask] = 0
        a_values[phase5_mask] = -max_acceleration
        
        phase5_indices = np.where(phase5_mask)[0]
        if len(phase5_indices) > 0:
            for i in phase5_indices:
                t = t_values[i]
                delta_t = t - (t_a + t_j)
                v_values[i] = v_values[last_phase4_idx] - max_acceleration * delta_t
                s_values[i] = s_values[last_phase4_idx] + v_values[last_phase4_idx] * delta_t - (max_acceleration * delta_t**2) / 2
            
            last_phase5_idx = phase5_indices[-1]
        else:
            last_phase5_idx = last_phase4_idx
        
        # Фаза 6: Уменьшение торможения до полной остановки
        phase6_mask = (t_values > t_a + (total_time - t_a - t_j))
        j_values[phase6_mask] = max_jerk
        
        phase6_indices = np.where(phase6_mask)[0]
        if len(phase6_indices) > 0:
            for i in phase6_indices:
                t = t_values[i]
                delta_t = t - (t_a + (total_time - t_a - t_j))
                a_values[i] = -max_acceleration + max_jerk * delta_t
                v_values[i] = v_values[last_phase5_idx] - max_acceleration * delta_t + (max_jerk * delta_t**2) / 2
                s_values[i] = s_values[last_phase5_idx] + v_values[last_phase5_idx] * delta_t - (max_acceleration * delta_t**2) / 2 + (max_jerk * delta_t**3) / 6
    
    # Корректировка чтобы последняя скорость была ровно 0
    v_values[-1] = 0
    
    return t_values, s_values, v_values, a_values, j_values


def plot_motion_profile(t, s, v, a, j, distance, max_jerk, max_acceleration, max_velocity):
    """
    Визуализирует профили движения
    """
    fig, axs = plt.subplots(4, 1, figsize=(10, 16), sharex=True)
    
    # График позиции
    axs[0].plot(t, s)
    axs[0].set_ylabel('Позиция (м)')
    axs[0].set_title(f'S-образный профиль движения\nДистанция={distance}м, Jmax={max_jerk}м/с³, Amax={max_acceleration}м/с², Vmax={max_velocity}м/с')
    axs[0].grid(True)
    
    # График скорости
    axs[1].plot(t, v)
    axs[1].set_ylabel('Скорость (м/с)')
    axs[1].axhline(y=max_velocity, color='r', linestyle='--', label='Max Velocity')
    axs[1].legend()
    axs[1].grid(True)
    
    # График ускорения
    axs[2].plot(t, a)
    axs[2].set_ylabel('Ускорение (м/с²)')
    axs[2].axhline(y=max_acceleration, color='r', linestyle='--', label='Max Acceleration')
    axs[2].axhline(y=-max_acceleration, color='r', linestyle='--')
    axs[2].legend()
    axs[2].grid(True)
    
    # График рывка
    axs[3].plot(t, j)
    axs[3].set_ylabel('Рывок (м/с³)')
    axs[3].axhline(y=max_jerk, color='r', linestyle='--', label='Max Jerk')
    axs[3].axhline(y=-max_jerk, color='r', linestyle='--')
    axs[3].set_xlabel('Время (с)')
    axs[3].legend()
    axs[3].grid(True)
    
    plt.tight_layout()
    return fig


# Параметры движения
distance = 10.0  # метров
max_jerk = 5.0   # м/с³
max_acceleration = 2.0  # м/с²
max_velocity = 1.5  # м/с

# Рассчитать и отобразить профиль движения
t, s, v, a, j = s_curve_motion_profile(distance, max_jerk, max_acceleration, max_velocity)
fig = plot_motion_profile(t, s, v, a, j, distance, max_jerk, max_acceleration, max_velocity)

# Вывести некоторую информацию о движении
print(f"Общее время движения: {t[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v):.2f} м/с (заданный лимит: {max_velocity} м/с)")
print(f"Максимальное ускорение: {max(a):.2f} м/с² (заданный лимит: {max_acceleration} м/с²)")
print(f"Конечная позиция: {s[-1]:.3f} м (заданная: {distance} м)")

plt.savefig('s_curve_motion_profile.png', dpi=300)
plt.show()


# Пример использования функции с другими параметрами
# Большее расстояние, когда точно достигается макс. скорость
print("\nПример 2: Большее расстояние с выходом на макс. скорость")
distance2 = 30.0
t2, s2, v2, a2, j2 = s_curve_motion_profile(distance2, max_jerk, max_acceleration, max_velocity)
fig2 = plot_motion_profile(t2, s2, v2, a2, j2, distance2, max_jerk, max_acceleration, max_velocity)
print(f"Общее время движения: {t2[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v2):.2f} м/с")
print(f"Конечная позиция: {s2[-1]:.3f} м")
plt.savefig('s_curve_motion_profile_long_distance.png', dpi=300)
plt.show()

# Небольшое расстояние, когда не достигается макс. скорость
print("\nПример 3: Малое расстояние без выхода на макс. скорость")
distance3 = 1.0
t3, s3, v3, a3, j3 = s_curve_motion_profile(distance3, max_jerk, max_acceleration, max_velocity)
fig3 = plot_motion_profile(t3, s3, v3, a3, j3, distance3, max_jerk, max_acceleration, max_velocity)
print(f"Общее время движения: {t3[-1]:.2f} секунд")
print(f"Максимальная скорость: {max(v3):.2f} м/с")
print(f"Конечная позиция: {s3[-1]:.3f} м")
plt.savefig('s_curve_motion_profile_short_distance.png', dpi=300)
plt.show()