function [est_north, est_east, num_solns] = circle_intersect(na, ea, nb, eb, ra, rb)
% perform circle intersection using technique used in john's thesis
% uses trig to solve for the 0, 1, or 2 intersection points.
% if there is no intersection, perhaps find the midpoint of the 
% line connecting the two closest points on the circles. 
%
% inputs: na = beacon a north position
% inputs: ea = beacon a east position
% inputs: nb = beacon b north position
% inputs: eb = beacon b east position
% inputs: ra = range to beacon a (m)
% inputs: rb = range to beacon b (m)
% outputs: est_north and est_east vehicle positions

% applies a coordinate transformation to place center of circ a at the origin 
% center of circle b on the (+?) x axis.   
% x is north and y is east !!! 

% need to add error checking ! 

est_north = [NaN ; NaN];
est_east = [NaN ; NaN];

% DISTPQ_THRESH = 20;
DISTPQ_THRESH = 1;


num_solns = 0;

debug=0;

if (debug==1)
  figure(6)
  clf
  hold on
  circle(ea, na, ra, 'y-.');
  plot(ea, na, 'g+');
  hold on
  circle(eb, nb, rb, 'y-.');
  plot(eb, nb, 'g+');
  axis('equal');
end

x2 = eb - ea;
y2 = nb - na;
z1 = ra;
z2 = rb;

SMALL = 0.00001;
COS_SMALL = 0.003;     % 4/25/95 changed from 0.003 to 0.001 and back;

d = sqrt(x2*x2+y2*y2);
if (d < SMALL) 
  disp(['circle_intersect: origins are two close, dist = ', num2str(d)]);
  return
end	

ctheta = ((z1*z1) - (z2*z2) + (d*d))/(2*d*z1);

phi_travel = atan2( nb-na, eb-ea );

if ((ctheta < (1+COS_SMALL)) & (ctheta > 1)) ctheta = 1.0; end
if ((ctheta > (-1-COS_SMALL)) & (ctheta < (-1))) ctheta = -1.0; end

if ((ctheta > 1) | (ctheta < -1)) 
%  disp([' circle_intersect: cos(theta) = ', num2str(ctheta), ' no intersections']);

  if ((ra<SMALL)|(rb<SMALL)) return
  end

  % compute midpoint

  % first find the point on circle a closest to beacon b: 
  theta11 = phi_travel + pi;
  theta12 = phi_travel;
  p1east = ea + ra*(cos(theta11));
  p1north = na + ra*(sin(theta11));
  p2east = ea + ra*(cos(theta12));
  p2north = na + ra*(sin(theta12));
  
  distp1 = dist(p1east, p1north, eb, nb);
  distp2 = dist(p2east, p2north, eb, nb);
  if (distp1 < distp2) 
    peast = p1east;
    pnorth = p1north;
  else 
    peast = p2east;
    pnorth = p2north;
  end

  q1east = eb + rb*(cos(theta11));
  q1north = nb + rb*(sin(theta11));
  q2east = eb + rb*(cos(theta12));
  q2north = nb + rb*(sin(theta12));
  
  distq1 = dist(q1east, q1north, ea, na);
  distq2 = dist(q2east, q2north, ea, na);
  if (distq1 < distq2) 
    qeast = q1east;
    qnorth = q1north;
  else 
    qeast = q2east;
    qnorth = q2north;
  end

  if (ctheta>1) theta = 0;
  elseif (ctheta<-1) theta = pi;
  end
  theta11 = phi_travel + theta;
  theta21 = phi_travel + (2*pi - theta);
  distpq = dist(peast, pnorth, qeast, qnorth);

  if (distpq < DISTPQ_THRESH )                  % threshold of 20 meters
     midpt_east = 0.5*(peast + qeast);
     midpt_north = 0.5*(pnorth + qnorth);
     est_east(1) = midpt_east;
     est_north(1) = midpt_north;
     num_solns = 1;

     if (debug==1)
       plot(peast, pnorth, 'ro');
       plot(qeast, qnorth, 'go');
       plot(midpt_east, midpt_north, 'wo');
       plot(midpt_east, midpt_north, 'w+');
       pause
     end
  end
  return
end


theta = acos( ctheta );
theta11 = phi_travel + theta;
theta12 = phi_travel + (2*pi-theta);

est_east(1) = ea + (ra * cos(theta11));
est_north(1) = na + (ra * sin(theta11));

linex = [ea; eb; est_east(1); ea];
liney = [na; nb; est_north(1); na];
if (debug==1) 
  plot(linex, liney, 'w');
  plot(est_east(1), est_north(1), 'bo');
end

if ((ctheta == 1) | (ctheta == -1))
%  disp([' circle_intersect: cos(theta) = ', num2str(ctheta), ' one intersection']);
  num_solns = 1;
  return
end

est_east(2) = ea + (ra * cos(theta12));
est_north(2) = na + (ra * sin(theta12));

if (debug==1) 
  linex = [ea; eb; est_east(2); ea];
  liney = [na; nb; est_north(2); na];
  plot(est_east(2), est_north(2), 'bo');
  plot(linex, liney, 'w');
%  pause
end

num_solns = 2;

return

