clear all
close all

dt = .1;       %timestep size
tf = 600;
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',[1.5 1.5 3.83274]);
Buoy = set(Buoy,'Cd',[1 1 1]);
Buoy = set(Buoy,'AreaW',[3.8 3.8 0]);
Buoy = set(Buoy,'CdW',[0.42 0.42 0]);  %Cd on pi*r^2 from Hoerner Figure 14, pp5-8.  Needs refinement

%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 "Foil"  
Lc = 0.4;  %Chord Length
Ls = 2*1.5;  %Span
S = Lc*Ls;
thickness = 0.15;
mass = 2*189.0;  %kg (For NACA0015 Foil)
displ = 2*0.0254;  %m^3 (For NACA0015 Foil)
Foil = RigidBody;
Foil = set(Foil,'name','Foil');
Foil = set(Foil,'x0',[0  0 -20]);  %Equilibrium Position in global coordinates.
Foil = set(Foil,'dt',dt,'N',N);  %Set timesteps
Foil = set(Foil,'dof',6);  %With 6 DOF.
Foil = set(Foil,'Fixed',0);  %Set free to move
Foil = set(Foil,'mass',mass,'S',0,'disp',displ,'g',9.81); 
Foil = set(Foil,'xG',[0.0 0 0]);
Foil = set(Foil,'xB',[0.0 0 0]);
Foil = set(Foil,'S11',0.0,'S22',0.0);  %m^4


I = zeros(3);
I(1,1) = 35.5; I(2,2) = 1.88; I(3,3) = 37.3;
Foil = set(Foil,'I',I);
clear I;
Foil = set(Foil,'Area',[0  0  S]);


mu_inf = zeros(6,6);     
mu_inf(1,1) = Lc*pi*1025*(thickness*Lc)^2; %m11 
mu_inf(3,3) = Lc*pi*1025*Lc^2;   %added mass for 2-d ellipse plate.  Newman pp 145
mu_inf(5,5) = Lc*pi*1025*(Lc^2-(thickness*Lc)^2);                     %m55

Foil = set(Foil,'mu_inf',mu_inf);


%Compute incident wave profile

t = get(Buoy,'t');

Type = 0;  %Type refers monochromatic(Type = 1) or PM spectrum (Type = 0) wave
A = 2.0;
wave = IncWave;
wave = set(wave,'A',A);
wave = set(wave,'Type',Type);
wave = set(wave,'beta',-1);

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


%Incident Wave profile
figure('Position', [100 100 1000 600])
set(gcf,'color','white');
plot(t,eta0(1:N));
legend('Incident wave profile (m)');

%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,[-7.5 0 0]));

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

%Add forces that act on two bodies
p_aB = [0.0  0.0  -1.5];  %Tether attachment point on Buoy (local CoordSys).
p_aF = [0.0  0.0  0.0];  %Tether attachment point on Foil (local CoordSys).

gamma = 1.4;
[Spring_Force_Coeffs] = AccumSpringForce(gamma);

b = 3000;
[Damping_Force_Coeffs] = DampingForce(b);

Buoy = add_Force_fcn(Buoy,@(body,RB,n) Hydraulic_tether(body,RB,n,'Foil',p_aB,p_aF,Spring_Force_Coeffs,Damping_Force_Coeffs));
Foil = add_Force_fcn(Foil,@(body,RB,n) Hydraulic_tether(body,RB,n,'Surface Buoy',p_aF,p_aB,Spring_Force_Coeffs,Damping_Force_Coeffs));



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

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

%Solve for motion
RB = Solve_RK4(RB);


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

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

%Plot Foil motion results
y = get(Foil,'x');
ydot = get(Foil,'xdot');
yddot = get(Foil,'xddot');

%plotting results

Bforce = get(Buoy,'Forces');
Fforce = get(Foil,'Forces');


%Cylinder kinematics

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


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

subplot(3,1,1), plot(t,y(:,1),t,y(:,3),t,y(:,5));
title('Foil Motions','FontName','times','FontSize', 12);
legend('surge','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)');


V = zeros(1,N);
AOA = zeros(1,N);
for n = 1:N
[V(n) AOA(n)] = relative_vel(Foil,-n,Buoy,wave);
end

figure
plot(AOA*180/pi,V,'x');
xlabel('Angle of Attack (degrees)');
ylabel('Speed (m/s)');

nu = 1.1E-6;  %kinematic viscosity (m^2/s)plo
figure
plot(AOA*180/pi,V*Lc/nu,'x');
xlabel('Angle of Attack (degrees)');
ylabel('Re_c - Re Number based on chord length');

figure
hist(AOA*180/pi,50);
xlabel('Angle of Attach (degrees)');
ylabel('timestep count');


beta = atan2(-ydot(:,3),ydot(:,1));
theta_optimum = 10*pi/180;
theta_max = 20*pi/180;
th_target = zeros(1,length(beta));
for i = 1:length(beta)
   th_target(i) = theta_target(beta(i),theta_optimum,theta_max);
end
figure
plot(t,y(:,5)*180/pi,t,th_target*180/pi)
xlabel('time (s)');
ylabel('Foil Pitch (degrees)');
legend('Actual','Target');
