% Task  : Transformed MVs. SISO Extraction casestudy
% compare
% Date  : 24. April 2020
% Author: Simen Bjorvand
% TODO  : 
% Var   : MVs=[]; CVs=[]; States=[]  DV=[].

clc;clear;%close all
%% Plotting options
set(0,'DefaultTextFontName','Times',...
'DefaultTextFontSize',16,...
'DefaultAxesFontName','Times',...
'DefaultAxesFontSize',16,...
'DefaultLineLineWidth',2,...
'DefaultLineMarkerSize',7.75,...
'DefaultStairLineWidth',2)
%% Colors
blue    = [0 0.4470 0.7410];
red     = [0.83 0 0.33];
green   = [0 0.5 0];
indigo  = [0.2930 0 0.5078];
lilac   = [0.7383    0.5898    0.7930];
hotpink = [0.8945    0.4102    0.5898];
cerulean = [0.1094    0.6641    0.8359];

%% Parameters
%initial values for states
par.ME0 = 1e4;
par.MR0 = 1e4; 
par.xAE0 = 3/14;
par.xWE0 = 1/14; 

%initial values for input and disturbance variables
par.F0 = 100; 

par.S0 = 100;
par.xAF0 = 0.3;

%other parameters
par.K = 1/3;
par.Kc1 = -1;
par.Kc2 = -1;
par.E0 = 140;
par.R0 = 60;

%tau, A and transformed input bias
par.tau = par.ME0/par.E0;
v0 = 0;        %update v0 in some help function later



%% calculate A = df/dx (x*,u*,d*)



%% Set up simulation
par.p   = [par.K     %1
           par.Kc1   %2                                
           par.Kc2   %3
           par.E0    %4
           par.R0    %5
           par.ME0   %6
           par.MR0   %7
           0         %8 parameter indicating the structure 
           0];       %9 parameter indicating CV transform  % Parameter list
       
x0      = [par.ME0   %1
           par.MR0   %2
           par.xAE0  %3
           par.xWE0];%4                                    % initial states
       
u0      = par.F0;                                       % physical MVs bias
  
d0      = [par.S0;par.xAF0];                                     % DVs bias

y0      = stateToOutput(x0,u0,d0,par.p);

%% variables which will be varied to make up the 30 cases
tsim    = 400;                                       % [s], simulation time

tstep_y = tsim*0.02;                   % [s], step time for setpoint change

tstep_d = tsim*0.02;                      % [s], step time for disturbances



ys = y0;
ys = 0.5;
 
d1 = 1.0*par.S0;                                           % change in DV 1
d2 = 1.0*par.xAF0;                                         % change in DV 2

ds = [d1;d2];


tauC = 10;


%input gain error
Ku = [1.0;1.0];



%% Simulation. Get PID tuning, and run simulations
[Kc,KI,par] = tunings(par,tauC,1,false);
y0 = stateToOutput(x0,u0,d0,par.p);
out = sim('Extraction');
IAE = out.IAE(end)

colorList = {blue,green,hotpink,lilac};
%% plotting
fig=figure('visible','on');

subplot(311)
plot(out.time,out.x(:,3))
% ylim([0,0.5])
hold on
plot(out.time,out.ys,'--','color',red)
% plot(out.time,out.y)
subplot(312)
plot(out.time,out.u)
hold on
subplot(313)
plot(out.time,out.v)
hold on







