% Matlab Example
%Requires MPC toolbox
% Adriana Reyes Lúa
% December 2019
%https://se.mathworks.com/help/mpc/gs/control-of-a-multi-input-single-output-plant.html
%https://se.mathworks.com/help/mpc/ug/plant-models-with-delays.html
%https://se.mathworks.com/help/mpc/gs/design-controller-using-the-command-line.html
%https://se.mathworks.com/help/mpc/ug/optimization-problem.html
%https://se.mathworks.com/help/mpc/ug/qp-solver.html
clear all
%% Times
Ts = 1;                 % sample time 1 min
Tstop = 400;             % simulation time
Tf = round(Tstop/Ts);   % number of simulation steps
P = 100;                % prediction horizon 100
M = 50;                  % control horizon 5
%%  Define plant model
% one measured output, 4 manipulated variables, zero measured disturbance, and one unmeasured disturbance
% state-space from transfer functions
plant=tf({-5,-10,12,8,1},{[8 1],[15 1],[10 1],[5 1],[15 1]},'IODelay',[2,3,3,1,6]);
plantActual=plant; %tf({-5,-10,12,8,1},{[8 1],[15 1],[10 1],[5 1],[15 1]},'IODelay',[2,3,3,1,6]);
plantActual=ss(plantActual);
plantActual=c2d(plantActual,Ts);
%plantActual=absorbDelay(plantActual);
%plant=tf({-5,-10,12,8},{[8 1],[15 1],[10 1],[5 1]},'IODelay',[2,3,3,1]);
%sys = ss(tf({-5,-10,12,8,1},{[8 1],[15 1],[10 1],[5 1],[15 1]},'IODelay',[2,3,3,1,6]));    
sys = ss(plant);    
% discrete time model
model = c2d(sys,Ts);    %Converts continuous-time dynamic system to discrete time
%model=absorbDelay(model);   %Do we have to use this?
% the first 4 signals are MVs and the last one is an unmeasured disturbance
model = setmpcsignals(model,'MV',[1 2 3 4],'UD',5);
plantActual = setmpcsignals(plantActual,'MV',[1 2 3 4],'UD',5);
%model=plantActual;
N = 4;                      %Number of MVs
%% Design MPC object
mpcobj = mpc(model,Ts,P,M); % M is either an integer (<= P) or a vector of blocked moves such that sum(M) <= P.
% Constraints for MV
mpcobj.ManipulatedVariables(1).Min = 0;
mpcobj.ManipulatedVariables(2).Min = 0;
mpcobj.ManipulatedVariables(3).Min = 0;
mpcobj.ManipulatedVariables(4).Min = 0;
mpcobj.ManipulatedVariables(1).Max = 1;
mpcobj.ManipulatedVariables(2).Max = 1;
mpcobj.ManipulatedVariables(3).Max = 1;
mpcobj.ManipulatedVariables(4).Max = 1;
mpcobj.ManipulatedVariables(1).RateMin = -1;
mpcobj.ManipulatedVariables(2).RateMin = mpcobj.ManipulatedVariables(1).RateMin;
mpcobj.ManipulatedVariables(3).RateMin =mpcobj.ManipulatedVariables(1).RateMin;
mpcobj.ManipulatedVariables(4).RateMin = mpcobj.ManipulatedVariables(1).RateMin;
mpcobj.ManipulatedVariables(1).RateMax = 1;
mpcobj.ManipulatedVariables(2).RateMax = mpcobj.ManipulatedVariables(1).RateMax ;
mpcobj.ManipulatedVariables(3).RateMax = mpcobj.ManipulatedVariables(1).RateMax ;
mpcobj.ManipulatedVariables(4).RateMax = mpcobj.ManipulatedVariables(1).RateMax ;
% % Weights for MV
mpcobj.Weights.ManipulatedVariables = [5 1 1 5];
%mpcobj.Weights.ManipulatedVariablesRate = [0.1000 0.1000 0.1000 0.1000];
mpcobj.Weights. OutputVariables = 50; 
%get(mpcobj)

% Add simulation options
%https://se.mathworks.com/help/mpc/ref/mpcsimopt.html
SimOptions = mpcsimopt(mpcobj);

% Define the unmeasured input disturbance model used by the controller.
% mpcobj.Model.Disturbance = tf(sqrt(1000),[1 0]);

%% Plant model mismatch; actual plant model
SimOptions.Model = plantActual;

%% Reference and measured diskturbance
r = zeros(Tf,1);                           % reference signal vector - here we are working on deviations

%Disturbances
t1=10;  d1=20-18;
t2=80;  d2=29-18;
t3=140; d3=24-18;
t4=180; d4=-1-18;
t5=280; d5=17-18;
t6=350; d6=22-18;
d = [zeros(t1,1);d1*ones(t2-t1,1);d2*ones(t3-t2,1);d3*ones(t4-t3,1);d4*ones(t5-t4,1);d5*ones(t6-t5,1);d6*ones(Tstop-t6,1)];      % unmeasured disturbance signal vector
%d = [zeros(Tf/2,1);15*ones(Tf/2,1)];     
SimOptions.Unmeas = d;                     % unmeasured input disturbance signal

% 
%% Simulate
%https://se.mathworks.com/help/mpc/ref/mpc.sim.html
%sim(mpcobj,Tf,r,SimOptions)
 [y,t,u,xp] = sim(mpcobj,Tf,r,SimOptions);             %output, time, MV;  states (initial model was a tf, so states have no physical  meaning

 results_size=length(y);
 T_MPC=18*ones(results_size,1)+y;
 Tamb_MPC=18*ones(results_size,1)+d;
 u1_MPC=u(:,1);
 u2_MPC=u(:,2);
 u3_MPC=u(:,3);
 u4_MPC=u(:,4);
 IAE=cumsum(abs(y));
 
%%
% figure
% x=linspace(1, size(d,1),size(d,1))
% plot(x,d)


 %Plot
PlotRoomMPC
% %Plot
% figure
% subplot(2,1,1)
% plot(0:Tf-1,y,0:Tf-1,r)
% title('Output')
% grid
% subplot(2,1,2)
% plot(0:Tf-1,u)
% title('Input')
% grid
% 
