% robust_stability_tools.m
% Набор функций для анализа робастной устойчивости

function robust_stability_square(ncffs, bcffs)
% robust_stability_square - строит годограф Цыпкина–Поляка и квадраты для
% проверки робастной устойчивости по квадратам.
% Входные данные:
%   ncffs - вектор номинальных коэффициентов [a_n ... a_0]
%   bcffs - вектор границ коэффициентов изменений [b_n ... b_0]

    % Построение вспомогательных полиномов U0, Ua, V0, Va
    rev_nc = flip(ncffs);
    tmp_u = rev_nc(1:2:end);
    for i = 1:length(tmp_u)
        if mod(i-1,2)==1, tmp_u(i) = -tmp_u(i); end
    end
    u0cffs = flip(tmp_u);
    uacffs = flip(flip(bcffs)(1:2:end)); %# 求 bcffs(end:-2:1) 然后反转

    rev_nc2 = ncffs(end-1:-2:1);
    tmp_v = rev_nc2;
    for i = 1:length(tmp_v)
        if mod(i-1,2)==1, tmp_v(i) = -tmp_v(i); end
    end
    v0cffs = flip(tmp_v);
    vacffs = flip(flip(bcffs(end-1:-2:1)));

    % Параметры для построения
    count = 5001;
    omega = linspace(0, 100, count);
    xy = zeros(2, count);
    for k = 1:count
        xy(1,k) = polyval(u0cffs, omega(k)) / polyval(uacffs, omega(k));
        xy(2,k) = polyval(v0cffs, omega(k)) / polyval(vacffs, omega(k));
    end

    % Вычисление корней полиномов wp и wm
    wp = conv(u0cffs, vacffs) - conv(v0cffs, uacffs);
    wm = conv(u0cffs, vacffs) + conv(v0cffs, uacffs);
    rts_wp = roots(wp);
    rts_wm = roots(wm);
    rts = [rts_wp; rts_wm];
    rts = rts(real(rts)>0 & abs(imag(rts))<1e-8);

    deltas = arrayfun(@(r) polyval(u0cffs,r)/polyval(uacffs,r), rts);
    labels = arrayfun(@(d) sprintf("%.3f", abs(d)), deltas, 'UniformOutput',false);
    cmap = jet(5);

    % Построение графика
    figure; hold on;
    plot(xy(1,:), xy(2,:), 'b', 'LineWidth',2);
    for k = 1:length(deltas)
        d = deltas(k);
        vtx = [ d,-d,-d, d, d; d, d,-d,-d, d];
        plot(vtx(1,:), vtx(2,:), 'Color', cmap(end-k+1,:), 'LineWidth',2);
    end
    grid on; axis equal;
    legend(labels);
    title('Робастная устойчивость: квадрат');
    hold off;
end

function min_radius = robust_stability_disk(ncffs, bcffs)
% robust_stability_disk - строит годограф и окружности радиусов роб.уст.
% Возвращает минимальный радиус.

    % Формирование полиномов
    U0 = flip(apply_alternating_signs(flip(ncffs(1:2:end))));
    Ua = flip(bcffs(1:2:end));
    V0 = flip(apply_alternating_signs(ncffs(end-1:-2:1)));
    Va = flip(bcffs(end-1:-2:1));

    % Производные
    U0p = polyder(U0); Ua_p = polyder(Ua);
    V0p = polyder(V0); Va_p = polyder(Va);

    % Построение A и B
    A = conv(U0p, Ua) - conv(U0, Ua_p);
    B = conv(V0p, Va) - conv(V0, Va_p);

    % Кубы
    Va3 = conv(conv(Va,Va),Va);
    Ua3 = conv(conv(Ua,Ua),Ua);

    % Уравнение
    T1 = conv(conv(U0, A), Va3);
    T2 = conv(conv(V0, B), Ua3);
    eqpoly = T1 + T2;

    % Корни и радиусы
    rts = roots(eqpoly);
    rts = unique(round(real(rts(imag(rts)==0 & real(rts)>0)),6));
    radii = arrayfun(@(r) norm([ polyval(U0,r)/polyval(Ua,r), polyval(V0,r)/polyval(Va,r) ]), rts);
    radii = sort(radii);

    % Годограф
    omega = linspace(0,100,10001);
    x = polyval(U0,omega)./polyval(Ua,omega);
    y = polyval(V0,omega)./polyval(Va,omega);
    figure; hold on;
    plot(x,y,'b');
    phi = linspace(0,2*pi,100);
    for r = radii
        plot(r*cos(phi), r*sin(phi),'--','DisplayName',sprintf('R=%.2f',r));
    end
    axis equal; grid on; legend; title('Робастная устойчивость: диск');
    hold off;

    if isempty(radii), min_radius = Inf; else min_radius = radii(1); end
end

function H = hurwitz_matrix_(coeffs)
% hurwitz_matrix_ - строит матрицу Гурвица для Routh-Hurwitz
    n = length(coeffs)-1;
    if coeffs(1)<=0, error('Старший коэффициент должен быть положительным.'); end
    H = zeros(n);
    num = 0;
    while num <= floor(n/2)
        cur_i = 2*num+1; cur_j = num+1; cnt=1;
        while cnt <= length(coeffs)
            try
                H(cur_i,cur_j) = coeffs(cnt);
            end
            switch mod(cnt-1,4)
                case 0, cur_i = cur_i-1;
                case {1,3}, cur_i = cur_i+1; cur_j = cur_j+1;
                case 2, cur_i = cur_i-1;
            end
            cnt = cnt+1;
        end
        num = num+1;
    end
end

function [is_stable, H, minors] = check_hurwitz_stability(coeffs)
% check_hurwitz_stability - проверка по главным минорам матрицы Гурвица
    n = length(coeffs)-1;
    H = hurwitz_matrix_(coeffs);
    disp('Матрица Гурвица:'); disp(H);
    minors = zeros(1,n);
    for k=1:n
        M = H(1:k,1:k);
        minors(k) = det(M);
        fprintf('Минор M%d = %.4f\n', k, minors(k));
    end
    is_stable = all(minors>0);
end

function is_robust = check_robust_stability_kharitonov(low, up)
% check_robust_stability_kharitonov - проверка по полиномам Харитонова
    if any(low<=0)
        disp('Есть не положительные коэффициенты -> не робастно устойчив.');
        is_robust = false; return;
    end
    l = length(low);
    % Формирование 4 полиномов
    nn = zeros(1,l); nv = nn; vn = nn; vv = nn;
    for i=1:l
        idx = mod(i-1,4);
        nn(i) = low(i)   * (idx<=1) + up(i)   * (idx>1);
        nv(i) = up(i)    * (idx>=1&&idx<=2) + low(i)*(~(idx>=1&&idx<=2));
        vn(i) = low(i)   * (idx>=1&&idx<=2) + up(i) *(~(idx>=1&&idx<=2));
        vv(i) = up(i)    * (idx<=1) + low(i)  * (idx>1);
    end
    polys = {nn,nv,vn,vv}; names = {'NN','NV','VN','VV'};
    for k=1:4
        fprintf('Полином %s:\n', names{k});
        [ok,~,~] = check_hurwitz_stability(polys{k});
        if ~ok
            disp('-> Не робастно устойчив.'); is_robust = false; return;
        end
    end
    disp('-> Многочлен робастно устойчив.'); is_robust = true;
end

function r_c = robust_stability_radii(A,B,C)
% robust_stability_radii - вычисление радиуса h-inf нормы
% A' = A + B*Delta*C, r_c = 1/||Delta||∞
    r_c = 1 / h_inf_norm_lmi_continuous(A,B,C);
end

% Вспомогательная функция для чередования знаков
function out = apply_alternating_signs(vec)
    out = vec;
    for i = 1:length(vec)
        if mod(i-1,2)==1, out(i) = -out(i); end
    end
end