clear all
close all

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

%Set up "Buoy"
Buoy = RigidBody;
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,'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);

%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,n) Mem_Radiation(body,n,L));
Buoy = add_Force_fcn(Buoy,@(body,n) F_Exciting(body,n,L,eta0));  

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

%Solve for motion
state = zeros(12,1);
for n = 1:get(Buoy,'N')-1
  disp(['Timestep = ' num2str(n)]); 
  state(1:6) = get_pos(Buoy,n);
  state(7:12) = get_vel(Buoy,n);

  Buoy = set(Buoy,'sv',state);
  k1 = dt*RHS(Buoy,n);    

  Buoy = set(Buoy,'sv',state+k1/2);
  k2 = dt*RHS(Buoy,n+.5);    

  Buoy = set(Buoy,'sv',state+k2/2);  
  k3 = dt*RHS(Buoy,n+.5);
  Buoy = set(Buoy,'sv',state+k3);
  k4 = dt*RHS(Buoy,n+1);

  R = k1/6+k2/3+k3/3+k4/6;

  state = state + R;
    
  Buoy = set_pos(Buoy,state(1:6),n+1);
  Buoy = set_vel(Buoy,state(7:12),n+1);
  Buoy = set_accel(Buoy,R(7:12)/dt,n+1);
end

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



