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
extptr = regexp(missionFileName,'\.cfg');
diary([missionFileName(1:extptr-1) '.txt'])
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
nWptsRoute = j;
fprintf('Found %d waypoints in %s.\n', j, routeFileName);
%
% Compute the duration. Assume constant ascent and descent rates, and also
% assume 1.5 m/s for straight segments.
%
totalDur = 0;
ascentRate = 1;     %m/s
descentRate = .5;   %m/s
speed = 1.5;        %m/s
wpTmSafety = 2;     %none. Safety factor for waypoint durations.
missTmSafety = 1.4;   %none. Safety factor for mission timer.
%
%              1       2       3     4       5       6       7       8
fprintf('%11.11s %11.11s %11.11s %4.4s %11.11s %11.11s %11.11s %11.11s \n',...
	'Long (deg)','Lat (deg)','Topo (m)','Type','Bearing (deg)',...
	'Horiz Dist (m)','Dist (m)','Slope (m/m)')
for(i=1:nWptsRoute)
  %
  % Print it back to the console.
  fprintf('%11.6f %11.6f %11.6f %4d %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;
nWptsMiss = 0;
foundLon = 0;
foundLat = 0;
foundWpt = 0;
foundWptDur = 0;
initDist = 500;    %Distance (assumed) from launch to first waypoint.
mtSet = 0;
foundMTimer=0;
bhvCntr = 0;
fileStr = fgets(missionFid);

while( fileStr ~= -1 )
  i = i + 1;
  latPtr = regexp(fileStr,'^[ ]*latitude[ ]*=','end');
  lonPtr = regexp(fileStr,'^[ ]*longitude[ ]*=','end');
  durPtr = regexp(fileStr,'^[ ]*duration[ ]*=','end');
  tmrPtr = regexp(fileStr,'^[ ]*behavior[ ]*missionTimer','end');
  wptPtr = regexp(fileStr,'^[ ]*behavior[ ]*waypoint','end');
  bhvPtr = regexp(fileStr,'^[ ]*behavior[ ]','end');
  timeOutPtr = regexp(fileStr,'^[ ]*timeOut[ ]*=','end');
  
  if( ~isempty(bhvPtr) )
    %
    % Count behaviors
    bhvCntr = bhvCntr + 1;
  end
  
  if( ~isempty(tmrPtr) )
    foundMTimer=1;
  end

  if( ~isempty(timeOutPtr) && foundMTimer ) 
    %
    % First check for an error
    if( mtSet )
      fprintf('Error: Found second mission timer on line %d.\n', i);
      fclose(missionFid);
      fclose(tempFid);
      return;
    end
    %
    % Compute overall mission duration.
    totalDur = initDist/speed;
    totalDur = totalDur - routeFile(1,3)/descentRate;
    totalDur = totalDur + routeFile(nWptsRoute,7)/speed;
    totalDur = totalDur - routeFile(nWptsRoute,3)/ascentRate;
    totalDur = totalDur*missTmSafety;
    fprintf('Setting missionTimer to %d seconds.\n',round(totalDur));
    if( totalDur > 600 && totalDur < 3600)
      fprintf('Setting missionTimer to %.1f minutes.\n',totalDur/60);
    elseif( totalDur >=3600 )
      fprintf('Setting missionTimer to %.1f hours.\n',totalDur/3600);
    end
    str = sprintf(' %d;\n',round(totalDur));
    dataStr = [fileStr(1:timeOutPtr),str];
    fileStr = dataStr;
    mtSet = 1;
    foundMTimer = 0;
  end

  if( ~isempty(wptPtr) )
    foundWpt = 1;
  end
  
  if( ~isempty(durPtr) )
    if( foundWpt )
      if( foundWptDur )
	fprintf('Error: Second waypoint-duration in a row on line %d.\n', i);
	fclose(missionFid);
	fclose(tempFid);
	return;
      else
	foundWptDur = 1;
	if( nWptsMiss < nWptsRoute - 1 )
	  dist = routeFile(nWptsRoute-nWptsMiss,7) - ...
		 routeFile(nWptsRoute-nWptsMiss-1,7);
	  %
	  % Set the duration according to the route file.
	  dur = dist/speed;
	  dur = dur*wpTmSafety;
	  fprintf('Distance from Wpt %d to %d is %.1f, dur = %d sec.\n', ...
		  nWptsRoute-nWptsMiss-1,nWptsRoute-nWptsMiss, dist, round(dur));
	elseif( nWptsMiss == nWptsRoute - 1 )
	  %
	  % This is the launch-to-first-waypoint duration
	  dur = initDist/speed;
	  dur = dur*wpTmSafety;
	  fprintf(['Assumed distance from descend termination ',...
		   'to Wpt %d is %.1f, with dur = %d sec.\n'], ...
		  nWptsRoute-nWptsMiss, initDist, round(dur));
	elseif( nWptsMiss == nWptsRoute )
	  %
	  % This is the launch-to-first-waypoint duration
	  fprintf(['Assumed distance from launch to Wpt %d ',...
		   'is %.1f, with dur = %d sec.\n'], ...
		  nWptsRoute-nWptsMiss, initDist, round(dur));
	elseif( nWptsMiss > nWptsRoute + 1 )
	  fprintf('Error: found too many durations on line %d.\n', i);
          %fclose(missionFid);
          %fclose(tempFid);
	  %return;
	end
	str = sprintf(' %d;\n',round(dur));
	dataStr = [fileStr(1:durPtr),str];
	fileStr = dataStr;
      end
    end
  end
  
  if( ~isempty(lonPtr) ) 
    if( foundLon )
      fprintf('Error: Second longitude in a row on line %d.\n', i);
      fclose(missionFid);
      fclose(tempFid);
      return;
    else
      foundLon = 1;
    end
    if( nWptsMiss > nWptsRoute )
      fprintf('Error: File %s contains more waypoints than does %s.\n',...
	      missionFileName, routeFileName);
      fclose(missionFid);
      fclose(tempFid);
      return;
    end
    if( nWptsMiss < nWptsRoute )
      str = sprintf(' %11.6f;\n',routeFile(nWptsRoute-nWptsMiss,1));
    else
      %
      % Repeat the last (really the first) waypoint.
      str = sprintf(' %11.6f;\n',routeFile(nWptsRoute-nWptsMiss+1,1));
    end
    dataStr = [fileStr(1:lonPtr),str];
    fileStr = dataStr;
  end

  if( ~isempty(latPtr) ) 
    if( foundLat )
      fprintf('Error: Second latitude in a row on line %d.\n', i);
      fclose(missionFid);
      fclose(tempFid);
      return;
    else
      foundLat = 1;
    end
    if( nWptsMiss > nWptsRoute )
      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( nWptsMiss < nWptsRoute )
      str = sprintf(' %11.6f;\n',routeFile(nWptsRoute-nWptsMiss,2));
    else
      %
      % Repeat the last (really the first) waypoint.
      str = sprintf(' %11.6f;\n',routeFile(nWptsRoute-nWptsMiss+1,2));
    end
    dataStr = [fileStr(1:latPtr),str];
    fileStr = dataStr;
  end
%  
% I probably could have done this with only foundWpt, but I added it later in
% the coding process.  foundLat and foundLon already existed, so I'm leaving
% them be.
%
  if( foundLon && foundLat && foundWptDur )
    % OK, got all of them.
    foundLat = 0;
    foundLon = 0;
    foundWpt = 0;
    foundWptDur = 0;
    nWptsMiss = nWptsMiss + 1;
  end
  %
  % Write it out, line by line.
  fprintf(tempFid,fileStr);
  %
  % Now read the next line.
  fileStr = fgets(missionFid);
end

fprintf(['Found %d behaviors, of which %d are waypoints, ',...
	 'in %d lines in file %s.\n'], bhvCntr, nWptsMiss, i, missionFileName);

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

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

return;
