%% Main simulated MPC file - nonlinear MPC (Casadi)

clc
close all
addpath('C:\Users\amund\OneDrive\Skrivebord\NTNU\TKP4555\Matlab\casadi-windows-matlabR2016a-v3.5.3')
import casadi.*

par = initHC();

T = 60;     % Time horizon
N = 60;     % Sampling intervals
dt = T/N;

%% Initial conditions

x1 = 0.03;       % Overflow concentration - initial value
x2 = 25E-6;       % Underflow concentration - initial value
x0 = [x1;x2];

u0 = 0.35;       % Initial value of the MV (Zo)

% Inital pressure values

P2 = 3*10^5;
P3 = 4*10^5;

%% Time (for plotting)

t = linspace(0, T, N);

%% Initializing values of u and x

xi = zeros(2,N);
ui = zeros(1,N);

%% The first initial values will be given the initial condition

xi(:,1) = x0;
ui(1) = u0;

%% Simulated values for in-flow and concentration (d1 & d2)

Qin1 = [0.0006.*ones(1,N/4) 0.0006.*ones(1,N/4) 0.0006.*ones(1,N/4) 0.0007.*ones(1,N/4)];
Betain1 = [0.0005.*ones(1,N/2),0.0009.*ones(1,N/2)];
Qin = awgn(Qin1, 110);
Betain = awgn(Betain1, 110);

xSP = 30E-6;

Q = 1E6;
R = 0.1;

%% Loop

elapsedTime = [];
for i = 2:N
    disp('Iteration number:')
    disp(i-1)
    PQ = PressFlowRelation(0.5,ui(i-1),P2,P3,Qin(i),par);
    P2 = PQ(:,3);
    P3 = PQ(:,4);
    xi(:,i) = ModelNMPC(xi(:,i-1), ui(i-1), Qin(i), Betain(i), dt, P2, P3);
    tic
    [ui(i)] = SimulatedMultipleShooting(xi(:,i), ui(i-1), Qin(i), Betain(i), dt, Q, R, xSP, P2, P3);
    elapsedTime = [elapsedTime;toc];
end

%% Plotting results

set(groot, 'defaultTextInterpreter','latex');
set(groot, 'defaultAxesTickLabelInterpreter','latex');
set(groot, 'defaultLegendInterpreter','latex');

xL = 20E-6;

figure('visible', 'on')
subplot(4,1,1)
stairs(t, ui.*100, 'b')
% title('MV - Overflow Valve Opening')
ylim([30 50])
ylabel('$Z_O$ [\%]')

subplot(4,1,2)
plot(t, xi(2,:).*1e6, 'b')
hold on
yline(xSP*1e6, '--r')
hold on
yline(xL*1e6, '--r')
% title('CV - Oil Concentration Underflow ($x_2$)')
ylim([0 50])
ylabel('$\beta_{U,o}$ [ppm]')

subplot(4,1,3)
plot(t, Qin.*3600, 'b')
ylim([2.0 2.6])
% title('D1 - Flowrate Inlet')
ylabel('$Q_{in}$ [m$^3$/h]')

subplot(4,1,4)
plot(t, Betain.*1e6, 'b')
% title('D2 - Oil Concentration Inlet')
xlabel('$t$ [s]')
ylim([400 1000]) 
ylabel('$\beta_{in,o}$ [ppm]')

% sgtitle('Simulated MPC Results', 'FontSize', 12)

% saveas(gcf,[pwd '/Figures/SimulatedMPC_2'],'epsc')