clear all
close all

dt = 0.1;       %timestep size
tf = 100;
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', 1485.11,'S',3.83274,'disp',1.66,'g',9.80665);
Buoy = set(Buoy,'x0',[0 0 0]);  %Equilibrium Position in global coordinates.
Buoy = set(Buoy,'xG',[0 0 -0.82]);
Buoy = set(Buoy,'xB',[0 0 -0.26]);
Buoy = set(Buoy,'S11',0.2591,'S22',.2591);  %S11 = S22 = (-1)(-1)(3.14159) m^4
I = zeros(3);
I(1,1) = 2694.71; I(2,2) = 2643.45; I(3,3) = 458.64;      
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 = '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
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"
Clump = RigidBody;
Clump = set(Clump,'name','Clump');
Clump = set(Clump,'x0',[0  0 -10]);  %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',248,'S',0,'disp',0.0310,'g',9.81);% thin square plate 2m and thickness = 0.0254 m 
%Clump = set(Clump,'mass',400.0,'S',0,'disp',0.03525,'g',9.81);
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) = 20.69; I(2,2) = 20.69; I(3,3) = 41.33;
Clump = set(Clump,'I',I);
clear I;
Clump = set(Clump,'Area',[1  1  1]);
Clump = set(Clump,'Cd',[0.01 0.01 1.17]);

mu_inf = zeros(6,6);     
mu_inf(1,1) = 0;                     %m11 = (pi.rho.a^2)times thickness, a = 1m
mu_inf(3,3) = 3220.1325*1.0*1.0;
mu_inf(5,5) = 402.5166*1.0*1.0;            %m55 = (1/8pi.rho.a^4)times thickness, a = 1m


% Sphere
% I = zeros(3);
% I(1,1) = 6.62; I(2,2) = 6.62; I(3,3) = 6.62;
% Clump = set(Clump,'I',I);
% clear I;
% Clump = set(Clump,'Area',[0.13  0.13  0.13]);
% Clump = set(Clump,'Cd',[0.5 0.5 0.5]);
% 
% mu_inf = zeros(1,6);     
% mu_inf(1) = (4/3)*3220.1325*(0.2034)^3;            %m11 = (pi.rho.a^2)times thickness, a = 1m
% mu_inf(2) = (4/3)*3220.1325*(0.2034)^3;
% mu_inf(3) = (4/3)*3220.1325*(0.2034)^3;
% mu_inf(5) = (8/15)*3220.1325*(0.2034)^5;           %m55 = (1/8pi.rho.a^4)times thickness, a = 1m


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

%Compute incident wave profile

%default values
%A = 1;
%T = 2;
%Type = 1 ( = monochromatic)

t = get(Buoy,'t');

Type = 0;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave
T = 4.0;  %wave period or peak wave period in seconds
SigH = 1.0;
wave = IncWave;
wave = set(wave,'T',T);
wave = set(wave,'Type',Type);

disp('Computing incident wave profile');
t1= dt:dt:L{7,1}.breaks(end);
eta0 = eta(wave,0,[t t(end)+t1]);

% 
% figure('Position', [100 100 1000 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));
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  -1.5];  %Tether attachment point on Buoy (local CoordSys).
p_aC = [0.0  0.0  0.0];  %Tether attachment point on Clump (local CoordSys).

k = 4000;
b = 100;

Buoy = add_Force_fcn(Buoy,@(body,RB,n) SD_Tether(body,RB,n,'Clump',p_aB,p_aC,k,b));
Clump = add_Force_fcn(Clump,@(body,RB,n) SD_Tether(body,RB,n,'Surface Buoy',p_aC,p_aB,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

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


%Cylinder kinematics

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

subplot(3,1,1),plot(t,x(:,3),t,x(:,5));
title('MOOS Buoy, 1485 kg ','FontName','times','FontSize', 12);
legend('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', [100 100 1000 600])
set(gcf,'color','white');

subplot(3,1,1), plot(t,y(:,3),t,y(:,5));
title('Steel Square Plate 1 m^2   mass 248 kg','FontName','times','FontSize', 12);
legend('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)');

%Relative motion

dz = y(:,3) - x(:,3);%relative heave displacement
dv = v2-v1; %relative heave velocity

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

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

subplot(3,1,2), plot(t,dz);
legend('Relative Heave disp (m)');

subplot(3,1,3), plot(t,dv);
legend('Relative velocity (m/s)');
xlabel('time (s)');


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

subplot(3,1,1), plot(t,Bforce{4}(:,1));
legend('Wave-Exciting Force - surge (N)');

subplot(3,1,2), plot(t,Bforce{4}(:,3));
legend('Wave-Exciting Force - heave (N)');

subplot(3,1,3), plot(t,Bforce{4}(:,5));
legend('Wave-Exciting Force - pitch (N)');


%power extraction 


for i =1:N;
    pow(i) =dv(i,1)*dv(i,1);
    pow(i) = 0.001*b*pow(i);
    
    p_drag(i) = -0.001*v2(i)*CForce{3}(i,3);
end


%Compare power values
%Ignore first 5 seconds

delt = 5/dt; delt = delt+1;

meanpow = mean(pow(delt:N));
meandrag = mean(p_drag(delt:N));

disp(['Mean Power dissipated-drag = ',num2str(meandrag),' kW'])
disp(['Mean Power take-off at damper = ',num2str(meanpow),' kW'])


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

subplot(2,1,1), plot(t,pow);
title('Power dissipated at damper (kW)','FontName','times','FontSize', 12);

subplot(2,1,2),plot(t,p_drag);
title('Power dissipated due to drag (kW)','FontName','times','FontSize', 12);


