function [bearing, distance] = relspara(long1,lat1,long2,lat2)
%[bearing, distance] = relspara(long1,lat1,long2,lat2)
%
%Advanced version of the function relspar. (Same functionality thow.)
%
%Given two point with geographical coordinates (long,lat - both in degrees]
%on the unit sphere the function returns the bearing angle of the second
%point at the first point (from north, in clockwise direction) and
%the distance of the second point from the first point along a great
%circle following the direction defined by the bearing.
%
%Note that abs(lat) =< 90 is expected; lat grows
%from 0 to 90 from the equator to the "north pole" and decreases from 0 to
%-90 towards the "south pole".
%Only abs(long) <= 180 is allowed, long is positive toward east.
%
%If point1 (long1,lat1) is at one of the poles then bearing is meant as
%the angle of the second point relative to the Greenwich meridian looked
%from north in anti-clockwise direction.
%Longitude is ignored in this case.
%
%Note that this routine is implemented on scalar input variables.
%Do not use vectors/matrices for long and lat!
%
%bearing and distance are returned in radians

%Check for correct number of input arguments
if nargin ~= 4
    error('4 arguments are required: relspar(long1,lat1,long2,lat2)')
end

%Check for correct number of output arguments
if nargout ~= 2
    error('2 variables are needed for output: [bearing, distance]')
end

%Check for dimensions
[rows, cols] = size(long1);
if (rows ~= 1 | cols ~= 1)
    error('long1 expected to be scalar')
end
[rows, cols] = size(long2);
if (rows ~= 1 | cols ~= 1)
    error('long2 expected to be scalar')
end
[rows, cols] = size(lat1);
if (rows ~= 1 | cols ~= 1)
    error('lat1 expected to be scalar')
end
[rows, cols] = size(lat2);
if (rows ~= 1 | cols ~= 1)
    error('lat2 expected to be scalar')
end

%Check data,
if abs(long1) > 180
    error('wrong longitude value has been found in long1')
end
if abs(long2) > 180
    error('wrong longitude value has been found in long2')
end
if abs(lat1) > 90
    error('wrong longitude value has been found in lat1')
end
if abs(lat2) > 90
    error('wrong longitude value has been found in lat2')
end

%Convert to radians
long1 = long1/180*pi;
long2 = long2/180*pi;
lat1 = lat1/180*pi;
lat2 = lat2/180*pi;

%Transform longitudes into eastern longitudes
if long1 < 0
    long1 = 2*pi+long1;
end
if long2 < 0
    long2 = 2*pi+long2;
end

%Transform latitudes into 0 <= lat <= pi values
lat1 = pi/2-lat1;
lat2 = pi/2-lat2;

%We have now two positions in spherical coordinates and radians
if lat1 == 0
    bearing = long2;
    distance = lat2;
elseif lat1 == pi
    bearing = long2;
    distance = pi-lat2;
elseif lat2 == 0
    bearing = 0;
    distance = lat1;
elseif lat2 == pi
    bearing = pi;
    distance = pi-lat1;
elseif long1 == long2
    if lat1 == lat2
        bearing = 0;
        distance = 0;
    else
        if lat1 > lat2
            bearing = 0;
            distance = lat1-lat2;
        else
            bearing = pi;
            distance = lat2-lat1;
        end
    end
elseif abs(long2-long1) == pi
    if (lat1 + lat2) <= pi
        bearing = 0;
        distance = lat1 + lat2;
    else
        bearing = pi;
        distance = 2*pi-(lat1+lat2);
    end
else
    dlong = long2 - long1;
    dlongsign = 1;
    if (dlong > pi)
        dlong = 2*pi-dlong;
        dlongsign = -1;
    elseif dlong < -pi
        dlong = 2*pi+dlong;
    elseif dlong < 0
        dlong = abs(dlong);
        dlongsign = -1;
    end
    cosd = cos(lat1)*cos(lat2)+sin(lat1)*sin(lat2)*cos(dlong);
    distance = acos(cosd);
    sind = sin(distance);
    cosangl = ( cos(lat2)-cos(lat1)*cosd ) / (sin(lat1)*sind);
    bearing = acos(cosangl);
    if dlongsign < 0
        bearing = 2*pi-bearing;
    end
end
end


function [pt_height] = h3ang(st_long, st_lat, st_height,   elev_angl,   pt_long, pt_lat)

% Usage:
% [pt_height] = h3ang(st_long, st_lat, st_height,   elev_angl,   pt_long, pt_lat)
%
% This routine determines the height of a point (pt_height) if the coordinates of the
% point are known (pt_long, pt_lat) and it is seen from a given station
% (st_long, st_lat, st_height) under a known elevation angle (elev_angl)
%
% pt_height is returned in kms above main sea level (MSL)
%
% Note that abs(lat) =< 90 is expected; lat grows
%  from 0 to 90 from the equator to the "north pole" and decreases from 0 to
%  -90 towards the "south pole".
% Only abs(long) <= 180 is allowed, long is positive toward east.
% st_height is expected in kms above MSL
% elev_angl is measured from the horizontal plane at the station in degrees,
%   positive values are towards the zenith
%   accepted values are -90 < elev_angl < +90
%
% Note that this routine is implemented on scalar input variables.
% Do not use vectors/matrices for longitudes, latitudes, and azimuths!
%
% This code uses routine RELSPARA.M
%

%Check for correct number of input arguments
if nargin ~= 6
    error(' 6 arguments are required: [pt_height] = h3ang(st_long, st_lat, st_height,   elev_angl,   pt_long, pt_lat)')
end

%Check for correct number of output arguments
if nargout ~= 1
    error('1 variable is needed for output: [pt_height] = h3ang(st_long, st_lat, st_height,   elev_angl,   pt_long, pt_lat)')
end

%Check for dimensions
[rows, cols] = size(st_long);
if (rows ~= 1 | cols ~= 1)
    error('st_long expected to be scalar')
end
[rows, cols] = size(st_lat);
if (rows ~= 1 | cols ~= 1)
    error('st_lat expected to be scalar')
end
[rows, cols] = size(st_height);
if (rows ~= 1 | cols ~= 1)
    error('st_height expected to be scalar')
end
[rows, cols] = size(elev_angl);
if (rows ~= 1 | cols ~= 1)
    error('elev_angl expected to be scalar')
end
[rows, cols] = size(pt_long);
if (rows ~= 1 | cols ~= 1)
    error('pt_long expected to be scalar')
end
[rows, cols] = size(pt_lat);
if (rows ~= 1 | cols ~= 1)
    error('pt_lat expected to be scalar')
end

%Check data
if abs(st_long) > 180
    error('wrong longitude value has been found in st_long')
end
if abs(st_lat) > 90
    error('wrong longitude value has been found in st_lat')
end
if abs(pt_long) > 180
    error('wrong longitude value has been found in pt_long')
end
if abs(pt_lat) > 90
    error('wrong longitude value has been found in pt_lat')
end
if (elev_angl <= (-90) | elev_angl >= 90)
    error('elev_angl is out of -90 < elev_angl < 90 range!')
end

st_R = 6378.14*(0.9983271+0.0016764*cos(2*(st_lat/180*pi))-0.0000035*cos(4*(st_lat/180*pi))); %Radius of the Earth in km
st_full_height = st_R + st_height;

[bearing, arch_dist] = relspara(pt_long, pt_lat, st_long, st_lat);

beta = elev_angl./180.*pi + pi/2;
gamma = pi - arch_dist - beta;

pt_full_height = sin(beta) ./ sin(gamma) .* st_full_height;

pt_R = 6378.14*(0.9983271+0.0016764*cos(2*(pt_lat/180*pi))-0.0000035*cos(4*(pt_lat/180*pi))); %Radius of the Earth in km

pt_height = pt_full_height - pt_R;

disp(sprintf('Height of the point is  %f km above MSL\n', pt_height))
end

% Usage:
% [pt_height] = h3ang(st_long, st_lat, st_height,   elev_angl,   pt_long, pt_lat)
%
% This routine determines the height of a point (pt_height) if the coordinates of the
% point are known (pt_long, pt_lat) and it is seen from a given station
% (st_long, st_lat, st_height) under a known elevation angle (elev_angl)
%
% pt_height is returned in kms above main sea level (MSL)
%
% Note that abs(lat) =< 90 is expected; lat grows
%  from 0 to 90 from the equator to the "north pole" and decreases from 0 to
%  -90 towards the "south pole".
% Only abs(long) <= 180 is allowed, long is positive toward east.
% st_height is expected in kms above MSL
% elev_angl is measured from the horizontal plane at the station in degrees,
%   positive values are towards the zenith
%   accepted values are -90 < elev_angl < +90
%
% Note that this routine is implemented on scalar input variables.
% Do not use vectors/matrices for longitudes, latitudes, and azimuths!

[pt_height] = h3ang(-67.113646, 18.048262, 0.07675,   15.34,   -65.41, 16.0)