clear all
close all

dt = 0.05;       %timestep size
tf = 100;
N = tf/dt;      %Number of timesteps to compute


%Set up "Clump1"
Clump1 = RigidBody;
Clump1 = set(Clump1,'name','Clump1');
Clump1 = set(Clump1,'x0',[0  0  -5]);  %Equilibrium Position in global coordinates.
Clump1 = set(Clump1,'dt',dt,'N',N);  %Set timesteps
Clump1 = set(Clump1,'dof',6);  %With 6 DOF.
Clump1 = set(Clump1,'Fixed',0);  %Set free to move
Clump1 = set(Clump1,'mass',6*1025,'S',0,'disp',6,'g',9.81);% thin plate 1m x 1m and thickness = 0.0654 m 
Clump1 = set(Clump1,'xG',[0 0 0]);
Clump1 = set(Clump1,'xB',[0 0 0]);
Clump1 = set(Clump1,'S11',0.0,'S22',0.0);  %m^4
I = zeros(3);
I(1,1) = 6.61; I(2,2) = 32.91; I(3,3) = 26.37;
Clump1 = set(Clump1,'I',I);
clear I;

mu_inf = zeros(1,6);     
%mu_inf(1) = 805.03;                           %m11 = pi.rho.a^2, a = 0.5m
%mu_inf(3) = 3220.1325;                        %m33 = pi.rho.b^2, b = 1m
mu_inf(3) = 1000;                           %m11 = pi.rho.a^2, a = 0.5m
mu_inf(1) = 1000;                        %m33 = pi.rho.b^2, b = 1m

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

%Set up "Clump2"
Clump2 = RigidBody;
Clump2 = set(Clump2,'name','Clump2');
Clump2 = set(Clump2,'x0',[0  0  -10]);  %Equilibrium Position in global coordinates.
Clump2 = set(Clump2,'dt',dt,'N',N);  %Set timesteps
Clump2 = set(Clump2,'dof',6);  %With 6 DOF.
Clump2 = set(Clump2,'Fixed',0);  %Set free to move
Clump2 = set(Clump2,'mass',102.5,'S',0,'disp',0.1,'g',9.81);% thin plate 1m x 1m and thickness = 0.0654 m 
Clump2 = set(Clump2,'xG',[0 0 0]);
Clump2 = set(Clump2,'xB',[0 0 0]);
Clump2 = set(Clump2,'S11',0.0,'S22',0.0);  %m^4
I = zeros(3);
I(1,1) = 6.61; I(2,2) = 32.91; I(3,3) = 26.37;


Clump2 = set(Clump2,'I',I);
clear I;

mu_inf = zeros(1,6);     
%mu_inf(1) = 805.03;                           %m11 = pi.rho.a^2, a = 0.5m
%mu_inf(3) = 3220.1325;                        %m33 = pi.rho.b^2, b = 1m
mu_inf(3) = 1000;                           %m11 = pi.rho.a^2, a = 0.5m
mu_inf(1) = 1000;                        %m33 = pi.rho.b^2, b = 1m
Clump2 = set(Clump2,'mu_inf',mu_inf);
clear mu_inf;

t = get(Clump1,'t');

%Add forces to Clump
Clump1 = add_Force_fcn(Clump1,@F_HydroStatic);
Clump1 = add_Force_fcn(Clump1,@F_Weight);
%Clump1 = add_Force_fcn(Clump1,@F_Skin);

%Add forces to Clump
Clump2 = add_Force_fcn(Clump2,@F_HydroStatic);
Clump2 = add_Force_fcn(Clump2,@F_Weight);
Clump2 = add_Force_fcn(Clump2,@F_Skin);

%Add forces that act on two bodies
p_aC1 = [0.0  0.0  0.0];  %Tether attachment point on Clump-top (local CoordSys).
p_aC2 = [0.0  0.0  0.0];  %Tether attachment point on Clump-bottom (local CoordSys).

k = 2000;
b = 20;

Clump1 = add_Force_fcn(Clump1,@(body,RB,n) SD_Tether(body,RB,n,'Clump2',p_aC1,p_aC2,k,b));
Clump2 = add_Force_fcn(Clump2,@(body,RB,n) SD_Tether(body,RB,n,'Clump1',p_aC2,p_aC1,k,b));

%Set Initial Conditions  %Offset position from equilibrium position.
Clump1 = set_pos(Clump1,[0 0 0 0 0 0],1);
Clump2 = set_pos(Clump2,[0 0 -1 0 0 0],1);

RB{1} = Clump1;
RB{2} = Clump2;

%Solve for motion
RB = Solve_RK4(RB);

Clump1 = RB{1};
Clump2 = RB{2};

%Plot Buoy motion results
x = get(Clump1,'x');
xdot = get(Clump1,'xdot');
xddot = get(Clump1,'xddot');

v1 = xdot(:,3);

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

v2 = ydot(:,3);

%plotting results

%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('Elliptical Plate 1 m x 2 m mass = 105.3 Kg (top) ','FontName','times','FontSize', 12);
legend('surge(m)','heave(m)','pitch(rad)');



subplot(3,1,2),plot(t,xdot(:,1),t,xdot(:,3));
legend('surge vel (m/s)','heave vel (m/s)');


subplot(3,1,3),plot(t,xddot(:,1),t,xddot(:,3));
legend('surge accel (m/s^2)','heave accel (m/s^2)');
xlabel('time (s)');


%plate 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('Elliptical Plate 1 m x 2 m mass = 105.3 Kg (bottom)','FontName','times','FontSize', 12);
legend('surge(m)','heave(m)','pitch(rad)');


subplot(3,1,2), plot(t,ydot(:,1),t,ydot(:,3));
legend('surge vel (m/s)','heave vel (m/s)');


subplot(3,1,3), plot(t,yddot(:,1),t,yddot(:,3));
legend('surge accel (m/s^2)','heave accel (m/s^2)');
xlabel('time (s)');
%xlim([150  300])

% % pitching motion of cylinder
% 
% figure('Position', [100 100 1000 600])
% set(gcf,'color','white');
% subplot(3,1,1),plot(t,x(:,5));
% title('Cylinder - Pitching motion ','FontName','times','FontSize', 12);
% legend('cylinder (rad)');
% 
% 
% subplot(3,1,2),plot(t,xdot(:,5));
% legend('cylinder (rad/s)');
% ylim([-1  1])
% 
% subplot(3,1,3),plot(t,xddot(:,5));
% legend('cylinder (rad/s^2)');
% xlabel('time (s)');
% ylim([-1  1])
% 
% % pitching motion of ellipse
% 
% figure('Position', [100 100 1000 600])
% set(gcf,'color','white');
% subplot(3,1,1),plot(t,y(:,5));
% title('Ellipse - Pitching motion ','FontName','times','FontSize', 12);
% legend('ellipse (rad)');
% ylim([-20  20])
% 
% subplot(3,1,2),plot(t,ydot(:,5));
% legend('ellipse (rad/s)');
% ylim([-20  20])
% 
% subplot(3,1,3),plot(t,yddot(:,5));
% legend('ellipse (rad/s^2)');
% xlabel('time (s)');
% ylim([-20  20])
% 
% 
% % %Calculating drag force on the plate
% % 
% % len = 1.0;
% % rho = 1025;
% % area = len^2;
% 
% %Calculating power
% 
% dv = v2-v1;
% 
% for i =1:N;
%     pow(i) =dv(i,1)*dv(i,1);
%     pow(i) = 0.001*b*pow(i);
%     
%     
%     F_tether(i) = -k*(x(i,3)-y(i,3))-b*(v1(i)-v2(i));
%     
% end
% 
% meanpow = mean(pow);
% 
% figure('Position', [50 50 800 900])
% set(gcf,'color','white');
% 
% subplot(3,1,1),plot(t,dv);
% set(gca, 'FontSize',20,'FontWeight','bold');
% title('Relative heave velocity (m/s)','FontSize',20,'FontWeight','bold');
% ylim([-20 20])
% 
% for i=1:N;
% 
%     pow_wav(i) = 1.025*9.81*(2*2)*56*2.0/48; %in kW
%     pow_ext(i) = meanpow;
%     
% end
% 
% subplot(3,1,2), plot(t,pow,t,pow_ext);
% set(gca, 'FontSize',20,'FontWeight','bold');
% title('Power take-off (kW)','FontSize', 20,'FontWeight','bold');
% ylim ([0  10])
% 
% subplot(3,1,3),plot(t,F_tether/1000);
% set(gca, 'FontSize',20,'FontWeight','bold');
% title(' Tether forces (kN)','FontSize', 20,'FontWeight','bold');
% xlabel('time (s)');
% ylim ([-40 40])



