clear all
close all

dt = 0.1;       %timestep size
tf = 240;
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

%VVVVVVVVV Change this code to reflect ESP Drifter Buoy characteristics VVVVVVVV
Buoy = set(Buoy,'mass', 1642.75,'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.
%******** Change this filename to be the WAMIT results for the ESP drifter*
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 "ESP"
ESP = RigidBody;
ESP = set(ESP,'name','ESP');
ESP = set(ESP,'dt',dt,'N',N);  %Set timesteps
ESP = set(ESP,'dof',6);  %With 6 DOF.
ESP = set(ESP,'Fixed',0);  %Set free to move

%VVVVVVVVVVV Change this code to reflect ESP characteristics ****************
ESP = set(ESP,'x0',[0  0  0]);  %Equilibrium Position in global coordinates.
ESP = set(ESP,'mass',315,'S',0,'disp',0.25,'g',9.81);% thin square plate 2m and thickness = 0.0254 m 
ESP = set(ESP,'xG',[0 0 0]);
ESP = set(ESP,'xB',[0 0 0]);
ESP = set(ESP,'S11',0.0,'S22',0.0);  %m^4

I = zeros(3);
I(1,1) = 20.69; I(2,2) = 20.69; I(3,3) = 41.33;

ESP = set(ESP,'I',I);
clear I;
ESP = set(ESP,'Area',[1  1  1]);
ESP = set(ESP,'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
%**************************************************************************


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

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

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

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


%Add forces to ESP
ESP = add_Force_fcn(ESP,@F_HydroStatic);
ESP = add_Force_fcn(ESP,@F_Weight);
ESP = add_Force_fcn(ESP,@(body,RB,n) F_Drag_Inc(body,RB,n,'Surface Buoy',0));


%Add forces that act on two bodies
%******* Change this code to reflect Buoy and ESP attachement points ******
p_aB = [0.0  0.0  -1.5];  %Tether attachment point on Buoy (local CoordSys).
p_aE = [0.0  0.0  0.0];  %Tether attachment point on ESP (local CoordSys).
%**************************************************************************


%****** Use this code to include ESP tension control algorithm ************
Buoy = add_Force_fcn(Buoy,@(body,RB,n) WinchControlledTether(body,RB,n,'ESP',p_aB,p_aE));
ESP = add_Force_fcn(ESP,@(body,RB,n) WinchControlledTether(body,RB,n,'Surface Buoy',p_aE,p_aB));
%**************************************************************************


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

RB{1} = Buoy;
RB{2} = ESP;

%Solve for motion
RB = Solve_RK4(RB);

Buoy = RB{1};
ESP = RB{2};

%Plot Buoy motion results
x = get(Buoy,'x');
xdot = get(Buoy,'xdot');
xddot = get(Buoy,'xddot');
v1 = xdot(:,3);

%Plot ESP motion results
y = get(ESP,'x');
ydot = get(ESP,'xdot');
yddot = get(ESP,'xddot');
v2 = ydot(:,3);


%plotting results
Bforce = get(Buoy,'Forces');
ESPForce = get(ESP,'Forces');

figure


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

subplot(2,1,1),plot(t,x(:,3),t,x(:,5));
title('Buoy Motions ','FontName','times','FontSize', 12);
legend('heave(m)','pitch(rad)');

subplot(2,1,2),plot(t,xdot(:,3),t,xdot(:,5));
legend('heave vel (m/s)','pitch vel (m/s)');
xlabel('time (s)');

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

subplot(2,1,1), plot(t,y(:,3),t,y(:,5),t,TargetDepth);
title('ESP Motions','FontName','times','FontSize', 12);
legend('heave (m)','pitch (rad)','target depth (m)');

subplot(2,1,2), plot(t,ydot(:,3),t,ydot(:,5));
legend('heave vel (m/s)','pitch vel (m/s)');
xlabel('time (s)');



%Forces and Power
Power = -ElongationRate.*Tension;
idx = find(ElongationRate < 0);  %Find when moving up.
mean_power = mean(Power);
mean_power_noregen = mean(Power(idx));
figure('Position', [100 100 1000 600])
set(gcf,'color','white');
plot(t,Power,t(1:length(t)-1:end),mean_power*ones(1,2),t(1:length(t)-1:end),mean_power_noregen*ones(1,2));
legend('Power (W)','Mean Power (with regen)','Mean Power (without regen)');



