close all
clear all



rho = 1025;
g = 9.81;


%Buoy Characteristics
buoyRadius = 66.0 * 0.0254 / 2.0;               %meters (66" in diameter)
mass = 691.0;                               %from SolidWorks in kg
S = pi * buoyRadius^2;        
displ = (mass / rho);
S11 = (pi * (buoyRadius)^4) / 4;    %(pi*r^4)/4 for a circle
I = 343.4;
zB = -0.19;
%zG = -0.2; %Andy's original value
zG = -.45;  %+10 inches




filename = 'WAMIT_Results\ESP_drifter';
%**************************************************************************

disp('Reading frequency domain hydrodynamic coefficients');
%Compute hydrodynamic coefficients 
FD_coefs = ReadWamitFD(filename);

tps = FD_coefs.tps;  %time period in seconds
tps = tps(3:end);

w = 2*pi./tps;

add_heave = rho*displ*FD_coefs.X{3,3}(3:end); %added mass of floating body (heave)
damp_heave = rho*displ*FD_coefs.Y{3,3}(3:end)./w;  %damping of floating body (heave)

add_pitch = rho*FD_coefs.X{5,5}(3:end); %added mass of floating body (heave)
damp_pitch = rho*FD_coefs.Y{5,5}(3:end);%./w;  %damping of floating body (heave)

efr = rho*g*FD_coefs.efr;
efi = rho*g*FD_coefs.efi;

Fex_heave = efr(:,3) + i*efi(:,3);
Fex_pitch = efr(:,5) + i*efi(:,5);





%Calculation of Response Amplitude Operator as a function of frequency
% response amplitude operator of Floating Body

for j = 1:99; 
  T(j) =2*pi/w(j);

  %Calculate Floating body Heave RAO 
  Heave_RAO(j) = Fex_heave(j)/(-(w(j).^2)*(add_heave(j)+mass)+rho*g*S+i*w(j)*damp_heave(j));
  
  %Calculate Floating body Roll RAO
  c55 = rho*g*displ*(S11/displ+zB-zG);
  Pitch_RAO(j) = Fex_pitch(j)/(-(w(j).^2)*(add_pitch(j)+I)+c55+i*w(j)*damp_pitch(j));
end
          
% 
% figure('Position', [50 50 1100 800])
% set(gcf,'color','white');
% 
% plot(w/(2*pi), abs(Heave_RAO),'Color','b','linewidth',1.5);
% ylabel('Heave RAO (m/m)','FontName','times', 'FontSize',12,'FontWeight','bold');
% xlabel('freq (Hz)','FontName','times', 'FontSize',12,'FontWeight','bold');
% set(gca,'FontName','times', 'FontSize',12);
% 

figure('Position', [50 50 1100 800])
set(gcf,'color','white');

plot(w/(2*pi), g*abs(Pitch_RAO)./(w'.^2),'Color','b','linewidth',1.5);
ylabel('Pitch RAO (Buoy Pitch/Wave Slope)','FontName','times', 'FontSize',12,'FontWeight','bold');
xlabel('freq (Hz)','FontName','times', 'FontSize',12,'FontWeight','bold');
set(gca,'FontName','times', 'FontSize',12);






% 
% figure('Position', [50 50 1100 800])
% set(gcf,'color','white');
% 
% subplot(2,1,1),plot(w,add_heave)
% xlabel('w');
% ylabel('Added Mass');
% legend('Heave');
% set(gca,'FontName','times');
% 
% subplot(2,1,2),plot(w,damp_heave)
% xlabel('w');
% ylabel('Damping');
% legend('Heave');
% set(gca,'FontName','times');
% 
% figure('Position', [50 50 1100 800])
% set(gcf,'color','white');
% 
% subplot(2,1,1),plot(w,efr(:,3));
% xlabel('w');
% ylabel('Re[Exciting Force]');
% legend('Heave');
% set(gca,'FontName','times');
% 
% subplot(2,1,2),plot(w,efi(:,3));
% xlabel('w');
% ylabel('Im[Exciting Force]');
% legend('Heave');
% set(gca,'FontName','times');
% 
% 
% subplot(2,1,1),plot(w,add_pitch)
% xlabel('w');
% ylabel('Added Moment of Inertia');
% legend('Pitch');
% set(gca,'FontName','times');
% 
% subplot(2,1,2),plot(w,damp_pitch)
% xlabel('w');
% ylabel('Damping');
% legend('Pitch');
% set(gca,'FontName','times');
% 
% 
% figure('Position', [50 50 1100 800])
% set(gcf,'color','white');
% 
% subplot(2,1,1),plot(w,efr(:,5));
% xlabel('w');
% ylabel('Re[Exciting Force]');
% legend('Pitch');
% set(gca,'FontName','times');
% 
% subplot(2,1,2),plot(w,efi(:,5));
% xlabel('w');
% ylabel('Im[Exciting Force]');
% legend('Pitch');
% set(gca,'FontName','times');




