% 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.5
         .5   .5
         4    .5
         0.5   0]; 

clc
tic

global gp msEq iaeWeights manWeights iaeTuning


i = 1; % iteration counter for command window printout


minTuningSp = [];
iaeSp = [];
exitFlagSp = [];

minTuningD = [];
iaeD = [];
exitFlagD = [];

minTuning = []; % matrices for storing results
costTuning = [];
iaeOptTun = [];
exitFlagTuning=[];

msSpace = 1.2; % the Ms search range


modelSpace = [0:0.1:5,6:.25:15,16:.5:50];



fprintf('Optimal Tuning for Ms=%.2f and 1st order model\n',msSpace)
fprintf('**********************************************\n')
fprintf('\n');
fprintf('\n');


for modelId = modelSpace;

x0Sp = [modelId/2 .5];
x0D  = [.2 .62];
x0   = [.2 .62];
    
gp = model(modelId); % getting the model
%x0 = simcImp(gp);
%x0=x0';
%% Finding the iaeWeights 

    fprintf('*Finding Optimal IAE Weights\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,:);


iaeWeights = [1; 1]; % cost function weigths
manWeights = [1, 0];


[minTuningSpTemp,iaeSpTemp, exitFlagSpTemp] =  fmincon(@costFun, x0Sp, [],[],[],[],[0;0],[],@conFun,opt);

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

[minTuningDTemp,iaeDTemp, exitFlagDTemp]    =  fmincon(@costFun, x0D, [],[],[],[],[0;0],[],@conFun,opt);

    minTuningD(:,i) = minTuningDTemp;
    iaeD(i) = iaeDTemp;
    exitFlagD(i) = exitFlagDTemp;
    x0D = minTuningDTemp;

    fprintf('%0.2f \t \t %.2f  %.2f \t %.2f\t %i \t \n',iaeDTemp, minTuningDTemp, msEq, exitFlagDTemp)
    fprintf('\n');


iaeWeights = [iaeSpTemp; iaeDTemp];



%% Generating curve
%x0  = [.4; .07]; % initial guess
manWeights = [.5, .5];
    fprintf('*Optimal Ms=%.2f Solution\n',msSpace)
    fprintf('costFun(%.1f, %.1f) \t minTuning \t lag \t exitFlag \t iterations left \n',manWeights)
    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'.

msEq = msSpace;
    

% Optimizing


[minTuningTemp, iaeTuningTemp, exitFlagTuningTemp] = fmincon(@costFun, x0, [],[],[],[],[0;0],[],@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

    fprintf('%0.2f \t \t \t %.2f  %.2f \t %.2f\t %i \t \t %i \n',iaeTuningTemp, minTuningTemp, modelId, exitFlagTuningTemp, length(modelSpace)-i)
    fprintf('\n');
    fprintf('\n');
    fprintf('\n');
i = i + 1; %updating iteration counter
end

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






%% Ploting

% Tuning versus lagConstant
figure(1)
plot(modelSpace,minTuning(1,:)./minTuning(2,:),[0 8 50],[0 8 8])
xlabel('Lag constant, $\tau_1$', 'interpreter','latex')
ylabel('Integral time, $\tau_I$', 'interpreter','latex')


figure(2)
plot(modelSpace,minTuning(1,:)./modelSpace,[0 50], [0 25]./[0 25])
xlabel('Lag constant, $\tau_1$', 'interpreter','latex')
ylabel('Controller gain, $K_c$', 'interpreter','latex')

%% Storing results
res.minSp.tuning = minTuningSp;
res.minSp.iae = iaeSp;
res.minSp.exitFlag = exitFlagSp;
res.minSp.lagConstant = modelSpace;

res.minD.tuning = minTuningD;
res.minD.iae = iaeD;
res.minD.exitFlag = exitFlagD;
res.minD.lagConstant = modelSpace;

res.minTuning.ms = msSpace;
res.minTuning.tuning = minTuning;
res.minTuning.costFun = costTuning;
res.minTuning.exitFlag = exitFlagTuning;
res.minTuning.iae = iaeOptTun;
res.minTuning.lagConstant = modelSpace;


save(['resModelLagMs120.mat'],'res')

%end
