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

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]])

# 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*lamb/(np.pi*w_0**2))**2)

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 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", P:"float|u.quantity.Quantity"=1, wavelength:"float|u.quantity.Quantity"=1.064*u.um, alpha:"float|u.quantity.Quantity"=184.4)->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 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 plotPotential(Positions, Powers, ComputedPotentials, axis, TrapDepthLabels):

    ## plot of the measured parameter vs. scan  parameter
    plt.figure(figsize=(9, 7))
    for i in range(np.size(ComputedPotentials, 0)):
        plt.plot(Positions[axis], ComputedPotentials[i][axis], label = 'P = ' + str(Powers[i]) + ' W; ' + TrapDepthLabels[i]) 
    if axis == 0:
        dir = 'X'
    elif axis == 1:
        dir = 'Y'
    else:
        dir = 'Z'
    # maxPotentialValue   = max(ComputedPotentials.flatten())
    # minPotentialValue   = min(ComputedPotentials.flatten())
    # PotentialValueRange = maxPotentialValue - minPotentialValue
    # upperlimit = 5
    # if maxPotentialValue > 0:
    #     upperlimit = maxPotentialValue
    # lowerlimit = min(ComputedPotentials.flatten()) - PotentialValueRange/6
    # plt.ylim([lowerlimit, upperlimit])
    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(prop={'size': 12, 'weight': 'bold'})
    plt.tight_layout()
    # plt.show()
    plt.savefig('pot_' + dir + '.png')

if __name__ == '__main__':

    # Powers = [0.1, 0.5, 2]
    # Powers = [5, 10, 20, 30, 40]
    Powers = [40]
    Polarizability = 160 # in a.u., should we use alpha = 136 or 160 or 184.4?
    # w_x, w_z = 34*u.um, 27.5*u.um # Beam Waists in the x and y directions
    w_x, w_z = 35*u.um, 35*u.um # Beam Waists in the x and y directions
    # w_x, w_z = 20.5*u.um, 20.5*u.um

    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

    TrappingPotential = []
    ComputedPotentials = []
    TrapDepthLabels = []

    gravity = False
    astigmatism = False

    tilt_gravity = True
    theta = 0 # in degrees
    tilt_axis = [1, 0, 0] # lab space coordinates are rotated about x-axis in reference frame of beam

    for p in Powers: 

        Power = p*u.W  # Single Beam Power
        
        TrapDepth = trap_depth(w_x, w_z, Power, alpha=Polarizability) 
        TrapDepthInKelvin = (TrapDepth/ac.k_B).to(u.uK)
        TrapDepthLabels.append("Trap Depth = " + str(round(TrapDepthInKelvin.value, 2)) + " " + str(TrapDepthInKelvin.unit))

        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]

        if not gravity and not astigmatism:
            TrappingPotential        = single_gaussian_beam_potential(Positions, np.asarray([w_x.value, w_z.value])*u.um, P = Power, alpha = Polarizability) 
            TrappingPotential        = TrappingPotential + np.zeros((3, len(TrappingPotential))) * TrappingPotential.unit
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK) 

        elif gravity and not astigmatism:
            # Influence of Gravity
            m = 164*u.u
            gravity_axis             = np.array([0, 0, 1]) 
            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) + gravitational_potential(gravity_axis_positions, m)
            TrappingPotential        = (TrappingPotential/ac.k_B).to(u.uK)
        
        ComputedPotentials.append(TrappingPotential)
    
    ComputedPotentials = np.asarray(ComputedPotentials)
    plotPotential(Positions, Powers, ComputedPotentials, axis, TrapDepthLabels)

# Influence of Astigmatism

# TrappingPotential = single_gaussian_beam_potential_harmonic_approximation(Positions, np.asarray([w_x.value, w_z.value])*u.um, depth = TrapDepth)
# TrappingPotential = (TrappingPotential/ac.k_B).to(u.uK)