clear all
close all

ii = 3;
jj = 3;

T = 2;  %Period of oscillation.
dt = 0.05;

filename = 'test25';
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

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




%ReadData;


%tau = 0:dt:10;
%L = ComputeIRF(w,F,tau);
%L = ComputeIRF_coefs(w,F,tau);


I = zeros(3);
I(1,1) = 1613.25; I(2,2) = 1615.53; I(3,3) = 1613.25;

Buoy = RigidBody;
Buoy = set(Buoy,'mass',3220.13,'S',3.14159,'disp',3.14159,'g',9.81);
Buoy = set(Buoy,'xG',[0 -.25 0]);
Buoy = set(Buoy,'xB',[0 -.5 0]);
Buoy = set(Buoy,'S11',0.79,'S22',0.79);  %m^4
Buoy = set(Buoy,'I',I);
Buoy = set(Buoy,'dt',dt);
%for i = 1:6 
%    for j = 1:6
%      mu_inf(i,j) = real(F{i,j}(end)); 
%    end
%end
Buoy = set(Buoy,'mu_inf',mu_inf);


tf = 50;
t = 0:dt:tf;

Buoy = set(Buoy,'N',length(t));

f = 1/T;
tc = 5;  %Ramp up time constant (seconds)
x = (1-exp(-t/tc)).*cos(2*pi*f*t);
xdot = -(1-exp(-t/tc)).*sin(2*pi*f*t)*2*pi*f+cos(2*pi*f*t).*exp(-t/tc)/tc;
xddot = -(1-exp(-t/tc)).*cos(2*pi*f*t)*(2*pi*f)^2 -2*pi*f*sin(2*pi*f*t).*exp(-t/tc)/tc -cos(2*pi*f*t).*exp(-t/tc)/(tc^2) -2*pi*f*exp(-t/tc).*sin(2*pi*f*t)/tc;

modes = [0 0 0 0 0 0];
modes(jj) = 1;
for n = 1:length(t)

end

for n = 1:length(t)
n
Buoy = set_pos(Buoy,x(n)*modes,n);
Buoy = set_vel(Buoy,xdot(n)*modes,n);
Buoy = set_accel(Buoy,xddot(n)*modes,n);
    
for i = 1:6
  F_R(n,i) = 0;
  for j = 1:6
    F_R(n,i) = F_R(n,i)-mu_inf(i,j)*xddot(n)*modes(j);
  end
end

RB = cell(1,1);
RB{1} = Buoy;
F_R(n,:) = F_R(n,:)+Mem_Radiation(Buoy,RB,n,L);

end


%Compute Frequency Domain Forces

a = spline(w,real(F{ii,jj}),2*pi*f);
b = spline(w,imag(F{ii,jj}),2*pi*f);

ForceFreq = cos(2*pi*f*t)*(2*pi*f)^2*a+sin(2*pi*f*t)*2*pi*f*b;

figure
plot(t,F_R(:,jj),t,ForceFreq)
