warning('off','all');
clearvars -except design_count

clc
format long e

situation = 1;

key = 0;
count_Inaccurate = 0;
count_Failed = 0;
count_Infeasible = 0;
count_Error_Other = 0;

Success_flag = 0;
Solved_flag = 0;
satisfy_flag = 0;

result_flag = 1;

s = tf('s');
Ts = 1e-3;
z = tf('z',Ts);

end_condition = 1e-2; 
count_end_condition = 3;
deep = 10000;

omega = logspace(-3,3,500);
[~,f_ten] = size(omega);


P = 10*(s^2+1)/(s^2*(s^2+2)); % 制御対象モデル

[m,n] = size(P);
check = 1;
C0 = (s+9)/(s+10);

W1 = tf(1);
W2 = tf(1);

ND = (s+10); % 数値的安定性を向上させるためのフィルタ
phi_n0 = [s;1];
phi_n = phi_n0/ND; % 分子多項式の基底を安定な伝達関数で表現
[ln,~] = size(phi_n);    % 制御器の分子多項式の基底の数

phi_d0 = [s;1];

phi_d = phi_d0/ND; % 分母多項式の基底を安定な伝達関数で表現
[ld,~] = size(phi_d); % 制御器の分母多項式の基底の数

C0_d = minreal((C0.den{1,1}*phi_d0)/ND);   % C0d = D0/ND
C0_n = minreal(C0*C0_d); % C0n = N0/ND = N0/D0 * (D0/ND) = minreal(C0 * C0d)

sys1 = minreal([1 ; W2*P*W1]/(1+C0*W2*P*W1)*[1,C0]);
H_inf_f0 = norm(sys1,inf);
clear sys1;
fprintf('H-infinity norm for initial controller = %d\n',H_inf_f0);


default_LMI_scale = 1;
scale_LMI(1:deep) = default_LMI_scale;

COMP_P = freqresp(P,omega);
COMP_W1 = freqresp(W1,omega);
COMP_W2 = freqresp(W2,omega);
COMP_phi_n = freqresp(phi_n,omega);
COMP_phi_d = freqresp(phi_d,omega);
COMP_C0_n = freqresp(C0_n,omega);
COMP_C0_d = freqresp(C0_d,omega);

COMP_G = COMP_W1.*COMP_P.*COMP_W2;

if exist('count','var') == 0 %% 前回の設計結果からの引継ぎ判定
    end_count = 0;
    Ztil = zeros(m+n,m+n,f_ten);
    eig_L_n = zeros(m,f_ten);
    eig_deco_n = zeros(m,m,f_ten);
    old_C_d = zeros(1,1,f_ten);
    old_C_n = zeros(n,m,f_ten);
    COMP_L_old = zeros(1,1,f_ten);

    gamma2_old = zeros(deep,1);
    rho_old = zeros(n,ln,deep);
    eta_old = zeros(1,ld,deep);
    count = 1;
end

while count < deep   % Update Loop
    if count == 1
        for freq =1:f_ten
            old_C_n(:,:,freq) = COMP_C0_n(:,:,freq);
            old_C_d(1,1,freq) = COMP_C0_d(1,1,freq);
            COMP_L_old(:,:,freq) = COMP_G(1,1,freq)*old_C_n(1,1,freq)/old_C_d(1,1,freq);
            Ztil(:,:,freq) = (old_C_d(1,1,freq) + old_C_n(:,:,freq)*COMP_G(:,:,freq))*eye(2); % 前回の解を用いて \tilde{Z} を作成(初期解用)
        end
        
        Relative = 0; % 評価値の相対誤差の初期値
        
        if exist('A0','var') == 0; A0 = [-C0.den{1,1}(:,2:ld);eye(ld-2),zeros(ld-2,1)];end
        P_Ly = lyap(A0.',eye(ld-1)); % 制御器の安定性のチェックのためのリアプノフ方程式の解(初期解用)
        gamma_b = 1e10;
    else
        for freq = 1:f_ten
            old_C_n(:,:,freq) = (rho_old(:,:,count-1)*COMP_phi_n(:,:,freq));
            old_C_d(1,1,freq) = (eta_old(:,:,count-1)*COMP_phi_d(:,1,freq));
            COMP_L_old(:,:,freq) = COMP_G(1,1,freq)*old_C_n(1,1,freq)/old_C_d(1,1,freq);          
            Ztil(:,:,freq)=[old_C_d(1,1,freq)*eye(n)+old_C_n(:,:,freq)*COMP_G(:,:,freq),zeros(n,m);zeros(m,n),old_C_d(1,1,freq)*eye(m)+COMP_G(:,:,freq)*old_C_n(:,:,freq)]; % 前回の解を用いて \tilde{Z} を作成
        end
        
        A0 = [-eta_old(:,2:ld,count-1);eye(ld-2),zeros(ld-2,1)];
        P_Ly = lyap(A0.',eye(ld-1)); % 制御器の安定性のチェックのためのリアプノフ方程式の解
        
        if Solved_flag == 1 %||count<4;
            fprintf('Iteration Number = %d\nRelative error of cost = %d\n\n',count-1,Relative);
            for i=1:n+1
                fprintf('rho(%d) = %d  eta(%d) = %d\n',i,rho_old(1,i,count-1),i,eta_old(1,i,count-1)); %調整されたパラメータの表示
            end
            fprintf('\n');
            fprintf('Cost γ = %d \n\n',sqrt(gamma2_old(count-1,1))); % 評価値 γ の表示 (gamma2はγの2乗)
            
            C_inf_d = eta_old(:,:,count-1)*phi_d0;
            C_inf_n = rho_old(:,:,count-1)*phi_n0;
            C_inf = C_inf_n/C_inf_d;
            
            if 1 % Robust control toolbox がない場合には0にしてください 制御器・閉ループの安定性のチェック
                F=loopsens(W2*P*W1*C_inf,eye(m));
                fprintf('Pole(C_inf):\n');disp(pole(C_inf));
                fprintf('Pole(Closed_Loop):\n');disp(F.Poles);
                Solved_flag = 0;
            end
        end
        
    end
    
    clear COMP_L_n;clear cal_r;clear Vec_eta;clear r;clear C_inf_d_now;clear C_inf_n_now;clear r;clear L;

   %% CVXによる凸最適化
    cvx_clear                           % CVXの設定を初期化
    cvx_precision best                  % CVXの解法の範囲 (変化量，標準偏差，最適点の許容偏差)を設定{best,high,...など}
    cvx_begin sdp                       % CVXをSDPの解法で開始
    cvx_solver sdpt3                    % ソルバーの指定 (sedumi,sdpt3など。それぞれ精度-時間などの優先が異なる)
    
    variable rho(n,ln) % 設計変数の定義 \eta , \rho, \gamma 2
    variable eta(1,ld-1)
    variable gamma2(1,1)
    
    expressions C_inf_n_now(n,m,f_ten); % 設計変数を含む記号の定義
    expressions C_inf_d_now(1,1,f_ten);
    expressions Vec_eta(1,ld);
    expressions COMP_L_n(m,m,f_ten);
    expressions cal_r(m,m);
    
    Vec_eta=[1,eta]; % 分母多項式を monic にするため最高次数の係数を1に固定
        
    % 目的関数
    minimize( gamma2 )

    subject to % これ以降は制約条件   
     
%% 制御器の安定制約(リアプノフ不等式)
    A = [-Vec_eta(2:ld);eye(ld-2),zeros(ld-2,1)]; % 制御器のA行列 (可制御正準システム)
    P_Ly*A + A.'*P_Ly < 0 % リアプノフ不等式による安定制約 (1回前の解より得られる P_Ly を利用)
    
    for freq=1:f_ten
        C_inf_n_now(:,:,freq) = rho*COMP_phi_n(:,:,freq);
        C_inf_d_now(1,1,freq) = Vec_eta*COMP_phi_d(:,1,freq);
        COMP_L_n(:,:,freq) = COMP_G(:,:,freq)*C_inf_n_now(:,:,freq);

%% 安定制約 (ナイキストの安定判別 更新範囲の制限 |1+L(k-1)|>|L(k)-L(k-1)| + CCCP )
        abs(COMP_L_n(1,1,freq) - (C_inf_d_now(1,1,freq)*COMP_L_old(:,:,freq))) - abs(1 + COMP_L_old(:,:,freq))*real(old_C_d(1,1,freq)*C_inf_d_now(1,1,freq)')/abs(old_C_d(1,1,freq)) < 0

%% 評価式(LMI制約)         
        Y = [1;COMP_G(:,:,freq)]*[C_inf_d_now(:,:,freq) C_inf_n_now(:,:,freq)]; % 解説論文本文中の(16)式
        Z = (C_inf_d_now(:,:,freq) + C_inf_n_now(:,:,freq)*COMP_G(:,:,freq))*eye(2); % 解説論文本文中の(17)式

        L11 = Z'*Ztil(:,:,freq)+Ztil(:,:,freq)'*Z-Ztil(:,:,freq)'*Ztil(:,:,freq);
        L21 = Y;
        L12 = L21';
        L22 = gamma2*eye(m+n);
        L{freq} = [L11,L12;L21,L22]*scale_LMI(count); % Infeasible の場合，数値条件を良化するためスケーリング
        L{freq} > 0 % 解説論文本文中の(19)式
    end
    cvx_end

% 最適計算のエラー判定部

    if strcmp(cvx_status,'Solved') == 0
        if strcmp(cvx_status,'Inaccurate/Solved') == 1
            count_Inaccurate=count_Inaccurate+1;
        elseif strcmp(cvx_status,'Infeasible')==1
            count_Infeasible = count_Infeasible + 1;
            count = count - 1;
            scale_LMI(count+1:deep) = scale_LMI(count+1)*0.1;
            fprintf('\nAuto Tune: LMI is scaled by = %d\n\n',scale_LMI(count+1));
            if scale_LMI(count+1) < 1e-8
                prompt = 'Do you try to calculate on this scale?\n(reset scaling:2, continue：1，end：0)\n';
                key = input(prompt);
                if size(key) == 0
                    key = 0;
                elseif key > 2
                    key = 0;
                elseif key < 0
                    key = 0;
                end
                if key == 0
                    if count > 1
                        fprintf('Optimization is terminated. Suboptimized result is displayed.\n\n');
                    else
                        fprintf('Optimization is terminated. Solution is not obtained.\n\n');
                    end
                    break;
                elseif key == 2
                    scale_LMI(count+1:deep) = default_LMI_scale;
                end
            end
        elseif strcmp(cvx_status,'Failed') == 1
            count_Failed = count_Failed + 1;
            count = count - 1;
            scale_LMI(count+1:deep) = scale_LMI(count+1)*0.1;
            fprintf('\nAuto Tune: LMI is scaled by = %d\n\n',scale_LMI(count+1));
            if scale_LMI(count+1) < 1e-8
                prompt = 'Do you try to calculate on this scale?\n(reset scaling:2, continue：1，end：0)\n';
                key = input(prompt);
                if size(key) == 0
                    key = 0;
                elseif key > 2
                    key = 0;
                elseif key < 0
                    key = 0;
                end
                if key == 0
                    if count > 1
                        fprintf('Optimization is terminated. Suboptimized resultis displayed.\n\n');
                    else
                        fprintf('Optimization is terminated. Solution is not obtained.\n\n');
                    end
                    break;
                elseif key == 2
                    scale_LMI(count+1:deep) = default_LMI_scale;
                end
            end
        else
            count_Error_Other = count_Error_Other + 1;
            fprintf('unKnown error on optimizing\n\n');
            prompt ='Do you try to continue design? (continue：1，end：0)\n';
            key = input(prompt);
            if size(key) == 0
                key = 0;
            elseif key > 1
                key = 0;
            elseif key < 0
                key = 0;
            end
            if key == 0
                if count > 1
                    gamma2_old(count,1) = 0; % 失敗データの消去
                    rho_old(:,:,count) = zeros(n,ln);
                    gamma2 = gamma2_old(count-1,1); % 前回データの回復
                    rho = rho_old(:,:,count-1);
                    fprintf('Optimization is terminated. Suboptmized result is displayed.\n\n');
                else
                    fprintf('Optimization is terminated. Solution is not obtained.\n\n');
                end
                break;
            end
        end
    end
    
    if strcmp(cvx_status,'Solved') == 1 ||strcmp(cvx_status,'Inaccurate/Solved') == 1
        C_inf_d = double(Vec_eta)*phi_d0;
        C_inf_n = double(rho)*phi_n0;
        C_inf = C_inf_n/C_inf_d; % 最適化された制御器

        
        sys = [1; W2*P*W1]/(1 + C_inf*W2*P*W1)*[1, C_inf];
        check_sys = freqresp(sys,omega);
        sig_c = zeros(1,f_ten);
        for i=1:f_ten
            sig_c(i) = max(svd(check_sys(:,:,i)));
        end
        
        if 1 % Robust control toolbox がない場合には0にしてください 制御器・閉ループの安定性のチェック
            F = loopsens(W2*P*W1*C_inf,eye(m));
            if isstable(C_inf)==1 && F.Stable==1 %&& max(sig_c)^2 <= double(gamma2);
                if isstable(C_inf) == 1
                    fprintf('All_consraints:Satisfied\n\n');
                    satisfy_flag = 1;
                    if F.Stable == 1 && exist('unstable_count','var') == 0
                        unstable_count = count;
                    end
                else
                    fprintf('Some consraint(s) are violated.\n\n');
                    pause;
                end
            end
        end
    end
%% 繰り返し部分の終了判定

    if satisfy_flag == 1
        if count >= 2
            Relative = (gamma2_old(count-1,1) - gamma2)/gamma2_old(count-1,1); % 評価値の相対誤差
            if abs(Relative) <= end_condition
                end_count = end_count + 1;
                if end_count >= count_end_condition
                    Success_flag = 1;
                    gamma2_old(count,1) = gamma2;
                    rho_old(:,:,count) = rho;
                    eta_old(:,:,count) = Vec_eta;
                    gamma_b = gamma2;
                    break;
                end
            else
            end_count = 0;
            end
            
            if Relative < 0
                fprintf('γ become large than the previous value. Relative error is %d.\n',Relative);
                pause;
            end
        end
 
    %% 設計データの保存
        gamma2_old(count,1) = gamma2;             
        rho_old(:,:,count) = rho;
        eta_old(:,:,count) = Vec_eta;
        gamma_b = gamma2;
        
        satisfy_flag = 0;
        Solved_flag = 1;
        if scale_LMI(count+1:deep) < 1e-3
            scale_LMI(count+1:deep) = default_LMI_scale; 
        end
    end
    
    count = count + 1;
end

fprintf('Optimized γ = %d\n\n',sqrt(gamma2));

%% 解析解の導出 (モデルベース フルオーダー制御器 Robust Control Toolbox が必要)

if 1
    [Ap,Bp,Cp,Dp] = ssdata(P);
    sys_P = ltisys(Ap,Bp,Cp,Dp);
    [sys_K,emax] = ncfsyn(sys_P,1);
    [Ak,Bk,Ck,Dk] = ltiss(sys_K);
    Kopt = -ss(Ak,Bk,Ck,Dk); % ncfsyn は正帰還フィードバックに基づいているため '-' を追加
    gopt = 1/emax;
    fprintf('Optimal γ with model-based full-order controller = %d\n',gopt);
    bode(C_inf,Kopt,omega)
end