%This script runs a state estimation on the chosen system

% clc;
% clear;

%% Prelimenary stuff

%Choosing test system
cd = loadcase('case4gs');   
casesystem = size(cd.bus, 1);
cd.bus(:,6) = zeros(length(cd.bus(:,6)), 1);
cd.branch((cd.branch(:,9) ~= 0),9) = zeros(size(cd.branch((cd.branch(:,9) ~= 0),9)));
pfres = runpf(cd);


%% Measurements are collected, Random errors are introduced

%Retrieving measurement data of the test system
n_lines = size(pfres.branch,1);

zdata1 = [ones(casesystem,2), pfres.bus(:,8), pfres.bus(:,1), zeros(casesystem,1), 0.001*ones(casesystem,1)]; %voltage measurements

zdata2 = [ones(n_lines*2,1), [4*ones(n_lines,1); 5*ones(n_lines,1)], [pfres.branch(:, 14); pfres.branch(:, 15)]/cd.baseMVA, [pfres.branch(:,1:2); pfres.branch(:,1:2)], 0.005*ones(n_lines*2,1)]; %active and reactive power flows of sending nodes

zdata3 = [ones(n_lines*2,1), [4*ones(n_lines,1); 5*ones(n_lines,1)], [pfres.branch(:, 16); pfres.branch(:, 17)]/cd.baseMVA, [pfres.branch(:,2),pfres.branch(:,1); pfres.branch(:,2),pfres.branch(:,1)], 0.005*ones(n_lines*2,1)]; %active and reactive power flows of receiving nodes

zdata4 = [ones(casesystem,1), 6*ones(casesystem,1), pfres.bus(:,8).*cos((pfres.bus(:,9)-pfres.bus(1,9))*pi/180), pfres.bus(:,1), zeros(casesystem,1), 0.001*ones(casesystem,1)]; %voltage phasor measurement, real component

zdata5 = [ones(casesystem,1), 7*ones(casesystem,1), pfres.bus(:,8).*sin((pfres.bus(:,9)-pfres.bus(1,9))*pi/180), pfres.bus(:,1), zeros(casesystem,1), 0.001*ones(casesystem,1)]; %voltage phasor measurement, imaginary component

zdata = [zdata1; zdata2; zdata3; zdata4; zdata5];

zdata = sortrows(zdata, 2);
zdata(:,1) = 1:size(zdata,1);

%Introducing random meaurement errors according to variances
for i = 1:size(zdata, 1)
    zdata(i,3) = normrnd( zdata(i,3), zdata(i,6))
end


%% Executing central estimation

%Central model is imported
[ybus, bsh, g, b] = getGridParameters(cd);

%WLS solver
x = wls_function(zdata, ybus, bsh, g, b);



% Displaying results

% Real Values
Vreal = pfres.bus(:,8);
delreal = pfres.bus(:,9);

disp('-------- Load flow ------------------');
disp('--------------------------');
disp('| Bus |    V   |  Angle  | ');
disp('| No  |   pu   |  Degree | ');
disp('--------------------------');
for m = 1:min(6,casesystem)
    fprintf('%4g', m); fprintf('  %8.4f', Vreal(m)); fprintf('   %8.4f', delreal(m)-delreal(1)); fprintf('\n');
end
disp('---------------------------------------------');

% Estimated Values 
V=x(:,2);
Del=x(:,3);
disp('-------- State Estimation ------------------');
disp('--------------------------');
disp('| Bus |    V   |  Angle  | ');
disp('| No  |   pu   |  Degree | ');
disp('--------------------------');
for m = 1:min(6,casesystem)
    fprintf('%4g', m); fprintf('  %8.4f', V(m)); fprintf('   %8.4f', Del(m)); fprintf('\n'); %!!!!!!
end
disp('---------------------------------------------');

figure(1)
nbus = casesystem;
K = 1:nbus;
plot(K,V,'r:*',K,Vreal,'b--o','linewidth',1.5)
title('Voltage Magnitude Comparision Result ')
xlabel('Bus number')
xlim([1 nbus])
ylabel('Voltage in p.u')
legend('Estimated Value','True Value',1)
grid on

figure(2)
j = 1:nbus;
plot(j,Del,'r:*',j,delreal,'b--o','linewidth',1.5)
title('Voltage Angle Comparision Result')
xlabel('Bus number')
xlim([1 nbus])
ylabel('Voltage angle in degree')
%legend('Estimated Value','True Value',1)
grid on