clear all
close all

%At-sea Experiment Modeling Analysis
%MOOS buoy-plate

dt = 0.1;       %timestep size
tf = 500;
N = tf/dt;      %Number of timesteps to compute

%Set up "Buoy"

Buoy = RigidBody;
Buoy = set(Buoy,'name','Surface Buoy');
Buoy = set(Buoy,'dt',dt,'N',N);  %Set timesteps
Buoy = set(Buoy,'dof',6);  %With 6 DOF.
Buoy = set(Buoy,'Fixed',0);  %Set free to move
Buoy = set(Buoy,'mass',1179.34,'S',3.83274,'disp',1.65,'g',9.80665);
Buoy = set(Buoy,'x0',[0 0 0]);  %Equilibrium Position in global coordinates.
Buoy = set(Buoy,'xG',[0 0 -0.63]);
Buoy = set(Buoy,'xB',[0 0 -0.26]);
Buoy = set(Buoy,'S11',0.2208,'S22',.2208);  %S11 = S22 = (-1)(-1)(3.14159) m^4
I = zeros(3);
I(1,1) = 1998.19; I(2,2) = 1947.66; I(3,3) = 466.13;      
Buoy = set(Buoy,'I',I);
clear I;
Buoy = set(Buoy,'Area',[0.1414 0.1414 3.83274]);
Buoy = set(Buoy,'Cd',[1 1 0]);
Buoy = set(Buoy,'AreaW',[1 1 0]);
Buoy = set(Buoy,'CdW',[1 1 0]);

%Read Frequency Domain Hydrodynamic Data from file.

filename = 'test25a';
disp('Reading frequency domain hydrodynamic coefficients');
FD_coefs = ReadWamitFD(filename);
for i = 1:6 
   for j = 1:6 
      mu_inf(i,j) = 1025*FD_coefs.X{i,j}(2); 
   end
end
Buoy = set(Buoy,'mu_inf',mu_inf);
clear mu_inf;

%Compute Impulse Response Functions from Frequency Domain Results

disp('Computing impulse response functions'); 
L = ComputeWamitIRF_coefs(filename);


%Set up "Clump"
radius = .6096;  %in m = 2feet!two 2 ft radius discs - cut out of a 4 x 8 steel plate  
mass = 588.31;  %kg
thck = .0635;  %  compute based upon radius and mass and density of steel.
Clump = RigidBody;
Clump = set(Clump,'name','Clump');
Clump = set(Clump,'x0',[0  0 -25.0]);  %Equilibrium Position in global coordinates.
Clump = set(Clump,'dt',dt,'N',N);  %Set timesteps
Clump = set(Clump,'dof',6);  %With 6 DOF.
Clump = set(Clump,'Fixed',0);  %Set free to move
Clump = set(Clump,'mass',mass,'S',0,'disp',thck*pi*radius^2,'g',9.81);% thin square plate 2m and thickness = 0.0254 m 
Clump = set(Clump,'xG',[0 0 0]);
Clump = set(Clump,'xB',[0 0 0]);
Clump = set(Clump,'S11',0.0,'S22',0.0);  %m^4

% Flat Plate

I = zeros(3);
I(1,1) = 55.22; I(2,2) = 55.22; I(3,3) = 110.08;
Clump = set(Clump,'I',I);
clear I;
Clump = set(Clump,'Area',[2*pi*radius^2  2*pi*radius^2  pi*radius^2]);
Clump = set(Clump,'Cd',[0.01 0.01 1.17]);

mu_inf = zeros(6,6);     
mu_inf(1,1) = 0;                     
mu_inf(3,3) = 0.65*(4/3)*pi*1025*radius^3;    %flat circular plate m33= 0.65*(4/3)*pi*1025*radius^3,Newman pp 147
mu_inf(5,5) = 0.22*(8/15)*pi*1025*radius^5;   %m55 = 0.22*(8/15)*pi*1025*radius^5

Clump = set(Clump,'mu_inf',mu_inf);
clear mu_inf;

%Compute incident wave profile

%default values
A = 0;%Significant wave amplitude;
%T = 2;
%Type = 1 ( = monochromatic)
t = get(Buoy,'t');

Type = 2;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave or Specified Spectrum (Type = 2)

wave = IncWave;
wave = set(wave,'Type',Type);

%Input wave profile based on at-sea measurements

d_hs = load('081120091317pdt.txt');
ts1 = d_hs(:,2);
len1 = length(ts1)-1;
dt1 = 0.14;
tlen1 = dt1*len1;
t1 = 0:dt1:tlen1;

t2 = 0:0.1:tf;%t2 is equal to t used for model run

ts2= interp1(t1,ts1,t2);

[f2, Pxx2] = tseries2spectrum(t2,ts2);

%%units of spectral energy, Pxx2 (Power/Hz)
%%units of Frequency, f2 (Hz)
wave = set(wave,'freq',f2);
wave = set(wave,'spectrum',Pxx2);

disp('Computing incident wave profile');
t1= dt:dt:L{7,1}.breaks(end);
tx = [t t(end)+t1];
eta0 = eta(wave,0,tx);
 
figure('Position', [50 100 1200 600])
set(gcf,'color','white');
subplot(2,1,1), plot(t2,ts2);
set(gca,'FontName','times','FontSize',16);
ylabel('elevation (m)');
xlabel('time (s)');

subplot(2,1,2), plot(tx,eta0);
set(gca,'FontName','times','FontSize',16);
ylabel('elevation (m)');
xlabel('time (s)');
 


%Add forces to Buoy
Buoy = add_Force_fcn(Buoy,@F_HydroStatic);
Buoy = add_Force_fcn(Buoy,@F_Weight);
Buoy = add_Force_fcn(Buoy,@(body,RB,n) Mem_Radiation(body,RB,n,L));
Buoy = add_Force_fcn(Buoy,@(body,RB,n) F_Exciting(body,RB,n,L,eta0));
Buoy = add_Force_fcn(Buoy,@(body,RB,n) F_Drag_Inc(body,RB,n,'Surface Buoy',wave));
Buoy = add_Force_fcn(Buoy,@(body,RB,n) F_Wind(body,RB,n,[-10 0 0]));

%Add forces to Clump
Clump = add_Force_fcn(Clump,@F_HydroStatic);
Clump = add_Force_fcn(Clump,@F_Weight);
Clump = add_Force_fcn(Clump,@(body,RB,n) F_Drag_Inc(body,RB,n,'Surface Buoy',wave));

%Add forces that act on two bodies
p_aB = [0.0  0.0  -2.09];  %Tether attachment point on Buoy (local CoordSys).
p_aC = [0.0  0.0  1.378];  %Tether attachment point on Clump (local CoordSys).

gamma = 1.4;
[Spring_Force_Coeffs] = AccumSpringForce(gamma);

b = 3000;
[Damping_Force_Coeffs] = DampingForce(b);

Buoy = add_Force_fcn(Buoy,@(body,RB,n) Hydraulic_tether(body,RB,n,'Clump',p_aB,p_aC,Spring_Force_Coeffs,Damping_Force_Coeffs));
Clump = add_Force_fcn(Clump,@(body,RB,n) Hydraulic_tether(body,RB,n,'Surface Buoy',p_aC,p_aB,Spring_Force_Coeffs,Damping_Force_Coeffs));

%Set Initial Conditions  %Offset position from equilibrium position.
Buoy = set_pos(Buoy,[0 0 0 0 0 0],1);
Clump = set_pos(Clump,[0 0 -1 0 0 0],1);

RB{1} = Buoy;
RB{2} = Clump;

%Solve for motion
RB = Solve_RK4(RB);

Buoy = RB{1};
Clump = RB{2};

%Plot Buoy motion results
x = get(Buoy,'x');
xdot = get(Buoy,'xdot');
xddot = get(Buoy,'xddot');
 
v1 = xdot(:,3);

%Plot Clump motion results
y = get(Clump,'x');
ydot = get(Clump,'xdot');
yddot = get(Clump,'xddot');

v2 = ydot(:,3);

%plotting results

Bforce = get(Buoy,'Forces');
Cforce = get(Clump,'Forces');

%Cylinder kinematics

figure('Position', [50 50 600 900])
set(gcf,'color','white');

subplot(3,1,1),plot(t,x(:,1),t,x(:,3),t,x(:,5));
title('MOOS Buoy, 1111.5 kg ','FontName','times','FontSize', 12);
legend('surge(m)','heave(m)','pitch(rad)');

subplot(3,1,2),plot(t,xdot(:,1),t,xdot(:,3),t,xdot(:,5));
legend('surge vel (m/s)','heave vel (m/s)','pitch vel (m/s)');

subplot(3,1,3),plot(t,xddot(:,1),t,xddot(:,3),t,xddot(:,5));
legend('surge accel (m/s^2)','heave accel (m/s^2)','pitch accel (m/s^2)');
xlabel('time (s)');

%plate kinematics
figure('Position', [50 50 600 900])
set(gcf,'color','white');

subplot(3,1,1), plot(t,y(:,1),t,y(:,3),t,y(:,5));
title('Circular Steel Plate r = 2 ft, mass 588.3 kg','FontName','times','FontSize', 12);
legend('surge(m)','heave(m)','pitch(rad)');

subplot(3,1,2), plot(t,ydot(:,1),t,ydot(:,3),t,ydot(:,5));
legend('surge(m/s)','heave vel (m/s)','pitch vel (m/s)');

subplot(3,1,3), plot(t,yddot(:,1),t,yddot(:,3),t,yddot(:,5));
legend('surge(m/s^2)','heave accel (m/s^2)','pitch accel (m/s^2)');
xlabel('time (s)');

%Calculate Elongation

xo1 = get(Buoy,'x0');
xo2 = get(Clump,'x0');

for k=1:length(t);
[elon(k),elonrate(k),s] = TetherElongations(xo1,x(k,:)',xdot(k,:)',p_aB,xo2,y(k,:)',ydot(k,:)',p_aC);
end

%Calculate Tension
for i=1:length(t);
tension(i)=ppval(Spring_Force_Coeffs,elon(i));
end

%Elongation and Tension about zero

elon =elon - mean(elon);
tension = tension - mean(tension);%tension in newtons 


%Calculate Spectral properties of elongation and tension

[elf2, elPxx2] = tseries2spectrum(t,elon);
[tf2, tPxx2] = tseries2spectrum(t,tension);

%Relative motion

figure('Position', [50 50 600 900])
set(gcf,'color','white');

subplot(3,1,1), plot(t,eta0(1:N));
legend('Incident wave profile (m)');

subplot(3,1,2), plot(t,elon);
legend('Elongation (m)');

subplot(3,1,3), plot(t,elonrate);
legend('Elongation Rate (m/s)');
xlabel('time (s)');

%Forces
figure('Position', [50 100 1200 600])
set(gcf,'color','white');

subplot(2,1,1), plot(t,tension);
L4=legend('Tether Tension (N)');
set(L4,'FontName','times','FontSize', 16,'FontWeight','Bold');
xlabel('time (s)','FontName','times','FontSize', 16,'FontWeight','Bold');
set(gca,'FontName','times','FontSize',16,'FontWeight','Bold');

subplot(2,1,2),plot(t,elon);
L3=legend('Elongation (m)');
set(L3,'FontName','times','FontSize', 16,'FontWeight','Bold');
set(gca,'FontName','times','FontSize',16,'FontWeight','Bold');

%Elongation and Tension spectrals plots

figure('Position', [50 100 1200 600])
set(gcf,'color','white');

subplot(2,1,1),plot(tf2,tPxx2);
set(gca,'FontName','times','FontSize',16);
xlabel('Frequency (Hz)');
ylabel('Power Spectrum (Power/Hz)');
L3=legend('Tension (N)');
set(L3,'FontName','times','FontSize', 16,'FontWeight','Bold');
xlim([0 1])
ylim([0 2500000])

subplot(2,1,2),plot(elf2,elPxx2);
set(gca,'FontName','times','FontSize',16);
xlabel('Frequency (Hz)');
ylabel('Power Spectrum (Power/Hz)');
L3=legend('Elongation (m)');
set(L3,'FontName','times','FontSize', 16,'FontWeight','Bold');
xlim([0 1])
ylim([0 0.04])
