import numpy as np
from scipy.optimize import curve_fit
from scipy import interpolate
from astropy import units as u, constants as ac

from Potentials import *
from Helpers import *

DY_POLARIZABILITY = 184.4 # in a.u, most precise measured value of Dy polarizability
DY_MASS = 164*u.u
DY_DIPOLE_MOMENT = 9.93 * ac.muB

#####################################################################
# AUXILIARY COMPUTATIONS
#####################################################################

def calculateHeatingRate(w_x, w_y, P, linewidth, detuning, wavelength = 1.064*u.um):
    U = trap_depth(w_x, w_y, P).decompose()
    Gamma_sr = ((linewidth * U) / (ac.hbar * detuning)).decompose()
    E_recoil = (ac.h**2 / (2 * DY_MASS * wavelength**2)).decompose()
    T_recoil = (E_recoil/ac.k_B).to(u.uK)
    HeatingRate = 2/3 * Gamma_sr * T_recoil
    return HeatingRate, T_recoil, Gamma_sr, U

def calculateAtomNumber(NCount, pixel_size, magnification, eta):
    sigma = 8.468e-14 * (u.m)**(2)
    return (1/eta * 1/sigma * NCount * pixel_size**2/magnification**2).decompose()

def meanThermalVelocity(T, m = DY_MASS):
    return 4 * np.sqrt((ac.k_B * T) /(np.pi * m))

def particleDensity(w_x, w_z, Power, N, T, m = DY_MASS): # For a thermal cloud
    v_x = calculateTrapFrequency(w_x, w_z, Power, dir = 'x')
    v_y = calculateTrapFrequency(w_x, w_z, Power, dir = 'y')
    v_z = calculateTrapFrequency(w_x, w_z, Power, dir = 'z')
    return N * (2 * np.pi)**3 * (v_x * v_y * v_z) *  (m / (2 * np.pi * ac.k_B * T))**(3/2)

def calculateParticleDensityFromMeasurements(v_x, dv_x, v_y, dv_y, v_z, dv_z, w_x, w_z, T_x, T_y, dT_x, dT_y, modulation_depth, N, m = DY_MASS):
        alpha_x =  [(v_x[0]/x)**(2/3) for x in v_x]
        dalpha_x = [alpha_x[i] * np.sqrt((dv_x[0]/v_x[0])**2 + (dv_x[i]/v_x[i])**2) for i in range(len(v_x))]
        alpha_y = [(v_z[0]/y)**2 for y in v_z]
        dalpha_y = [alpha_y[i] * np.sqrt((dv_z[0]/v_z[0])**2 + (dv_z[i]/v_z[i])**2) for i in range(len(v_z))]

        avg_alpha = [(g + h) / 2 for g, h in zip(alpha_x, alpha_y)]
        new_aspect_ratio = (w_x * avg_alpha) / w_z

        avg_T = [(g + h) / 2 for g, h in zip(T_x, T_y)]
        avg_dT = [0.5 * np.sqrt(g**2 + h**2) for g, h in zip(dT_x, dT_y)]

        pd = np.zeros(len(modulation_depth))
        dpd = np.zeros(len(modulation_depth))
        
        for i in range(len(modulation_depth)):
            particle_density = (N * (2 * np.pi)**3 * (v_x[i] * v_y[i] * v_z[i]) *  (m / (2 * np.pi * ac.k_B * avg_T[i]))**(3/2)).decompose()
            pd[i] = particle_density.value
            dpd[i] = (((N * (2 * np.pi)**3 * (m / (2 * np.pi * ac.k_B * avg_T[i]))**(3/2)) * ((dv_x[i] * v_y[i] * v_z[i]) + (v_x[i] * dv_y[i] * v_z[i]) + (v_x[i] * v_y[i] * dv_z[i]) - (1.5*(v_x[i] * v_y[i] * v_z[i])*(avg_dT[i]/avg_T[i])))).decompose()).value  
        
        pd = pd*particle_density.unit
        dpd = dpd*particle_density.unit
        
        return pd, dpd, avg_T, avg_dT, new_aspect_ratio
    
def thermaldeBroglieWavelength(T, m = DY_MASS):
    return np.sqrt((2*np.pi*ac.hbar**2)/(m*ac.k_B*T))

def scatteringLength(B, FR_choice = 1, ABKG_choice = 1):
    # Dy 164 a_s versus B in 0 to 8G range
    # should match SupMat of PhysRevX.9.021012, fig S5 and descrption
    # https://journals.aps.org/prx/supplemental/10.1103/PhysRevX.9.021012/Resubmission_Suppmat.pdf
    
    if FR_choice == 1: # new values
        
        if ABKG_choice == 1:
            a_bkg = 85.5 * ac.a0
        elif ABKG_choice == 2:
            a_bkg = 93.5 * ac.a0
        elif ABKG_choice == 3:
            a_bkg = 77.5 * ac.a0
        
        #FR resonances
        #[B11 B12 B2 B3 B4 B51 B52 B53 B6 B71 B72 B81 B82 B83 B9]
        resonanceB  = [1.295, 1.306, 2.174, 2.336, 2.591, 2.74, 2.803, 2.78, 3.357, 4.949, 5.083, 7.172, 7.204, 7.134, 76.9] * u.G #resonance position
        #[wB11 wB12 wB2 wB3 wB4 wB51 wB52 wB53 wB6 wB71 wB72 wB81 wB82 wB83 wB9]
        resonancewB = [0.009, 0.010, 0.0005, 0.0005, 0.001, 0.0005, 0.021, 0.015, 0.043, 0.0005, 0.130, 0.024, 0.0005, 0.036, 3.1] * u.G #resonance width
        
    else: # old values
        
        if ABKG_choice == 1:
            a_bkg = 87.2 * ac.a0
        elif ABKG_choice == 2:
            a_bkg = 95.2 * ac.a0
        elif ABKG_choice == 3:
            a_bkg = 79.2 * ac.a0

        #FR resonances
        #[B1 B2 B3 B4 B5 B6 B7 B8]
        resonanceB  = [1.298, 2.802, 3.370, 5.092, 7.154, 2.592, 2.338, 2.177] * u.G #resonance position
        #[wB1 wB2 wB3 wB4 wB5 wB6 wB7 wB8]
        resonancewB = [0.018, 0.047, 0.048, 0.145, 0.020, 0.008, 0.001, 0.001] * u.G #resonance width
        
    #Get scattering length
    np.seterr(divide='ignore')
    a_s = a_bkg * np.prod([(1 - resonancewB[j] / (B - resonanceB[j])) for j in range(len(resonanceB))])
    return a_s, a_bkg

def dipolarLength(mu = DY_DIPOLE_MOMENT, m = DY_MASS):
    return (m * ac.mu0 * mu**2) / (12 * np.pi * ac.hbar**2)

def scatteringCrossSection(B):
    return 8 * np.pi * scatteringLength(B)[0]**2 + ((32*np.pi)/45) * dipolarLength()**2

def calculateElasticCollisionRate(w_x, w_z, Power, N, T, B): #For a 3D Harmonic Trap
    return (particleDensity(w_x, w_z, Power, N, T) * scatteringCrossSection(B) * meanThermalVelocity(T) / (2 * np.sqrt(2))).decompose()

def calculatePSD(w_x, w_z, Power, N, T):
    return (particleDensity(w_x, w_z, Power, N, T) * thermaldeBroglieWavelength(T)**3).decompose()

def convert_modulation_depth_to_alpha(modulation_depth):
    fin_mod_dep = [0, 0.5, 0.3, 0.7, 0.9, 0.8, 1.0, 0.6, 0.4, 0.2, 0.1]
    fx = [3.135, 0.28, 0.690, 0.152, 0.102, 0.127, 0.099, 0.205, 0.404, 1.441, 2.813]
    dfx = [0.016, 0.006, 0.005, 0.006, 0.003, 0.002, 0.002,0.002, 0.003, 0.006, 0.024]
    fz = [2.746, 1.278, 1.719, 1.058, 0.923, 0.994, 0.911, 1.157, 1.446, 2.191, 2.643]
    dfz = [0.014, 0.007, 0.009, 0.007, 0.005, 0.004, 0.004, 0.005, 0.007, 0.009, 0.033]

    alpha_x =  [(fx[0]/x)**(2/3) for x in fx]
    dalpha_x = [alpha_x[i] * np.sqrt((dfx[0]/fx[0])**2 + (dfx[i]/fx[i])**2) for i in range(len(fx))]
    alpha_z = [(fz[0]/z)**2 for z in fz]
    dalpha_z = [alpha_z[i] * np.sqrt((dfz[0]/fz[0])**2 + (dfz[i]/fz[i])**2) for i in range(len(fz))]

    avg_alpha = [(g + h) / 2 for g, h in zip(alpha_x, alpha_z)]
    sorted_fin_mod_dep, sorted_avg_alpha = zip(*sorted(zip(fin_mod_dep, avg_alpha)))

    f = interpolate.interp1d(sorted_fin_mod_dep, sorted_avg_alpha)

    return f(modulation_depth), fin_mod_dep, alpha_x, alpha_z, dalpha_x, dalpha_z

def convert_modulation_depth_to_temperature(modulation_depth):
    fin_mod_dep = [1.0, 0.8, 0.6, 0.4, 0.2, 0.0, 0.9, 0.7, 0.5, 0.3, 0.1]
    T_x = [22.1, 27.9, 31.7, 42.2, 98.8, 145.8, 27.9, 33.8, 42.4, 61.9, 136.1]
    dT_x = [1.7, 2.6, 2.4, 3.7, 1.1, 0.6, 2.2, 3.2, 1.7, 2.2, 1.2]
    T_y = [13.13, 14.75, 18.44, 26.31, 52.55, 92.9, 13.54, 16.11, 21.15, 35.81, 85.8]
    dT_y = [0.05, 0.05, 0.07, 0.16, 0.28, 0.7, 0.04, 0.07, 0.10, 0.21, 0.8]

    avg_T = [(g + h) / 2 for g, h in zip(T_x, T_y)]
    sorted_fin_mod_dep, sorted_avg_T = zip(*sorted(zip(fin_mod_dep, avg_T)))

    f = interpolate.interp1d(sorted_fin_mod_dep, sorted_avg_T)

    return f(modulation_depth), fin_mod_dep, T_x, T_y, dT_x, dT_y

#####################################################################
# COMPUTE/EXTRACT TRAP POTENTIAL AND PARAMETERS                     #
#####################################################################

def trap_depth(w_1, w_2, P, alpha = DY_POLARIZABILITY):
    return 2*P/(np.pi*w_1*w_2) * (1 / (2 * ac.eps0 * ac.c)) * alpha * (4 * np.pi * ac.eps0 * ac.a0**3)

def calculateTrapFrequency(w_x, w_z, Power, dir = 'x', m = DY_MASS):
    TrapDepth = trap_depth(w_x, w_z, Power)
    TrapFrequency = np.nan
    if dir == 'x':
        TrapFrequency = ((1/(2 * np.pi)) * np.sqrt(4 * TrapDepth / (m*w_x**2))).decompose()
    elif dir == 'y':
        zReff = np.sqrt(2) * z_R(w_x, 1.064*u.um) * z_R(w_z, 1.064*u.um) / np.sqrt(z_R(w_x, 1.064*u.um)**2 + z_R(w_z, 1.064*u.um)**2)
        TrapFrequency = ((1/(2 * np.pi)) * np.sqrt(2 * TrapDepth/ (m*zReff**2))).decompose()
    elif dir == 'z':
        TrapFrequency = ((1/(2 * np.pi)) * np.sqrt(4 * TrapDepth/ (m*w_z**2))).decompose()
    return round(TrapFrequency.value, 2)*u.Hz

def calculateCrossedBeamTrapFrequency(delta, Waists, Powers, dir = 'x', m = DY_MASS, wavelength=1.064*u.um):
    
    TrapDepth_1 = trap_depth(Waists[0][0], Waists[1][0], Powers[0])
    TrapDepth_2 = trap_depth(Waists[0][1], Waists[1][1], Powers[1])
    
    w_x1 = Waists[0][0]
    w_z1 = Waists[1][0]

    w_y2 = Waists[0][1]
    w_z2 = Waists[1][1] 
        
    zReff_1 = np.sqrt(2) * z_R(w_x1, wavelength) * z_R(w_z1, wavelength) / np.sqrt(z_R(w_x1, wavelength)**2 + z_R(w_z1, wavelength)**2)
    zReff_2 = np.sqrt(2) * z_R(w_y2, wavelength) * z_R(w_z2, wavelength) / np.sqrt(z_R(w_y2, wavelength)**2 + z_R(w_z2, wavelength)**2)
    
    wy2alpha = np.sqrt(((np.cos(np.radians(90 - delta)) / w_y2)**2 + (np.sin(np.radians(90 - delta)) / (2 * zReff_2))**2)**(-1))
    zR2alpha = np.sqrt(((np.sin(np.radians(90 - delta)) / w_y2)**2 + (np.cos(np.radians(90 - delta)) / (2 * zReff_2))**2)**(-1))    
   
    TrapFrequency = np.nan
    if dir == 'x':
        v_1 = (1/(2 * np.pi)) * np.sqrt(4/m * (TrapDepth_1 / w_x1**2))
        v_2 = (1/(2 * np.pi)) * np.sqrt(4/m * (TrapDepth_2 / zR2alpha**2))
        TrapFrequency = (np.sqrt(v_1**2 + v_2**2)).decompose()
    elif dir == 'y':
        v_1 = (1/(2 * np.pi)) * np.sqrt(1/m * (2 * TrapDepth_1/ zReff_1**2))
        v_2 = (1/(2 * np.pi)) * np.sqrt(1/m * (4 * TrapDepth_2/ wy2alpha**2))
        TrapFrequency = (np.sqrt(v_1**2 + v_2**2)).decompose()
    elif dir == 'z':
        v_1 = (1/(2 * np.pi)) * np.sqrt(4/m * (TrapDepth_1 / w_z1**2))
        v_2 = (1/(2 * np.pi)) * np.sqrt(4/m * (TrapDepth_2 / w_z2**2))
        TrapFrequency = (np.sqrt(v_1**2 + v_2**2)).decompose()

    return round(TrapFrequency.value, 2)*u.Hz

def extractTrapFrequency(Positions, TrappingPotential, axis):
    tmp_pos = Positions[axis, :]
    tmp_pot = TrappingPotential[axis]
    center_idx = np.argmin(tmp_pot)
    lb = int(round(center_idx - len(tmp_pot)/200, 1))
    ub = int(round(center_idx + len(tmp_pot)/200, 1))
    xdata = tmp_pos[lb:ub]
    Potential = tmp_pot[lb:ub]
    p0 = [1e3, tmp_pos[center_idx].value, -100]
    try:
        popt, pcov = curve_fit(harmonic_potential, xdata, Potential, p0)
        v = popt[0]
        dv = pcov[0][0]**0.5 
    except:
        popt = np.nan
        pcov = np.nan
        v    = np.nan
        dv   = np.nan

    return v, dv, popt, pcov

def computeTrapPotential(w_x, w_z, Power, options):

    axis = options['axis']
    extent = options['extent']
    gravity = options['gravity']
    astigmatism = options['astigmatism']
    modulation = options['modulation']
    crossed = options['crossed']

    if modulation:
        aspect_ratio = options['aspect_ratio']
        current_ar = w_x/w_z
        w_x = w_x * (aspect_ratio / current_ar)

    TrappingPotential = []
    TrapDepth = trap_depth(w_x, w_z, Power) 
    IdealTrapDepthInKelvin = (TrapDepth/ac.k_B).to(u.uK)
    
    projection_axis = np.array([0, 1, 0]) # default

    if axis == 0:
        projection_axis = np.array([1, 0, 0]) # radial direction (X-axis)
    
    elif axis == 1:
        projection_axis = np.array([0, 1, 0]) # propagation direction (Y-axis)
    
    elif axis == 2:
        projection_axis = np.array([0, 0, 1]) # vertical direction (Z-axis)
    
    else:
        projection_axis = np.array([1, 1, 1]) # vertical direction (Z-axis)
    
    x_Positions      = np.arange(-extent, extent, 0.05)*u.um 
    y_Positions      = np.arange(-extent, extent, 0.05)*u.um
    z_Positions      = np.arange(-extent, extent, 0.05)*u.um 
    Positions        = np.vstack((x_Positions, y_Positions, z_Positions)) * projection_axis[:, np.newaxis]

    if not crossed:
        IdealTrappingPotential        = single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power) 
        IdealTrappingPotential        = IdealTrappingPotential * (np.ones((3, len(IdealTrappingPotential))) * projection_axis[:, np.newaxis])
        IdealTrappingPotential        = (IdealTrappingPotential/ac.k_B).to(u.uK) 

        if gravity and not astigmatism:
            # Influence of Gravity
            m = DY_MASS
            gravity_axis             = np.array([0, 0, 1]) 
            tilt_gravity             = options['tilt_gravity']
            theta                    = options['theta']
            tilt_axis                = options['tilt_axis']
            if tilt_gravity:
                R = rotation_matrix(tilt_axis, np.radians(theta))
                gravity_axis = np.dot(R, gravity_axis)
            gravity_axis_positions   = np.vstack((x_Positions, y_Positions, z_Positions)) * gravity_axis[:, np.newaxis]
            TrappingPotential        = single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power)
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis]) + gravitational_potential(gravity_axis_positions, m)
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
            
        elif not gravity and astigmatism:
            # Influence of Astigmatism
            foci_disp_single = options['foci_disp_single']
            TrappingPotential        = astigmatic_single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power, del_y = foci_disp_single)
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis])
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
        
        elif gravity and astigmatism:
            # Influence of Gravity and Astigmatism
            m = DY_MASS
            gravity_axis             = np.array([0, 0, 1]) 
            tilt_gravity             = options['tilt_gravity']
            theta                    = options['theta']
            tilt_axis                = options['tilt_axis']
            foci_disp_single         = options['foci_disp_single']
            if tilt_gravity:
                R = rotation_matrix(tilt_axis, np.radians(theta))
                gravity_axis = np.dot(R, gravity_axis)
            gravity_axis_positions   = np.vstack((x_Positions, y_Positions, z_Positions)) * gravity_axis[:, np.newaxis]
            TrappingPotential        = astigmatic_single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power, del_y = foci_disp_single)
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis]) + gravitational_potential(gravity_axis_positions, m)
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
        
        else:
            TrappingPotential = IdealTrappingPotential
    
        infls = np.where(np.diff(np.sign(np.gradient(np.gradient(TrappingPotential[axis].value)))))[0]
        
        try:
            if TrappingPotential[axis][0] > TrappingPotential[axis][-1]:
                EffectiveTrapDepthInKelvin = max(TrappingPotential[axis][infls[1]:-1]) - min(TrappingPotential[axis][infls[0]:infls[1]])
            elif TrappingPotential[axis][0] < TrappingPotential[axis][-1]:
                EffectiveTrapDepthInKelvin = max(TrappingPotential[axis][0:infls[0]]) - min(TrappingPotential[axis][infls[0]:infls[1]])            
            else:
                EffectiveTrapDepthInKelvin = IdealTrapDepthInKelvin
        except:
            EffectiveTrapDepthInKelvin = np.nan

        TrapDepthsInKelvin = [IdealTrapDepthInKelvin, EffectiveTrapDepthInKelvin]

        v_x = calculateTrapFrequency(w_x, w_z, Power, dir = 'x')
        v_y = calculateTrapFrequency(w_x, w_z, Power, dir = 'y')
        v_z = calculateTrapFrequency(w_x, w_z, Power, dir = 'z')
        CalculatedTrapFrequencies = [v_x, v_y, v_z]

        v, dv, popt, pcov = extractTrapFrequency(Positions, IdealTrappingPotential, axis)
        if np.isinf(v):
            v = np.nan
        if np.isinf(dv):
            dv = np.nan

        IdealTrapFrequency = [v, dv]
        
        if options['extract_trap_frequencies']:
            v, dv, popt, pcov = extractTrapFrequency(Positions, TrappingPotential, axis)
            if np.isinf(v):
                v = np.nan
            if np.isinf(dv):
                dv = np.nan
            TrapFrequency = [v, dv]
            ExtractedTrapFrequencies = [IdealTrapFrequency, TrapFrequency]
        else:
            ExtractedTrapFrequencies = [IdealTrapFrequency]
        
        return Positions, IdealTrappingPotential, TrappingPotential, TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies

    else:
        waists = np.vstack((np.asarray([w_x[0].value, w_z[0].value])*u.um, np.asarray([w_x[1].value, w_z[1].value])*u.um))
        IdealTrappingPotential        = crossed_beam_potential(Positions, waists, Power, options) 
        IdealTrappingPotential        = IdealTrappingPotential * (np.ones((3, len(IdealTrappingPotential))) * projection_axis[:, np.newaxis])
        IdealTrappingPotential        = (IdealTrappingPotential/ac.k_B).to(u.uK) 

        if gravity and not astigmatism:
            # Influence of Gravity
            m = DY_MASS
            gravity_axis             = np.array([0, 0, 1]) 
            tilt_gravity             = options['tilt_gravity']
            theta                    = options['theta']
            tilt_axis                = options['tilt_axis']
            if tilt_gravity:
                R = rotation_matrix(tilt_axis, np.radians(theta))
                gravity_axis = np.dot(R, gravity_axis)
            gravity_axis_positions   = np.vstack((x_Positions, y_Positions, z_Positions)) * gravity_axis[:, np.newaxis]
            TrappingPotential        = crossed_beam_potential(Positions, waists, Power, options) 
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis]) + gravitational_potential(gravity_axis_positions, m)
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
            
        elif not gravity and astigmatism:
            # Influence of Astigmatism
            TrappingPotential        = astigmatic_crossed_beam_potential(Positions, waists, Power, options)
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis])
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
        
        elif gravity and astigmatism:
            # Influence of Gravity and Astigmatism
            m = DY_MASS
            gravity_axis             = np.array([0, 0, 1]) 
            tilt_gravity             = options['tilt_gravity']
            theta                    = options['theta']
            tilt_axis                = options['tilt_axis']
            if tilt_gravity:
                R = rotation_matrix(tilt_axis, np.radians(theta))
                gravity_axis = np.dot(R, gravity_axis)
            gravity_axis_positions   = np.vstack((x_Positions, y_Positions, z_Positions)) * gravity_axis[:, np.newaxis]
            TrappingPotential        = astigmatic_crossed_beam_potential(Positions, waists, Power, options)
            TrappingPotential        = TrappingPotential * (np.ones((3, len(TrappingPotential))) * projection_axis[:, np.newaxis]) + gravitational_potential(gravity_axis_positions, m)
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
        
        else:
            TrappingPotential = IdealTrappingPotential

        infls = np.where(np.diff(np.sign(np.gradient(np.gradient(TrappingPotential[axis].value)))))[0]
        
        try:
            if TrappingPotential[axis][0] > TrappingPotential[axis][-1]:
                EffectiveTrapDepthInKelvin = max(TrappingPotential[axis][infls[1]:-1]) - min(TrappingPotential[axis][infls[0]:infls[1]])
            elif TrappingPotential[axis][0] < TrappingPotential[axis][-1]:
                EffectiveTrapDepthInKelvin = max(TrappingPotential[axis][0:infls[0]]) - min(TrappingPotential[axis][infls[0]:infls[1]])            
            else:
                EffectiveTrapDepthInKelvin = IdealTrapDepthInKelvin
        except:
            EffectiveTrapDepthInKelvin = np.nan
        
        TrapDepthsInKelvin = [IdealTrapDepthInKelvin, EffectiveTrapDepthInKelvin]

        v_x = calculateCrossedBeamTrapFrequency(options['delta'], [w_x, w_z], Power, dir = 'x')
        v_y = calculateCrossedBeamTrapFrequency(options['delta'], [w_x, w_z], Power, dir = 'y')
        v_z = calculateCrossedBeamTrapFrequency(options['delta'], [w_x, w_z], Power, dir = 'z')
        CalculatedTrapFrequencies = [v_x, v_y, v_z]

        v, dv, popt, pcov = extractTrapFrequency(Positions, IdealTrappingPotential, axis)
        if np.isinf(v):
            v = np.nan
        if np.isinf(dv):
            dv = np.nan

        IdealTrapFrequency = [v, dv]
        
        if options['extract_trap_frequencies']:
            v, dv, popt, pcov = extractTrapFrequency(Positions, TrappingPotential, axis)
            if np.isinf(v):
                v = np.nan
            if np.isinf(dv):
                dv = np.nan
            TrapFrequency = [v, dv]
            ExtractedTrapFrequencies = [IdealTrapFrequency, TrapFrequency]
        else:
            ExtractedTrapFrequencies = [IdealTrapFrequency]

        return Positions, IdealTrappingPotential, TrappingPotential, TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies

def extractWaist(Positions, TrappingPotential):
    tmp_pos     = Positions.value
    tmp_pot     = TrappingPotential.value
    center_idx  = np.argmin(tmp_pot)
    
    TrapMinimum = tmp_pot[center_idx]
    TrapCenter  = tmp_pos[center_idx]

    lb = int(round(center_idx - len(tmp_pot)/30, 1))
    ub = int(round(center_idx + len(tmp_pot)/30, 1))
    xdata = tmp_pos[lb:ub]
    Potential = tmp_pot[lb:ub]

    p0=[TrapMinimum, 30, TrapCenter, 0]
    popt, pcov = curve_fit(gaussian_potential, xdata, Potential, p0)
    return popt, pcov

def computeIntensityProfileAndPotentials(Power, waists, wavelength, options, alpha = DY_POLARIZABILITY):
    w_x = waists[0]
    w_z = waists[1]
    extent = options['extent']
    modulation = options['modulation']
    mod_func = options['modulation_function']

    if not modulation:
        extent = 50
        x_Positions      = np.arange(-extent, extent, 1)*u.um 
        y_Positions      = np.arange(-extent, extent, 1)*u.um
        z_Positions      = np.arange(-extent, extent, 1)*u.um 
        
        idx = np.where(y_Positions==0)[0][0]

        wavelength = 1.064*u.um
        
        xm,ym,zm = np.meshgrid(x_Positions, y_Positions, z_Positions, sparse=True, indexing='ij')

        ## Single Gaussian Beam
        A = 2*Power/(np.pi*w(ym, w_x, wavelength)*w(ym, w_z, wavelength))
        intensity_profile = A * np.exp(-2 * ((xm/w(ym, w_x, wavelength))**2 + (zm/w(ym, w_z, wavelength))**2))
        I = intensity_profile[:, idx, :].to(u.MW/(u.cm*u.cm))
        
        U_tilde = (1 / (2 * ac.eps0 * ac.c)) * alpha * (4 * np.pi * ac.eps0 * ac.a0**3)
        U = - U_tilde * I
        U = (U/ac.k_B).to(u.uK)
        
        return [x_Positions, z_Positions], [w_x.value, 0, w_z.value, 0], I, U, [0, 0, 0, 0]

    else:
        mod_amp = options['modulation_amplitude']
        x_Positions      = np.arange(-extent, extent, 1)*u.um 
        y_Positions      = np.arange(-extent, extent, 1)*u.um
        z_Positions      = np.arange(-extent, extent, 1)*u.um 

        mod_amp   = mod_amp * w_x
        n_points  = len(x_Positions)
        dx, xmod_Positions = modulation_function(mod_amp, n_points, func = mod_func)
        
        idx = np.where(y_Positions==0)[0][0]

        xm,ym,zm,xmodm = np.meshgrid(x_Positions, y_Positions, z_Positions, xmod_Positions, sparse=True, indexing='ij')

        ## Single Modulated Gaussian Beam
        A = 2*Power/(np.pi*w(y_Positions[idx] , w_x, wavelength)*w(y_Positions[idx], w_z, wavelength))
        intensity_profile = A * 1/(2*mod_amp) * np.trapz(np.exp(-2 * (((xmodm - xm)/w(ym, w_x, wavelength))**2 + (zm/w(ym, w_z, wavelength))**2)), dx = dx, axis = -1)
        I = intensity_profile[:, idx, :].to(u.MW/(u.cm*u.cm))
        
        U_tilde = (1 / (2 * ac.eps0 * ac.c)) * alpha * (4 * np.pi * ac.eps0 * ac.a0**3)
        U = - U_tilde * I
        U = (U/ac.k_B).to(u.uK)

        poptx, pcovx = extractWaist(x_Positions, U[:, np.where(z_Positions==0)[0][0]])
        poptz, pcovz = extractWaist(z_Positions, U[np.where(x_Positions==0)[0][0], :])
        
        extracted_waist_x = poptx[1]
        dextracted_waist_x = pcovx[1][1]**0.5
        extracted_waist_z = poptz[1]
        dextracted_waist_z = pcovz[1][1]**0.5
        
        return [x_Positions, z_Positions], [extracted_waist_x, dextracted_waist_x, extracted_waist_z, dextracted_waist_z], I, U, [poptx, pcovx, poptz, pcovz]