def rect_to_cyl_vec(vx,vy,vz,X,Y,Z,cyl=False):
    """
    NAME:
       rect_to_cyl_vec
    PURPOSE:
       transform vectors from rectangular to cylindrical coordinates vectors
    INPUT:
       vx - 
       vy - 
       vz - 
       X - X
       Y - Y
       Z - Z
       cyl - if True, X,Y,Z are already cylindrical
    OUTPUT:
       vR,vT,vz
    HISTORY:
       2010-09-24 - Written - Bovy (NYU)
    """
    if not cyl:
        R,phi,Z= rect_to_cyl(X,Y,Z)
    else:
        phi= Y
    vr=+vx*sc.cos(phi)+vy*sc.sin(phi)
    vt= -vx*sc.sin(phi)+vy*sc.cos(phi)
    return (vr,vt,vz)

def radec_to_lb(ra,dec,degree=False,epoch=2000.0):
    """
    NAME:
       radec_to_lb
    PURPOSE:
       transform from equatorial coordinates to Galactic coordinates
    INPUT:
       ra - right ascension
       dec - declination
       degree - (Bool) if True, ra and dec are given in degree and l and b will be as well
       epoch - epoch of ra,dec (right now only 2000.0 and 1950.0 are supported)
    OUTPUT:
       l,b
       For vector inputs [:,2]
    HISTORY:
       2009-11-12 - Written - Bovy (NYU)
       2014-06-14 - Re-written w/ numpy functions for speed and w/ decorators for beauty - Bovy (IAS)
    """
    import math as m
    import numpy as nu
    import scipy as sc
    #First calculate the transformation matrix T
    theta,dec_ngp,ra_ngp= get_epoch_angles(epoch)
    T= sc.dot(sc.array([[sc.cos(theta),sc.sin(theta),0.],[sc.sin(theta),-sc.cos(theta),0.],[0.,0.,1.]]),sc.dot(sc.array([[-sc.sin(dec_ngp),0.,sc.cos(dec_ngp)],[0.,1.,0.],[sc.cos(dec_ngp),0.,sc.sin(dec_ngp)]]),sc.array([[sc.cos(ra_ngp),sc.sin(ra_ngp),0.],[-sc.sin(ra_ngp),sc.cos(ra_ngp),0.],[0.,0.,1.]])))
    #Whether to use degrees and scalar input is handled by decorators
    XYZ= nu.array([nu.cos(dec)*nu.cos(ra),
                   nu.cos(dec)*nu.sin(ra),
                   nu.sin(dec)])
    galXYZ= nu.dot(T,XYZ)
    b= nu.arcsin(galXYZ[2])
    l= nu.arctan2(galXYZ[1]/sc.cos(b),galXYZ[0]/sc.cos(b))
    l[l<0.]+= 2.*nu.pi
    out= nu.array([l,b])
    return out.T

    def getObservation(self):
        obs = array(impl.getObs())
        if self.verbose:
            print(('obs', obs))
        obs.resize(self.outdim)
        if self.extraObservations:
            cartpos = obs[-1]
            if self.markov:
                angle1 = obs[1]
            else:
                angle1 = obs[0]
            obs[-1 + self.extraRandoms] = 0.1 * cos(angle1) + cartpos
            obs[-2 + self.extraRandoms] = 0.1 * sin(angle1) + cartpos
            if self.numPoles == 2:
                if self.markov:
                    angle2 = obs[3]
                else:
                    angle2 = obs[1]
                obs[-3 + self.extraRandoms] = 0.05 * cos(angle2) + cartpos
                obs[-4 + self.extraRandoms] = 0.05 * sin(angle2) + cartpos

        if self.extraRandoms > 0:
            obs[-self.extraRandoms:] = randn(self.extraRandoms)

        if self.verbose:
            print(('obs', obs))
        return obs

def sparse_orth(d):
    """ Constructs a sparse orthogonal matrix.
    
    The method is described in:
    Gi-Sang Cheon et al., Constructions for the sparsest orthogonal matrices,
    Bull. Korean Math. Soc 36 (1999) No.1 pp.199-129
    """
    from scipy.sparse import eye
    from scipy import r_, pi, sin, cos

    if d % 2 == 0:
        seq = r_[0:d:2, 1:d - 1:2]
    else:
        seq = r_[0:d - 1:2, 1:d:2]
    Q = eye(d, d).tocsc()
    for i in seq:
        theta = random() * 2 * pi
        flip = (random() - 0.5) > 0;
        Qi = eye(d, d).tocsc()
        Qi[i, i] = cos(theta)
        Qi[(i + 1), i] = sin(theta)
        if flip > 0:
            Qi[i, (i + 1)] = -sin(theta)
            Qi[(i + 1), (i + 1)] = cos(theta)
        else:
            Qi[i, (i + 1)] = sin(theta)
            Qi[(i + 1), (i + 1)] = -cos(theta)
        Q = Q * Qi;
    return Q

    def residuals(self, p,errors, f,freq_val):
        theta = self.theta
#        Isrc = 19.6*pow((750.0/freq_val[f]),0.495)*2 
#        Isrc = 19.6*pow((750.0/freq_val[f]),0.495)*(2.28315426-0.000484307905*freq_val[f]) # Added linear fit for Jansky to Kelvin conversion.
#        Isrc = 19.74748409*pow((750.0/freq_val[f]),0.49899785)*(2.28315426-0.000484307905*freq_val[f]) # My fit solution for 3C286
        Isrc = 25.15445092*pow((750.0/freq_val[f]),0.75578842)*(2.28315426-0.000484307905*freq_val[f]) # My fit solution for  3C48
#        Isrc = 4.56303633*pow((750.0/freq_val[f]),0.59237327)*(2.28315426-0.000484307905*freq_val[f]) # My fit solution for 3C67
        PAsrc = 33.0*sp.pi/180.0 # for 3C286, doesn't matter for unpolarized. 
#        Psrc = 0.07 #for 3C286 
        Psrc = 0 #for #3C48,3C67 
        Qsrc = Isrc*Psrc*sp.cos(2*PAsrc) 
        Usrc = Isrc*Psrc*sp.sin(2*PAsrc) 
        Vsrc = 0
        XXsrc0 = Isrc-Qsrc
        YYsrc0 = Isrc+Qsrc
#        XXsrc = (0.5*(1+sp.cos(2*theta[i]))*XXsrc0-sp.sin(2*theta[i])*Usrc+0.5*(1-sp.cos(2*theta[i]))*YYsrc0)
#        YYsrc = (0.5*(1-sp.cos(2*theta[i]))*XXsrc0+sp.sin(2*theta[i])*Usrc+0.5*(1+sp.cos(2*theta[i]))*YYsrc0)
        source = sp.zeros(4*self.file_num)
        for i in range(0,len(source),4):
            source[i] = (0.5*(1+sp.cos(2*theta[i]))*XXsrc0-sp.sin(2*theta[i])*Usrc+0.5*(1-sp.cos(2*theta[i]))*YYsrc0)
            source[i+1] = 0
            source[i+2] = 0
            source[i+3] = (0.5*(1-sp.cos(2*theta[i]))*XXsrc0+sp.sin(2*theta[i])*Usrc+0.5*(1+sp.cos(2*theta[i]))*YYsrc0)
        err = (source-self.peval(p,f))/errors
        return err

def distance_fn(p1, l1, p2, l2, units='m'):
    """
    Simplified Vincenty formula.
    Returns distance between coordinates.
    """

    assert (units in ['km', 'm', 'nm']), 'Units must be km, m, or nm'

    if units == 'km':
        
        r = 6372.7974775959065
        
    elif units == 'm':
        
        r = 6372.7974775959065 * 0.621371
        
    elif units == 'nm':
        
        r = 6372.7974775959065 * 0.539957

#    compute Vincenty formula

    l = abs(l1 - l2)
    num = scipy.sqrt(((scipy.cos(p2) * scipy.sin(l)) ** 2) +\
        (((scipy.cos(p1) * scipy.sin(p2)) - (scipy.sin(p1) * scipy.cos(p2) * scipy.cos(l))) ** 2))
    den = scipy.sin(p1) * scipy.sin(p2) + scipy.cos(p1) * scipy.cos(p2) * scipy.cos(l)
    theta = scipy.arctan(num / den)
    distance = abs(int(round(r * theta)))

    return distance

def CalcXY2GPSParam_2p(x1,x2,g1,g2,K=[0,0]):
	# Kx = dLng/dx; Ky = dlat/dy;
	# In China:
	# Kx = (133.4-1.2*lat)*1e3
	# Ky = (110.2+0.002*lat)*1e3

	X1 = array(x1)
	Y1 = array(g1)
	X2 = array(x2)
	Y2 = array(g2)
	detX = X2-X1
	detY = Y2-Y1
	lat = Y1[1]
	if K[0] == 0:
		Kx = (133.4-1.2*lat)*1e3
		Ky = (110.2+0.002*lat)*1e3
		K = array([Kx,Ky])
	else:
		Kx = K[0]
		Ky = K[1]
	detKY = detY*K

	alpha =  myArctan(detX[0],detX[1]) - myArctan(detKY[0],detKY[1])
	A = array([[sp.cos(alpha),sp.sin(alpha)],[-sp.sin(alpha),sp.cos(alpha)]])
	X01 = X1 - dot(linalg.inv(A),Y1*K) 
	X02 = X2 - dot(linalg.inv(A),Y2*K)
	X0 = (X01+X02) /2

	return A,X0,K

    def testSnrFuncs(self):
        """test for signal to noise ratio functions"""

        # trivial
        data_triv = sp.ones((3, 10))
        snr_triv_test = sp.ones(3)
        assert_equal(
            snr_peak(data_triv, 1.0),
            snr_triv_test)
        assert_equal(
            snr_power(data_triv, 1.0),
            snr_triv_test)
        assert_equal(
            snr_maha(data_triv, sp.eye(data_triv.shape[1])),
            snr_triv_test)

        # application
        data = sp.array([
            sp.sin(sp.linspace(0.0, 2 * sp.pi, 100)),
            sp.sin(sp.linspace(0.0, 2 * sp.pi, 100)) * 2,
            sp.sin(sp.linspace(0.0, 2 * sp.pi, 100)) * 5,
        ])
        assert_equal(
            snr_peak(data, 1.0),
            sp.absolute(data).max(axis=1))
        assert_equal(
            snr_power(data, 1.0),
            sp.sqrt((data * data).sum(axis=1) / data.shape[1]))
        assert_almost_equal(
            snr_maha(data, sp.eye(data.shape[1])),
            sp.sqrt((data * data).sum(axis=1) / data.shape[1]))

    def form_point_set(self, histo, point_set):
        (slices, numbins) = histo.shape
        phases = numpy.arange(numbins)
        phases = phases * (360. / numbins)
        phases += phases[1] / 2.
        phi_step = phases[0]
        
        for time in xrange(slices):
            z = float(time)
            for bin in xrange(numbins):
                r = histo[time,bin]
                theta = phi_step * (bin+1)
                theta *= (scipy.pi / 180.)
                x = r*scipy.cos(theta)
                y = r*scipy.sin(theta)
                point_set.InsertNextPoint(x, y, z)

            for bin in xrange(numbins):
                curbin = bin
                lastbin = bin-1
                if lastbin < 0:
                    lastbin = numbins-1

                r = (histo[time,bin] -  histo[time,lastbin]) / 2.
                theta = curbin * 360. / numbins
                x = r*scipy.cos(theta)
                y = r*scipy.sin(theta)
                point_set.InsertNextPoint(x, y, z)

 def __init__(self,alphai,eparall,eperp,nrj):
   """
       Incident wave above a surface.
       Coordinates:
         - z is perpendicular to the surface, >0 going UP (different from H Dosch's convention)
         - x is the projection of the wavevector on the surface
         - y is parallel to the surface
       
       alphai: incident angle, with respect to the surface
       eparallel: component of the electric field parallel to the incident plane (vertical plane)
       eperp: component of the electric field perpendicular to the incident plane (along y)
       nrj: values of the energy of the incident wave, in eV
       
       alphai *or* nrj can be arrays, but not together
   """
   self.alphai=alphai
   self.eparall=eparall
   self.eperp=eperp
   self.ex=scipy.sin(alphai)*eparall
   self.ey=eperp
   self.ez=scipy.cos(alphai)*eparall
   self.kx= 2*pi/W2E(nrj)*scipy.cos(alphai)
   self.ky= 2*pi/W2E(nrj)*0
   self.kz=-2*pi/W2E(nrj)*scipy.sin(alphai)
   self.nrj=nrj

 def rotate(self, angle, mask=None):
     """Rotate the grids (arena centered)
     
     Grids to be rotated can be optionally specified by bool/index array
     *mask*, otherwise population is rotated. Specified *angle* can be a
     scalar value to be applied to the population or a population- or
     mask-sized array depending on whether *mask* is specified.
     """
     rot2D = lambda psi: [[cos(psi), sin(psi)], [-sin(psi),  cos(psi)]]
     if mask is not None and type(mask) is np.ndarray:
         if mask.dtype.kind == 'b':
             mask = mask.nonzero()[0]
         if type(angle) is np.ndarray and angle.size == mask.size:
             for i,ix in enumerate(mask):
                 self._phi[ix] = np.dot(self._phi[ix], rot2D(angle[i]))
         elif type(angle) in (int, float, np.float64):
             angle = float(angle)
             self._phi[mask] = np.dot(self._phi[mask], rot2D(angle))
         else:
             raise TypeError, 'angle must be mask-sized array or float'
         self._psi[mask] = np.fmod(self._psi[mask]+angle, 2*pi)
     elif mask is None:
         if type(angle) is np.ndarray and angle.size == self.num_maps:
             for i in xrange(self.num_maps):
                 self._phi[i] = np.dot(self._phi[i], rot2D(angle[i]))
         elif type(angle) in (int, float, np.float64):
             angle = float(angle)
             self._phi = np.dot(self._phi, rot2D(angle))
         else:
             raise TypeError, 'angle must be num_maps array or float'
         self._psi = np.fmod(self._psi+angle, 2*pi)
     else:
         raise TypeError, 'mask must be bool/index array'

def ned2ecef(lat, lon, alt, n, e, d):
    X0, Y0, Z0 = coord.geodetic2ecef(lat, lon, alt)
    lat, lon = radians(lat), radians(lon)
    
    pitch = math.pi/2 + lat
    yaw = -lon 
    
    my = mat('[%f %f %f; %f %f %f; %f %f %f]' %
        (cos(pitch), 0, -sin(pitch),
         0,1,0,
         sin(pitch), 0, cos(pitch)))
    
    mz = mat('[%f %f %f; %f %f %f; %f %f %f]' %
        (cos(yaw), sin(yaw),0,
         -sin(yaw),cos(yaw),0,
         0,0,1))
    
    mr = mat('[%f %f %f; %f %f %f; %f %f %f]' %
        (-cos(lon)*sin(lat), -sin(lon), -cos(lat) * cos(lon), 
         -sin(lat)*sin(lon), cos(lon), -sin(lon)*cos(lat),
         cos(lat), 0, -sin(lat)))
    
    geo = mat('[%f; %f; %f]' % (X0, Y0, Z0))
    ned = mat('[%f; %f; %f]' % (n, e, d))
    res = mr*ned + geo
    return res[0], res[1], res[2]  

def Spm2(k,dx):
    from scipy import sqrt,sin,exp
    im = sqrt(-1)
    Sp = exp(im*k*dx)*(1 - 0.5*(im*sin(im*k*dx)))
    Sm = (1 + 0.5*(im*sin(im*k*dx)))
    
    return Sp, Sm

def lla2ecef(lla: Sequence[float], cst: ConstantsFile, lla_as_degrees: bool=False) -> Tuple[float, float, float]:
    """
    converts LLA (Latitude, Longitude, Altitude) coordinates
    to ECEF (Earth-Centre, Earth-First) XYZ coordinates.
    """

    lat, lon, alt = lla
    if lla_as_degrees:
        lat = radians(lat)
        lon = radians(lon)

    a = cst.semi_major_axis
    b = cst.semi_minor_axis
    # calc. ellipsoid flatness
    f = (a - b) / a
    # calc. eccentricity
    e = sqrt(f * (2 - f))

    # Calculate length of the normal to the ellipsoid
    N = a / sqrt(1 - (e * sin(lat)) ** 2)
    # Calculate ecef coordinates
    x = (N + alt) * cos(lat) * cos(lon)
    y = (N + alt) * cos(lat) * sin(lon)
    z = (N * (1 - e ** 2) + alt) * sin(lat)
    # Return the ecef coordinates
    return x, y, z

    def f(self, x):
        B1 = 0.5 * sin(x[0]) - 2*cos(x[0]) + sin(x[1]) -1.5*cos(x[1])
        B2 = 1.5 * sin(x[0]) - cos(x[0]) + 2*sin(x[1]) -0.5*cos(x[1])

        f1 = 1 + (self._A1-B1)**2 + (self._A2-B2)**2
        f2 = (x[0]+3)**2 + (x[1]+1)**2
        return -array([f1, f2])

def binary_ephem(P, T, e, a, i, O_node, o_peri, t):
   # Grados a radianes
   d2rad = pi/180.
   rad2d = 180./pi
   i = i*d2rad
   O_node = (O_node*d2rad)%(2*pi)
   o_peri = (o_peri*d2rad)%(2*pi)
 
   # Anomalia media
   M = ((2.0*pi)/P)*(t - T)  # radianes
    
   if M >2*pi: M = M - 2*pi 
   M=M%(2*pi)

   # Anomalia excentrica (1ra aproximacion)
   E0 = M  + e*sin(M) + (e**2/M) * sin(2.0*M)

   for itera in range(15):
      M0 = E0 - e*sin(E0)
      E0 = E0 + (M-M0)/(1-e*cos(E0))

   true_anom = 2.0*arctan(sqrt((1+e)/(1-e))*tan(E0/2.0))
 
   #radius = (a*(1-e**2))/(1+e*cos(true_anom))
   radius = a*(1-e*cos(E0))

   theta = arctan( tan(true_anom + o_peri)*cos(i) ) + O_node
   rho = radius * (cos(true_anom + o_peri)/cos(theta - O_node))
   
   # revuelve rho ("), theta (grad), Anomalia excentrica (grad), Anomalia verdadera (grad)
   return rho, (theta*rad2d)%360. #, E0*rad2d, M*rad2d, true_anom*rad2d

    def get_bl(self,ra=None,dec=None):
        """
        http://scienceworld.wolfram.com/astronomy/GalacticCoordinates.html
        """
        if ra==None: ra=self.get_column("RA")
        if dec==None: dec=self.get_column("DEC")
        if type(ra)==float: 
            ral=scipy.zeros(1)
            ral[0]=ra
            ra=ral
        if type(dec)==float: 
            decl=scipy.zeros(1)
            decl[0]=dec
            dec=decl

        c62=math.cos(62.6*D2R)
        s62=math.sin(62.6*D2R)
        
        b=scipy.sin(dec*D2R)*c62
        b-=scipy.cos(dec*D2R)*scipy.sin((ra-282.25)*D2R)*s62
        b=scipy.arcsin(b)*R2D
        
        cosb=scipy.cos(b*D2R)
        #l minus 33 degrees
        lm33=(scipy.cos(dec*D2R)/cosb)*scipy.cos((ra-282.25))
        l=scipy.arccos(lm33)*R2D+33.0
        return b,l

def evaluateSphericalVariation (phi,theta,cntPhi,cntTheta):
  global conf,cons
  success = False

  alpha0     =sp.zeros([dim['alpha']],complex)
  alpha0[conf['id0']]=sp.cos(phi)*sp.sin(theta)+0j
  alpha0[conf['id1']]=sp.sin(phi)*sp.sin(theta)+0j
  alpha0[conf['id2']]=sp.cos(theta)+0j

#  if (sp.absolute(alpha0[conf['id0']]) <= 1e-10): 
#    alpha0[conf['id0']]=0.0+0j
#  if (sp.absolute(alpha0[conf['id1']]) <= 1e-10): 
#    alpha0[conf['id1']]=0.0+0j
#  if (sp.absolute(alpha0[conf['id2']]) <= 1e-10): 
#    alpha0[conf['id2']]=0.0+0j
#  
  # normalize coefficients for alpha -> defines net-power
  alpha0[:]=alpha0[:]/sp.linalg.norm(alpha0[:])*cons['alpha_norm']
  __,res   = MemoryPulseFunctional.evaluateFunctional(alpha0,1.0+0j)
 
  myRes    = sp.zeros([conf['entries']+3])
  myRes[0] = alpha0[conf['id0']].real
  myRes[1] = alpha0[conf['id1']].real
  myRes[2] = alpha0[conf['id2']].real
  myRes[3:]= res
 
  print "### spherical map: phi/pi={0:5.3f}, theta/pi={1:5.3f}, fun={2:f}".format(phi/sp.pi,theta/sp.pi,myRes[conf['funval']])

  myRes[conf['funval']] = min(conf['cutoff'],res[conf['funval']])

  return myRes,cntPhi,cntTheta

def f_Refl_Thin_Film_fit(Data):
    strain = Data[0]
    DW = Data[1]

    dat = Data[2]
    wl = dat[0]
    N = dat[2]

    phi = dat[3]
    t_l = dat[4]
    thB_S = dat[6]
    G = dat[7]
    F0 = dat[8]
    FH = dat[9]
    FmH = dat[10]
    th = dat[19]

    thB = thB_S - strain * tan(thB_S)  # angle de Bragg dans chaque lamelle

    eta = 0
    res = 0

    n = 1
    while (n <= N):
        g0 = sin(thB[n] - phi)  # gamma 0
        gH = -sin(thB[n] + phi)  # gamma H
        b = g0 / gH
        T = pi * G * ((FH[n]*FmH[n])**0.5) * t_l * DW[n] / (wl * (abs(g0*gH)**0.5))
        eta = (-b*(th-thB[n])*sin(2*thB_S) - 0.5*G*F0[n]*(1-b)) / ((abs(b)**0.5) * G * DW[n] * (FH[n]*FmH[n])**0.5)
        S1 = (res - eta + (eta*eta-1)**0.5)*exp(-1j*T*(eta*eta-1)**0.5)
        S2 = (res - eta - (eta*eta-1)**0.5)*exp(1j*T*(eta*eta-1)**0.5)
        res = (eta + ((eta*eta-1)**0.5) * ((S1+S2)/(S1-S2)))
        n += 1
    return res

 def __init__(self,id,matName,orientation,source=0.0):
     self.id = id
     self.matName = matName
     self.orientation = orientation
     self.source = source
     self.T = array([[cos(orientation),-sin(orientation)],
                     [sin(orientation), cos(orientation)]])