%function plotMission(mission,whichMap,handles)
function plotMission(mission,whichMap,handles)
	global index, global origin, global firstPoint, global fighandles,
	global referenceLines, global auvMarks

if ~checkMissionFile(mission,1), return, end
figure(gcf) , hold on;

title(mission{1}.name)	
grid on
if strcmp(whichMap,'geo'), setGeoAxis
else, axis('equal'), end
set(gca,'Box','on')

if isempty(origin), origin = findOrigin(mission); end
firstPoint = 1;

if nargin == 3, fighandles = handles; else fighandles = []; end

saveVerts('init',0,0,0);

lastPointIndex = -1;
sonarPointIndex = -1;
for index=length(mission):-1:1,
    %pause(0.1)
    %drawnow;
	name = lower(mission{index}.name);
	switch name,
	case {'trackline'},
		if lastPointIndex > 0,
			plotTrackline(mission{index},whichMap,mission{lastPointIndex})
		else
			plotTrackline(mission{index},whichMap)
		end
		firstPoint = 0;
		lastPointIndex = index;
		if sonarPointIndex > 0,
			plotSonarMode(mission{sonarPointIndex},mission{index});
		end
		
	case {'waypoint'},
		if lastPointIndex > 0,
			plotWaypoint(mission{index},whichMap,mission{lastPointIndex})
		else
			plotWaypoint(mission{index},whichMap)
		end
		firstPoint = 0;
		lastPointIndex = index;
		if sonarPointIndex > 0,
			plotSonarMode(mission{sonarPointIndex},mission{index});
		end
		
	case {'controlsonarklein'},
		if lastPointIndex < 1,
			sonarPointIndex = index;
		else
			plotSonarMode(mission{index},mission{lastPointIndex});
			sonarPointIndex = -1;
		end
        
    case {'controlsonaredgetech'},
		if lastPointIndex < 1,
			sonarPointIndex = index;
		else
			plotSonarMode(mission{index},mission{lastPointIndex});
			sonarPointIndex = -1;
		end
	end	
end

% This is a QROUTE hack
if isfield(mission{1},'initiallat') & isfield(mission{1},'initiallong') ...
	& isfield(mission{1},'latitude') & isfield(mission{1},'longitude'),
	plot([mission{1}.initiallong mission{1}.longitude], ...
		[mission{1}.initiallat mission{1}.latitude],'r-o')
elseif isfield(mission{1},'initialn') & isfield(mission{1},'initiale') ...
	& isfield(mission{1},'northing') & isfield(mission{1},'easting'),
	plot([mission{1}.initiale mission{1}.easting]-origin(1), ...
		[mission{1}.initialn mission{1}.northing]-origin(2),'r-o')
end

title(mission{1}.name)	
grid on
hold off
if strcmp(whichMap,'geo'), setGeoAxis
else, axis('equal'), end
set(gca,'Box','on')

if nargin >= 2,
	if strcmp(whichMap,'utm'),
		xlabel('East (meters)')
		ylabel('North (meters)')
	elseif strcmp(whichMap,'geo'),
		xlabel('Longitude (degrees)')
		ylabel('Latitude (degrees)')
	end
end

% add reference lines
if ~isempty(referenceLines),
	for ii = 1:size(referenceLines,1),
		if ( strcmp(whichMap,'utm') & referenceLines(ii,5) == 1 ) | ...
			( strcmp(whichMap,'geo') & referenceLines(ii,5) == 2 ),
			h = line([referenceLines(ii,1) referenceLines(ii,2)], ...
				[referenceLines(ii,3) referenceLines(ii,4)]);
			set(h,'Color','k','Marker','o');
		end
	end
end

% add AUV marks
hold on
if ~isempty(auvMarks),
	for ii = 1:size(auvMarks,1),
		if ( strcmp(whichMap,'utm') & auvMarks(ii,3) == 1 ) | ...
			( strcmp(whichMap,'geo') & auvMarks(ii,3) == 2 ),			
			plot(auvMarks(ii,1),auvMarks(ii,2),'y*')
			plot(auvMarks(ii,1),auvMarks(ii,2),'bo')
		end
	end
end

% add sonar overlaps if box checked
addOverlap(whichMap);
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,y] = getXY(bstruct,useOrigin)
	global origin
x = [];
y = [];
if nargin == 1, useOrigin = 1; end
if isfield(bstruct,'latitude') & isfield(bstruct,'longitude'),
	x = bstruct.longitude;
	y = bstruct.latitude;
elseif isfield(bstruct,'northing') & isfield(bstruct,'easting'),
	x = bstruct.easting;
	y = bstruct.northing;
	if useOrigin,
		x = x - origin(1);
		y = y - origin(2);
	end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,y] = getiXY(bstruct,useOrigin)
	global origin
if nargin == 1, useOrigin = 1; end
x = [];
y = [];
if isfield(bstruct,'initiallat') & isfield(bstruct,'initiallong'),
	x = bstruct.initiallong;
	y = bstruct.initiallat;
elseif isfield(bstruct,'initialn') & isfield(bstruct,'initiale'),
	x = bstruct.initiale;
	y = bstruct.initialn;
	if useOrigin,
		x = x - origin(1);
		y = y - origin(2);
	end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function origin = findOrigin(mission)
for index=length(mission):-1:1,
	name = lower(mission{index}.name);
	if strcmp(name,'waypoint'),
		[x,y] = getXY(mission{index},0);
		break;
	elseif strcmp(name,'trackline'),
		[x,y] = getiXY(mission{index},0);
		break;
	end
end
if exist('y') & exist('x'), origin = [x y];
else origin = [0 0]; end


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plotSonarMode(sstruct,bstruct)
if isfield(sstruct,'setmode'), 
    val = getfield(sstruct,'setmode');
    if strcmp(val,'normal'), marker = '^';
    elseif strcmp(val,'lowpower'), marker = 'v';
    else marker = []; end
elseif isfield(sstruct,'setmodesbp'),
    val = getfield(sstruct,'setmodesbp');
    if strcmp(val,'on'), marker = '^';
    elseif strcmp(val,'off'), marker = 'v';
    else marker = []; end
else
    return
end
[x,y] = getXY(bstruct); 
if ~isempty(marker), 
    h = plot(x,y,['m' marker],'LineWidth',2,'MarkerSize',15);
    set(h,'Tag','MyTag')
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plotTrackline(bstruct,whichMap,prev_bstruct)
	global index, global firstPoint
[ix,iy] = getiXY(bstruct);
[x,y] = getXY(bstruct);
c = 'b';
if isfield(bstruct,'depth'),
	if getfield(bstruct,'depth') > 5, c = 'k'; end
end
h = line([ix x],[iy y],'Color',c,'Marker','s','LineStyle','-','LineWidth',2);
set(h,'Tag','MyTag','UserData',index)
plotCaptureRadius(bstruct,whichMap)
addnumber(x,y,int2str(index-1))
addsonar(ix,x,iy,y,whichMap,bstruct);
if firstPoint,
	h = plot(ix,iy,'ro','LineWidth',2,'MarkerSize',15);
	set(h,'Tag','MyTag')
elseif nargin == 3,
	[px,py] = getXY(prev_bstruct);
	plot([px ix],[py iy],'k:','LineWidth',2)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plotWaypoint(bstruct,whichMap,prev_bstruct)
	global index, global firstPoint
[x,y] = getXY(bstruct);
c = 'b';
if isfield(bstruct,'depth'),
	if getfield(bstruct,'depth') > 5, c = 'k'; end
end
h = plot(x,y,[c 'o'],'LineWidth',2,'MarkerSize',10);
set(h,'Tag','MyTag','UserData',index)
plotCaptureRadius(bstruct,whichMap)
addnumber(x,y,int2str(index-1))
if firstPoint,
	h = plot(x,y,'ro','LineWidth',2,'MarkerSize',15);
	set(h,'Tag','MyTag')
elseif nargin == 3,
	[px,py] = getXY(prev_bstruct);
	plot([px x],[py y],'k:','LineWidth',2)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
function addsonar(ix,x,iy,y,whichMap,bstruct)
	global fighandles, global origin, global mission
if isempty(fighandles), return, end
try,

    if get(fighandles.sonar_checkbox,'Value'),
	if (bstruct.duration > 170+bstruct.captureradius),
		
		range = selectSonarRange;
	    	
		verts = [];
		
		if (strcmp(whichMap,'geo')),
			% 1 degree = about 60 nm. 100 meter = .054 nm.
			range = distdim(range,'m','nm');
			range = nm2deg(range);
			az = azimuth(iy,ix,y,x,selectGeoid);
			[lat,lon] = reckon('rh',iy,ix,range,az+90);		
			verts = [lon,lat];
			[lat,lon] = reckon('rh',iy,ix,range,az+270);		
			verts = [verts;lon,lat];
			[lat,lon] = reckon('rh',y,x,range,az+270);		
			verts = [verts;lon,lat];
			[lat,lon] = reckon('rh',y,x,range,az+90);		
			verts = [verts;lon,lat];
            		verts = [verts;verts(1,1),verts(1,2)]; % close off the polygon
		else
			% old: ix, 90, x, 90, x, 270, ix, 270
			% utm and geo should make sure they match the rotation order
            		XY = [(x-ix);(y-iy)];
			R1 = [0 1; -1 0]; % 90 deg rotation
			R2 = [0 -1; 1 0]; % 270 deg rotation
			xy = (R1*XY*range/norm(XY,2) + [ix;iy]);
			verts = [xy(1),xy(2)];
			xy = (R2*XY*range/norm(XY,2) + [ix;iy]);
			verts = [verts;xy(1),xy(2)];
			xy = (R2*XY*range/norm(XY,2) + [x;y]);
			verts = [verts;xy(1),xy(2)];
			xy = (R1*XY*range/norm(XY,2) + [x;y]);
			verts = [verts;xy(1),xy(2)];
            		verts = [verts;verts(1,1),verts(1,2)]; % close off the polygon
		end
		[allverts] = saveVerts('save',verts(:,1),verts(:,2),[ix,x,iy,y]); % save latest set of patch vertices and trackline ends
	end
    end
    
catch, errorbox('addsonar: Can''t get sonar checkbox value'),
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%
function addnumber(x,y,str)
	global fighandles
if isempty(fighandles), return, end
try,
	if get(fighandles.number_checkbox,'Value'),
		addtext(x,y,str)
	end
catch, errorbox('Can''t get number checkbox value'), end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
function addtext(x,y,str)
	global textHandle
try,
	pos = get(textHandle,'Position');
	if isequal(pos,[x y 0]),
		tstr = get(textHandle,'String');
		set(textHandle,'String',[tstr ',' str])
		return
	end
catch, end
textHandle = text(x,y,[' ' str],'VerticalAlignment','top','FontSize',12, ...
	'FontWeight','bold','Tag','MyTag','Color',[0 .8 0]);

	
%%%%%%%%%%%%%%%%%%%%%%%%%%%
function plotCaptureRadius(bstruct,whichMap)
	global origin
if isempty(bstruct.captureradius), return, end
[x,y] = getXY(bstruct);
if strcmp(whichMap,'utm'),
	x = x - bstruct.captureradius;
	y = y - bstruct.captureradius;
	w = 2*bstruct.captureradius;
	rectangle('Curvature',[1 1],'Position',[x y w w],'LineStyle',':')
elseif strcmp(whichMap,'geo'),
	az = (1:2:360)';
	geoid = selectGeoid;
	y = y*ones(length(az),1);
	x = x*ones(length(az),1);
	r = bstruct.captureradius*ones(length(az),1);
	[lat,lon] = reckon('rh',y,x,r,az,geoid);
	plot(lon,lat,'k:')
end
