%% assemble_survey.m
%% This code will concatenate individual missions, previously processed via 
%% process_auvmission.m, into a contiguous [X,Y,Z] survey.   

clear all; plotit = 1; 

m1c1a=0; m1c1b=0;

survey_assembly_setup; missionlist

[N x] = size(missionlist);

%% Initialize Variables
s=[]; t=[]; dens=[]; f=[]; obs=[]; o2=[]; ph=[]; Depth=[]; time=[]; 
Roll=[]; Pitch=[]; Yaw=[]; ILAT=[]; ILON=[]; IDIST=[0]; XPOS=[]; YPOS=[];

for M = 1:N;
 eval(['cd ' missionlist(M,:)]); load ctd %mission_sd 

 if m1c1a & M==N	% add W-E segment of mission 5 (pre-turn)
  ep(2)=NEcorner(1); ep(4)=NEcorner(2); 
  idx=find(diff(yaw)>5); idx=[1:idx(1)]; 
  O2=O2(idx); S=S(idx); T=T(idx); PH=PH(idx); depth=depth(idx); mtime=mtime(idx);
  roll=roll(idx); pitch=pitch(idx); yaw=yaw(idx); Xpos=Xpos(idx); Ypos=Ypos(idx);
  F=F(idx); OBS=OBS(idx); D=D(idx);
 end

 if m1c1b & M==1	% add N-S segment of mission 5 (post-turn)
  ep(1)=NEcorner(1); ep(3)=NEcorner(2);
  idx=find(diff(yaw)>5); idx=[idx(1)+5:length(yaw)]; 
  O2=O2(idx); S=S(idx); T=T(idx); PH=PH(idx); depth=depth(idx); mtime=mtime(idx);
  roll=roll(idx); pitch=pitch(idx); yaw=yaw(idx); Xpos=Xpos(idx); Ypos=Ypos(idx);
  F=F(idx); OBS=OBS(idx); D=D(idx);
 end

 %% now that mission endpoints are adjusted, calculate distance 
 [dist phas]=sw_dist(ep([1 2]),ep([3 4]),'km');

 o2=[o2;O2]; s=[s;S]; t=[t;T]; ph=[ph;PH]; Depth=[Depth;depth]; time=[time;mtime];
 Roll=[Roll;roll]; Pitch=[Pitch;pitch]; Yaw=[Yaw;yaw]; XPOS=[XPOS;Xpos]; YPOS=[YPOS;Ypos]; 
 f=[f;F]; obs=[obs;OBS]; dens=[dens;D];

 ILAT=[ILAT; [linspace(ep(1),ep(2),length(S))]'];
 ILON=[ILON; [linspace(ep(3),ep(4),length(S))]'];
 IDIST=[IDIST; max(IDIST)+[linspace(0,dist,length(S))]'];
 if M<N; 
  EP(M,:)= ep([1 3]); 
 else; 
  EP=[EP;ep([1 3;2 4])]; 
 end
 cd ..
 % eval(['cd ' wdir]);
end

eplat=EP(:,1); eplon=EP(:,2); 
IDIST(1)=[];

if plotit
 figure(1); clf; 
 set(gcf,'units','pixels','position',[20 50 350 400],'color',[1 1 1],'menubar','none');
 axes('position',[.2 .15 .7 .7]);
 AX=[-122.5 -121.7 36.5 37]; gry=[.7 .7 .7];
 wclandfill(AX,gry); hold on; plot_iso; 
 set(gca,'tickdir','out','fontsize',12,'fontweight','bold');
 plot(ILON,ILAT,'r'); p=plot(eplon,eplat,'ro'); set(p,'markerfacecolor','r');
 %% M1
 p=plot(-122.03,36.75,'ko'); 
 set(p,'markerfacecolor','k','markersize',8);
 tx=text(-122.03,36.73,'M1'); 
 set(tx,'fontweight','bold','fontsize',15,'color','k');
 
 figure(2); clf; set(gcf,'units','pixels','position',[10 100 800 500]);
 load wcmask; idx=find(LAT>=AX(3) & LAT<=AX(4) & LON>=AX(1) & LON<=AX(2));
 p=plot3(ILON,ILAT,Depth,'r.'); set(p,'markersize',3);
 grid on; set(gca,'zdir','reverse'); hold on; box on; 
 plot3(LON(idx),LAT(idx),0*idx,'g.'); 
 
end

eval(['save ' surveyfilename ' time ILAT ILON XPOS YPOS IDIST Depth t s dens o2 ph f obs Pitch Roll Yaw eplon eplat']);  

