%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 = 50;
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 = 421.4;                        %from SW in kg
espWPArea = pi * espRadius^2;        
espWtH2O = 65;
espDisp = (espMass - espWtH2O) / 1025.0;
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 buoyance 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;

%Set up "Winch"
Winch = RigidBody;
Winch = set(Winch,'name','Winch');
Winch = set(Winch,'dt',dt,'N',N);  %Set timesteps
Winch = set(Winch,'dof',6);  %With 6 DOF.
Winch = set(Winch,'Fixed',0);  %Set free to move

%VVVVVVVVVVV Change this code to reflect Winch characteristics ****************
Winch = set(Winch,'x0',[0  0  .5]);  %Equilibrium Position in global coordinates.
Winch = set(Winch,'mass', 138.0,'S', 0.0,'disp', 0.0,'g',9.81);% GM
Winch = set(Winch,'xG',[0 0 0.0]); % GM
Winch = set(Winch,'xB',[0 0 0.0]); % GM
Winch = set(Winch,'S11',0.0,'S22',0.0);  % GM

I = zeros(3);
I(1,1) = 6.35; I(2,2) = 7.4; I(3,3) = 6.35;

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

mu_inf = zeros(6,6);     
Winch = set(Winch,'mu_inf',mu_inf);
clear mu_inf;




%Compute incident wave profile
Type = 1;  %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 = 1;  %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 to ESP
Winch = add_Force_fcn(Winch,@F_Weight);


%Add forces that act on two bodies
%******* Change this code to reflect Buoy and ESP attachement points ******
p_aWonB = get(Winch,'x0');  %Winch attachment point on Buoy (in Buoy local CoordSys).
p_aBonW = [0.0  0.0  0.0];  %Buoy attachment point on Winch (in Winch local CoordSys).
p_aE = [0.0  0.0  0.0];  %Tether attachment point on ESP (local CoordSys).
p_aW = [0.45  0.0  0.0];  %Tether attachment point on Winch (local CoordSys).
%**************************************************************************

k = 100000;
b = 5000;
[Spring_Force_Coeffs] = LinearSpringForce(k);
[Damping_Force_Coeffs] = LinearDampingForce(b);
%****** Use this code to include  Motor forces on winch and vice versa (translation and torque ************
Winch = add_Force_fcn(Winch,@(body,RB,n) MotorConnection(body,RB,n,'Surface Buoy',p_aBonW,p_aWonB,Spring_Force_Coeffs,Damping_Force_Coeffs));
Buoy = add_Force_fcn(Buoy,@(body,RB,n) MotorConnection(body,RB,n,'Winch',p_aWonB,p_aBonW,Spring_Force_Coeffs,Damping_Force_Coeffs));
%**************************************************************************


k = 1000;
b = 0;
[Spring_Force_Coeffs] = LinearSpringForce(k);
[Damping_Force_Coeffs] = LinearDampingForce(b);

%****** Use this code to include ESP forces on winch and vice versa (translation and torque ************
Winch = add_Force_fcn(Winch,@(body,RB,n) CableConnection(body,RB,n,'ESP',p_aW,p_aE,Spring_Force_Coeffs,Damping_Force_Coeffs));
ESP = add_Force_fcn(ESP,@(body,RB,n) CableConnection(body,RB,n,'Winch',p_aE,p_aW,Spring_Force_Coeffs,Damping_Force_Coeffs));
%**************************************************************************


%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);
Winch = set_pos(Winch,[0 0 0 0 0 0],1);

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

%Solve for motion
RB = Solve_RK4(RB);

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


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

%Plot Winch motion results
z = get(Winch,'x');
zdot = get(Winch,'xdot');
zddot = get(Winch,'xddot');
v3 = zdot(:,3);

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

xo1 = get(ESP,'x0');
xo2 = get(Buoy,'x0');
xo3 = get(Winch,'x0');
