close all
clear all

shape = 'cyl';
    
%bar quantities are dimension values that the wdra3d results were run for.

a_bar = 1;  %radius
d_bar = 4;  %draft
rho_bar = 1025;  %density
g_bar = 9.81;   %gravity
A_bar = 1.0;  %Wave amplitude
S_bar= pi*a_bar^2;
switch (shape)
    case ('cyl')
        V_bar= d_bar*S_bar;
    case ('hemi')
        V_bar= (2*pi*a_bar^3)/3;
end
m_bar = rho_bar*V_bar;  %Floating body, body mass equals mass of displaced water.


%primed values are non-dimensional
d_prime = d_bar/a_bar;  %aspect ratio
A_prime = A_bar/a_bar;  %wave amplitude



%Unmarked values are dimensional values in for the results plotted here
g = 9.81;
rho = 1025;
a = 1;
d = d_prime*a;
S = pi*a^2;
switch (shape)
    case ('cyl')
        V = d*S;
    case ('hemi')
        V = (2*pi*a^3)/3;
end
m = rho*V;

switch (shape)
    case ('cyl')
        filename = ['t_cyl_d' num2str(d_bar)]
    case ('hemi')
        filename = ['hemisphere_fine']
end

%Read dimensional Results
FD_coefs = Readwdra3dResults([filename '.out']);

%Nondimensionalize Results  
k_prime = FD_coefs.k*a_bar;
w_prime = FD_coefs.w*sqrt(a_bar/g_bar);
T_prime = FD_coefs.T*sqrt(g_bar/a_bar);

%(skipping cross coupling elements for now)
for i = 1:3
  mu_prime{i,i} = real(FD_coefs.F{i,i})/(rho_bar*V_bar);
  lambda_prime{i,i} = imag(FD_coefs.F{i,i})./(rho_bar*V_bar*FD_coefs.w);
  X_prime{i} = (FD_coefs.F{7,i}/A_bar)./(rho_bar*V_bar*(FD_coefs.w).^2);
end
for i = 4:6
  mu_prime{i,i} = real(FD_coefs.F{i,i})/(rho_bar*V_bar*S_bar);
  lambda_prime{i,i} = imag(FD_coefs.F{i,i})./(rho_bar*V_bar*S_bar*FD_coefs.w);
  X_prime{i} = (FD_coefs.F{7,i}/A_bar)./(rho_bar*V_bar*a_bar*(FD_coefs.w).^2);
end

%Plot nondimensional results
fh1 = figure('Position', [181   637   560   420]);
subplot(2,1,1)
plot(k_prime,mu_prime{3,3}, ...
     k_prime,lambda_prime{3,3}, ...
     k_prime,real(X_prime{3}), ...
     k_prime,imag(X_prime{3}) ...
  )
xlabel(['$ \bar{k} \bar{a}  $'],'interpreter','latex','fontsize',18);
legend('mu','lambda','real(F)','imag(F)');
title('Nondimensional Heave Hydrodynamics');

subplot(2,1,2)
plot(k_prime,mu_prime{5,5}, ...
     k_prime,lambda_prime{5,5}, ...
     k_prime,real(X_prime{5}), ...
     k_prime,imag(X_prime{5}) ...
  )
xlabel(['$ \bar{k} \bar{a}  $'],'interpreter','latex','fontsize',18);
legend('mu','lambda','real(F)','imag(F)');
title('Nondimensional Roll Hydrodynamics');

    
%Dimensionalize Results
k = k_prime/a;
w = w_prime/sqrt(a/g);
T = T_prime/sqrt(g/a);

%(skipping cross coupling elements for now)
for i = 1:3
  mu{i,i} = mu_prime{i,i}*(rho*V);
  lambda{i,i} = lambda_prime{i,i}.*(rho*V*w);
  X{i} = X_prime{i}.*(rho*V*w.^2);
end
for i = 4:6
  mu{i,i} = mu_prime{i,i}*(rho*V*S);
  lambda{i,i} = lambda_prime{i,i}.*(rho*V*S*w);
  X{i} = X_prime{i}.*(rho*V*a*w.^2);
end



%Plot dimensional Hydrodynamics
fh1 = figure('Position', [181   637   560   420]);
subplot(2,1,1)
plot(T,mu{3,3}, ...
     T,lambda{3,3}, ...
     T,real(X{3}), ...
     T,imag(X{3}) ...
  )
xlabel(['Wave Period (s)']);
legend('mu','lambda','real(F)','imag(F)');
title('Dimensional Heave Hydrodynamics');

subplot(2,1,2)
plot(T,mu{5,5}, ...
     T,lambda{5,5}, ...
     T,real(X{5}), ...
     T,imag(X{5}) ...
  )
xlabel(['Wave Period (s)']);
legend('mu','lambda','real(F)','imag(F)');
title('Dimensional Roll Hydrodynamics');


i = sqrt(-1);
%Compute motion transfer functions
%Heave Motions
RAO{3} = X{3}./(-(m+mu{3,3}).*w.^2+i*w.*lambda{3,3}+rho*g*S);
figure('Position', [ 180   116   560   420]);
subplot(2,1,1)
plot(T,real(RAO{3}),T,imag(RAO{3}),T,abs(RAO{3}))
xlabel(['Wave Period (s)']);
title('Cylinder Heave Motions');
subplot(2,1,2)
plot(T,angle(RAO{3})*180/pi)
xlabel(['Wave Period (s)']);
ylabel('Phase angle (deg)');

%Compute motion transfer functions
GM = .5*d - 0.75*d + ((1/64)*pi*(2*a)^4)/V;
I = rho*V*(3*a^2+d^2)/12;

%Roll Motions
RAO{5} = X{5}./(-(I+mu{5,5}).*w.^2+i*w.*lambda{5,5}+rho*V*g*GM);
figure('Position', [ 180   116   560   420]);
subplot(2,1,1)
plot(T,real(RAO{5}),T,imag(RAO{5}),T,abs(RAO{5}))
xlabel(['Wave Period (s)']);
title('Cylinder Roll Motions');
subplot(2,1,2)
plot(T,angle(RAO{5})*180/pi)
xlabel(['Wave Period (s)']);
ylabel('Phase angle (deg)');

