clc;
clear;
close all;

%% Point A - Spherical Coordinate System
rA = 7;
thetaA = 40;
phiA = 30;

thetaA = deg2rad(thetaA);
phiA = deg2rad(phiA);

xA = rA*sin(thetaA)*cos(phiA);
yA = rA*sin(thetaA)*sin(phiA);
zA = rA*cos(thetaA);

%% Point B - Spherical Coordinate System
rB = 9;
thetaB = 50;
phiB = 130;

thetaB = deg2rad(thetaB);
phiB = deg2rad(phiB);

xB = rB*sin(thetaB)*cos(phiB);
yB = rB*sin(thetaB)*sin(phiB);
zB = rB*cos(thetaB);

%% Distance between A and B
d = sqrt((xB-xA)^2 + ...
         (yB-yA)^2 + ...
         (zB-zA)^2);

%% Display Results
fprintf('\n');
fprintf('====================================================\n');
fprintf('       SPHERICAL COORDINATE RESULTS\n');
fprintf('====================================================\n\n');

fprintf('Point A - Given in Spherical Coordinates\n');
fprintf('Original SCS: (r, theta, phi) = (7, 40, 30)\n');
fprintf('After conversion to Cartesian:\n');
fprintf('A = (%.3f, %.3f, %.3f)\n\n', xA, yA, zA);

fprintf('Point B - Given in Spherical Coordinates\n');
fprintf('Original SCS: (r, theta, phi) = (9, 50, 130)\n');
fprintf('After conversion to Cartesian:\n');
fprintf('B = (%.3f, %.3f, %.3f)\n\n', xB, yB, zB);

fprintf('====================================================\n');
fprintf('Distance between A and B:\n');
fprintf('d = %.3f units\n', d);
fprintf('====================================================\n');

%% 3-D Plot
figure;

plot3([xA xB], [yA yB], [zA zB], 'k--', 'LineWidth', 2);

hold on;

plot3(xA, yA, zA, 'bo', ...
      'MarkerSize', 8, 'MarkerFaceColor', 'b');

plot3(xB, yB, zB, 'ro', ...
      'MarkerSize', 8, 'MarkerFaceColor', 'r');

text(xA, yA, zA, '  A (SCS)', 'FontSize', 10);
text(xB, yB, zB, '  B (SCS)', 'FontSize', 10);

xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');

title('Distance Between Two Points in Spherical Coordinates');

grid on;
axis equal;

legend('Distance AB', 'Point A', 'Point B');

view(45, 30);