clear all; clear classes; close all; clc;
addpath(genpath('~/Library/oscar-3.3/Classes'));

% Define the grid for the simulation
N_px = 256;
grid_size = 5e-3;
G1 = Grid(N_px,grid_size);

% Define the beam
w0 = 100e-6;
lambda = 1064e-9;
zR = pi*w0^2/lambda;
Ein=E_Field(G1,'w',w0,'R', 0.3, 'mode','HG 0 0');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculate accumulated Gouy Phase from -0.3m to 0.3m
% and calculate the beam parameter with Fit_TEM00 function

z = linspace(-0.3,0.3,100);

for ii = 1:length(z)

    GP_test(ii) = Calculate_Gouy_Phase(Ein,z(ii));
    Ein_z = Propagate_E(Ein,z(ii));
    [Fit_w(ii),Fit_R(ii), Fit_w0(ii), Fit_z0(ii)] = Fit_TEM00(Ein_z);

end

% The waist size and position can be calculated from the Gouy phase plot,
% which is suppose to be symmetric respect to it

y0 = (max(GP_test)+min(GP_test))/2; % find the central point of the plot
z0 = interp1(GP_test,z,y0,'linear'); % interpolate in order to find the z at that point

wz0 = w0 * sqrt( 1/2 + sqrt(1/4- z0^2*lambda^2/(pi^2*w0^4) ) );

Ein_z0 = Propagate_E(Ein,z0); % propagate the beam to the waist position
Fit_TEM00(Ein_z0) % check the beam parameters at that position

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure
plot(z,GP_test, LineWidth=6)
xlabel('z [m]')
ylabel('Gouy phase [rad]')
title('Accumulated Gouy Phase')
grid
ax = gca;
ax.FontSize = 16;

figure
plot(z,Fit_w, LineWidth=6)
xlabel('z [m]')
ylabel('beam size [m]')
title('beam propagation')
grid
ax = gca;
ax.FontSize = 16;

figure
plot(Fit_z0-z,Fit_w0, '.',MarkerSize=16)
hold on
plot(abs(z0),wz0, '.',MarkerSize=26)
xlabel('waist position [m]')
ylabel('waist size [m]')
title('Waist size and distance calculated from the fit')
legend('from Fit\_TEM00', 'from Gouy phase plot')
grid
ax = gca;
ax.FontSize = 16;

