%% plot ITD + natural frequencies
l = 0.25;   % [m]

sizeMS = size(MS_A);

% degrees of freedom / number of sensors
n = sizeMS(1,1);

% length of 1 part of support
p = l/n;

% x points
for i = 1:n
    x(i) = i*p;
end

%% axis x, y ,z values
% support dimensions
a = 0.25;
b = 0.1;

% vectors of y dim (same for all modes)
y_A = [fliplr(x), 0];
y_B = [fliplr(x), 0];

Y_AB = [y_A; y_B];

% vectors of x dim (same for all modes)
x_A(1:length(y_A)) = b/2;
x_B(1:length(y_B)) = -b/2;

X_AB = [x_A; x_B];

% vectors of z dim
% 3rd mode
z_A1 = [real(MS_A(:, 9))', 0];
z_B1 = [real(MS_B(:, 9))', 0];

Z_AB1 = [z_A1; z_B1];

% 2nd mode
z_A2 = [real(MS_A(:, 7))', 0];
z_B2 = [real(MS_B(:, 7))', 0];

Z_AB2 = [z_A2; z_B2];

% 1st mode
z_A3 = [real(MS_A(:, 5))',0];
z_B3 = [real(MS_B(:, 5))',0];

Z_AB3 = [z_A3; z_B3];

% 4th mode
z_A4 = [real(MS_A(:, 3))',0];
z_B4 = [real(MS_B(:, 3))',0];

Z_AB4 = [z_A4; z_B4];

%% final natural frequency
% calculated as average of f_A and f_B
f = (f_A + f_B)/2;

%% plot 3D
figure()

% viewpoint set
az = 135; % rotation around z axis
el = 45; % top view

% subplot 1st mode
subplot(2, 2, 1)
hSurface = surf(X_AB, Y_AB, -Z_AB3);
set(hSurface, 'FaceColor', [1 0 0], 'FaceAlpha', 0.5);

view(az, el);
axis([-b/2, b/2, 0, a+0.01, -0.6, 0.6])
title(sprintf('1st mode shape, f_1 = %.3f Hz', f(3)))
xlabel('w [m]')
ylabel('l [m]')
zlabel('\psi [-]')

% subplot 2nd mode
subplot(2, 2, 2)
hSurface = surf(X_AB, Y_AB, -Z_AB2);
set(hSurface, 'FaceColor', [1 0 0], 'FaceAlpha', 0.5);

view(az, el);
axis([-b/2, b/2, 0, a+0.01, -0.6, 0.6])
title(sprintf('2nd mode shape, f_2 = %.3f Hz', f(2)))
xlabel('w [m]')
ylabel('l [m]')
zlabel('\psi [-]')

% subplot 3rd mode
subplot(2, 2, 3)
hSurface = surf(X_AB, Y_AB, Z_AB1);
set(hSurface, 'FaceColor', [1 0 0], 'FaceAlpha', 0.5);

view(az, el);
axis([-b/2, b/2, 0, a+0.01, -0.6, 0.6])
title(sprintf('3rd mode shape, f_3 = %.3f Hz', f(1)))
xlabel('w [m]')
ylabel('l [m]')
zlabel('\psi [-]')

% subplot 4th mode
subplot(2, 2, 4)
hSurface = surf(X_AB, Y_AB, Z_AB4);
set(hSurface, 'FaceColor', [1 0 0], 'FaceAlpha', 0.5);

view(az, el);
axis([-b/2, b/2, 0, a+0.01, -0.6, 0.6])
title(sprintf('4th mode shape, f_4 = %.3f Hz', f(4)))
xlabel('w [m]')
ylabel('l [m]')
zlabel('\psi [-]')