clear

lat1 = 36.0;
lon1 = -123.0;

lat2 = 36.0 + .01;
lon2 = -123.0 + 1/60.0;

R = 6378137;

lat1Rad = lat1 * pi / 180.0;
lon1Rad = lon1 * pi / 180.0;
lat2Rad = lat2 * pi / 180.0;
lon2Rad = lon2 * pi / 180.0;

xlat = lat2Rad - lat1Rad;
xlon = lon2Rad - lon1Rad;
a = sin(xlat/2)^2 + cos(lat1)*cos(lat2)*sin(xlon/2)^2;
c = 2.0 * atan2(sqrt(a), sqrt(1-a));

dist = R * c;
%angle  = (180.0/pi) * atan2(sin(xlon)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(xlon))

tempXlon = 0.0;
a = sin(xlat/2)^2 + cos(lat1)*cos(lat2)*sin(tempXlon/2)^2;
c = 2.0 * atan2(sqrt(a), sqrt(1-a));

tempDist = R * c;

trigAng = (180.0/pi) * real(acos(dist / tempDist))

if( (lat2>lat1) && (lon2>lon1) )
  ang = trigAng;
  fprintf('Quad 1\n');
end
if( (lat2>lat1) && (lon2<lon1) )
  ang = 360.0 - trigAng;
  fprintf('Quad 4\n');
end
if( (lat2<lat1) && (lon2>lon1) )
  ang = 180.0 - trigAng;
  fprintf('Quad 2\n');
end
if( (lat2<lat1) && (lon2<lon1) )
  ang = 180 + trigAng;
  fprintf('Quad 3\n');
end

%Deal with exactly 0, 90, 180, 270
if( (lat2==lat1) && (lon2<lon1) )
  ang = 270.0 - trigAng;
  fprintf('lat2=lat1\n');
end
if( (lat2==lat1) && (lon2>lon1) )
  ang = 90.0 - trigAng;
  fprintf('lat2=lat1\n');
end
if( (lat2>lat1) && (lon2==lon1) )
  ang = 0.0 - trigAng;
  fprintf('lat2=lat1\n');
end
if( (lat2<lat1) && (lon2==lon1) )
  ang = 180.0 - trigAng;
  fprintf('lat2=lat1\n');
end


fprintf('distance = %f   heading = %f \n', dist, ang)