% Projections PID

clear all

addpath('matlab2tikz/src');

filename = 'case2';

s=tf('s');
% tp = 1
%gp = 1/(s+1)/(.3*s+1)*exp(-1*s);
%gp_w = 1./(1i.*w+1).*exp(-1i.*w);

% tp = inf
gp = 1/(s+1)/(0.5*s+1)*exp(-s);
%%


domain = 0:.05:1;

ms_constraint = 1.5;

k = [];
w = [];
tuning = [];

iteration=[];

for n = 1:1:length(domain)
    for m = abs(round(cos(n*pi/2)*(length(domain)+1-n)+sin(n*pi/2))):(-1)^(n+1):abs(round(sin(n*pi/2)*(length(domain)+1-n)+cos(n*pi/2)))
        k_init = 0;
        fprintf('iteration I: %i, D: %i\t|',n,m)
        i = domain(n);
        d = domain(m);
        gc = @(k) k*((1-i-d)+i/s+d*s);
        [x_k,fval,exitflag] = fmincon(@(x_k) (ms_constraint-ms(gp,gc(x_k)))^2,k_init,[],[],[],[],0,[],[],optimset('Display','off','algorithm','interior-point'));
        p=0;
        
        while fval > 0.001 %huristics for flat optimum
            
            if p == 4
                k_init = 4000;
                fprintf('#')
            elseif p > 5
                break
            else
                k_init = 10-p;
                fprintf('*')
            end

            [x_k,fval,exitflag] = fmincon(@(x_k) (ms_constraint-ms(gp,gc(x_k)))^2,k_init,[],[],[],[],0,inf,[],optimset('Display','off','algorithm','interior-point'));
            
            p = p+1;

        end
        iteration(end+1) = p;
        fprintf('\t')
        k(end+1,1)= x_k;
        w(end+1,1) = ms2(gp,gc(x_k));
        fprintf('| ms: %.2f, gain: %.2f\t', ms(gp,gc(x_k)),x_k)
        fprintf(' | fval: %.2f, exitflag: %i\n', fval,exitflag)
        %k_init = 0.1;max(k(end),2);
        tuning(end+1,:) = [i d];
    end
end
    
%%
sol.pid.cntr_discription = 'C = k((1-i-d)+i/s+d*s)';
sol.pid.pros_discription = 'G = 1/(s)*exp(-s)';
sol.pid.k = k;
sol.pid.id_levels = tuning;
sol.w = w;
sol.iterations = iteration;

i = tuning(:,1);
d = tuning(:,2);

% frequency domain
gc = k.*((1-i-d)+i./(1i.*w)+d.*(1i.*w));
gp_w = 1./(1i.*w+1)./(.3*1i.*w+1).*exp(-1*1i.*w);

S = 1./(1+gp_w.*gc);

real_S = real(S);
imag_S = imag(S);

sol.pid.real_s = real_S;
sol.pid.imag_s = imag_S;
sol.pid.ms = sqrt(real_S.^2+imag_S.^2);

save([filename,'.mat'],'sol')


%%

figure(5)
ternpcolor(i, d, iteration+1); ternlabel('I', 'D', 'P');

%% gain plot
figure(1)
clf

i = sol.pid.id_levels(:,1);
d = sol.pid.id_levels(:,2);
k = sol.pid.k;

ternpcolor(i, d, k); ternlabel('I', 'D', 'P');
shading interp
figure(2)

plot(sol.pid.real_s,sol.pid.imag_s,'ro')

xlabel('real(S)')
ylabel('imag(S)')
axis square
axis([-ms_constraint ms_constraint -ms_constraint ms_constraint])


figure(4)
clf
ternpcolor(i, d, sol.pid.ms); ternlabel('I', 'D', 'P');
shading interp

%% optimize
%global gp iaeWeights manWeights msEq
%global msEq
%profile on
load('res0.mat')
%iaeWeights = [res.POpid.minSp.iae, res.POpid.minD.iae]';
iaeWeights = [res.minSp.iae, res.minD.iae]';
manWeights = [.5, .5];
tuning = [sol.pid.k, sol.pid.id_levels];
msEq = 1.01;
x0 = [0 0.1 0.1]';
lb = [0 0 0]';
ub = [Inf Inf Inf]';
sol_test=[];
gc = @(x)x(1)*((1-x(2)-x(3))+x(2)/s+x(3)*s);

%gc = @(x) x(1)*(1 + x(2)/s + x(3)*s);
%gc = @(x) x(1)*(1 + 1/x(2)/s + x(3)*s);
%gc = @(x) (x(1)+ x(2)/s + x(3)*s);
res.ms=[];
res.cost=[];
res.tuning=[];
opt=optimset('TolFun',1e-2,'tolX',1e-2,'display','none');
fprintf('************************************\n')
for msEq = 1.5
fprintf('ms %.2f || ',msEq)
% tic
%  sol_tuning = fmincon(@(x) -x(1),x0,[],[],[],[],lb,ub,@conFun2, optimset('Display','off','algorithm','sqp'));
%  x1 = sol_tuning;
%  gc = @(k,i,d)k*((1-i-d)+i/s+d*s);
%  ms0=ms(gp,gc(x1(1),x1(2),x1(3)));
%  fprintf('ms %.3f | time: %.2f | tuning: %.4f, %.4f, %.4f |',ms0,toc,x1)
tic
[sol_tuning,cost,exitflag] = fmincon(@(x) costFun_pid(gp,gc(x),iaeWeights,manWeights) ,x0,[],[],[],[],lb,ub,@(x) conFunEq(gp,gc(x),msEq,opt), optimset('Display','off','algorithm','sqp'));
x0 = sol_tuning;
sol_test(end+1,:) = sol_tuning'; 
ms1=ms(gp,gc(x0),opt);
res.ms(end+1)=ms1;
res.cost(end+1)=cost;
res.tuning(end+1,:)=sol_tuning';
fprintf('ms %.3f | time: %.2f | cost: %.2f \t| tuning: %.4f, %.4f, %.4f | exitflg: %i \n',ms1,toc,cost,x0,exitflag)
end

figure(6)
plot(res.ms,res.cost)
figure(7)
i = res.tuning(:,2);
d = res.tuning(:,3);
ternplot(i, d,(1-d-i), '-'); ternlabel('I', 'D', 'P');
% profile viewer

%% Cost Function

%global gp iaeWeights manWeights iaeTuning

gc = @(x)x(1)*((1-x(2)-x(3))+x(2)/s+x(3)*s);

%iaeWeights = [res.POpid.minSp.iae, res.POpid.minD.iae]';
iaeWeights = [res.minSp.iae, res.minD.iae]';
manWeights = [1, 0];

tuning = [sol.pid.k, sol.pid.id_levels];

J = [];
for n = 1:length(tuning)
   J(n,1) = costFun_pid(gp,gc(tuning(n,:)),iaeWeights,manWeights);
end
[j_min, j_min_indx] = min(J);
%%
J(end-11:end)=J(end-12)*ones(12,1)
%%

i = sol.pid.id_levels(:,1);
d = sol.pid.id_levels(:,2);
figure(3)
subplot(2, 2, 1)
ternplot(i, d,(1-d-i), '.'); ternlabel('I', 'D', 'P');
subplot(2, 2, 2)
ternpcolor(i, d, log(J)); ternlabel('I', 'D', 'P');
shading interp
subplot(2, 2, 3)
[C,h]=terncontour(i, d,1-i-d,log(J),120); ternlabel('I', 'D', 'P');
clabel(C,h)
subplot(2, 2, 4)
ternsurf(i, d, log(J));

%%
figure(4)
[C,h]=terncontour(i, d,1-i-d,log(log(J)),300); ternlabel('I', 'D', 'P');
clabel(C,h)

tuning(j_min_indx,:)
%%

clear d_critical i_critical
d_critical = [];
i_critical = [];
d0=1;
for n = 0:0.001:1
   d_critical(end+1) = fsolve(@(d) (1-d-n)^2-4*d*n,d0);
   i_critical(end+1) = n;
   d0 = d_critical(end);
end
i0=0;
%for n = domain(1:20)
%   i_critical(end+1) = fsolve(@(i) 0.5-sqrt(i*n)+i+n,i0);
%   d_critical(end+1) = n;
%   i0 = i_critical(end);
%end



figure(4)
ternplot(i_critical, d_critical,1-i_critical-d_critical, 'o');
matlab2tikz( 'projTest.tex' );






