function [] = swp( missionFileName, routeFileName)
%
% PURPOSE: Extract waypoints from Dave's .rte file and substitute into an
%          existing mission file.  Each file must have the same number of
%          waypoints.
%
if( ~isstr(missionFileName) || ~isstr(routeFileName) )
  fprintf([...
      ' The first argument must be the name of the mission file in a string.\n',...
      ' The second must be the name of the route file in a string.\n']);
  return
end
missionFid = fopen(missionFileName,'r+t');
if( missionFid == -1 )   
  fprintf(' File %s not found!\n',missionFileName);
  return
end
routeFid = fopen(routeFileName);
if( routeFid == -1 )   
  fprintf(' File %s not found!\n',routeFileName);
  fclose(missionFid);
  return
end
tempFid = fopen('Temp.cfg','w');
%
% First extract the wapoints from the .rte file.  Ignore the intermediates.
i=-1;
j=0;
fileStr = ' ';
while( fileStr ~= -1 )
  i = i + 1;
  fileStr = fgetl(routeFid);
  if( ~ischar(fileStr) ) continue; end
  regPtr = regexp(fileStr(1),'^[ ]*[0-9\-]');
  if( isempty(regPtr) ) continue; end
  data = sscanf(fileStr,'%f %f %f %d %f %f %f %f');
  if( data(4) == 0 ) continue; end
  j = j + 1;
  routeFile(j,:) = data';
end
nRouteWpts = j;
fprintf('Found %d waypoints in %s.\n', j, routeFileName);
for(i=1:length(routeFile(:,1)))
  fprintf('%11.6f %11.6f %11.6f %3d %11.6f %11.6f %11.6f %11.6f\n', routeFile(i,:));
end
fclose(routeFid);
%
% Now read in the mission file and substitute waypoints:
i=0;
nMissWpts = 0;
lastLon = 0;
lastLat = 0;
fileStr = fgets(missionFid);
while( fileStr ~= -1 )
  i = i + 1;
  latPtr = regexp(fileStr,'latitude[ ]*=[ ]*','end');
  lonPtr = regexp(fileStr,'longitude[ ]*=[ ]*','end');

  if( lonPtr ) 
    if( lastLon )
      fprintf('Error: Second longitude in a row on line %d.\n', i);
      fclose(missionFid);
      fclose(tempFid);
      return;
    else
      lastLon = 1;
    end
    if( nMissWpts >= nRouteWpts + 1)
      fprintf('Error: File %s contains more waypoints than does %s.\n',...
	      missionFileName, routeFileName);
      fclose(missionFid);
      fclose(tempFid);
      return;
    end
    if( nMissWpts < nRouteWpts )
      str = sprintf('%11.6f;\n',routeFile(end-nMissWpts,1));
    else
      %
      % Repeat the last (really the first) waypoint.
      str = sprintf('%11.6f;\n',routeFile(end-nMissWpts+1,1));
    end
    dataStr = [fileStr(1:lonPtr),str];
    fileStr = dataStr;
  end

  if( latPtr ) 
    if( lastLat )
      fprintf('Error: Second latitude in a row on line %d.\n', i);
      fclose(missionFid);
      fclose(tempFid);
      return;
    else
      lastLat = 1;
    end
    if( nMissWpts >= nRouteWpts + 1 )
      fprintf(['Error: The mission file %s contains more waypoints than does the\n' ...
	      'route file %s.\n'],...
	      missionFileName, routeFileName);
      fclose(missionFid);
      fclose(tempFid);
      return;
    end
    if( nMissWpts < nRouteWpts )
      str = sprintf('%11.6f;\n',routeFile(end-nMissWpts,2));
    else
      %
      % Repeat the last (really the first) waypoint.
      str = sprintf('%11.6f;\n',routeFile(end-nMissWpts+1,2));
    end
    dataStr = [fileStr(1:latPtr),str];
    fileStr = dataStr;
  end

  if( lastLon && lastLat )
    % OK, got both of them.
    lastLat = 0;
    lastLon = 0;
    nMissWpts = nMissWpts + 1;
  end
  %
  % Write it out, line by line.
  fprintf(tempFid,fileStr);
  %
  % Now read the next line.
  fileStr = fgets(missionFid);
end

fprintf('Found %d waypoints in %d lines in file %s.\n', nMissWpts, i, missionFileName);

if( nMissWpts < nRouteWpts )
  fprintf(['Warning:  The route file %s contains more waypoints than does the\n' ...
	   'mission file %s.\n'], routeFileName, missionFileName);
end
fclose(missionFid);
fclose(tempFid);

system(['cp -p ' missionFileName ' ' [missionFileName '.save'] ]);
system(['cp -p ' 'Temp.cfg ' missionFileName]);

return;
