% format long
source "rw.m"
d = [
  151.581 20
  244.285 20
  255.235 20
  269.408 10
  233.510 10
  148.655 10
  182.453 20
  ];

% od razu ujednolićmy miano w błędach do m
d(:,2) /= 1e3;

alpha = [111.4985 15];

% korzystajmy konsekwentnie z radianów
alpha = grad2rad([alpha(1) alpha(2)/1e4]);

X_osnowa = [
  1400.200  2389.750 
  1450.080  2550.150 
  1359.880  2640.360
  ];

X0 = [
  1250.180  2409.860 
  1220.120  2589.820
  ];

%% jeżeli nie były by podane wsp. przybliżone (X0) to można policzyć je
%% tak jak poniżej ze zdefiniowanej funkcji
% linearsection(X_osnowa(2,:), d(2,1), X_osnowa(1,:), d(1,1) )
% linearsection(X_osnowa(2,:), d(5,1), X_osnowa(1,:), d(4,1) )

d0 = [
  distance(X_osnowa, X0(1,:))
  distance(X_osnowa, X0(2,:))
  distance(X0(1,:), X0(2,:))
  ];

alpha0 = angle(X0(1,:), X0(2,:), X_osnowa(3,:));

%% wektor wyrazów wolnych
L = [
  d0 - d(:,1)
  alpha0 - alpha(1,1)
  ];

%% macierz wag
P = diag([d(:,2) ; alpha(:,2)].^(-2));

%% macierz współczynników przy niewiadomych
A = zeros(size(d,1)+size(alpha,1),4);
A(1:3,1:2) = dme_dist(X0(1,:), X_osnowa)(:,1:2);
A(4:6,3:4) = dme_dist(X0(2,:), X_osnowa)(:,1:2);
A(7,:) = dme_dist(X0(1,:),X0(2,:));
A(8,:) = dme_angle(X0(1,:),X0(2,:),X_osnowa(3,:))([1,2,5,6]);

dX = - inv(A'*P*A) * (A'*P*L);

%% wyrównane współrzędne obu punktów
Xw = X0 + reshape(dX,2,2)'

%% wektor poprawek
V = A*dX+L;

%% wyrównane obserwacje
yw = [d(:,1) ; alpha(1,1)] + V 

%% cały szereg kontroli
A'*P*V;
V'*P*V;
L'*P*A*dX + L'*P*L;

obserwacje_obliczone_z_wyrownanych_wspolrzednych = [
  distance(X_osnowa,Xw(1,:))
  distance(X_osnowa,Xw(2,:))
  distance(Xw(1,:),Xw(2,:))
  angle(Xw(1,:),Xw(2,:),X_osnowa(3,:))
  ];

abs(yw - obserwacje_obliczone_z_wyrownanych_wspolrzednych);

sigma0 = sqrt(V'*P*V/(size(A,1)-size(A,2)))

CX = sigma0^2 * (A'*P*A)^-1;
mX = sqrt(diag(CX))
mP = sqrt(sum(reshape(mX,2,2).^2))

%% macierz Jacobiego to są współczynniki przy niewiadomych
%% z równania poprawki do kierunku
D = dme_bearing(X0(2,:), X0(1,:))(1:4);

%% błąd średni azymutu B-A
mu_Az_AB_grad = rad2grad(sqrt(D*CX*D'));


%% elipsy błędu średniego
[aA, bA, phiA] = errorellipse(inv(A'*P*A)(1:2,1:2), sigma0);
[aB, bB, phiB] = errorellipse(inv(A'*P*A)(3:4,3:4), sigma0);

%% rysowanie elips, Octave wymaga instalacji dodatkowego pakietu.
% pkg load geometry
% hold on
% axis("equal");
% plot(Xw(:,2), Xw(:,1), '.', "markersize", 40)
% plot(X_osnowa(:,2),X_osnowa(:,1),"sr", "markersize", 10, "markerfacecolor", "red")
% scale = 4000;
% drawEllipse([flip(Xw(1,:)) scale*aA scale*bA 90-rad2deg(phiA)], "linewidth", 4);
% drawEllipse([flip(Xw(2,:)) scale*aB scale*bB 90-rad2deg(phiB)], "linewidth", 4);

% drawEdge(
% flip(
% [
%   ones(3,1)*Xw(1,:),
%   ones(3,1)*Xw(2,:)
%   Xw(1,:)
%   ones(2,1)*X_osnowa(2,:)
%   ]
% ,2),
% flip(
% [
%   X_osnowa(:,:),
%   X_osnowa(:,:),
%   Xw(2,:),
%   X_osnowa(1,:)
%   X_osnowa(3,:)
%   ]
% ,2),
% "color" , "black")
% pause
