clear all
close all

dt = 0.1;       %timestep size
tf = 75;
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',3220.13,'S',3.14159,'disp',3.14159,'g',9.81);
Buoy = set(Buoy,'x0',[0 0 0]);
Buoy = set(Buoy,'xG',[0 0 -.25]);
Buoy = set(Buoy,'xB',[0 0 -0.5]);
Buoy = set(Buoy,'S11',0.79,'S22',0.79);  %m^4
I = zeros(3);
I(1,1) = 1613.25; I(2,2) = 1615.53; I(3,3) = 1613.25;
Buoy = set(Buoy,'I',I);
clear I;

%Read Frequency Domain Hydrodynamic Data from file.
disp('Reading frequency domain hydrodynamic coefficients'); 
FD_coefs = ReadData('tcyl.out');
for i = 1:6 
   mu_inf(i) = real(FD_coefs.F{i,i}(end)); 
end
Buoy = set(Buoy,'mu_inf',mu_inf);
clear mu_inf;

%Compute Impulse Response Functions from Frequency Domain Results
tau = 0:dt:10;
disp('Computing impulse response functions'); 
L = ComputeIRF_coefs(FD_coefs.w,FD_coefs.F,tau);


%Set up "Clump"
Clump = RigidBody;
Clump = set(Clump,'name','Clump');
Clump = set(Clump,'x0',[0 0 -20]);  %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',100.0,'S',0,'disp',0.0654,'g',9.81);
Clump = set(Clump,'xG',[0 0 0]);
Clump = set(Clump,'xB',[0 0 0.05]);
Clump = set(Clump,'S11',0.0,'S22',0.0);  %m^4
I = zeros(3);
I(1,1) = 1.6362; I(2,2) = 1.6362; I(3,3) = 1.6362;
Clump = set(Clump,'I',I);
clear I;


%Compute incident wave profile
t = get(Buoy,'t');
T = 6;  %seconds
wave = IncWave;
wave = set(wave,'T',T);
disp('Computing incident wave profile'); 
eta0 = eta(wave,0,[t t(end)+tau]);




%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));

%Add forces to Clump
Clump = add_Force_fcn(Clump,@F_HydroStatic);
Clump = add_Force_fcn(Clump,@F_Weight);

%Add forces that act on two bodies
p_a1 = [0 0 0];  %Tether attachment point on Buoy (local CoordSys).
p_a2 = [0 0 0];  %Tether attachment point on Clump (local CoordSys).
k = 1000;
b = 10;
Buoy = add_Force_fcn(Buoy,@(body,RB,n) SD_Tether(body,RB,n,'Clump',p_a1,p_a2,k,b));
Clump = add_Force_fcn(Clump,@(body,RB,n) SD_Tether(body,RB,n,'Surface Buoy',p_a1,p_a2,k,b));

%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 0 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');
figure
plot(t,x(:,3),t,xdot(:,3),t,xddot(:,3));
legend('pos (m)','vel (m/s)','accel (m^2/s)');
xlabel('time (s)');

%Plot Clump motion results
x = get(Clump,'x');
xdot = get(Clump,'xdot');
xddot = get(Clump,'xddot');
figure
plot(t,x(:,3),t,xdot(:,3),t,xddot(:,3));
legend('pos (m)','vel (m/s)','accel (m^2/s)');
xlabel('time (s)');



