function [T_violation,J_end] = main(inmode,involume,inplot,inmatrix,inbo)
% main.m runs the simulink model hwtank.mdl with different temperature 
% set points and starting conditions.
%
%Function arguments:
% inmode:   Which case to simulate:{1 = constant set point case,
%                                   2 = optimized case,
%                                   3 = two-phase case}
% involume:  The maximum volume of water contained in the tank
% inplot:    Whether to plot state profiles (1) or not(0) 
% inmatrix:  Contains the two phase procedure, only matters if mode = 3 
% inbo:      The back-off, (back off = set point-constraint) defines
%            how far away from the constraint the smallest set points are
%
%Function output:
% T_violation:  The number of hours that T<T_hws in the simulation
% J_end:        The total cost at the end of simulation
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

simdays = 3; %controls the amount of days to simulate for.

%Default arguments: 
%Reads the function arguments. If not specified the defualt args are used
mode = 1; V_max = 200; ploton=0; backoff=0;
rulem=[1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1];
if nargin > 0, mode = inmode;end
if nargin > 1, V_max = involume;end
if nargin > 2, ploton = inplot;end
if nargin > 3 
    if isempty(inmatrix)
                  % if the matrix is empty the default one should be used
    else
        rulem = inmatrix;     % otherwise use the incoming matrix instead
    end
end;
if nargin > 4, backoff = inbo;end

warning off
%clc
%clear all
close all

%% Initializing
global par
par.c_p  = 4.19;                      % heat capacity of water [kJ/(kgC)]
par.rho  = 1;                                   % density of water [kg/l]
par.Q_loss = 0;                              % loss of heat from the tank

%disturbances and set point limitations
T_hws  = 50;      % Temperature wanted on the hot water reaching the user
T_cw   = 5;                               % Temperature of the cold water
T_smax = 90;                             % Maximum tepmperature set point
T_smin = T_hws+backoff;           % adjusts minimum temperature set point  
V_min  = 50;                                  % minimum water in the tank
%V_max is defined on line 24

totfailures = [];
Ekfailures = [];
Eendy= [];

%simulation parameters (initial conditions)
T_s = T_smax;        %For base case
par.V = V_max;       %volume starting point
par.T = T_smax;      %Temperature starting point

%Controller tunings
Kc1  = 3;    % gain for controller of volume
Kc2  = 0.21; % gain for controller of temperature
tau1 = 1/80; % Really 80 but simulink uses tuning paramer I=1/tau_i in its
tau2 = 1/80; % PI controllers.
%input limitations
q_in_min = 0; % dm^3/min
q_in_max = 10;% dm^3/min
Q_min    = 0; % kJ/s
Q_max    = 5; % kJ/s

%Generating the demand/price and the setpoints:
%
% In general we prefer to load a pregenerated profiles (longprofile)
% to ensure that we get comparable results for all experiments.
% For the hot water setpoint T_hws and cold water temperature T_cw a 
% constant value is wanted. These are created using the "ext" variable. 
% The electricity price are loaded from a price.csv file.
%
test=load('langprofil.mat');
a = test.testlong;   %contains the pregrnerated demand and price profiles
profile.time=(a.time'+0.167)*3600; 
ext = ones(length(a.time),1);    % used to extend the constant set points

%Generating variable containing all disturbances for the given period:
profile.signals.values =  [a.flow, T_hws.*ext, T_cw.*ext, a.price];

simlength = 24*simdays;                 % number of hours to simulate for

% Load pregenerated average energy demand and average price profiles, 
% this is just used in case II:
av.demand = csvread('average.csv',0,0,[0,0,23,0]);       % q_hw, dm^3/min
predictedprice = getPriceHour(4,1,4,31);    % 31 because we need the full
                      % prediction horizon at the end of the 30th day too
switch mode  
    case 1                      % CONSTANT TEMPERATURE CASE, T_s = T_smax
        T_s =T_smax;                             % constant max set point
        
        % Define the matrices used to store state variables
        ptime_base=[]; %changing time from seconds to hours.
        sp_base=[]; % extracting setpoints
        y_base=[]; % T, V and modified cost fx
        d_base=[]; % disturbances
        u_base=[]; % inputs
        
        mdl = 'hwtank.mdl';
        load_system(mdl);
        set_param('hwtank/Temp_s', 'Value', num2str(T_s));
        out1 = sim(mdl,'SrcWorkspace','current','StopTime', ...
            (num2str(3600*simlength)), 'SaveFinalState', 'on', ...
            'LoadInitialState', 'off', 'SaveCompleteFinalSimState', ...
            'on','FinalStateName', 'xFinal');
        
        %update the state variables
        ptime_base=[out1.get('time')/3600]; %time from seconds to hours.
        sp_base=[out1.get('sp')]; %extracting setpoints
        y_base=[out1.get('y')]; % T, V and modified cost fx
        d_base=[out1.get('d')]; % disturbances
        u_base=[out1.get('u')]; % inputs
        
        if ploton
            figure(3)
            subplot(311)
            plot(ptime_base,d_base(:,1))
            set(gca,'FontSize',14)
            title('Hot water demand')
            ylabel('q_o_u_t, dm^3/min')
            hold on
            
            subplot(312)
            plot(ptime_base,u_base(:,2),'red')
            set(gca,'FontSize',14)
            ylabel('q_i_n, dm^3/min')
            hold on
            
            subplot(313)
            plot(ptime_base,y_base(:,1))
            set(gca,'FontSize',14)
            ylabel('V, dm^3')
        end
        
        % If a 30 day simulation, save the data points to .mat file
        if simdays == 30 && backoff
            if exist(['_datasetsV',num2str(V_max),'.mat'],'file')
                save(['_datasetsV',num2str(V_max)],'ptime_base',...
                'sp_base','y_base','d_base','u_base','-append');
            else 
                 save(['_datasetsV',num2str(V_max)],'ptime_base',...
                'sp_base','y_base','d_base','u_base');
            end
        end
                
        % Variables for calculating T_violation and J_end
        listlength= length(y_base(:,2));
        yvar = y_base;
    case 2        % OPTIMIZED CASE, T_s = changing according to predictor
        
        %Bounds
        Emax = par.rho*par.c_p*V_max*(T_smax - T_cw) %kJs 
        Emin = par.rho*par.c_p*V_min*(T_hws - T_cw); %kJ
        E0   = Emax; % intial condition
        Qmax = Q_max;
        
        
        %plotting
        if ploton
            figure(1);   % start the MPC-plot (must be done outside loop)
        end
        %Make lists to store state variables
        ptime_optim=[]; %changing time from seconds to hours.
        sp_optim=[]; %extracting setpoints
        y_optim=[]; % T, V and modified cost fx
        d_optim=[]; % disturbances
        u_optim=[]; % inputs
        
        
        %% iteration
        for i=1:simlength
            Ns=24; % size of horizon (in sample times)
            j = i;
            while j>24
                j= j-24
            end
            % shifting the average demand and price function
            Qdemand = [av.demand(j:Ns);av.demand(1:j-1)]*(par.rho...
                      *par.c_p*(T_hws -T_cw))/60 ;%  kJ/s
            prices  = [predictedprice(i:Ns+(i-1))];
            %startposition for both price and demand depends on time of day
            dT      = 3600;  % sampling time of 1hour
            
            %% formulating the optimization problem
            constraints=[]; % vector containing all constraints
            Qk=sdpvar(Ns,1); % decision variables as a vector of size Ns
            Tri=tril(ones(Ns,Ns),0);
            %state trajectory: Equivalent to computing Ek+1=Ek+(Qk-Qdk)*dT
            %k=0...Ns
            Ek=Tri*(Qk*dT-Qdemand*dT)+E0;
            %update the constraints
            constraints=constraints+set(Emin<=Ek<=Emax);
            constraints=constraints+set(Ek(end)==Emax);
            constraints=constraints+set(0<=Qk<=Qmax);
            
            cost=prices'*Qk*dT;  % cost function, what we want to opt!!

            %% solve optimization
            opts = sdpsettings('solver','sedumi','sedumi.numtol',1e-3);
            opts.verbose = 0;
            tic
            sol=solvesdp(constraints,cost,opts) %solving the "MPC"
            toc
            if sol.problem == 0
                %filler, don't need to do anything if it works
            else % stop if the solver doesnt find a solution
                display('Hmm, something went wrong!');
                sol.info
                yalmiperror(sol.problem)
            end
            cost=double(cost) %the results from the optimization
            Qv=double(Qk);
            Ev=double(Ek);
            
            %%Transform from energy set points to temperature set point
            Ts=double(Ev/(par.rho*par.c_p*par.V)+ T_cw);
            %Modify result if reccomended set points breaks constraints
            if Ts(1) > T_smax
                T_s = T_smax
            elseif Ts(1)< T_smin
                T_s = T_smin
            else
                T_s = Ts(1)
            end
            Volume = par.V % for bug testing purposes
            
            %% Simulate with set point
            mdl = 'hwtank.mdl';
            load_system(mdl);
            if i == 1
                %Simulate until end of step. Save state.
                set_param('hwtank/Temp_s', 'Value', num2str(T_s));
                out3 = sim(mdl,'SrcWorkspace','current','StopTime', ...
                    (num2str(dT*(i+4))), 'SaveFinalState', 'on', ...
                    'LoadInitialState', 'off', ...
                    'SaveCompleteFinalSimState', 'on',...
                    'FinalStateName', 'xFinal');
                y_step = out3.get('y'); % the y variable (T,V and cost)
                
            else
                %Load SimState and run until end of step.
                %The start time value must remain unchanged.
                set_param('hwtank/Temp_s', 'Value', num2str(T_s));
                xFinal = out3.get('xFinal');
                out3= sim(mdl, 'SrcWorkspace','current', 'StopTime', ...
                    num2str(dT*(i+4)), 'SaveFinalState', 'on', ...
                    'LoadInitialState', 'on', 'InitialState','xFinal',...
                    'SaveCompleteFinalSimState', 'on',...
                    'FinalStateName', 'xFinal');
                y_step = out3.get('y'); % the y variable (T,V and cost)
            end
            % extracting initial conditions for next iter.
            Tnew = y_step(end,2);
            Vnew = y_step(end,1);
            Enew = Vnew*par.rho*par.c_p*(Tnew -T_cw);
            E0   = Enew;
            par.V    = Vnew;
            
            % scale, so Ek stays within the interval [0 1]
            Ev=(Ev-Emin)/(Emax-Emin)
            if ploton
                subplot(411)
                stairs((1+(i-1):Ns+(i-1)),Ev)
                xlabel('Time,h')
                ylabel('Energy level')
                hold on
                subplot(412)
                stairs((1:Ns),Qv)
                ylabel('Heat power')
                xlabel('Time,h')
                
                subplot(413)
                stairs((1+i:Ns+i),prices,'Color',[i/simlength 0 1])
                xlabel('Time,h')
                ylabel('Energy prices')
                hold on
                subplot(414)
                stairs((1+(i-2):Ns+(i-2)),Ts,'Color',[i/simlength 0 1])
                xlabel('Time,h')
                ylabel('Teperature setpoint')
                hold on
            end
            %changing time from seconds to hours.
            ptime_optim=[ptime_optim;(out3.get('time')/3600)];
            sp_optim=[sp_optim;out3.get('sp')]; %extracting setpoints
            y_optim=[y_optim;out3.get('y')]; % T, V and modified cost fx
            d_optim=[d_optim;out3.get('d')]; % disturbances
            u_optim=[u_optim;out3.get('u')]; % inputs
            
        end
        
        if ploton
            figure(2)
            subplot(611)
            plot(ptime_optim, sp_optim(:,1),'green',...
                 ptime_optim, y_optim(:,1))
            set(gca,'FontSize',14)
            title('Model implementation ')
            ylabel('V, dm^3')
            
            subplot(612)
            plot(ptime_optim,u_optim(:,2),'red')
            set(gca,'FontSize',14)
            ylabel('q_i_n, dm^3/min')
            hold on
            
            subplot(613)
            plot(ptime_optim, sp_optim(:,2),'green', ...
                 ptime_optim,y_optim(:,2))
            set(gca,'FontSize',14)
            ylabel('T, ^oC')
            hold on
            
            subplot(614)
            plot(ptime_optim,u_optim(:,1),'red')
            set(gca,'FontSize',14)
            ylabel('Q, kW')
            hold on
            
            subplot(615)
            plot(ptime_optim,d_optim(:,4))
            set(gca,'FontSize',14)
            ylabel('P, EUR/kWh')
            hold on
            
            subplot(616)
            plot(ptime_optim,y_optim(:,3))
            set(gca,'FontSize',14)
            ylabel('J, EUR')
            % hold on
            
            figure(3)
            subplot(311)
            plot(ptime_optim,d_optim(:,1))
            set(gca,'FontSize',14)
            title('Hot water demand')
            ylabel('q_o_u_t, dm^3/min')
            hold on
            
            subplot(312)
            plot(ptime_optim,u_optim(:,2),'red')
            set(gca,'FontSize',14)
            ylabel('q_i_n, dm^3/min')
            hold on
            
            subplot(313)
            plot(ptime_optim,y_optim(:,1))
            set(gca,'FontSize',14)
            ylabel('V, dm^3')
        
        end
        if simdays == 30 && backoff
            if exist(['_datasetsV',num2str(V_max),'.mat'],'file')
                save(['_datasetsV',num2str(V_max)],'ptime_optim',...
                'sp_optim','y_optim','d_optim','u_optim','-append');
            else
                 save(['_datasetsV',num2str(V_max)],'ptime_optim',...
                'sp_optim','y_optim','d_optim','u_optim');
            end
        end
        T_s = 90; % return simulink file to original condition 
        set_param('hwtank/Temp_s', 'Value', num2str(T_s));
        
        %%Calculating the final cost and temperature violation
        listlength= length(y_optim(:,2)); %length of dataset
        yvar=y_optim;        
        
    case 3                      %%Two phase case, T_s = either max or min
        
        %plotting
        ptime_sim=[]; %changing time from seconds to hours.
        sp_sim=[]; %extracting setpoints
        y_sim=[]; % T, V and modified cost fx
        d_sim=[]; % disturbances
        u_sim=[]; % inputs
        
        
        %% iteration
        for i=1:simlength
            Ns=24; % size of horizon (in sample times)
            j = i; % the hour of the day
            while j>24
                j= j-24;
            end            
            if rulem(j)==1             % determening the course of action
                T_s= T_smax;
            else
                T_s = T_smin;
            end
            %Running the Simulink model
            mdl = 'hwtank.mdl';
            load_system(mdl);
            if i == 1 % if the very first step of simulation..
                %Simulate until end of step and save the final state:
                set_param('hwtank/Temp_s', 'Value', num2str(T_s));
                out3 = sim(mdl,'SrcWorkspace','current','StopTime', ...
                    (num2str((i+4)*3600)), 'SaveFinalState', 'on', ...
                    'LoadInitialState', 'off', ...
                    'SaveCompleteFinalSimState', 'on',...
                    'FinalStateName', 'xFinal');
            else
                %Load the SimState and run for an additional step. 
                %The start time value must remain constant
                set_param('hwtank/Temp_s', 'Value', num2str(T_s));
                xFinal = out3.get('xFinal');
                out3 = sim(mdl, 'SrcWorkspace','current','StopTime', ...
                    num2str((i+4)*3600), 'SaveFinalState', 'on', ...
                    'LoadInitialState', 'on', 'InitialState','xFinal',...
                    'SaveCompleteFinalSimState', 'on',...
                    'FinalStateName', 'xFinal');
            end
            %update the state variables after each simulation
            ptime_sim=[ptime_sim;(out3.get('time')/3600)]; % sec to hours
            sp_sim=[sp_sim;out3.get('sp')];        % extracting setpoints
            y_sim=[y_sim;out3.get('y')];      % T, V and modified cost fx
            d_sim=[d_sim;out3.get('d')];                   % disturbances
            u_sim=[u_sim;out3.get('u')];                         % inputs
            
        end
        
        % variables for calculating T_violation and J_end
        listlength= length(y_sim(:,2));
        yvar=y_sim;
        
        %saving if serious
        if simdays == 30 && backoff && isempty(inmatrix)
            if exist(['_datasetsV',num2str(V_max),'.mat'],'file')
                save(['_datasetsV',num2str(V_max)],'ptime_sim',...
                'sp_sim','y_sim','d_sim','u_sim','-append');
            else
                 save(['_datasetsV',num2str(V_max)],'ptime_sim',...
                'sp_sim','y_sim','d_sim','u_sim');
            end
        end

    otherwise % you tried to run a case that didn't exist
        error(['Mode = ',num2str(mode),...
        '. The only implemented modes are 1,2 and 3.'])
   
end

%%calculating the temperature violation and total cost.
n = 0;
for k=1:listlength
    if yvar(k,2) < 50
        n=n+1;
    end
end
T_violation= (n/listlength)*simlength                % hours of violation
J_end = yvar(end,3)                                                 % EUR

T_s = T_smax;                % return simulink file to original condition
set_param('hwtank/Temp_s', 'Value', num2str(T_s));
end

