% iSoSuG accuracy check

function dist = algorithm(sigma_crs_deg, sigma_gps_m)
% true sonde coordinates
slat = deg2rad(50.91535); 
slon = deg2rad(-11.825333);

% six DF coordinates and courses
dflat = deg2rad([50.920283,50.9149,50.920233,50.91235,50.9134,50.918317]);
dflon = deg2rad([-11.81795,-11.818517,-11.830917,-11.8337,-11.829283,-11.821517]);
maxdist = 2000;

ncoords = length(dflat);
ncross = (ncoords-1)*ncoords/2;

% calculate distances and true courses
[dfdist,dfcrs] = distbear(dflat, dflon, slat, slon);

% add random error to the courses (degrees)
dfcrs = dfcrs + deg2rad(sigma_crs_deg*randn(1,ncoords));
% add random error to the positions (meters)
dflat = dflat + (sigma_gps_m*randn(1,ncoords))/6366710;
dflon = dflon + (sigma_gps_m*randn(1,ncoords))/6366710;


%clat = zeros(1,ncross);
%clon = zeros(1,ncross);

index = 1;
for i=1:ncoords
    for j=(i+1):ncoords
            [clat(index),clon(index), okay] = intersection(dflat(i), dflon(i), dfcrs(i), dflat(j), dflon(j), dfcrs(j));
            % ignore points without crosssings
            
            if (okay)
                if (distbear(slat, slon, clat(index), clon(index)) * 6366710 < maxdist)
                    index = index + 1;
                else
                    clat(index) = [];
                    clon(index) = [];
                end
            else
                    clat(index) = [];
                    clon(index) = [];
            end
            % ignore points with too high distance
            
    end
end

[estlat, estlon] = middlepoint(clat, clon);

[dist, crs] = distbear(slat, slon, estlat, estlon);

%estlat = rad2deg(estlat);
%estlon = rad2deg(estlon);

%slat = rad2deg(slat);
%slon = rad2deg(slon);

%dlat = estlat-slat;
%dlon = estlon-slon;

dist = 6366710 * dist;

end





