close all
clear all

animate = 1;

print_results = 0;

tf = 15;

Hs = 2;
T = 9;

Vmean = 3*.515;

%Compute incident wave profile
wave = IncWave;
wave = set(wave,'A',Hs/2);         %Amplitude = 1 meter
wave = set(wave,'T',T);
wave = set(wave,'Type',3);      %Type 1 = Monochromatic wave  Type 3 = PM spectrum
wave = set(wave,'beta',-1);     %Waves moving towards negative x
wave = set(wave,'tau',0);     %No ramp
wave = set(wave,'n_freq',100);

f = get(wave,'freq');
H = get(wave,'spectrum');

%Plot Spectrum
figure
plot(f,H,'-x')
xlabel('freq (Hz)');
ylabel('m^2/s');
title(['PM:  Hs =' num2str(Hs) '  Tp = ' num2str(T) ' s']);
grid on


dt = .05; 
disp('Computing incident wave profile');
t = 0:dt:tf;
eta0 = eta(wave,0,t);

x = 0:.2:2*Vmean*t(end);
figure('Position',[    87         775        2236         694]);
pl1 = plot(x,NaN(size(x)));
hold all;
pl2 = plot(t,NaN,'ko');

verticalExtent = 1.5*max([max(eta0) -min(eta0)]);
ylim([-verticalExtent verticalExtent]);
xlim([x(1) x(end)]);
grid on;
xlabel('m');
ylabel('m');
th = title(['PM:  Hs =' num2str(Hs) '  Tp = ' num2str(T) ' s' '   t = 0s']);

eta0 = NaN(length(t),1);

xx = 0;

heave = NaN(length(t),1);
heave_dot = NaN(length(t),1);
surge_dot = NaN(length(t),1);
surge = NaN(length(t),1);
pitch = NaN(length(t),1);
eta0 = NaN(length(t),1);
Boat_u = NaN(length(t),1);
Boat_w = NaN(length(t),1);


u = NaN(length(t),1);
v = NaN(length(t),1);
w = NaN(length(t),1);

for k = 1:length(t)
    
  tk = t(k)
  eta0_k = eta(wave,0,t(k));  %Wave elevation at origin (measurement)
  eta0(k) = eta0_k;  %Save for plotting later
  eta_k = eta(wave,x,t(k));  %Compute wave-elevation everywhere at timestep k (for plotting)

  heave(k) = eta(wave,xx,tk);
  pitch(k) = deta_dx(wave,xx,tk);  %Should be in radians
  surge(k) = xx;
  
  [u(k) v(k) w(k)] = vel(wave,xx,0,tk);
  
  Boat_u(k) = Vmean*cos(pitch(k))-u(k);
  Boat_w(k) = Vmean*sin(pitch(k))-w(k);
  
  xx = xx+dt*Boat_u(k);  %Compute surge position based on resultant velocity of boat.  heave position comes from wave-height so no need to advance that.
  
  %Update Plot  
  if(animate == 1)
    set(pl1,'YData',eta_k);
    set(th,'string',['PM:  Hs =' num2str(Hs) '  Tp = ' num2str(T) ' s' '   t = ' num2str(tk) 's']);

    set(pl2,'XData',surge(k));
    set(pl2,'YData',heave(k));
    %drawnow;
    pause(dt/100);
  end
  
end

mean_Boat_u = nanmean(Boat_u);
%mean_Boat_u = (surge(end)-surge(2))/(t(end)-t(2))
%surge = surge-mean_Boat_u*t';
surge = detrend(surge);

Boat_u_accel = [0 ; diff(Boat_u)/dt];
Boat_w_accel = [0 ; diff(Boat_w)/dt];

figure('Position',[  286         130         954        1340])
ah1 = subplot(3,1,1)
plot(t,surge)
hold all
plot(t,heave);
xlabel('t');
ylabel('m');
legend('Surge Pos about mean forward speed','Heave');

ah2 = subplot(3,1,2)
plot(t,Boat_u-mean_Boat_u)
hold all
plot(t,Boat_w)
ylabel('m/s');
xlabel('t');
title(['Mean Boat Surge Vell = ' num2str(mean_Boat_u) ' m/s']);
legend('Surge Vel about mean forward speed','Heave Vel');

ah3 = subplot(3,1,3)
plot(t,Boat_u_accel)
hold all
plot(t,Boat_w_accel)
ylabel('m/s^2');
xlabel('t');
legend('Surge Accel','Heave Accel')

linkaxes([ah1 ah2 ah3],'x');


PitchRate = [0 ; diff(pitch)/dt];
PitchAccel = [0 ; diff(PitchRate)/dt];

figure('Position',[   1291         132         954        1340])
ah1 = subplot(3,1,1)
plot(t,pitch*180/pi)
ylabel('t');
xlabel('deg');
legend('Pitch');

ah2 = subplot(3,1,2)
plot(t,PitchRate*180/pi)
ylabel('deg/s');
xlabel('t');
legend('Pitch Rate');

ah3 = subplot(3,1,3)
plot(t,PitchAccel*180/pi)
ylabel('deg/s^2');
xlabel('t');
legend('Pitch Accel');

linkaxes([ah1 ah2 ah3],'x');

if(print_results)

fid = fopen('SurgeMotions.txt','w');
fprintf(fid,"%d %f %d\n",length(t),dt,2);
for k = 1:length(t)
   fprintf(fid,"%f %f %f %f\n",t(k), surge(k), Boat_u(k),Boat_u_accel(k));
end
fclose(fid);

fid = fopen('HeaveMotions.txt','w');
fprintf(fid,"%d %f %d\n",length(t),dt,2);
for k = 1:length(t)
   fprintf(fid,"%f %f %f %f\n",t(k), heave(k), Boat_w(k),Boat_w_accel(k));
end
fclose(fid);

fid = fopen('PitchMotions.txt','w');
fprintf(fid,"%d %f %d\n",length(t),dt,2);
for k = 1:length(t)
   fprintf(fid,"%f %f %f %f\n",t(k), pitch(k)/2, PitchRate(k)/2,PitchAccel(k)/2);
end
fclose(fid);
end