
% Script for generating the PO curve for a given process gp
% written by: Chriss Grimholt, 11. jan 2012                 

% tuning parameters are given for the PI controller gc = K + I/s
xInit = [0.2, 0.6
         .5   .5
         4    .5
         0.5   0]; 

%for i = 1:4
clc
tic

global gp msEq iaeWeights manWeights iaeTuning


modelId = 4;


gp = model(modelId); % getting the model

%% Finding the iaeWeights 

    fprintf('Finding Optimal IAE Weights\n')
    fprintf('***************************\n')
    fprintf('\n')
    fprintf('costFun(iae) \t minTuning \t Ms \t exitFlag \n')
    fprintf('-------------------------------------------------\n')


msEq = 1.59; % Ms for the iaeWeights 
opt = optimset('algorithm','active-set','Display','off','TolCon',1e-4); %'active-set', 'trust-region-reflective', 'interior-point', 'interior-point-convex', 'levenberg-marquardt', 'trust-region-dogleg', 'lm-line-search', or 'sqp'.

%x0 = xInit(modelId,:);
x0 = [.5 0];

iaeWeights = [1; 1]; % cost function weigths
manWeights = [1, 0];
res.minSp.iae = iaeTuning;

[minTuningSp,iaeSp, exitFlagSp] =  fmincon(@costFun, x0, [],[],[],[],[0;0],[],@conFun,opt); %@conFun


    fprintf('%0.2f \t \t %.2f  %.2f \t %.2f\t %i \t \n',iaeSp, minTuningSp, msEq, exitFlagSp)
%%
x0  = [0.4; 0.07]; % initial guess
manWeights = [0, 1];

[minTuningD,iaeD, exitFlagD]    =  fmincon(@costFun, x0, [],[],[],[],[0;0],[],@conFun,opt);

    fprintf('%0.2f \t \t %.2f  %.2f \t %.2f\t %i \t \n',iaeD, minTuningD, msEq, exitFlagD)
    fprintf('\n');
    fprintf('\n');
    fprintf('\n');
    fprintf('\n');

res.minSp.iae = iaeTuning;
    
iaeWeights = [iaeSp; iaeD];



%% Generating curve

i = 1; % iteration counter for command window printout
minTuning = []; % matrices for storing results
costTuning = [];
iaeOptTun = [];
costFun05 = [];

msSpace = 1.59; %1.1:0.01:3; % the Ms search range


%x0  = [.4; .07]; % initial guess


a = [0:0.1:.9,.91:0.01:1];


    fprintf('Generating the PO Curve\n')
    fprintf('***********************\n')
    fprintf('\n')
    fprintf('Number of iterations: %i \n', length(msSpace))
    fprintf('\n')
    fprintf('costFun  costFun(.5) \t iae(ys, d) \t minTuning \t manWeights \t exitFlag \t iterations left \n')
    fprintf('-------------------------------------------------------------------------------------------------------\n')
    
opt = optimset('algorithm','active-set','Display','off'); %'active-set', 'trust-region-reflective', 'interior-point', 'interior-point-convex', 'levenberg-marquardt', 'trust-region-dogleg', 'lm-line-search', or 'sqp'.

for run = 1:length(a)

    manWeights = [a(run), 1-a(run)];
    
    msEq = msSpace;

% Optimizing


[minTuningTemp, iaeTuningTemp, exitFlagTuningTemp] = fmincon(@costFun, x0, [],[],[],[],[0;0.001],[],@conFun,opt);

minTuning(:,i) = minTuningTemp; % storing resutlts
costTuning(i) = iaeTuningTemp;
exitFlagTuning(i) = exitFlagTuningTemp;
iaeOptTun(i,:) = iaeTuning;
x0 = minTuningTemp; % updating the initial value for the next Ms step

manWeights = [.5 .5];
costFun05(i)  = costFun(minTuningTemp);

manWeights  = [a(run), 1-a(run)];

fprintf('%0.2f \t %0.2f \t \t %.2f  %.2f \t %.3f  %.3f \t %.2f %.2f\t %i \t \t %i \n',iaeTuningTemp, costFun05(i), iaeTuning, minTuningTemp,  [a(run), 1-a(run)], exitFlagTuningTemp, length(a)-i)

i = i + 1; %updating iteration counter
end

    fprintf('\n')
    fprintf('Calculation Finished!\n')
    fprintf('=====================\n')
toc

%% Ploting

% J versus Ms
figure(1)
plot(a,iaeOptTun)
xlabel('Robustness, $M_s$', 'interpreter','latex')
ylabel('Performance, $J$', 'interpreter','latex')

TV = tvTuning(gp,minTuning);
%% Storing results
res.minSp.tuning = minTuningSp;
res.minSp.costFun = iaeSp;
res.minSp.exitFlag = exitFlagSp;

res.minD.tuning = minTuningD;
res.minD.costFun = iaeD;
res.minD.exitFlag = exitFlagD;

res.minTuning.ms = msSpace;
res.minTuning.tuning = minTuning;
res.minTuning.costFun = costTuning;
res.minTuning.costFun05 = costFun05;
res.minTuning.exitFlag = exitFlagTuning;
res.minTuning.iae = iaeOptTun;
res.minTuning.tv = TV;

modelName={0,1,8,'Inf'};
save(['res',num2str(modelName{modelId}),'.mat'],'res')

%end
