---

% Решение задачи оптимального управления (Вариант 14)
% ИСПРАВЛЕННАЯ ВЕРСИЯ: Гарантия того, что время идет вперед (tf > t0)

function solve_optimal_control_v14_fixed()
    % Ищем параметр p, где конечное время tf = 1 + p^2. 
    % Если мы ожидаем tf около 3 сек, то p_guess около 1.41 (т.к. 1 + 1.41^2 ~ 3)
    p_guess = 1.5; 
    
    % Создаем сетку для нормализованного времени tau от 0 до 1
    tau = linspace(0, 1, 100);
    
    % Задаем структуру начального приближения для bvp4c
    solinit = bvpinit(tau, @guess_func, p_guess);

    % Настройки решателя
    options = bvpset('RelTol', 1e-4, 'AbsTol', 1e-6, 'NMax', 5000);

    disp('Идет поиск оптимального решения (в прямом времени)...');
    try
        % Решение краевой задачи
        sol = bvp4c(@ode_system, @boundary_conditions, solinit, options);
        
        % Извлечение результатов с учетом нашей подстановки tf = 1 + p^2
        p_opt = sol.parameters; 
        T_opt = 1 + p_opt^2; % Найденное оптимальное время tf
        
        t = 1 + sol.x * (T_opt - 1); % Возврат к реальному времени t (t0 = 1)
        
        x1 = sol.y(1, :);
        x2 = sol.y(2, :);
        psi1 = sol.y(3, :);
        psi2 = sol.y(4, :);
        
        % Расчет оптимального управления
        u = zeros(size(t));
        for i = 1:length(t)
            u(i) = (psi2(i) - 2*psi1(i)) / (0.4 * x2(i)^2 + 1e-6);
        end
        
        fprintf('Решение успешно найдено!\n');
        fprintf('Оптимальное конечное время tf = %.4f сек\n', T_opt);
        
        % Построение графиков
        figure('Name', 'Оптимальное управление: Вариант 14', 'Color', 'w');
        
        subplot(3,1,1);
        plot(t, x1, 'b-', t, x2, 'r-', 'LineWidth', 1.5);
        title(sprintf('Фазовые траектории (Оптимальное время t_f = %.3f)', T_opt));
        xlabel('Время t'); ylabel('x(t)');
        legend('x_1(t)', 'x_2(t)', 'Location', 'best');
        grid on;
        
        subplot(3,1,2);
        plot(t, psi1, 'b--', t, psi2, 'r--', 'LineWidth', 1.5);
        title('Сопряженные переменные \psi(t)');
        xlabel('Время t'); ylabel('\psi(t)');
        legend('\psi_1(t)', '\psi_2(t)', 'Location', 'best');
        grid on;
        
        subplot(3,1,3);
        plot(t, u, 'g', 'LineWidth', 1.5);
        title('Оптимальное управление u(t)');
        xlabel('Время t'); ylabel('u(t)');
        grid on;
        
    catch ME
        disp('Ошибка при решении:');
        disp(ME.message);
    end
end

% -----------------------------------------------------------------------
function dydtau = ode_system(~, y, p)
    % Вычисляем реальное T из параметра p
    T = 1 + p^2;
    
    x1 = y(1); x2 = y(2);
    psi1 = y(3); psi2 = y(4);
    
    u = (psi2 - 2*psi1) / (0.4 * x2^2 + 1e-6);
    
    dx1_dt = -17*x1 - 50*x2 - 2*u;
    dx2_dt = 3*x1 + 8*x2 + u;
    dpsi1_dt = 0.6*x1 + 17*psi1 - 3*psi2;
    dpsi2_dt = 0.4*x2*u^2 + 50*psi1 - 8*psi2;
    
    t0 = 1; 
    dt_dtau = T - t0;
    
    dydtau = dt_dtau * [dx1_dt; dx2_dt; dpsi1_dt; dpsi2_dt];
end

% -----------------------------------------------------------------------
function res = boundary_conditions(ya, yb, p)
    % ya - t=1, yb - t=tf
    x1_0 = ya(1);
    psi2_0 = ya(4);
    
    x1_f = yb(1);
    x2_f = yb(2);
    psi1_f = yb(3);
    psi2_f = yb(4);
    
    u_f = (psi2_f - 2*psi1_f) / (0.4 * x2_f^2 + 1e-6);
    H_f = -(0.3*x1_f^2 + 0.2*x2_f^2*u_f^2) + psi1_f*(-17*x1_f - 50*x2_f - 2*u_f) + psi2_f*(3*x1_f + 8*x2_f + u_f);
    
    res = [
        x1_0 - 0;      
        psi2_0 - 0;    
        x1_f - 1;      
        x2_f - 1;      
        H_f            
    ];
end

% -----------------------------------------------------------------------
function y_guess = guess_func(tau)
    % Немного изменим начальное приближение, чтобы помочь решателю
    x1 = tau;           
    x2 = tau; 
    psi1 = 1;           
    psi2 = 1;
    
    y_guess = [x1; x2; psi1; psi2];
end