close all
clear all
clc


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

%% Optimum calculation

%initial setup
S0 = S0();
S=S0.S;
%scenario number
n_scenario = size(sample_path.D,1);

%Extention of sample path
sample_path.E = [S.E sample_path.E];
sample_path.D = [S.D*ones(n_scenario,1) sample_path.D]; 
sample_path.P = [S.P*ones(n_scenario,1) sample_path.P]; 

%time horizon
t = size(sample_path.D,2);

tic
T_Cost = [];
for w = 1:n_scenario
    w
%Choosing the price policy
Price_horizon = sample_path.P(w,:);
Demand_horizon = sample_path.D(w,:);
Energy_horizon = sample_path.E(1,:);

%optimize the system
        %%% EQUALITY CONSTRAINT
            %Demand constraint
        Aeq_D = kron(eye(t),[1 1 S0.eff_d 0 0 0]);
        Beq_D = Demand_horizon';
            %Transition function
        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]);
        Beq_T = S.R*ones(t,1);
        % Aeq & Beq integration
        Aeq = [Aeq_D;Aeq_T];
        Beq = [Beq_D;Beq_T];
        
        %%% INEQUALITY CONSTRAINTS : Ax<=b
            %Charging limitation : WS+GS+S <=R.max
        A_1 = kron(eye(t), [0 0 0 1 1 1]);
        B_1 = S0.R_max*ones(t,1);
            %Discharging limitation : Q,SD < amount in Storage
        A_2 = kron(eye(t),[0 0 1 0 0 -1]);
        B_2 = zeros(t,1);
            %Charing rate limitation
        A_3 = kron(eye(t),[0 0 0 1 1 0]);
        B_3 = S0.rate_c*ones(t,1);
            %Discharing rate limitation
        A_4 = kron(eye(t),[0 0 1 0 0 0]);
        B_4 = S0.rate_d*ones(t,1);
            %Waste incinerator : Heat supply amount
        A_5 = kron(eye(t),[1 0 0 1 0 0]);
        B_5 = Energy_horizon';
        % A&B integration
        A = [A_1;A_2;A_3;A_4;A_5];
        B = [B_1;B_2;B_3;B_4;B_5];
        
        %%% Objective function
        f = kron(Price_horizon,[0 1 0 0 1 0]);
        
        %%% Boundaries - lower and upper
        lb = zeros(6*t,1);
        ub = ones(6*t,1)*inf;
       
        %%% Optimization
        w_opt = linprog(f,A,B,Aeq,Beq,lb,ub);
        
        % Data store from openloop optimization
        u.WD = w_opt(1:6:6*t);
        u.GD = w_opt(2:6:6*t);
        u.SD = w_opt(3:6:6*t);
        u.WS = w_opt(4:6:6*t);
        u.GS = w_opt(5:6:6*t);
        x.R = w_opt(6:6:6*t);
        
        M_Cost = sum((u.GD+u.GS).*Price_horizon');
        
T_Cost = [T_Cost; M_Cost];
end
Avg_Cost = mean(T_Cost);

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

save('P05_Opt')

%% Plot
Line_thick = 1.1
Font = 14;

figure()
subplot(5,1,1)
stairs(1:t,u.WD,'LineWidth',Line_thick)
%legend('$Q_{WD}$')
axis([0 t 0 300])
%xlabel('Time period, $t$')
ylabel('$Q^{WD}$ [MWh]','FontSize',Font)
set(gca,'FontSize',Font)


subplot(5,1,2)
stairs(1:t,u.GD,'LineWidth',Line_thick)
%legend('$Q_{GD}$')
axis([0 t 0 300])
%xlabel('Time period, $t$')
ylabel('$Q^{GD}$ [MWh]','FontSize',Font)
set(gca,'FontSize',Font)


subplot(5,1,3)
stairs(1:t,u.SD,'LineWidth',Line_thick)
%legend('$Q_{GD}$')
axis([0 t 0 300])
%xlabel('Time period, $t$')
ylabel('$Q^{SD}$ [MWh]','FontSize',Font)
set(gca,'FontSize',Font)

subplot(5,1,4)
stairs(1:t,u.WS,'LineWidth',Line_thick)
%legend('$Q_{WS}$')
axis([0 t 0 300])
%xlabel('Time period, $t$')
ylabel('$Q^{WS}$ [MWh]','FontSize',Font)
set(gca,'FontSize',Font)

subplot(5,1,5)
stairs(1:t,u.GS,'LineWidth',Line_thick)
%legend('$Q_{GS}$')
axis([0 t 0 300])
xlabel('Time period, $t$','FontSize',Font)
set(gca,'FontSize',Font)

ylabel('$Q^{GS}$ [MWh]','FontSize',Font)

Font = 30;
figure()
stairs(1:t,x.R,'LineWidth',1.5)
%legend('$Q_{GS}$')
axis([0 t 0 S0.R_max])
xlabel('Time period, $t$','FontSize',Font)
ylabel('$Q^{Storage}$ [MWh]','FontSize',Font)
set(gca,'FontSize',Font)


