close all
clear

% % % define 2 waypoints for back-and-forth mission
BFlat = [38.0676139 38.0671535]; % cal maritime
BFlon = [-122.2334361 -122.2319716];

% two more points on edges of box
EDlat = [38.0682473,38.0664355]; % cal maritime
EDlon = [-122.2323525 -122.2330123];

% % define 2 waypoints for back-and-forth mission
% BFlat = [38.0619248 38.0616946]; % sherman island
% BFlon = [-121.7840561 -121.7846623];
% 
% % two more points on edges of box
% EDlat = [38.0615658 38.0624148]; % sherman island
% EDlon = [-121.7842466 -121.7847508];

% define survey path
nsr = 2; % rows
nsc = 2; % columns

% convert to x-y
%set x,y zero point here
lat0 = 38.0686148; % wlat(1);
lon0 = -122.2322184; % wlon(1);

% convert lat/lon to x-y with simple spherical projection
D = 1.1119897552896e5; % m/degree
BFx = (BFlon-lon0)*D*cosd(lat0);
BFy = (BFlat-lat0)*D;

EDx = (EDlon-lon0)*D*cosd(lat0);
EDy = (EDlat-lat0)*D;

% get rotation angle for box- x axis parallel to back-forth path
theta = -atan2d((BFy(2)-BFy(1)),(BFx(2)-BFx(1)));
Rmtx = [cosd(theta) -sind(theta); sind(theta) cosd(theta)];

% convert to xp yp
BFp = Rmtx*[BFx;BFy];
EDp = Rmtx*[EDx;EDy];

% figure out box bounds
maxX=max(BFp(1,:));
minX=min(BFp(1,:));
maxY=max(EDp(2,:));
minY=min(EDp(2,:));
boxPlot = [[minX maxX maxX minX minX];[minY minY maxY maxY minY]];

% Make survey points
xpvals = linspace(minX,maxX,nsc);
ypvals = linspace(minY,maxY,nsr);

kk=1;
for jj=nsr:-1:1
    if (rem(jj,2)==0) % even
        for ii=1:nsc
            xp(kk)=xpvals(ii);
            yp(kk)=ypvals(jj);
            kk=kk+1;
        end
    else
        for ii=nsc:-1:1
            xp(kk)=xpvals(ii);
            yp(kk)=ypvals(jj);
            kk=kk+1;
        end
    end
end

% plot in xp yp coords
figure(2)
plot(BFp(1,:),BFp(2,:),'r+--', EDp(1,:),EDp(2,:),'b+',boxPlot(1,:),boxPlot(2,:),'k:',xp,yp,'bo-');
axis('equal')

% Rotate back
theta = atan2d((BFy(2)-BFy(1)),(BFx(2)-BFx(1)));
Rmtx = [cosd(theta) -sind(theta); sind(theta) cosd(theta)];
Pxy=Rmtx*[xp;yp];
Bxy = Rmtx*boxPlot;

% convert to lat-lon again
Plat = lat0+Pxy(2,:)/D;
Plon = lon0+Pxy(1,:)/D/cosd(lat0);

Blat = lat0+Bxy(2,:)/D;
Blon = lon0+Bxy(1,:)/D/cosd(lat0);

% Plot on map
figure(1)
%geoplot(BFlat,BFlon,'+--b', EDlat,EDlon,'bo',Plat,Plon,'m*:')
geoplot(BFlat,BFlon,'+--b', Plat([1:end,1]),Plon([1:end,1]),'m:')
legend('Baseline Waypoints','Survey Region')
% geoplot(BFlat,BFlon,'+--r', EDlat,EDlon,'b+',Blat,Blon,'k:')
% legend('Baseline Waypoints','Box Edge Points','Optimal route Box')
geobasemap streets
% for ii=1:length(Plat)
%     text(Plat(ii),Plon(ii),num2str(ii))
% end
for ii=1:length(BFlat)
    text(BFlat(ii),BFlon(ii),num2str(ii))
end

% export waypoints file
ff=fopen("surveyX.waypoints","w");
fprintf(ff,"QGC WPL 110\n");
fprintf(ff,"0	1	0	16	0	0	0	0	38.0686148	-122.2322184	2.590000	1\n");
ip=1;
for ii=1:length(Plat)
    fprintf(ff,"%d 0 0 19 10.00 0.0 0.0 0.0 %12.7f %14.7f 0 1\n",ip,Plat(ii),Plon(ii));
    ip=ip+1;
    fprintf(ff,"%d 0	3	42702	86.00000000	50.00000000	10.00000000	0.00000000	0.00000000	0.00000000	0.000000	1 \n",ip);
    ip=ip+1;
end
fclose(ff);