%clear all
clear
close all
global PIDValues

PIDValues = zeros(1,7);

yold = 0;
TC_P = 0;
TC_I = 0;
TC_D = 0;

dt = 0.05;       %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

espRadius = 25.5 * 0.0254 / 2.0;        %meters (25.5" in diameter)
espMass = 620;                        %from SW in kg
espWPArea = pi * espRadius^2;        
espWtH2O = 3450;  %N
espDisp = ((espMass * 9.8) - espWtH2O) / (1025.0 * 9.8);
espLength = espDisp / (pi * espRadius^2); 
espPJASubmergedX = espRadius * 2.0 * espLength;
espPJASubmergedY = espPJASubmergedX;
espPJASubmergedZ = espWPArea;

buoyRadius = 66.0 * 0.0254 / 2.0;               %meters (66" in diameter)
buoyMass = 691.0;                               %from SW in kg
buoyWPArea = pi * buoyRadius^2;        
buoyDisp = ((buoyMass + espMass) / 1025.0) - espDisp;                                
buoyWPA2ndMoment = (pi * buoyRadius^4) / 4;    %(pi*r^4)/4 for a circle
buoyPJASubmergedX = buoyRadius * 2.0 * 0.686;
buoyPJASubmergedY = buoyPJASubmergedX;
buoyPJASubmergedZ = buoyWPArea;

%VVVVVVVVV Change this code to reflect ESP Drifter Buoy characteristics VVVVVVVV
Buoy = set(Buoy,'mass', buoyMass,'S', buoyWPArea,'disp', buoyDisp,'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.2]); %GM Center of mass X, Y, Z actual values from SW are X-0.01 Y0.08 Z0.00
Buoy = set(Buoy,'xB',[0 0 -0.19]); %GM Center of buoyancy X, Y, Z actual values from SW are X-0.00 Y-0.19 Z0.00
Buoy = set(Buoy,'S11',buoyWPA2ndMoment,'S22', buoyWPA2ndMoment);  %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) = 319.9; I(2,2) = 343.4; I(3,3) = 122.0; %GM SW moment of inertia about body's coordinated system    
Buoy = set(Buoy,'I',I);
clear I;
Buoy = set(Buoy,'Area',[buoyPJASubmergedX buoyPJASubmergedY buoyPJASubmergedZ]); %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 = 'WAMIT_Results\ESP_drifter';
%filename = 'test25a';
%**************************************************************************

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  -10]);  %Equilibrium Position in global coordinates.
ESP = set(ESP,'mass', espMass,'S', 0,'disp', espDisp,'g',9.81);% GM
ESP = set(ESP,'xG',[0 0 -1.03]); % GM
ESP = set(ESP,'xB',[0 0 -.75]); % GM
ESP = set(ESP,'S11',0.0,'S22',0.0);  % GM

I = zeros(3);
I(1,1) = 551.5; I(2,2) = 551.6; I(3,3) = 27.9; % GM

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

ro = get(Buoy,'rho');                          %kg /m^3
b = espLength / 2.0;
a = espRadius;
espAMX = 3 * 3.14 * ro * b^2 * (espRadius * 2.0) / 4.0;
espAMY = 3 * 3.14 * ro * a^2 * (espRadius * 2.0) / 4.0;
mu_inf = zeros(6,6);     
mu_inf(1,1) = espAMX;    %GM added mass in surge horizontal 
mu_inf(2,2) = espAMX;    %GM added mass in surge horizontal 
mu_inf(3,3) = espAMY;    %GM added mass heave Z
mu_inf(5,5) = 0.0;        %GM added mass pitch 
%**************************************************************************

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 = 8.0;  %GM wave period or peak wave period in seconds
A = 2.0;  %GM Wave amplitude in meters
wave = IncWave;
wave = set(wave,'T',T);
wave = set(wave,'A',A);
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).
%**************************************************************************


if(1==0)
    %****** Use this code to include ESP tension control algorithm (comment out section below) ************
    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));
    %**************************************************************************
else
    %****** Use this code to model fixed cable length (comment out add_forces above) ************
    k = 100000;  %N/m
    [Spring_Force_Coeffs] = LinearSpringForce(k);
    % 
    b = 130;  %N/(m/s)
    [Damping_Force_Coeffs] = LinearDampingForce(b);
    % 
    Buoy = add_Force_fcn(Buoy,@(body,RB,n) Hydraulic_tether(body,RB,n,'ESP',p_aB,p_aE,Spring_Force_Coeffs,Damping_Force_Coeffs));
    ESP = add_Force_fcn(ESP,@(body,RB,n) Hydraulic_tether(body,RB,n,'Surface Buoy',p_aE,p_aB,Spring_Force_Coeffs,Damping_Force_Coeffs));
    %**************************************************************************
end

%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 (-espWtH2O / k) 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{end}(i,1:3));
end

%Buoy Motions
figure('Units','normalized','Position', [.05 .55 .4 .4])
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])
figure('Units','normalized','Position', [.05 .05 .4 .4])
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])
figure('Units','normalized','Position', [.55 .55 .4 .4])
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])
figure('Units','normalized','Position', [.55 .05 .4 .4])
set(gcf,'color','white');

subplot(3,1,1)
plot(t,ElongationRate)
legend('Payout Rate (m/s)');
ylabel('m/s');
% titleText = sprintf('Tension Control k: %5.1f   b: %5.1f   ESP Weight in H2O: %5.1f', gTC_k, gTC_b, espWtH2O);
% title(titleText);

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

subplot(3,1,3)
plot(PIDValues(:,6))
% 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)');

figure('Units','normalized','Position', [.25 .05 .6 .25])
plot(t,Tension/4.45)
title(sprintf('Cable Tension with A (Hs/2) = %5.2d and ESP Wt in H20 = %4.0f N b = %5.3f', A, espWtH2O, b))
ylabel('Tension (lbf)');
%axis([0 100 0 1000]);
grid


