% Code to optimize the charge discharge of a battery considering a variable
% price of the electricity.

% Solutions to exercise 3.1 and 3.2 can be easily derived from this code.
% In the following i use the notion of SOC as opposed to SOE. SOC is
% SOE/E_bar. Rest should be the same.

clc;
clear all;
close all;


% ----
% Battery optimal charging/discharging under a TOU tariff
% ----

% price signal
T = 24;
DeltaT = 1;
E_bar = 15;
P_bar = 10;
Bmax = P_bar * ones(T, 1);
Bmin = -P_bar * ones(T, 1);
SOC_0 = 0.5;
SOCMAX = 1 * ones(T, 1);
SOCMIN = 0 * ones(T, 1);


SOCMAX(11:12) = 0.9;

SOCMIN(20:23)=0.3;



c = ones(T, 1);
time = 0:DeltaT:T-1;

c(1:7) = 0.05;
c(8:10) = 0.08;
c(11:17) = 0.15;
c(18:21) = 0.35;
c(22:T) = 0.05;


plot(time, c)
ylim([0, max(c)*1.1])
xticks([0:23])
xlim([0, 23])
grid on
xlabel('Time [hours]')
ylabel('Price signal (CHF/kWh)')



f = c;

sigma = tril(ones(T, T));


A0 = sigma * DeltaT / E_bar;
b0 = SOCMAX - SOC_0;

A1 = -sigma * DeltaT / E_bar;
b1 = -(SOCMIN - SOC_0);

A = vertcat(A0, A1);
b = vertcat(b0, b1);



x_lin = linprog(f, A, b, [], [], Bmin, Bmax);

% Adding a quadratic cost enforces convexity (and gets rid of power
% oscillations).
x_qua = quadprog(eye(T)*0.01, f, A, b, [], [], Bmin, Bmax);

% You can here switch between x_lin and x_qua to compare solutions.
x = x_qua;

SOC = SOC_0 + sigma * DeltaT / E_bar * x;
SOC = vertcat(SOC_0, SOC(1:end-1));


close all;


subplot(3, 1, 1)
plot(time, c)
ylim([0, max(c)*1.1])
xticks([0:23])
xlim([0, 23])
title('Price');


subplot(3, 1, 2)
plot(time, x)
xticks([0:23])
xlim([0, 23])
title('Battery power');

subplot(3, 1, 3)
plot(time, SOC)
xticks([0:23])
xlim([0, 23])
title('SOC');


% Electricity bill
fprintf('Price of electricity: %.1f CHF. \n', f' * x * DeltaT);

% Deliberate code error to stop execution. Comment out to continue

nop


% ----
% Demand peak shaving
% ----

close all;

D = ones(T, 1)*3;
D(18:21) = 15;
PMAX = 12;

stairs(time, D)
hold on
stairs(time, time * 0 + PMAX, 'r--')
hold off
xticks([0:23])
xlim([0, 23])
grid on
xlabel('Time [hours]')
ylabel('Active power (kW)')
legend('Power demand', 'Contractual power');

% We have a contractual limitation of 12 kW.

A = vertcat(A0, A1, eye(T));
b = vertcat(b0, b1, PMAX - D);

x = quadprog(eye(T)*0.1, f, A, b, [], [], Bmin, Bmax);


P = D + x;

hold on
stairs(time, P)
legend('Power demand', 'Contractual power', 'Corrected power');
hold on












