% XNAVAI  locate a point on a spheroid using circular or hyperbolic navigation
%
% SYNOPSIS:
%   function [pnt]=xnavai(sosopos,dist,refpoint,mindex)
%
% DESCRIPTION:
%   XNAVAI locates a point from a spheroid using circular or hyperbolic
%   navigation.
%
% ARGUMETNS:
%     INPUT:    sosopos         matrix of the size 2 by 2 which contains the
%                               positions (latitude, longitude) of the sound
%                               sources used for this point in radiants.
%                               The matrix should look like:
%                                   [soso_lat1, soso_long1;
%                                    soso_lat2, soso_long2]
%               dist            vector which contains the distance of each
%                               sound source in km used for this point.
%               refpoint        vector with length 2, which contains the
%                               position of the refpoint in radiants.
%                               The vector should look like:
%                                   [ref_lat, ref_lon]
%               mindex          =2 --> circular navigation
%                               =3 --> hyperbolic navigation
%
%    OUTPUT:    pnt             vector with length 2, which contains the 
%                               calculated floatposition in radiants.#
%                               The vector will look like:
%                                   [pnt_lat, pnt_lon]
%
% Maximum number of iteration steps set to 20

function [pnt]=xnavai(sosopos,dist,refpoint,mindex)

a(1:3)=999;
dt1=999;
dt2=999;
dt3=999;
dt4=999;
dt5=999;
dt6=999;

TOL=0.1;
INCR=0.000156788;
ERAD=6377;

imax=20;
icnt=0;
stop=0;
pnt=[NaN, NaN];

if mindex==2 % the circular case
  if ~(isnan(dist(1)) || isnan(dist(2)))   % if neither dist(1) nor dist(2) = NaN
  		d1=dist(1);
     	d2=dist(2);
      sosoposition(1,:) = sosopos(1,:);
      sosoposition(2,:) = sosopos(2,:);
  elseif ~(isnan(dist(1)) || isnan(dist(3)))   % if neither dist(1) nor dist(3) = NaN
  	   d1=dist(1);
      d2=dist(3);
      sosoposition(1,:) = sosopos(1,:);
      sosoposition(2,:) = sosopos(3,:);
  elseif ~(isnan(dist(2)) || isnan(dist(3)))   % if neither dist(1) nor dist(3) = NaN
  		d1=dist(2);
     d2=dist(3);
      sosoposition(1,:) = sosopos(2,:);
      sosoposition(2,:) = sosopos(3,:);
  end
elseif mindex==3 % the hyperbolic case
  d1=dist(2)-dist(1);
  d2=dist(3)-dist(1);
  sosoposition(1,:) = sosopos(1,:);
  sosoposition(2,:) = sosopos(2,:);
  sosoposition(3,:) = sosopos(3,:);
end

istop=0;

while ((icnt < imax) && ~stop)

  istop=istop+1;

  if istop > 20
     pnt=[NaN, NaN];
     stop=1;
  end

  ps=refpoint;
  
  a(1)=artoa.data.calculateEllipk4(ps,sosoposition(1,:));
  a(2)=artoa.data.calculateEllipk4(ps,sosoposition(2,:));
  if mindex==3
    a(3)=artoa.data.calculateEllipk4(ps,sosoposition(3,:));
  end
  
  
  if ((a(1)<0) || (a(2)<0) || (a(3)<0))
    stop=1;
  else  
  
    if mindex==2
      cr1=d1-a(1);
      cr2=d2-a(2);
    elseif mindex==3
      cr1=d1-a(2)+a(1);
      cr2=d2-a(3)+a(1);
    end
                                  % check if the error is small enough  
    err=abs(cr1)+abs(cr2);

    if (err <= TOL)
      pnt=refpoint;
      stop=1;
    else
                                  % if not, go on incrementing position
      ps(1)=refpoint(1);
      ps(2)=refpoint(2)+INCR;
      dt1=artoa.data.calculateEllipk4(ps,sosoposition(1,:));
      dt2=artoa.data.calculateEllipk4(ps,sosoposition(2,:));
      if mindex==3
        dt3=artoa.data.calculateEllipk4(ps,sosoposition(3,:));
      end
  
          
      if ((dt1<0) || (dt2<0) || (dt3<0))
        stop=1;
      else  
      
        ps(1)=refpoint(1)+INCR;
        ps(2)=refpoint(2);
        dt4=artoa.data.calculateEllipk4(ps,sosoposition(1,:));
        dt5=artoa.data.calculateEllipk4(ps,sosoposition(2,:));
        if mindex==3
          dt6=artoa.data.calculateEllipk4(ps,sosoposition(3,:));
        end
            
        if ((dt4<0) || (dt5<0) || (dt6<0))
          stop=1;
        else  
          if mindex==2
            b1=dt1-a(1);
            b2=dt2-a(2);
            b3=dt4-a(1);
            b4=dt5-a(2);
          elseif mindex==3
            b1=(dt2-dt1)-(a(2)-a(1));
            b2=(dt3-dt1)-(a(3)-a(1));
            b3=(dt5-dt4)-(a(2)-a(1));
            b4=(dt6-dt4)-(a(3)-a(1));
          end
          
          dnom=(b1*b4-b2*b3)*ERAD;
          
          c(2)=(cr1*b4-b3*cr2)/dnom;
          c(1)=(cr2*b1-b2*cr1)/dnom;

          im=max(abs(c))==abs(c);
          cc=c(im);
          
          if mindex==2
            im=min(abs(a(1:2)))==abs(a(1:2));
            aa=a(im);
          elseif mindex==3
            im=min(abs(a(1:3)))==abs(a(1:3));
            aa=a(im);
          end
          
          cp=2*cc*ERAD/aa;
          
          if cp < 1
            cp=1;
          end
 
          refpoint=refpoint+c/cp;
         
        end
      end      
    end
  end  
end