close all
clear all

meters_per_foot = .3048;

Points = {'Buoy Planned','Buoy Actual', 'East Anchor Planned', 'East Anchor Actual', 'West Anchor Planned', 'West Anchor Actual'};

Lat(1) = 36.7723;  % Buoy Planned
Lat(2) = 36.7722;  % Buoy Actual
Lat(3) = 36.77230;   % East Anchor Planned
Lat(4) = 36.77254;   % East Anchor Actual
Lat(5) = 36.7723;   % West Anchor Planned
Lat(6) = 36.77209;   % West Anchor Actual Deployed
Lat(7) = 36.77185;   % West Anchor Actual Adjusted


Lon(1) = -121.8645;   % Buoy Planned
Lon(2) = -121.864250; % Buoy Actual
Lon(3) = -121.8623;   % East Anchor Planned
Lon(4) = -121.86189;  % East Anchor Actual
Lon(5) = -121.8668;   % West Anchor Planned
Lon(6) = -121.86721;  % West Anchor Actual Deployed
Lon(7) = -121.8666;  % West Anchor Actual Adjusted

Depth = NaN(7,1);
Depth(2) = 255*meters_per_foot;
Depth(4) = 244*meters_per_foot;




[Easting, Northing, UTMZone] = deg2utm(Lat,Lon);

Dist = haversineDist(Lat(1), Lon(1), Lat(2), Lon(2))

Dist = haversineDist(Lat(3), Lon(3), Lat(4), Lon(4))

Dist = haversineDist(Lat(5), Lon(5), Lat(6), Lon(6))


Dist = haversineDist(Lat(2), Lon(2), Lat(4), Lon(4))

Dist = haversineDist(Lat(2), Lon(2), Lat(7), Lon(7))

figure

plot(Lon(1:2:5),Lat(1:2:5),'o');
hold all
plot(Lon(2:2:6),Lat(2:2:6),'x');
plot(Lon(7),Lat(7),'kx');


axis equal
grid on

figure
plot((Easting([2 4 7])-Easting(2)),Northing([2 4 7])-Northing(2),'x')
hold all;
axis equal
grid on


Angle = atan2(Northing(7)-Northing(2),-(Easting(7)-Easting(2)));

SwiftPos_x(1) = -185;  SwiftPos_y(1) = -50;  SwiftDepth(1) = 263*meters_per_foot;
SwiftPos_x(2) = -185;  SwiftPos_y(2) = 50;  SwiftDepth(2) = 265*meters_per_foot;
SwiftPos_x(3) = -85;  SwiftPos_y(3) = 50;  SwiftDepth(3) = 260*meters_per_foot;
SwiftPos_x(4) = -85;  SwiftPos_y(4) = -50;  SwiftDepth(4) = 259*meters_per_foot;

SwiftEasting = Easting(2) + SwiftPos_x*cos(Angle) + SwiftPos_y*sin(Angle); 
SwiftNorthing = Northing(2) - SwiftPos_x*sin(Angle) + SwiftPos_y*cos(Angle); 


f
[SwiftLat, SwiftLon] = utm2deg(SwiftEasting,SwiftNorthing,['10 S'; '10 S'; '10 S'; '10 S'])



plot(SwiftEasting-Easting(2),SwiftNorthing-Northing(2),'o');



figure
plot(Lon([2 4 7]),Lat([2 4 7]),'x');
hold all;
plot(SwiftLon,SwiftLat,'o');
axis equal
grid on
legend('WEC','Swift Buoys');

i = 1; disp([num2str(i) ': 23SW: ' num2str(SwiftLat(i),'%.5f') '   ' num2str(SwiftLon(i),'%.5f')  '  Depth = ' num2str(SwiftDepth(i),'%.1f') 'm']);
disp(['A:  ' num2str(Lat(7),'%.5f') '   ' num2str(Lon(7),'%.5f')]);
i = 2; disp([num2str(i) ': 22NW: ' num2str(SwiftLat(i),'%.5f') '   ' num2str(SwiftLon(i),'%.5f')  '  Depth = ' num2str(SwiftDepth(i),'%.1f') 'm']);
disp(['W:  ' num2str(Lat(2),'%.5f') '   ' num2str(Lon(2),'%.5f') '  Depth = ' num2str(Depth(2),'%.1f') 'm']);
i = 3; disp([num2str(i) ': 25NE: ' num2str(SwiftLat(i),'%.5f') '   ' num2str(SwiftLon(i),'%.5f')  '  Depth = ' num2str(SwiftDepth(i),'%.1f') 'm']);
i = 4; disp([num2str(i) ': 24SE: ' num2str(SwiftLat(i),'%.5f') '   ' num2str(SwiftLon(i),'%.5f')  '  Depth = ' num2str(SwiftDepth(i),'%.1f') 'm']);
disp(['D:  ' num2str(Lat(4),'%.5f') '   ' num2str(Lon(4),'%.5f') '  Depth = ' num2str(Depth(4),'%.1f') 'm']);


num2str(1000*haversineDist(SwiftLat(1), SwiftLon(1), SwiftLat(2), SwiftLon(2)),'%.3f')
num2str(1000*haversineDist(SwiftLat(3), SwiftLon(3), SwiftLat(4), SwiftLon(4)),'%.3f')

num2str(1000*haversineDist(SwiftLat(1), SwiftLon(1), SwiftLat(3), SwiftLon(3)),'%.3f')
num2str(1000*haversineDist(SwiftLat(2), SwiftLon(2), SwiftLat(4), SwiftLon(4)),'%.3f')

num2str(1000*haversineDist(SwiftLat(2), SwiftLon(2), SwiftLat(3), SwiftLon(3)),'%.3f')
num2str(1000*haversineDist(SwiftLat(1), SwiftLon(1), SwiftLat(4), SwiftLon(4)),'%.3f')

num2str(1000*haversineDist(Lat(2), Lon(2), SwiftLat(1), SwiftLon(1)),'%.3f')
num2str(1000*haversineDist(Lat(2), Lon(2), SwiftLat(2), SwiftLon(2)),'%.3f')

num2str(1000*haversineDist(Lat(2), Lon(2), SwiftLat(3), SwiftLon(3)),'%.3f')
num2str(1000*haversineDist(Lat(2), Lon(2), SwiftLat(4), SwiftLon(4)),'%.3f')