%% Elementary constants
clear;
load qf
epsilon=8.854187817e-12;
c = 299792458;                      %Speed of light [m/s]
h = 6.626070040e-34;                %Planck constant [J/Hz]
hbar = h/2/pi;  
u0 = 4*pi*1e-7;                     %Vacuum permeability [N/A^2]
e0 = 1/u0/(c^2);                    %Vacuum permittivity [A^2*s^4/kg/m^3]
qe = 1.6021766208e-19;              %Elementary charge [C]
G = 6.67408e-11;                    %Gravitational constant [m^3/kg/s^2]
kB = 1.38064852e-23;                %Boltzmann constant [J/K]
me = 9.10938356e-31;                %Electron rest mass [kg]
mp = 1.672621898e-27;               %Proton mass [kg]
uB = qe*hbar /2/me;                 %Bohr magneton [J/T]
uN = qe*hbar /2/mp;                 %Nuclear magnton [J/T]
alpha = u0*qe^2*c/(2*h);            %Fine structure constant
u = 1.660539040e-27;                %Atomic mass unit [kg]
a0 = 4*pi*e0*hbar^2/me/qe^2;        %Bohr radius [m]
Eh = hbar^2/(me*a0^2);              %Rydberg energy [eV]
%% Laser Properties
wavelength=532e-9;                   %Laser wavlength [m]
k=2*pi/wavelength;                   %Wave vector [m^-1]
P=10;                                 %Power of each beam [W]
wz0=60e-6;                           %Beam waist in z-theta-direction[m]
wy0=250e-6;
zRy=pi*wy0^2./wavelength;        %Rayleigh wavelength
zRz=pi*wz0^2./wavelength;
theta=0.5/180*pi;                    %Half angle of interference
x_ext =wz0./tan(theta);

%Dysprosium Properties
polar532=350*qe^2*a0^2/Eh;          %Dynamical Polarizability of Dy at 532nm
polar1064=193*qe^2*a0^2/Eh;
mDy = 164*u;                        %Dysprosium mass [kg]
%% Trapping frequency as Plane wave
vz=sqrt(P.*polar532*k^2*sin(theta)^2/(pi^3*wz0*wy0*c*mDy*epsilon));

d = wavelength/(2*tan(theta));      %Fringe distance
focus = 100e-3;
D = wavelength*focus/d;
%% Gaussian beams

%Coordinate System
n=[2001,3,2001];
xmax=(x_ext.*3/2);
xmin=-(x_ext).*3/2;
ymax=2.*wy0;
ymin=-2.*wy0;
zmax=2.*wz0;
zmin=-2.*wz0;

for i=1:3
    x=linspace(xmin,xmax,n(1));
    y=linspace(ymin,ymax,n(2));
    z=linspace(zmin,zmax,n(3));
    [X,Y,Z]=meshgrid(x,y,z);

    %Rotation of the coordinte system
    xTheta=X.*cos(theta)+Z.*sin(theta);
    xPhi=X.*cos(theta)-Z.*sin(theta);
    zTheta=-X.*sin(theta)+Z.*cos(theta);
    zPhi=X.*sin(theta)+Z.*cos(theta);

    %Electric field
    E0=sqrt(2*P/pi/(wz0*wy0)/c/epsilon);        %Electric field at 0 

    wz1=wz0*sqrt(1+(xTheta./zRz).^2);            %Beam waist in z-direction for Beam 1
    wy1=wy0*sqrt(1+(xTheta./zRy).^2);            %Beam waist in y-direction for Beam 1
    wz2=wz0*sqrt(1+(xPhi./zRz).^2);              %Beam waist in z-direction for Beam 2
    wy2=wy0*sqrt(1+(xPhi./zRy).^2);              %Beam waist in y-direction for Beam 1

    R1y=xTheta.*(1+(zRy./xTheta).^2);             %Rayleigh length Beam 1
    R1z=xTheta.*(1+(zRz./xTheta).^2);
    R2y=xPhi.*(1+(zRy./xPhi).^2);                 %Rayleigh length Beam 2
    R2z=xPhi.*(1+(zRz./xPhi).^2);

    R1y(:,floor(n(1)/2)+1,floor(n(3)/2)+1)=Inf;  %Rayleigh length ->0
    R1z(:,floor(n(1)/2)+1,floor(n(3)/2)+1)=Inf;
    R2y(:,floor(n(1)/2)+1,floor(n(3)/2)+1)=Inf;
    R2z(:,floor(n(1)/2)+1,floor(n(3)/2)+1)=Inf;

    gouyPhase1=atan(wavelength.*xTheta./(pi.*wz0.*wy0));
    gouyPhase2=atan(wavelength.*xPhi./(pi.*wz0.*wy0));
    
   
    E1=E0.*sqrt(wz0.*wy0)./sqrt(wz1.*wy1).*exp(-(zTheta./wz1).^2-(Y./wy1).^2).*exp(1i*(k.*xTheta-gouyPhase1)+1i.*k.*(zTheta.^2./2./R1z+Y.^2./2./R1y));    %Electric field Beam 1
    E2=E0.*sqrt(wz0.*wy0)./sqrt(wz2.*wy2).*exp(-(zPhi./wz2).^2-(Y./wy2).^2).*exp(1i*(k.*xPhi-gouyPhase2)+1i.*k.*(zPhi.^2./2./R2z+Y.^2./2./R2y));          %Electric field Beam 2

  
    %Intensity and trapping Potential

    Itot=abs(E1+E2).^2./2*c*epsilon;
    
    if i==1 %Set y=0
        Itot_y=squeeze(Itot(2,:,:));
        Imax=Itot_y(1001,1001);
        
        figure
        imagesc(x,z, Itot_y')
        title('x-z Interference Pattern for \theta=', theta*180/pi);
        colormap(QF);
        caxis([0 Imax]); 
        
        %z-profile for x=0
        Udip=0.5.*Itot_y.*polar532./epsilon./c;
        Umid=squeeze(Udip(floor(n(1)/2)+1,:));
        zrange = find((-pi/(3*k*sin(theta))) < z & z < (pi./(3*k*sin(theta))));
        zfit = z(zrange(1):zrange(length(zrange)));
        Ufit = Umid(zrange(1):zrange(length(zrange)));
        zfit = zfit(:);
        Ufit = Ufit(:);
        [pks, locs]=findpeaks(Umid);
        p = polyfitn(zfit,Ufit,'constant x^2');

%         figure
%         plot(z,Umid, zfit, Ufit,'.', zfit, polyvaln(p,zfit))
%         title('Trapping potential in z-direction for x=y=0')
%         %Fitted Trapping frequency
%         nu_zfixed=sqrt(-p.Coefficients(2)*2/mDy)/2/pi;

        
        %FWHM of x-profile (actually 1/e^2)
        Uz0=squeeze(Udip(:,floor(n(1)/2)+1));
        Ufwhm= find(Uz0>Uz0(floor(n(1)/2)+1)/exp(2));
        x_ext_fwhm=abs(x(Ufwhm(1))-x(Ufwhm(length(Ufwhm))));
        x_range=x(Ufwhm);
        
        %x-profile for z=0
        xrange=find((-x_ext_fwhm/10)<x & x<(x_ext_fwhm/10));
        xfit=x(xrange(1):xrange(length(xrange)));
        Ufitx = Uz0(xrange(1):xrange(length(xrange)));
        
        px= polyfitn(xfit,Ufitx, 'constant x^2');
        
%         figure
%         plot(x,Uz0,xfit, Ufitx, '.',xfit, polyvaln(px,xfit) )
%         xline(x_ext_fwhm/2)
%         xline(-x_ext_fwhm/2)
%         title('x-Potential')
%         nu_xfixed=sqrt(-px.Coefficients(2)*2/mDy)/2/pi;
%         
       

        %z-profile x=x_ext
        Ux_ext=squeeze(Udip(Ufwhm(1),:));
        Ux_ext_fit=Ux_ext(zrange(1):zrange(length(zrange)));

        p_xext = polyfitn(zfit,Ux_ext_fit,'constant x^2');

%         figure
%         plot(z,Ux_ext, zfit, Ux_ext_fit,'.', zfit, polyvaln(p_xext,zfit))
%         %Fitted Trapping Frequency
%         nu_zxext=sqrt(-p_xext.Coefficients(2)*2/mDy)/2/pi;


    else
        if i==2
            Itot_z=squeeze(Itot(:,:,2));
%   
%             figure
%             imagesc(x,y,Itot_z)
%             title('x-y Interference Pattern for \theta=', theta*180/pi);
%             colormap(QF);
%             %caxis([min(min(Itot_z)) max(max(Itot_z))])
            
            %Gaussian profile y
            Udip_y=0.5.*Itot_z.*polar532./epsilon./c;
            Umid_y=squeeze(Udip_y(:,floor(n(1)/2)+1));
            
            Ufwhm_y= find(Umid_y>Umid_y(floor(n(1)/2)+1)/exp(2));
            y_ext_fwhm=abs(y(Ufwhm_y(1))-y(Ufwhm_y(length(Ufwhm_y))));
            y_range=y(Ufwhm_y);
            
            yrange = find( -y_ext_fwhm/20 < y & y < y_ext_fwhm/20);
            yfit = y(yrange(1):yrange(length(yrange)));
            Ufit_y = Umid_y(yrange(1):yrange(length(yrange)));
            
            yfit = yfit(:);
            Ufit_y = Ufit_y(:);

            p_ext_y = polyfitn(yfit,Ufit_y, 'constant x^2');
            
%             figure
%             plot(y,Umid_y, yfit, Ufit_y,'.', yfit, polyvaln(p_ext_y,yfit))
%             title('y profile for \theta=', theta*180/pi);
            
            nu_y=sqrt(-p_ext_y.Coefficients(2)*2/mDy)/2/pi;
           
            
            %Gaussian profile x
            Udip_x=0.5.*Itot_z.*polar532./epsilon./c;
            Umid_x=squeeze(Udip_x(floor(n(1)/2)+1,:));
            
            Ufwhm_x= find(Umid_x>Umid_x(floor(n(1)/2)+1)/exp(2));
            x_ext_fwhm=abs(x(Ufwhm_x(1))-x(Ufwhm_x(length(Ufwhm_x))));
            x_range=x(Ufwhm_x);
            
            xrange = find( -x_ext_fwhm/20 < x & x < x_ext_fwhm/20);
            xfit = x(xrange(1):xrange(length(xrange)));
            Ufit_x = Umid_x(xrange(1):xrange(length(xrange)));
            
            xfit = xfit(:);
            Ufit_x = Ufit_x(:);

            p_ext_x = polyfitn(xfit,Ufit_x,'constant x^2');
            
%             figure
%             plot(x,Umid_x, xfit, Ufit_x,'.', xfit, polyvaln(p_ext_x,xfit))
%             title('x profile for \theta=', theta*180/pi);
            
            nu_x=sqrt(-p_ext_x.Coefficients(2)*2/mDy)/2/pi;
            

        else
            Itot_x=squeeze(Itot(:,2,:));
%             figure
%             imagesc(y,z,Itot_x')
%             title('y-z Interference Pattern for \theta=', theta*180/pi);
%             colormap(QF);
%             %caxis([min(min(Itot_x)) max(max(Itot_x))])
         end
    end

            tempn=n(3);
            n(3)=n(2);
            n(2)=n(1);
            n(1)=tempn;
end 

Trap_f= table(theta*180/pi,P, wavelength, vz, nu_zfixed,nu_zxext, nu_xfixed,nu_x, nu_y,'VariableNames',{'Theta', 'Laser Power','Laser Wavelength', 'vz Plane wave','vz Gaussian Beam', 'vz Gaussian Beam at x_ext','vx in x-z-Plane','vx in x-y-Plane', 'vy in x-y-Plane'}); 


%% Peaks for shallow angle
npeaks=length(pks);
smax=(npeaks-1)/2;
fringeratios=zeros(smax,3);
for s=1:1:smax+1
    ratio=pks((npeaks+1)/2+s-1)/pks((npeaks+1)/2);
    fringeratios(s,1)=s-1;
    fringeratios(s,2)=pks((npeaks+1)/2+s-1);
    fringeratios(s,3)=ratio;
end