clear all
close all

dt = 0.1;       %timestep size
tf = 160;
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); %GM mass, waterplane area, displacement, g
Buoy = set(Buoy,'x0',[0 0 0]);  %GM Equilibrium Position in global coordinates.
Buoy = set(Buoy,'xG',[0 0 -0.82]); %GM Center of mass X, Y, Z
Buoy = set(Buoy,'xB',[0 0 -0.26]); %GM Center of buoyance X, Y, Z
Buoy = set(Buoy,'S11',0.2591,'S22',.2591);  %GM 2nd moment of area of Waterplane in x (S11) and Y (S22) (pi*r^4)/4 for a cricle
I = zeros(3);
I(1,1) = 2694.71; I(2,2) = 2643.45; I(3,3) = 458.64; %GM SW moment of inertia about body's coordinated system    
Buoy = set(Buoy,'I',I);
clear I;
Buoy = set(Buoy,'Area',[0.35 0.35 2.33]); %GM projected area of submerged parts in X, Y, Z
Buoy = set(Buoy,'Cd',[50 1 0]); %GM Cds for submerged part leave these alone
Buoy = set(Buoy,'AreaW',[1 1 0]); %GM Area of super surface elements for wind drag
Buoy = set(Buoy,'CdW',[1 1 0]); % GM Cds for wind
%**************************************************************************



%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);% GM
ESP = set(ESP,'xG',[0 0 0]); % GM
ESP = set(ESP,'xB',[0 0 0]); % GM
ESP = set(ESP,'S11',0.0,'S22',0.0);  % GM

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

ESP = set(ESP,'I',I);
clear I;
ESP = set(ESP,'Area',[1  1  1]); % GM
ESP = set(ESP,'Cd',[1.5 1.5 1.0]); % GM

mu_inf = zeros(6,6);     
mu_inf(1,1) = 0;                     %GM added mass in surge horizontal x
mu_inf(3,3) = 3220.1325*1.0*1.0; %GM added mass heave Z
mu_inf(5,5) = 402.5166*1.0*1.0;      %GM added mass pitch 0 for a sphere
%**************************************************************************


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

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

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


%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  -0.5];  %GM Tether attachment point on Buoy (local CoordSys).
p_aE = [0.0  0.0  0.0];  %GM 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 -10 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');

xo1 = get(ESP,'x0');
xo2 = get(Buoy,'x0');
for i = 1:N
    [TargetDepth(i) TargetDepthRate(i)] = TargetDepth_Profiling(t(i));
    [Elongation(i),ElongationRate(i),s] = TetherElongations(xo1,y(i,:)',ydot(i,:)',p_aE,xo2,x(i,:)',xdot(i,:)',p_aB);
    Tension(i) = norm(ESPForce{4}(i,1:3));
end

%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 (rad/s)');
xlabel('time (s)');


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

subplot(2,1,1)
plot(t,y(:,3)-TargetDepth');
title('ESP Depth Error','FontName','times','FontSize', 12);
ylabel('Depth Error (m)');

subplot(2,1,2)
plot(t,ydot(:,3)-TargetDepthRate');
title('ESP Depth Rate Error','FontName','times','FontSize', 12);
ylabel('Velocity Error (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');

subplot(3,1,1)
plot(t,ElongationRate)
legend('Payout Rate (m/s)');
ylabel('m/s');

subplot(3,1,2)
plot(t,Tension/4.45)
legend('Tether Tension (lbs)');
ylabel('lbs');

subplot(3,1,3)
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)');
ylabel('W');
xlabel('time (s)');


