close all
clear all
clc

%% Load
addpath('../Exogenous_information')
addpath('../P01_PFA')
load('sample_path.mat')

%% Optimum calculation

%initial setup
S0 = S0(); 
S = S0.S;

%Scenario number
n_scenario = size(sample_path.D,1);
%total time horizon
N = size(sample_path.D,2);

%Open loop optimization time horizon
t = 20;

%Extention of sample path
sample_path.E = [sample_path.E sample_path.E(1,1:t+1)];

%Demand model without stochastic value used for prediction
z = 0:N-1;
D_mean = 200;
amplitude = 50;
Dmin = 100;
Dmax = 300;
D = D_mean + amplitude * sin(2*2*pi*z/(N-1) -pi/2);
D = min(max(D,Dmin),Dmax);
D = [S.D D D(1,1:t+1)];

%% setting

%Price_scneario parameters.
Theta_up = 20;
Theta_mid = 0;
Theta_down = -20;

% %% simulation
Cost2 =[];
for w = 1:1%n_scenario
% initial states
S = S0.S;

%Plotting Arrays
xSim = [];
uSim = [];
timeSim = [];

for k = 1:N

        %exogenous information model
        Price_horizon =  normrnd(0,50,1,t) + S.P;
        Demand_horizon = [S.D D(1,k+1:k+t-1)];
        Energy_horizon = sample_path.E(1,k:k+t-1);
        
%% Demand SCENARIOS : 9 trees.
        dt.S1_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_up) (Demand_horizon(3)+2*Theta_up) (Demand_horizon(4:t)+2*Theta_up)];
        dt.S2_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_up) (Demand_horizon(3)+Theta_up+Theta_mid) (Demand_horizon(4:t)+Theta_up+Theta_mid)]; 
        dt.S3_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_up) (Demand_horizon(3)+Theta_up+Theta_down) (Demand_horizon(4:t)+Theta_up+Theta_down)];
        dt.S4_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_mid) (Demand_horizon(3)+Theta_mid+Theta_up) (Demand_horizon(4:t)+Theta_mid+Theta_up)];
        dt.S5_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_mid) (Demand_horizon(3)+Theta_mid+Theta_mid) (Demand_horizon(4:t)+Theta_mid+Theta_mid)];
        dt.S6_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_mid) (Demand_horizon(3)+Theta_mid+Theta_down) (Demand_horizon(4:t)+Theta_mid+Theta_down)];
        dt.S7_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_down) (Demand_horizon(3)+Theta_down+Theta_up) (Demand_horizon(4:t)+Theta_down+Theta_up)];
        dt.S8_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_down) (Demand_horizon(3)+Theta_down+Theta_mid) (Demand_horizon(4:t)+Theta_down+Theta_mid)];
        dt.S9_Demand_horizon = [Demand_horizon(1) (Demand_horizon(2)+Theta_down) (Demand_horizon(3)+Theta_down+Theta_down) (Demand_horizon(4:t)+Theta_down+Theta_down)];        

%% Process constraint : Same for all senarios.
        %%% EQUALITY CONSTRAINT
            %Demand constraint => when eqaulity : infeasible => inequality.
        S_Aeq_D = kron(eye(t),[1 1 S0.eff_d 0 0 0]);
        S_Beq_D = [dt.S1_Demand_horizon'
            dt.S2_Demand_horizon'
            dt.S3_Demand_horizon'
            dt.S4_Demand_horizon'
            dt.S5_Demand_horizon'
            dt.S6_Demand_horizon'
            dt.S7_Demand_horizon'
            dt.S8_Demand_horizon'
            dt.S9_Demand_horizon'];
        
            %Transition function
        S_Aeq_T = kron(tril(ones(t)),[0 0 1 -S0.eff_c -S0.eff_c 0])+kron(eye(t),[0 0 0 0 0 1]);
        S_Beq_T = S.R*ones(t,1);
        
        %%% INEQUALITY CONSTRAINTS : Ax<=b
            %Charging limitation : WS+GS+S <=R.max
        S_A_1 = kron(eye(t), [0 0 0 1 1 1]);
        S_B_1 = S0.R_max*ones(t,1);
            %Discharging limitation : Q,SD < amount in Storage
        S_A_2 = kron(eye(t),[0 0 1 0 0 -1]);
        S_B_2 = zeros(t,1);
            %Charing rate limitation
        S_A_3 = kron(eye(t),[0 0 0 1 1 0]);
        S_B_3 = S0.rate_c*ones(t,1);
            %Discharing rate limitation
        S_A_4 = kron(eye(t),[0 0 1 0 0 0]);
        S_B_4 = S0.rate_d*ones(t,1);
            %Waste incinerator : Heat supply amount
        S_A_5 = kron(eye(t),[1 0 0 1 0 0]);
        S_B_5 = Energy_horizon';
        % A&B integration
        S_A = [S_A_1; S_A_2; S_A_3; S_A_4; S_A_5];
        S_B = [S_B_1; S_B_2; S_B_3; S_B_4; S_B_5];
        
        S_f = kron(Price_horizon,[-0.02 1 0 -0.01 1 0]);
        
%% Non-anticipativity constraints
nas = [1 1 1 1 1 0];
naz = zeros(1,6);

na_A1 = [nas kron(ones(1,t-1),naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*7), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*1), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*6), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*2), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*5), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*3), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*4), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*4), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*3), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*5), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*2), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*6), naz) -nas kron(ones(1,t-1),naz) kron(ones(1,t*1), naz)
    nas kron(ones(1,t-1),naz) kron(ones(1,t*7), naz) -nas kron(ones(1,t-1),naz) ];

na_A2 = [naz nas kron(ones(1,t-2),naz) naz -nas kron(ones(1,t-2),naz) kron(ones(1,t*7),naz)
        naz nas kron(ones(1,t-2),naz) kron(ones(1,t),naz) naz -nas kron(ones(1,t-2),naz) kron(ones(1,t*6),naz)
        kron(ones(1,t*3),naz) naz nas kron(ones(1,t-2),naz) naz -nas kron(ones(1,t-2),naz) kron(ones(1,t*4),naz)
        kron(ones(1,t*3),naz) naz nas kron(ones(1,t-2),naz) kron(ones(1,t),naz) naz -nas kron(ones(1,t-2),naz) kron(ones(1,t*3),naz)
        kron(ones(1,t*6),naz) naz nas kron(ones(1,t-2),naz) naz -nas kron(ones(1,t-2),naz) kron(ones(1,t),naz)
        kron(ones(1,t*6),naz) naz nas kron(ones(1,t-2),naz) kron(ones(1,t),naz) naz -nas kron(ones(1,t-2),naz)];
    

na_Aeq = [na_A1; na_A2];
na_Beq = zeros(size(na_Aeq,1),1);

%% Integration of linprog factors
        Aeq_T = kron(eye(9),S_Aeq_T);
        Aeq_D = kron(eye(9),S_Aeq_D);
        Aeq = [Aeq_D; Aeq_T; na_Aeq];
        
        Beq = [S_Beq_D; kron(ones(9,1),S_Beq_T)];
        Beq = [Beq; na_Beq];
        
        A = kron(eye(9),S_A);
        B = kron(ones(9,1),S_B);
        f = kron(ones(1,9),S_f);
        lb = zeros(6*9*t,1);
        ub = ones(6*9*t,1)*inf;
       
%%        %%% Optimization
        w_opt = linprog(f,A,B,Aeq,Beq,lb,ub);
        
        % Data store from openloop optimization
        u.S1.WD = w_opt(1:6:6*t);
        u.S1.GD = w_opt(2:6:6*t);
        u.S1.SD = w_opt(3:6:6*t);
        u.S1.WS = w_opt(4:6:6*t);
        u.S1.GS = w_opt(5:6:6*t);
        u.S1.R = w_opt(6:6:6*t);
        
        u.S2.WD = w_opt(6*t+1:6:6*2*t);
        u.S2.GD = w_opt(6*t+2:6:6*2*t);
        u.S2.SD = w_opt(6*t+3:6:6*2*t);
        u.S2.WS = w_opt(6*t+4:6:6*2*t);
        u.S2.GS = w_opt(6*t+5:6:6*2*t);
        u.S2.R = w_opt(6*t+6:6:6*2*t);
        
        u.S3.WD = w_opt(6*2*t+1:6:6*3*t);
        u.S3.GD = w_opt(6*2*t+2:6:6*3*t);
        u.S3.SD = w_opt(6*2*t+3:6:6*3*t);
        u.S3.WS = w_opt(6*2*t+4:6:6*3*t);
        u.S3.GS = w_opt(6*2*t+5:6:6*3*t);
        u.S3.R = w_opt(6*2*t+6:6:6*3*t);
        
        u.S4.WD = w_opt(6*3*t+1:6:6*4*t);
        u.S4.GD = w_opt(6*3*t+2:6:6*4*t);
        u.S4.SD = w_opt(6*3*t+3:6:6*4*t);
        u.S4.WS = w_opt(6*3*t+4:6:6*4*t);
        u.S4.GS = w_opt(6*3*t+5:6:6*4*t);
        u.S4.R = w_opt(6*3*t+6:6:6*4*t);
        
        u.S5.WD = w_opt(6*4*t+1:6:6*5*t);
        u.S5.GD = w_opt(6*4*t+2:6:6*5*t);
        u.S5.SD = w_opt(6*4*t+3:6:6*5*t);
        u.S5.WS = w_opt(6*4*t+4:6:6*5*t);
        u.S5.GS = w_opt(6*4*t+5:6:6*5*t);
        u.S5.R = w_opt(6*4*t+6:6:6*5*t);
        
        u.S6.WD = w_opt(6*5*t+1:6:6*6*t);
        u.S6.GD = w_opt(6*5*t+2:6:6*6*t);
        u.S6.SD = w_opt(6*5*t+3:6:6*6*t);
        u.S6.WS = w_opt(6*5*t+4:6:6*6*t);
        u.S6.GS = w_opt(6*5*t+5:6:6*6*t);
        u.S6.R = w_opt(6*5*t+6:6:6*6*t);
        
        u.S7.WD = w_opt(6*6*t+1:6:6*7*t);
        u.S7.GD = w_opt(6*6*t+2:6:6*7*t);
        u.S7.SD = w_opt(6*6*t+3:6:6*7*t);
        u.S7.WS = w_opt(6*6*t+4:6:6*7*t);
        u.S7.GS = w_opt(6*6*t+5:6:6*7*t);
        u.S7.R = w_opt(6*6*t+6:6:6*7*t);
        
        u.S8.WD = w_opt(6*7*t+1:6:6*8*t);
        u.S8.GD = w_opt(6*7*t+2:6:6*8*t);
        u.S8.SD = w_opt(6*7*t+3:6:6*8*t);
        u.S8.WS = w_opt(6*7*t+4:6:6*8*t);
        u.S8.GS = w_opt(6*7*t+5:6:6*8*t);
        u.S8.R = w_opt(6*7*t+6:6:6*8*t);
        
        u.S9.WD = w_opt(6*8*t+1:6:6*9*t);
        u.S9.GD = w_opt(6*8*t+2:6:6*9*t);
        u.S9.SD = w_opt(6*8*t+3:6:6*9*t);
        u.S9.WS = w_opt(6*8*t+4:6:6*9*t);
        u.S9.GS = w_opt(6*8*t+5:6:6*9*t);
        u.S9.R = w_opt(6*8*t+6:6:6*9*t);
        
        % Take an action
        uk = [u.S1.WD(1);u.S1.GD(1);u.S1.SD(1);u.S1.WS(1);u.S1.GS(1)];
        
        S.D-uk(1)-uk(2)-uk(3)
        
        % Exogenous information realization
        S.P = sample_path.P(w,k);
        S.D = sample_path.D(w,k);
        
        %Simulating the plant behavior during dt
        S.R = S.R + [0 0 -1 S0.eff_c S0.eff_c]*uk;
        
        %Data storing
        xSim = [xSim, S.R];
        uSim = [uSim, uk];
                
end

Cost1 = sum(sample_path.P(w,:).*(uSim(2,:) + uSim(5,:)));
Cost2 = [Cost2; Cost1];
end
Cost = mean(Cost2);

for i = 1:size(Cost2,1)
Avg_cost_cum(i) = mean(Cost2(1:i));
end

save('P04_D_scn')
