import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
from astropy import units as u, constants as ac

#####################################################################
# HELPER FUNCTIONS                                                  #
#####################################################################

def orderOfMagnitude(number):
    return math.floor(math.log(number, 10))

def rotation_matrix(axis, theta):
    """
    Return the rotation matrix associated with counterclockwise rotation about
    the given axis by theta radians.
    In 2-D it is just,
        thetaInRadians = np.radians(theta)
        c, s = np.cos(thetaInRadians), np.sin(thetaInRadians)
        R = np.array(((c, -s), (s, c)))
    In 3-D, one way to do it is use the Euler-Rodrigues Formula as is done here   
    """
    axis = np.asarray(axis)
    axis = axis / math.sqrt(np.dot(axis, axis))
    a = math.cos(theta / 2.0)
    b, c, d = -axis * math.sin(theta / 2.0)
    aa, bb, cc, dd = a * a, b * b, c * c, d * d
    bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
    return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
                     [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
                     [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])

#####################################################################
# BEAM PARAMETERS                                                   #
#####################################################################

# Rayleigh range
def z_R(w_0:np.ndarray, lamb:float)->np.ndarray:
    return np.pi*w_0**2/lamb

# Beam Radius
def w(pos, w_0, lamb):
    return w_0*np.sqrt(1+(pos / z_R(w_0, lamb))**2)

#####################################################################
# RELEVANT PARAMETERS FOR EVAPORATIVE COOLING                       #
#####################################################################

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

def particleDensity(w_x, w_z, Power, Polarizability, N, T, m = 164*u.u): # For a thermal cloud
    v_x = calculateTrapFrequency(w_x, w_z, Power, Polarizability, dir = 'x')
    v_y = calculateTrapFrequency(w_x, w_z, Power, Polarizability, dir = 'y')
    v_z = calculateTrapFrequency(w_x, w_z, Power, Polarizability, 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 thermaldeBroglieWavelength(T, m = 164*u.u):
    return np.sqrt((2*np.pi*ac.hbar**2)/(m*ac.k_B*T))

def scatteringLength(B):
    a_bkg = 87 * ac.a0
    #resonanceWidth = 0.005 * u.G
    #resonanceB = 0.5 * u.G
    
    #return a_bkg * (1 - resonanceWidth/(B - resonanceB))
    return a_bkg

def dipolarLength(mu = 9.93 * ac.muB, m = 164*u.u):
    return (m * ac.mu0 * mu**2) / (8 * np.pi * ac.hbar**2)

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

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

def calculatePSD(w_x, w_z, Power, Polarizability, N, T):
    return (particleDensity(w_x, w_z, Power, Polarizability, N, T, m = 164*u.u) * thermaldeBroglieWavelength(T)**3).decompose()

#####################################################################
# POTENTIALS                                                        #
#####################################################################

def gravitational_potential(positions: "np.ndarray|u.quantity.Quantity", m:"float|u.quantity.Quantity"):
    return m * ac.g0 * positions

def single_gaussian_beam_potential(positions: "np.ndarray|u.quantity.Quantity", waists: "np.ndarray|u.quantity.Quantity", alpha:"float|u.quantity.Quantity", P:"float|u.quantity.Quantity"=1, wavelength:"float|u.quantity.Quantity"=1.064*u.um)->np.ndarray:
    A = 2*P/(np.pi*w(positions[1,:], waists[0], wavelength)*w(positions[1,:], waists[1], wavelength))
    U_tilde = (1 / (2 * ac.eps0 * ac.c)) * alpha * (4 * np.pi * ac.eps0 * ac.a0**3)
    U = - U_tilde * A * np.exp(-2 * ((positions[0,:]/w(positions[1,:], waists[0], wavelength))**2 + (positions[2,:]/w(positions[1,:], waists[1], wavelength))**2)) 
    return U

def astigmatic_single_gaussian_beam_potential(positions: "np.ndarray|u.quantity.Quantity", waists: "np.ndarray|u.quantity.Quantity", del_y:"float|u.quantity.Quantity", alpha:"float|u.quantity.Quantity", P:"float|u.quantity.Quantity"=1, wavelength:"float|u.quantity.Quantity"=1.064*u.um)->np.ndarray:
    A = 2*P/(np.pi*w(positions[1,:] - (del_y/2), waists[0], wavelength)*w(positions[1,:] + (del_y/2), waists[1], wavelength))
    U_tilde = (1 / (2 * ac.eps0 * ac.c)) * alpha * (4 * np.pi * ac.eps0 * ac.a0**3)
    U = - U_tilde * A * np.exp(-2 * ((positions[0,:]/w(positions[1,:] - (del_y/2), waists[0], wavelength))**2 + (positions[2,:]/w(positions[1,:] + (del_y/2), waists[1], wavelength))**2)) 
    return U

def single_gaussian_beam_potential_harmonic_approximation(positions: "np.ndarray|u.quantity.Quantity", waists: "np.ndarray|u.quantity.Quantity", depth:"float|u.quantity.Quantity"=1, wavelength:"float|u.quantity.Quantity"=1.064*u.um)->np.ndarray:
    U = - depth * (1 - (2 * (positions[0,:]/waists[0])**2) - (2 * (positions[2,:]/waists[1])**2) - (0.5 * positions[1,:]**2 * np.sum(np.reciprocal(z_R(waists, wavelength)))**2))
    return U
    
def harmonic_potential(pos, v, xoffset, yoffset, m = 164*u.u):
    U_Harmonic = ((0.5 * m * (2 * np.pi * v*u.Hz)**2 * (pos*u.um - xoffset*u.um)**2)/ac.k_B).to(u.uK) + yoffset*u.uK
    return U_Harmonic.value

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

def trap_depth(w_1:"float|u.quantity.Quantity", w_2:"float|u.quantity.Quantity", P:"float|u.quantity.Quantity", alpha:float)->"float|u.quantity.Quantity":
    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, Polarizability, m = 164*u.u, dir = 'x'):
    TrapDepth = trap_depth(w_x, w_z, Power, alpha=Polarizability)
    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 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)/150, 1))
    ub = int(round(center_idx + len(tmp_pot)/150, 1))
    xdata = tmp_pos[lb:ub]
    Potential = tmp_pot[lb:ub]
    p0=[1e3, tmp_pos[center_idx].value, np.argmin(tmp_pot.value)]
    popt, pcov = curve_fit(harmonic_potential, xdata, Potential, p0)
    v = popt[0]
    dv = pcov[0][0]**0.5 
    return v, dv, popt, pcov

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

    axis = options['axis']
    extent = options['extent']
    gravity = options['gravity']
    astigmatism = options['astigmatism']
        
    TrappingPotential = []
    TrapDepth = trap_depth(w_x, w_z, Power, alpha=Polarizability) 
    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)
    
    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 
    Positions        = np.vstack((x_Positions, y_Positions, z_Positions)) * projection_axis[:, np.newaxis]

    IdealTrappingPotential        = single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power, alpha = Polarizability) 
    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 = 164*u.u
        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, alpha = Polarizability)
        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
        disp_foci = options['disp_foci']
        TrappingPotential        = astigmatic_single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power, del_y = disp_foci, alpha = Polarizability)
        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 = 164*u.u
        gravity_axis             = np.array([0, 0, 1]) 
        tilt_gravity             = options['tilt_gravity']
        theta                    = options['theta']
        tilt_axis                = options['tilt_axis']
        disp_foci                = options['disp_foci']
        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 = disp_foci, alpha = Polarizability)
        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

    if TrappingPotential[axis][0] > TrappingPotential[axis][-1]:
        EffectiveTrapDepthInKelvin = TrappingPotential[axis][-1] - min(TrappingPotential[axis])
    elif TrappingPotential[axis][0] < TrappingPotential[axis][-1]:
        EffectiveTrapDepthInKelvin = TrappingPotential[axis][0] - min(TrappingPotential[axis])

    TrapDepthsInKelvin = [IdealTrapDepthInKelvin, EffectiveTrapDepthInKelvin]

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

    v, dv, popt, pcov = extractTrapFrequency(Positions, IdealTrappingPotential, axis)
    IdealTrapFrequency = [v, dv]
    v, dv, popt, pcov = extractTrapFrequency(Positions, TrappingPotential, axis)
    TrapFrequency = [v, dv]
    ExtractedTrapFrequencies = [IdealTrapFrequency, TrapFrequency]

    # Gamma_elastic = calculateElasticCollisionRate(w_x, w_z, Power, Polarizability, N = 1.13 * 1e7, T = 22 * u.uK, B = 0 * u.G)
    # PSD = calculatePSD(w_x, w_z, Power, Polarizability, N = 1.13 * 1e7, T = 22 * u.uK).decompose()

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

#####################################################################
# PLOT TRAP POTENTIALS                                              #
#####################################################################

def plotHarmonicFit(Positions, TrappingPotential, TrapDepthsInKelvin, axis, popt, pcov):
    v = popt[0]
    dv = pcov[0][0]**0.5
    happrox = harmonic_potential(Positions[axis, :].value, *popt)
    plt.figure()
    plt.plot(Positions[axis, :].value, happrox, '-r', label = '\u03BD = %.1f \u00B1 %.2f Hz'% tuple([v,dv]))
    plt.plot(Positions[axis, :], TrappingPotential[axis], 'ob', label = 'Gaussian Potential')
    plt.xlabel('Distance (um)', fontsize= 12, fontweight='bold')
    plt.ylabel('Trap Potential (uK)', fontsize= 12, fontweight='bold')
    plt.ylim([-TrapDepthsInKelvin[0].value, max(TrappingPotential[axis].value)])
    plt.tight_layout()
    plt.grid(visible=1)
    plt.legend(prop={'size': 12, 'weight': 'bold'})
    plt.show()

def generate_label(v, dv):
    unit = 'Hz'
    if v <= 0.0:
        v        = np.nan
        dv       = np.nan
        unit = 'Hz'
    elif v > 0.0 and orderOfMagnitude(v) > 2:
        v    = v / 1e3 # in kHz
        dv   = dv / 1e3 # in kHz
        unit = 'kHz'
    tf_label = '\u03BD = %.1f \u00B1 %.2f %s'% tuple([v,dv,unit])
    return tf_label

def plotPotential(Positions, ComputedPotentials, axis, Params = [], listToIterateOver = [], save = False):

    plt.figure(figsize=(9, 7))
    for i in range(np.size(ComputedPotentials, 0)):
        
        if i % 2 == 0:
            j = int(i / 2)
        else:
            j = int((i - 1) / 2)
        
        IdealTrapDepthInKelvin = Params[j][0][0]
        EffectiveTrapDepthInKelvin = Params[j][0][1]
        
        idealv = Params[j][2][0][0]
        idealdv = Params[j][2][0][1]
        
        v = Params[j][2][1][0]
        dv = Params[j][2][1][1]
        
        if listToIterateOver:
            if np.size(ComputedPotentials, 0) == len(listToIterateOver):
                plt.plot(Positions[axis], ComputedPotentials[i][axis], label = 'Trap Depth = ' + str(round(EffectiveTrapDepthInKelvin.value, 2)) + ' ' + str(EffectiveTrapDepthInKelvin.unit) + '; ' + generate_label(v, dv)) 
            else:
                if i % 2 == 0:
                    plt.plot(Positions[axis], ComputedPotentials[i][axis], '--', label = 'Trap Depth = ' + str(round(IdealTrapDepthInKelvin.value, 2)) + ' ' + str(IdealTrapDepthInKelvin.unit) + '; ' + generate_label(idealv, idealdv)) 
                elif i % 2 != 0:
                    plt.plot(Positions[axis], ComputedPotentials[i][axis], label = 'Effective Trap Depth = ' + str(round(EffectiveTrapDepthInKelvin.value, 2)) + ' ' + str(EffectiveTrapDepthInKelvin.unit) + '; ' + generate_label(v, dv)) 
        else:
            if i % 2 == 0:
                plt.plot(Positions[axis], ComputedPotentials[i][axis], '--', label = 'Trap Depth = ' + str(round(IdealTrapDepthInKelvin.value, 2)) + ' ' + str(IdealTrapDepthInKelvin.unit) + '; ' + generate_label(idealv, idealdv))
            elif i % 2 != 0:
                plt.plot(Positions[axis], ComputedPotentials[i][axis], label = 'Effective Trap Depth = ' + str(round(EffectiveTrapDepthInKelvin.value, 2)) + ' ' + str(EffectiveTrapDepthInKelvin.unit) + '; ' + generate_label(v, dv))
    if axis == 0:
        dir = 'X'
    elif axis == 1:
        dir = 'Y'
    else:
        dir = 'Z'
    plt.xlabel(dir + ' Direction (um)', fontsize= 12, fontweight='bold')
    plt.ylabel('Trap Potential (uK)', fontsize= 12, fontweight='bold')
    plt.tight_layout()
    plt.grid(visible=1)
    plt.legend(loc=3, prop={'size': 12, 'weight': 'bold'})
    if save:
        plt.savefig('pot_' + dir + '.png')
    plt.show()

#####################################################################
# FUNCTION CALLS BELOW                                              #
#####################################################################

if __name__ == '__main__':            

    Power = 40*u.W
    Polarizability = 184.4 # in a.u, most precise measured value of Dy polarizability
    w_x, w_z = 27.5*u.um, 33.8*u.um # Beam Waists in the x and y directions
    
    options = {
        'axis': 1, # axis referenced to the beam along which you want the dipole trap potential
        'extent': 1e4, # range of spatial coordinates in one direction to calculate trap potential over
        'gravity': True,
        'astigmatism': False,
        'tilt_gravity': True,
        'theta': 5, # in degrees
        'tilt_axis': [1, 0, 0], # lab space coordinates are rotated about x-axis in reference frame of beam
        'disp_foci': 3 * z_R(w_0 = np.asarray([30]), lamb = 1.064)[0]*u.um # difference in position of the foci along the propagation direction (Astigmatism)
    }
    
    ComputedPotentials = [] 
    Params = []  

    Positions, IdealTrappingPotential, TrappingPotential, TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies = computeTrapPotential(w_x, w_z, Power, Polarizability, options)
    ComputedPotentials.append(IdealTrappingPotential)
    ComputedPotentials.append(TrappingPotential)
    Params.append([TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies])

    ComputedPotentials = np.asarray(ComputedPotentials)
    plotPotential(Positions, ComputedPotentials, options['axis'], Params)

    # v, dv, popt, pcov = extractTrapFrequency(Positions, TrappingPotential, options['axis'])
    # plotHarmonicFit(Positions, TrappingPotential, TrapDepthsInKelvin, options['axis'], popt, pcov)

    # Power = [10, 20, 25, 30, 35, 40]*u.W # Single Beam Power
    # for p in Power: 
    #     Positions, IdealTrappingPotential, TrappingPotential, TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies = computeTrapPotential(w_x, w_z, p, Polarizability, options)
    #     ComputedPotentials.append(IdealTrappingPotential)
    #     ComputedPotentials.append(TrappingPotential)
    #     Params.append([TrapDepthsInKelvin, CalculatedTrapFrequencies, ExtractedTrapFrequencies])
        
    # ComputedPotentials = np.asarray(ComputedPotentials)
    # plotPotential(Positions, ComputedPotentials, options['axis'], Params)