---

% Решение задачи оптимального управления (Вариант 14)
% Метод множителей Лагранжа, сведение к краевой задаче (BVP) с неизвестным временем tf

function solve_optimal_control_v14()
    % Начальное приближение для неизвестного времени tf
    T_guess = 3; 
    
    % Создаем сетку для нормализованного времени tau от 0 до 1
    tau = linspace(0, 1, 100);
    
    % Задаем структуру начального приближения для bvp4c
    solinit = bvpinit(tau, @guess_func, T_guess);

    % Настройки решателя (задаем допуски и увеличиваем макс. число итераций)
    options = bvpset('RelTol', 1e-4, 'AbsTol', 1e-6, 'NMax', 5000);

    disp('Идет поиск оптимального решения...');
    try
        % Решение краевой задачи
        sol = bvp4c(@ode_system, @boundary_conditions, solinit, options);
        
        % Извлечение результатов
        T_opt = sol.parameters; % Найденное оптимальное время 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');
        
        % 1. Фазовые траектории
        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;
        
        % 2. Сопряженные переменные
        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;
        
        % 3. Оптимальное управление
        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);
        disp('Попробуйте изменить начальное приближение (T_guess или guess_func).');
    end
end

% -----------------------------------------------------------------------
% 1. Система дифференциальных уравнений
% -----------------------------------------------------------------------
function dydtau = ode_system(~, y, T)
    % Переменные состояния и сопряженные переменные
    x1 = y(1); x2 = y(2);
    psi1 = y(3); psi2 = y(4);
    
    % Уравнение управления (с добавлением 1e-6 от деления на 0)
    u = (psi2 - 2*psi1) / (0.4 * x2^2 + 1e-6);
    
    % Производные по реальному времени t
    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;
    
    % Масштабный множитель перехода к нормализованному времени tau:
    % dt = (T - t0) * dtau
    t0 = 1; 
    dt_dtau = T - t0;
    
    % Возвращаем вектор производных по tau
    dydtau = dt_dtau * [dx1_dt; dx2_dt; dpsi1_dt; dpsi2_dt];
end

% -----------------------------------------------------------------------
% 2. Краевые условия и условия трансверсальности
% -----------------------------------------------------------------------
function res = boundary_conditions(ya, yb, T)
    % ya - вектор значений на левом конце (t = 1)
    % yb - вектор значений на правом конце (t = tf)
    
    % На левом конце (t = 1)
    x1_0 = ya(1);
    psi2_0 = ya(4);
    
    % На правом конце (t = tf)
    x1_f = yb(1);
    x2_f = yb(2);
    psi1_f = yb(3);
    psi2_f = yb(4);
    
    % Гамильтониан на правом конце должен быть равен 0
    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);
    
    % Формируем вектор невязок (все они должны быть сведены решателем к 0)
    res = [
        x1_0 - 0;      % 1: x1(1) = 0
        psi2_0 - 0;    % 2: psi2(1) = 0 (условие трансверсальности, т.к. x2(1) свободно)
        x1_f - 1;      % 3: x1(tf) = 1
        x2_f - 1;      % 4: x2(tf) = 1
        H_f            % 5: H(tf) = 0 (условие трансверсальности, т.к. tf свободно)
    ];
end

% -----------------------------------------------------------------------
% 3. Функция начального приближения траекторий
% -----------------------------------------------------------------------
function y_guess = guess_func(tau)
    % tau меняется от 0 до 1.
    % Формируем простейшую линейную интерполяцию от заданных начальных
    % до заданных конечных точек, чтобы помочь решателю сходиться быстрее.
    x1 = tau;           % от 0 до 1
    x2 = 0.5 + 0.5*tau; % от 0.5 до 1 (чтобы x2 не был нулем в начале)
    psi1 = 0;           % костаты начинаем с 0
    psi2 = 0;
    
    y_guess = [x1; x2; psi1; psi2];
end