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 = [S.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 = 1.3;
Theta_mid = 1.0;
Theta_down = 0.7;

% %% 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_R =  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);
    
%% Process constraint : Same for all senarios.
        %%% EQUALITY CONSTRAINT
            %Demand constraint
        S_Aeq_D = kron(eye(t),[1 1 S0.eff_d 0 0 0]);
        S_Beq_D = 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);
        % Aeq & Beq integration
        S_Aeq = [S_Aeq_D; S_Aeq_T];
        S_Beq = [S_Beq_D; S_Beq_T];
        
        %%% 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];
        
%% PRICE SCENARIOS : 9 trees.
f_form = [-0.02 1 0 -0.01 1 0]
% Scenario. 01 : Present-UP-UP-Costant
        pt.S1_Price_horizon = [Price_R(1) (Theta_up*Price_R(2)) Theta_up*(Theta_up*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S1_f = kron(pt.S1_Price_horizon,f_form);
        
% Scenario. 02 : Present-UP-MID-Costant
        pt.S2_Price_horizon = [Price_R(1) (Theta_up*Price_R(2)) Theta_mid*(Theta_up*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S2_f = kron(pt.S2_Price_horizon,f_form);

% Scenario. 03 : Present-UP-DOWN-Costant
        pt.S3_Price_horizon = [Price_R(1) (Theta_up*Price_R(2)) Theta_down*(Theta_up*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S3_f = kron(pt.S3_Price_horizon,f_form);

% Scenario. 04 : Present-MID-UP-Costant
        pt.S4_Price_horizon = [Price_R(1) (Theta_mid*Price_R(2)) Theta_up*(Theta_mid*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S4_f = kron(pt.S4_Price_horizon,f_form);
        
% Scenario. 05 : Present-MID-MID-Costant
        pt.S5_Price_horizon = [Price_R(1) (Theta_mid*Price_R(2)) Theta_mid*(Theta_mid*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S5_f = kron(pt.S5_Price_horizon,f_form);

% Scenario. 06 : Present-MID-DOWN-Costant
        pt.S6_Price_horizon = [Price_R(1) (Theta_mid*Price_R(2)) Theta_down*(Theta_mid*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S6_f = kron(pt.S6_Price_horizon,f_form);

% Scenario. 07 : Present-DOWN-UP-Costant
        pt.S7_Price_horizon = [Price_R(1) (Theta_down*Price_R(2)) Theta_up*(Theta_down*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S7_f = kron(pt.S7_Price_horizon,f_form);

% Scenario. 08 : Present-DOWN-MID-Costant
        pt.S8_Price_horizon = [Price_R(1) (Theta_down*Price_R(2)) Theta_mid*(Theta_down*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S8_f = kron(pt.S8_Price_horizon,f_form);
        
% Scenario. 09 : Present-DOWN-DOWN-Costant
        pt.S9_Price_horizon = [Price_R(1) (Theta_down*Price_R(2)) Theta_down*(Theta_down*Price_R(3))*ones(1,t-2)];
        %%% Objective function
        S9_f = kron(pt.S9_Price_horizon,f_form);

%% 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 = kron(eye(9),S_Aeq);
        Aeq = [Aeq; na_Aeq];
        Beq = kron(ones(9,1),S_Beq);
        Beq = [Beq; na_Beq];
        A = kron(eye(9),S_A);
        B = kron(ones(9,1),S_B);
        f = [S1_f S2_f S3_f S4_f S5_f S6_f S7_f S8_f S9_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)];
        
        % 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;
        
        %For plotting
        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_P_scn')
