close all
clear all



b = .25;  
a = -1; 
c = 1; 

v = .5; %.0785/2;  %m/s

rho = 1025;  %kg/m^3


alpha0 = 40*pi/180;   phi0 = -pi/2; %-pi/4; %pi/2;
beta0 = 0.0;    phi1 = 0;
h0 = 1*1.3;       phi2 = 0;

T = 10;
f = 1/T;  %hz
p = 2*pi*f;  %Frequency

k = p*b/v;

J0 = besselj(0,k);
J1 = besselj(1,k);
Y0 = bessely(0,k);
Y1 = bessely(1,k);
C = ((J1.*(J1+Y0)+Y1.*(Y1-J0)) - i*(Y1.*Y0+J1.*J0))./((J1+Y0).^2+(Y1-J0).^2);
clear J0 J1 Y0 Y1;

c2 = c^2;
T1 = -(2+c2)*sqrt(1-c2)/3+c*acos(c);
T2 = c*(1-c2)-(1+c2)*sqrt(1-c2)*acos(c)+c*(acos(c))^2;
T3 = -(1-c2)*(5*c2+4)/8+c*(7+2*c2)*sqrt(1-c2)*acos(c)/4-(1/8+c2)*(acos(c))^2;
T4 = c*sqrt(1-c2)-acos(c);
T5 = -(1-c2)+2*c*sqrt(1-c2)*acos(c)-(acos(c))^2;
T6 = T2;
T7 = c*(7+2*c2)*sqrt(1-c2)/8-(1/8+c2)*acos(c);
T8 = -(1+2*c2)*sqrt(1-c2)/3+c*acos(c);

T10 = sqrt(1-c2)+acos(c);
T11 = (2-c)*sqrt(1-c2)+(1-2*c)*acos(c);
T12 = 2*T4+T11;
T13 = -.5*(T7+(c-a)*T1);
T14 = 1/16+a*c/2;
T15 = T4+T10;
T16 = T1-T8-(c-a)*T4+T11/2;

T18 = T5-T4*T10;
T19 = T4*T11;
T20 = -sqrt(1-c2)+acos(c);


dt = 1/f/100;
t = 0:dt:1/f;  %100 timesteps through period

alpha = alpha0*exp(i*(p*t+phi0));
alphadot = i*p*alpha;
alphaddot = -p^2*alpha;
beta = beta0*exp(i*(p*t+phi1));
betadot = i*p*beta;
betaddot = -p^2*beta;
h = h0*exp(i*(p*t+phi2));
hdot = i*p*h;
hddot = -p^2*h;



Q = v*alpha+hdot+b*(.5-a)*alphadot+T10*v*beta/pi+b*T11*betadot/(2*pi);
P = -rho*b^2*(v*pi*alphadot+pi*hddot-pi*b*a*alphaddot-v*T4*betadot-T1*b*betaddot)-2*pi*rho*v*b*C*Q;
M_alpha = -rho*b^2*(pi*(.5-a)*v*b*alphadot+pi*b^2*(1/8+a^2)*alphaddot+T15*v^2*beta+T16*v*b*betadot+2*T13*b^2*betaddot-a*pi*b*hddot)+2*rho*v*b^2*pi*(a+.5)*C*Q;

S = (sqrt(2)/2)*(2*C*Q-b*alphadot-2*sqrt(1-c2)*v*beta/pi+T4*b*betadot/pi);

P_beta = 0;

Px = -(pi*rho*b*imag(S).^2+imag(alpha).*imag(P)+imag(beta).*imag(P_beta));

%Compute average horizontal force
a1 = abs(C)^2;
a2 = b^2*(a1*(1/k^2+(.5-a)^2)+.25-(.5-a)*real(C)-imag(C)/k);
a4 = b*(a1*(-sin(phi2-phi0)/k+(.5-a)*cos(phi2-phi0))-real(C)*cos(phi2-phi0)/2+imag(C)*sin(phi2-phi0)/2);
b2 = b^2*(-a/2-real(C)/k^2+(.5-a)*imag(C)/k);
b4 = b*((.5+imag(C)/k)*cos(phi2-phi0)+real(C)*sin(phi2-phi0)/k)/2;

Pxbar = -pi*rho*b*p^2*(a1*h0^2+(a2+b2)*alpha0^2+2*(a4+b4)*alpha0*h0);

%Wbar = pi*rho*b*p^2*h0^2*real(C);

PowerIn = -mean(imag(P(1:end-1)).*imag(hdot(1:end-1)))-mean(imag(M_alpha(1:end-1)).*imag(alphadot(1:end-1)))

PowerOut = -mean(Px(1:end-1))*v

eff = PowerOut/PowerIn

figure
plot(t,imag(h),t,imag(hdot),t,imag(hddot))
xlabel('time (s)');
ylabel('m');
legend('h','hdot','hddot');

figure
plot(t,180*imag(alpha)/pi,t,180*imag(alphadot)/pi,t,180*imag(alphaddot)/pi)
xlabel('time (s)');
ylabel('deg');
legend('alpha','alphadot','alphaddot');


figure
plot(t,imag(P),t,Px)
xlabel('time (s)');
ylabel('N');
legend('P','Px');