%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Calculating Dorado's waypoints (the three vertices of a triangular
% transect around a drifting ESP).
%
% August 2012.
%
% Yanwu Zhang
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
radiusearth = 6371e3; % In meters.
hr2min = 60; % 1 hour = 60 minutes
min2sec = 60; % 1 minute = 60 seconds
day2hr = 24; % 1 day = 24 hours
hr2sec = hr2min * min2sec;
day2sec = day2hr * hr2sec;
deg2rad = pi/180.0;
%
Lat_lon_CALCOFI_Line67_Station70 = [36.12 -123.48];
distance_OneDegOfLat = radiusearth * (deg2rad*1.0);
distance_OneDegOfLon = ...
    (radiusearth*cos(Lat_lon_CALCOFI_Line67_Station70(1)*deg2rad)) * (deg2rad*1.0);
ratio_x_to_y = cos(Lat_lon_CALCOFI_Line67_Station70(1)*deg2rad);
%
radius_of_circle_circumscribed_in_triangle = 3e3; % In meters.
distance_center_to_vertex_of_triangle = radius_of_circle_circumscribed_in_triangle * 2;
length_one_side_of_triangle = ...
    (radius_of_circle_circumscribed_in_triangle / tan(30.0*deg2rad)) * 2;
%
%%%%%%%%%%%%%%%%%%%% Enter parameters %%%%%%%%%%%%%%%%%%%%%%%%
Lat_lon_ESP_start = ...
    input('latitude and longitude (in decimal degrees) of ESP''s start location (e.g., [36.12 -123.48]) = ___ ? ');
speed_ESP = input('Estimated drifting speed of ESP (in m/s) = ___ ? ');
direction_ESP = input('Estimated drifting direction of ESP (in degrees. North is zero degree. East is 90 degrees.) = ___ ? ');
Lat_lon_Dorado_start = ...
    input('latitude and longitude (in decimal degrees) of Dorado''s start location (e.g., [36.15 -123.42]) = ___ ? ');
horiz_speed_Dorado = input('Dorado''s horizontal component of velocity (in m/s) = ___ ? ');
num_LapsOfTriangle_Dorado = input('Number of laps of the triangle to be traveled by Dorado using this set of triangular waypoints = ___ ? ');
%
%Lat_lon_ESP_start = Lat_lon_CALCOFI_Line67_Station70;
%speed_ESP = 0.2; % Estimated drifting speed of ESP (in m/s).
%direction_ESP = 165.0; % Estimated drifting direction (in degrees. North is zero degree. East is 90 degrees).
%horiz_speed_Dorado = 1.1; % In m/s.
%num_LapsOfTriangle_Dorado = 1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
time_OneLapDorado = (length_one_side_of_triangle * 3) / horiz_speed_Dorado;
%
%
% Preliminary result:
%
time_drift_ESP = time_OneLapDorado * num_LapsOfTriangle_Dorado;
distance_drift_ESP = speed_ESP * time_drift_ESP;
deltaY_drift_ESP = distance_drift_ESP * cos(direction_ESP*deg2rad);
deltaX_drift_ESP = distance_drift_ESP * sin(direction_ESP*deg2rad);
%
Lat_lon_ESP_end(1) = Lat_lon_ESP_start(1) + (deltaY_drift_ESP/distance_OneDegOfLat);
Lat_lon_ESP_end(2) = Lat_lon_ESP_start(2) + (deltaX_drift_ESP/distance_OneDegOfLon);
%
Lat_lon_ESP_onethird(1) = Lat_lon_ESP_start(1) + (deltaY_drift_ESP/3.0/distance_OneDegOfLat);
Lat_lon_ESP_onethird(2) = Lat_lon_ESP_start(2) + (deltaX_drift_ESP/3.0/distance_OneDegOfLon);
%
% Now set the one-third-point of the ESP-drift segment as the center of the triangle, also the origin of coordinate.
% Then calculate the lat/lon of the three vertices of Dorado's triangluar transect.
%
northing_vertex1 = ...
    distance_center_to_vertex_of_triangle * cos(direction_ESP*deg2rad);
easting_vertex1 = ...
    distance_center_to_vertex_of_triangle * sin(direction_ESP*deg2rad);
%
northing_vertex2 = ...
    distance_center_to_vertex_of_triangle * cos((direction_ESP+120.0)*deg2rad);
easting_vertex2 = ...
    distance_center_to_vertex_of_triangle * sin((direction_ESP+120.0)*deg2rad);
%
northing_vertex3 = ...
    distance_center_to_vertex_of_triangle * cos((direction_ESP-120.0)*deg2rad);
easting_vertex3 = ...
    distance_center_to_vertex_of_triangle * sin((direction_ESP-120.0)*deg2rad);
%
Lat_lon_vertex1(1) = Lat_lon_ESP_onethird(1) + (northing_vertex1/distance_OneDegOfLat);
Lat_lon_vertex1(2) = Lat_lon_ESP_onethird(2) + (easting_vertex1/distance_OneDegOfLon);
%
Lat_lon_vertex2(1) = Lat_lon_ESP_onethird(1) + (northing_vertex2/distance_OneDegOfLat);
Lat_lon_vertex2(2) = Lat_lon_ESP_onethird(2) + (easting_vertex2/distance_OneDegOfLon);
%
Lat_lon_vertex3(1) = Lat_lon_ESP_onethird(1) + (northing_vertex3/distance_OneDegOfLat);
Lat_lon_vertex3(2) = Lat_lon_ESP_onethird(2) + (easting_vertex3/distance_OneDegOfLon);
%
%
figure; orient landscape; hold on;
H_ESP_start = plot(Lat_lon_ESP_start(2), Lat_lon_ESP_start(1), ...
    'rx', 'linewidth', 1.5);
set(H_ESP_start, 'markersize', 10.0);
H_ESP_track = plot([Lat_lon_ESP_start(2) Lat_lon_ESP_onethird(2) Lat_lon_ESP_end(2)], ...
    [Lat_lon_ESP_start(1) Lat_lon_ESP_onethird(1) Lat_lon_ESP_end(1)], ...
    'r-', 'linewidth', 1.5);
H_CenterOfTriangle = plot(Lat_lon_ESP_onethird(2), Lat_lon_ESP_onethird(1), ...
    'ro', 'linewidth', 1.5);
set(H_CenterOfTriangle, 'markerfacecolor', 'r', 'markersize', 10.0);
%Harrow = arrow([Lat_lon_ESP_onethird(2) Lat_lon_ESP_onethird(1)], ...
%    [Lat_lon_ESP_end(2) Lat_lon_ESP_end(1)], 40.0, 'baseangle', 20.0, 'tipangle', 25.0, 'width', 0.5);
%set(Harrow, 'edgecolor', [1.0 0.0 0.0], 'facecolor', [1.0 0.0 0.0]);
H_Dorado_start = plot(Lat_lon_Dorado_start(2), Lat_lon_Dorado_start(1), ...
    'b*', 'linewidth', 1.5);
set(H_Dorado_start, 'markersize', 10.0);
H_triangle = plot([Lat_lon_vertex1(2) Lat_lon_vertex2(2) Lat_lon_vertex3(2) Lat_lon_vertex1(2)], ...
    [Lat_lon_vertex1(1) Lat_lon_vertex2(1) Lat_lon_vertex3(1) Lat_lon_vertex1(1)], 's--', 'linewidth', 1.5);
Htext_vertex1 = text(Lat_lon_vertex1(2)+0.002, Lat_lon_vertex1(1)+0.002, '1');
set(Htext_vertex1, 'fontsize', 14);
Htext_vertex1 = text(Lat_lon_vertex2(2)+0.002, Lat_lon_vertex2(1)+0.002, '2');
set(Htext_vertex1, 'fontsize', 14);
Htext_vertex1 = text(Lat_lon_vertex3(2)+0.002, Lat_lon_vertex3(1)+0.002, '3');
set(Htext_vertex1, 'fontsize', 14);
Hlegend = legend([H_ESP_start H_ESP_track H_Dorado_start H_CenterOfTriangle], ...
    'ESP start location', 'ESP drifting track', 'Dorado start location', 'Center of triangle (at 1/3 of ESP track)');
set(Hlegend, 'fontsize', 12);
xlabel('Longitude', 'fontsize', 12);
ylabel('Latitude', 'fontsize', 12);
title('Preliminary', 'fontsize', 18);
set(gca, 'dataaspectratio',[1.0 ratio_x_to_y ratio_x_to_y], ...
    'plotboxaspectratio',[1.0 ratio_x_to_y ratio_x_to_y], 'fontsize', 12);
%
%
index_vertex_DoradoFirstReaches = input('Dorado first reaches which vertex (1, 2, or 3) = ___ ? ');
switch index_vertex_DoradoFirstReaches
    case 1
        Lat_lon_vertex_DoradoFirstReaches = Lat_lon_vertex1;
    case 2
        Lat_lon_vertex_DoradoFirstReaches = Lat_lon_vertex2;
    case 3
        Lat_lon_vertex_DoradoFirstReaches = Lat_lon_vertex3;
end
%
% Final result:
% 
distance_Dorado_transit = ...
    sqrt(((Lat_lon_Dorado_start(2)-Lat_lon_vertex_DoradoFirstReaches(2))*distance_OneDegOfLon)^2 + ...
    ((Lat_lon_Dorado_start(1)-Lat_lon_vertex_DoradoFirstReaches(1))*distance_OneDegOfLat)^2);
time_Dorado_transit = distance_Dorado_transit / horiz_speed_Dorado;
time_drift_ESP = time_Dorado_transit + (time_OneLapDorado*num_LapsOfTriangle_Dorado);
distance_drift_ESP = speed_ESP * time_drift_ESP;
deltaY_drift_ESP = distance_drift_ESP * cos(direction_ESP*deg2rad);
deltaX_drift_ESP = distance_drift_ESP * sin(direction_ESP*deg2rad);
%
Lat_lon_ESP_end(1) = Lat_lon_ESP_start(1) + (deltaY_drift_ESP/distance_OneDegOfLat);
Lat_lon_ESP_end(2) = Lat_lon_ESP_start(2) + (deltaX_drift_ESP/distance_OneDegOfLon);
%
Lat_lon_ESP_onethird(1) = Lat_lon_ESP_start(1) + (deltaY_drift_ESP/3.0/distance_OneDegOfLat);
Lat_lon_ESP_onethird(2) = Lat_lon_ESP_start(2) + (deltaX_drift_ESP/3.0/distance_OneDegOfLon);
%
% Now set the one-third-point of the ESP-drift segment as the center of the triangle, also the origin of coordinate.
% Then calculate the lat/lon of the three vertices of Dorado's triangluar transect.
%
northing_vertex1 = ...
    distance_center_to_vertex_of_triangle * cos(direction_ESP*deg2rad);
easting_vertex1 = ...
    distance_center_to_vertex_of_triangle * sin(direction_ESP*deg2rad);
%
northing_vertex2 = ...
    distance_center_to_vertex_of_triangle * cos((direction_ESP+120.0)*deg2rad);
easting_vertex2 = ...
    distance_center_to_vertex_of_triangle * sin((direction_ESP+120.0)*deg2rad);
%
northing_vertex3 = ...
    distance_center_to_vertex_of_triangle * cos((direction_ESP-120.0)*deg2rad);
easting_vertex3 = ...
    distance_center_to_vertex_of_triangle * sin((direction_ESP-120.0)*deg2rad);
%
Lat_lon_vertex1(1) = Lat_lon_ESP_onethird(1) + (northing_vertex1/distance_OneDegOfLat);
Lat_lon_vertex1(2) = Lat_lon_ESP_onethird(2) + (easting_vertex1/distance_OneDegOfLon);
%
Lat_lon_vertex2(1) = Lat_lon_ESP_onethird(1) + (northing_vertex2/distance_OneDegOfLat);
Lat_lon_vertex2(2) = Lat_lon_ESP_onethird(2) + (easting_vertex2/distance_OneDegOfLon);
%
Lat_lon_vertex3(1) = Lat_lon_ESP_onethird(1) + (northing_vertex3/distance_OneDegOfLat);
Lat_lon_vertex3(2) = Lat_lon_ESP_onethird(2) + (easting_vertex3/distance_OneDegOfLon);
%
switch index_vertex_DoradoFirstReaches
    case 1
        Lat_lon_vertex_DoradoFirstReaches_final = Lat_lon_vertex1;
    case 2
        Lat_lon_vertex_DoradoFirstReaches_final = Lat_lon_vertex2;
    case 3
        Lat_lon_vertex_DoradoFirstReaches_final = Lat_lon_vertex3;
end
%
figure; orient landscape; hold on;
H_ESP_start = plot(Lat_lon_ESP_start(2), Lat_lon_ESP_start(1), ...
    'rx', 'linewidth', 1.5);
set(H_ESP_start, 'markersize', 10.0);
H_ESP_track = plot([Lat_lon_ESP_start(2) Lat_lon_ESP_onethird(2) Lat_lon_ESP_end(2)], ...
    [Lat_lon_ESP_start(1) Lat_lon_ESP_onethird(1) Lat_lon_ESP_end(1)], ...
    'r-', 'linewidth', 1.5);
H_CenterOfTriangle = plot(Lat_lon_ESP_onethird(2), Lat_lon_ESP_onethird(1), ...
    'ro', 'linewidth', 1.5);
set(H_CenterOfTriangle, 'markerfacecolor', 'r', 'markersize', 10.0);
%Harrow = arrow([Lat_lon_ESP_onethird(2) Lat_lon_ESP_onethird(1)], ...
%    [Lat_lon_ESP_end(2) Lat_lon_ESP_end(1)], 40.0, 'baseangle', 20.0, 'tipangle', 25.0, 'width', 0.5);
%set(Harrow, 'edgecolor', [1.0 0.0 0.0], 'facecolor', [1.0 0.0 0.0]);
H_Dorado_start = plot(Lat_lon_Dorado_start(2), Lat_lon_Dorado_start(1), ...
    'b*', 'linewidth', 1.5);
set(H_Dorado_start, 'markersize', 10.0);
plot([Lat_lon_Dorado_start(2) Lat_lon_vertex_DoradoFirstReaches_final(2)], ...
    [Lat_lon_Dorado_start(1) Lat_lon_vertex_DoradoFirstReaches_final(1)], '--');
H_triangle = plot([Lat_lon_vertex1(2) Lat_lon_vertex2(2) Lat_lon_vertex3(2) Lat_lon_vertex1(2)], ...
    [Lat_lon_vertex1(1) Lat_lon_vertex2(1) Lat_lon_vertex3(1) Lat_lon_vertex1(1)], 's--', 'linewidth', 1.5);
Htext_vertex1 = text(Lat_lon_vertex1(2)+0.002, Lat_lon_vertex1(1)+0.002, '1');
set(Htext_vertex1, 'fontsize', 14);
Htext_vertex1 = text(Lat_lon_vertex2(2)+0.002, Lat_lon_vertex2(1)+0.002, '2');
set(Htext_vertex1, 'fontsize', 14);
Htext_vertex1 = text(Lat_lon_vertex3(2)+0.002, Lat_lon_vertex3(1)+0.002, '3');
set(Htext_vertex1, 'fontsize', 14);
Hlegend = legend([H_ESP_start H_ESP_track H_Dorado_start H_CenterOfTriangle], ...
    'ESP start location', 'ESP drifting track', 'Dorado start location', 'Center of triangle (at 1/3 of ESP track)');
set(Hlegend, 'fontsize', 12);
xlabel('Longitude', 'fontsize', 12);
ylabel('Latitude', 'fontsize', 12);
title('Final', 'fontsize', 18);
set(gca, 'dataaspectratio',[1.0 ratio_x_to_y ratio_x_to_y], ...
    'plotboxaspectratio',[1.0 ratio_x_to_y ratio_x_to_y], 'fontsize', 12);
%
disp('[latitude longitude] of the three vertices of the triangle are:');
Lat_lon_vertex1
Lat_lon_vertex2
Lat_lon_vertex3
%
%cd C:\work\CANON_BIOSPACE\2012\September_2012;
%