clear all
close all

dt = 0.1;       %timestep size
tf = 200;
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',12760,'S',14.4,'disp',14.4,'g',9.81);
Buoy = set(Buoy,'x0',[0 0 0]);
Buoy = set(Buoy,'xG',[0 0 -0.5]);
Buoy = set(Buoy,'xB',[0 0 -0.5]);
Buoy = set(Buoy,'S11',3.6,'S22',3.6);  %S11 = S22 = (-1.5)(-1.5)(14.4) m^4
I = zeros(3);
I(1,1) = 10378.13; I(2,2) = 44404.80; I(3,3) = 42533.33;
Buoy = set(Buoy,'I',I);
clear I;


%Read Frequency Domain Hydrodynamic Data from file.
disp('Reading frequency domain hydrodynamic coefficients'); 
FD_coefs = ReadData('box1.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',2000.0,'S',0,'disp',0.376704,'g',9.81);% thin plate 2.4m x 2.4m and thickness = 0.0654 m
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) = 962.85; I(2,2) = 962.85; I(3,3) = 1920.00;
Clump = set(Clump,'I',I);
clear I;

mu_inf = zeros(1,6);
%mu_inf(3) = 805.03; % m33 = (pi.rho.len^2)/4
mu_inf(3) = 805.03*5.76;

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

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

% figure('Position', [100 100 900 600])
% set(gcf,'color','white');
% 
% plot(eta0)

%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);
Clump = add_Force_fcn(Clump,@F_Drag);

%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 = 2500;works for periods from 9-12 s
k = 2500;
b = 100;

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

v1 = xdot(:,3);

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

v2 = ydot(:,3);

%plotting results


figure('Position', [100 100 1000 600])
set(gcf,'color','white');
subplot(3,1,1),plot(t,x(:,3));
title('Box 6 x 2.4 x 2 - 1 m freeboard','FontName','times','FontSize', 12);
legend('pos (m)');
xlabel('time (s)');
ylim([-3 3])

subplot(3,1,2),plot(t,xdot(:,3));
legend('vel (m/s)')
xlabel('time (s)');
ylim([-3 3])


subplot(3,1,3),plot(t,xddot(:,3));
legend('accel (m/s^2)')
xlabel('time (s)');
ylim([-3 3])

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

subplot(3,1,1),plot(t,y(:,3));
title('Thin Plate 2.4 x 2.4m mass = 2000 Kg','FontName','times','FontSize', 12);
legend('pos (m)');
xlabel('time (s)');
ylim([-20 10])

subplot(3,1,2),plot(t,ydot(:,3));
legend('vel (m/s)')
xlabel('time (s)');
ylim([-15 15])

subplot(3,1,3),plot(t,yddot(:,3));
legend('accel (m/s^2)')
xlabel('time (s)');
ylim([-15 15])


%Calculating power


dv = v2-v1;


for i =1:N;
    pow(i) =dv(i,1)*dv(i,1);
    pow(i) = b*pow(i);
    
    
    F_tether(i) = -k*(x(i,3)-y(i,3))-b*(v1(i)-v2(i));
end

meanpow = mean(pow);

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

subplot(3,1,1),plot(t,dv);
title('Relative vertical velocity (m/s)','FontName','times','FontSize', 12);
xlabel('time (s)');

ylim([-20 20])

for i=1:N;

    pow_wav(i) = 1025*9.81*(4*4)*147*2.4/8;
    pow_ext(i) = meanpow;
end


subplot(3,1,2), plot(t,pow,t,pow_ext)
title('Power (W)','FontName','times','FontSize', 12);
xlabel('time (s)');

ylim([0 10000])

subplot(3,1,3),plot(t,pow_wav);


title('Average Wave Power (W)','FontName','times','FontSize', 12);
xlabel('time (s)');
ylim ([0 9000000])


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

plot(t,F_tether/1000)

grid on
title('Tether forces (kN)','FontName','times','FontSize', 12);
xlabel('time (s)','FontName','times','FontSize', 12);

ylim([-40 10])

