close all
clear all
clc


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

%% Optimum calculation

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)];
sample_path.D = [sample_path.D]; 
sample_path.P = [sample_path.P]; 

%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)];

% parameter tuning for D
theta_array = linspace(0.1, 2.0, 20)

%% simulation

Modified_cost = [];
for kk = 1:size(theta_array,2)
    theta = theta_array(kk);
Cost2 =[];
for w = 1:n_scenario
ss
%Initial setup
S0 = S0();
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);
    
    %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)*theta;
            %Discharing rate limitation
        A_4 = kron(eye(t),[0 0 1 0 0 0]);
        B_4 = S0.rate_d*ones(t,1)*theta;
            %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.02 1 0 -0.01 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:t:6*t);
        u.GD = w_opt(2:t:6*t);
        u.SD = w_opt(3:t:6*t);
        u.WS = w_opt(4:t:6*t);
        u.GS = w_opt(5:t:6*t);
        
        %realization
        S.P = sample_path.P(w,k);
        S.D = sample_path.D(w,k);
        %Energy_horizon = sample_path.E(1,k);
        
        % Take an action
        uk = [u.WD(1);u.GD(1);u.SD(1);u.WS(1);u.GS(1)];
        
        %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,1:N).*(uSim(2,1:N) + uSim(5,1:N)));
Cost2 = [Cost2; Cost1];
end
Cost = mean(Cost2);
Modified_cost = [Modified_cost, Cost];
end

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

save('P03_CFA_R_search')

%% Plot
plot_settings
close all

figure()
plot(theta_array,Modified_cost,'LineWidth',1.5)
axis([min(theta_array) max(theta_array) min(Modified_cost) max(Modified_cost)])
xlabel('A parameter for the maximum charging and discharging rates, $\theta_{R}$','FontSize',33)
ylabel('Expected Cost [NOK]','FontSize',33)
set(gca,'FontSize',33)
