% suppress warning of shadowing intrinsic 
% angle  function
warning('off', 'Octave:shadowed-function')

%% angle funtions compute angle
function angle = angle(Xl, Xc, Xp)

  angle = azimuth(Xc, Xp) - azimuth(Xc, Xl);

end

warning('on', 'Octave:shadowed-function')

%% azimuth
function azimuth = azimuth(X1, X2)

  deltaX = X2 - X1;
  azimuth = atan2(deltaX(:,2),deltaX(:,1));

end

%% compute distance on cartesian plane
%% for set of points
%%
%% works also if one arg is a matrix
%% [X1 Y1; X2 Y2; ... and so on ]
function distance = distance(Xp, Xk)
  distance = sqrt(sum((Xk - Xp).^2,2));
end

%%dme design matrix entries for bearing
%% returns entries in order for:
%%dXl dYl dXp dYp dXc dYc
function [dme] = dme_angle(Xl, Xc, Xp)
  dcl = distance(Xc, Xl);
  dcp = distance(Xc, Xp);

  dme = [ 
    +(Xl(:,2) - Xc(:,2)) ./ dcl.^2, ...
    -(Xl(:,1) - Xc(:,1)) ./ dcl.^2, ...
    -(Xp(:,2) - Xc(:,2)) ./ dcp.^2, ...
    +(Xp(:,1) - Xc(:,1)) ./ dcp.^2
    ];

  dme = [
    dme, ...
    -(dme(:,1) + dme(:,3)), ...
    -(dme(:,2) + dme(:,4))
    ];
end

%%dme design matrix entries for bearing
function [dme] = dme_bearing(Xp, Xk)
  npoints = max(size(Xp,1), size(Xk,1));
  d = distance(Xp, Xk);
  dme = [ 
    +(Xk(:,2) - Xp(:,2)) ./ d.^2, ...
    -(Xk(:,1) - Xp(:,1)) ./ d.^2, ...
    -(Xk(:,2) - Xp(:,2)) ./ d.^2, ...
    +(Xk(:,1) - Xp(:,1)) ./ d.^2, ...
    -1 * ones(npoints,1)
    ];
end

%%dme design matrix entries for distance observables
function [dme] = dme_dist(Xp, Xk)
  az = azimuth(Xp, Xk);
  dme = [-cos(az) -sin(az) cos(az) sin(az)];
end

%% converts grad to rad
function rad = grad2rad(grad)
  rad = grad*pi/200;
end

%% converts rad to grad
function rad = rad2grad(grad)
  rad = grad*200/pi;
end

%% angle1 : angle at X1
%% angle2 : angle at X2
function coordinates = intersection(X1, angle1, X2, angle2)
  dist3 = distance(X1,X2);

  azX1X3 = azimuth(X1,X2)-angle1;

  dist1 = dist3/(sin(angle1+angle2)) * sin(angle2);

  X3(1) = X1(1) + dist1 * cos(azX1X3);
  X3(2) = X1(2) + dist1 * sin(azX1X3);

  coordinates = X3;
end

%% X1     two element vector with x and y of first point
%% dist1  distance measured from point 1
function coordinates = linearsection(X1, dist1, X2, dist2)

  dist3 = distance(X1,X2);

  angles = acos([
    (-dist2^2 + dist1^2 + dist3^2) / (2 * dist1 * dist3)
    (-dist1^2 + dist2^2 + dist3^2) / (2 * dist2 * dist3)
    ]);

  coordinates = intersection(X1, angles(1), X2, angles(2));
end

%% compute error ellipse
function [a, b, phi] = errorellipse(invATPA, sigma0 = 1, F = 1)
  lambda = eig(invATPA) ;
  a = sigma0 * sqrt(2*lambda(1) * F);
  b = sigma0 * sqrt(2*lambda(2) * F);
  phi = 1/2 * atan2(2*invATPA(1,2), invATPA(1,1)-invATPA(2,2));
end
