def phase(filtered, carf, sampf,tarr,bitclock,baudrate):
    uphasearr = []   # Establishing arrays to hold the entire unfiltered phase
    lphasearr = []   # in both the upper and lower sideband frequencies    
    deltaf = 50  # This is determined from baudrate and modulation scheme (MSK)

    window = 125  # This is the window the phase is calculated and averaged over for a single bit (1/6 of a full bit). This bit phase is in turn averaged over the whole second later on
    phasebitsize = len(filtered[0])/window/baudrate   # data points in a bit in phase time series (6)
    rawbitsize = len(filtered[0])/baudrate    # data points in a bit in raw signal time series (750)
    
    bins = len(filtered[0])/window - phasebitsize    # Lose a full bits worth of data points(6) to start in sync with bitclock
    time = np.array(tarr)   # Just to not f up the 'tarr' array created a 'time' array

    for k in range(0,len(filtered)):
        modu = carf[k] + deltaf   # The sideband frequencies used in the 
        modl = carf[k] - deltaf   # MSK modulation scheme
        
        startbin = (np.abs(time - bitclock[k])).argmin()    # Start measuring the phase at start of measured bitclock
        endbin = startbin - rawbitsize   # Endbin will be negative to make sure it is even splitting the time series into chunks 1/6 of a bit in length

        uy = filtered[k]*sin((2.0)*(pi)*modu*time)   # Crunching the phase in segments 
        ux = filtered[k]*cos((2.0)*(pi)*modu*time)   # 1/6 of a bit in length                
        uysum = np.split(uy[startbin:endbin],bins)  # Summed over this whole segment for 
        uxsum = np.split(ux[startbin:endbin],bins)  # phase measurement
        uphase = -arctan((sum(uysum, axis = 1))/(sum(uxsum, axis = 1)))   # a phase for upper and lower sidebands in MSK modulation
                                                
        ly = filtered[k]*sin((2.0)*(pi)*modl*time)  # Crunching the phase in segments 
        lx = filtered[k]*cos((2.0)*(pi)*modl*time)   # 1/6 of a bit in length      
        lysum = np.split(ly[startbin:endbin],bins)  # Summed over this whole segment for
        lxsum = np.split(lx[startbin:endbin],bins)  # phase measurement         
        lphase = -arctan((sum(lysum, axis = 1))/(sum(lxsum, axis = 1)))  # this is the lower sidebands phase
        
        lphasearr.extend([lphase])  # Adding the arrays of uppper phase
        uphasearr.extend([uphase])  # and lower phase for each frequency
    
    return uphasearr, lphasearr  # Each element in array has 1194 datapoints

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 ecef2geodetic(x, y, z):
    """Convert ECEF coordinates to geodetic.
    J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
    to geodetic coordinates," IEEE Transactions on Aerospace and \
    Electronic Systems, vol. 30, pp. 957-961, 1994."""
    a = 6378.137
    b = 6356.7523142
    esq = 6.69437999014 * 0.001
    e1sq = 6.73949674228 * 0.001

    # return h in kilo
    r = sqrt(x * x + y * y)
    Esq = a * a - b * b
    F = 54 * b * b * z * z
    G = r * r + (1 - esq) * z * z - esq * Esq
    C = (esq * esq * F * r * r) / (pow(G, 3))
    S = sqrt(1 + C + sqrt(C * C + 2 * C))
    P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
    Q = sqrt(1 + 2 * esq * esq * P)
    r_0 =  -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
        P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
    U = sqrt(pow((r - esq * r_0), 2) + z * z)
    V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
    Z_0 = b * b * z / (a * V)
    h = U * (1 - b * b / (a * V))
    lat = arctan((z + e1sq * Z_0) / r)
    lon = arctan2(y, x)
    return degrees(lat), degrees(lon), h

    def velocity(self, mass, time=0., anomaly_offset=1e-3):
        """Returns the radial velocities and proper motions in km/s.

        Returns an (N, 2) array with the radial velocities and the proper motions due to the binary orbital motions of the N binaries.

        Arguments:
        - `mass`: primary mass of the star in solar masses.
        - `time`: 
        """
        nbinaries = self.size
        mean_anomaly = (self.phase + time / self.period) * 2. * sp.pi
        ecc_anomaly = mean_anomaly
        old = sp.zeros(nbinaries) - 1.
        count_iterations = 0
        while (abs(ecc_anomaly - old) > anomaly_offset).any() and count_iterations < 20:
            old = ecc_anomaly
            ecc_anomaly = ecc_anomaly - (ecc_anomaly - self.eccentricity * sp.sin(ecc_anomaly) - mean_anomaly) / (1. - self.eccentricity * sp.cos(ecc_anomaly))
            count_iterations += 1

        theta_orb = 2. * sp.arctan(sp.sqrt((1. + self.eccentricity) / (1. - self.eccentricity)) * sp.tan(ecc_anomaly / 2.))
        seperation = (1 - self.eccentricity ** 2) / (1 + self.eccentricity * sp.cos(theta_orb))
        thdot = 2 * sp.pi * sp.sqrt(1 - self.eccentricity ** 2) / seperation ** 2
        rdot = seperation * self.eccentricity * thdot * sp.sin(theta_orb) / (1 + self.eccentricity * sp.cos(theta_orb))

        vtotsq = (thdot * seperation) ** 2 + rdot ** 2
        vlos = (thdot * seperation * sp.sin(self.theta - theta_orb) + rdot * sp.cos(self.theta - theta_orb)) * sp.sin(self.inclination)
        vperp = sp.sqrt(vtotsq - vlos ** 2)
        velocity = sp.array([vlos, vperp]) * self.semi_major(mass) / (self.period * (1 + 1 / self.mass_ratio)) * 4.74057581
        return velocity

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 ccd_stats(energy, npix, pix_size, z_sam_det):
    NA = sp.sin(sp.arctan(0.5*npix*pix_size/z_sam_det))
    l = energy_to_wavelength(energy)
    axial_res = 2*l/NA**2.
    lateral_res = l/(2.*NA)
    
    print 'NA: %1.2e\nAxial resolution: %1.2e\nLateral resolution: %1.2e' % (NA, axial_res, lateral_res)

    def log_reconstruction_parameters(self):
        """
        h - object size\nz - sam-det dist\npix - # of pix\ndel_x_d - pixel size
        """
        dx_d = CXP.experiment.dx_d
        x = (CXP.p/2.)*dx_d
        l = energy_to_wavelength(CXP.experiment.energy)
        h = min(CXP.experiment.beam_size)
        pix = CXP.p
        z=CXP.experiment.z
        NF = lambda nh, nl, nz: nh**2./(nl*nz)
        del_x_s = lambda l, z, x: (l*z)/(2.*x)
        nNF = NF(h, l, z)
        OS = lambda l, z, x, h, pix: ((pix*del_x_s(l, z, x))**2.)/(h**2.)
        nOS = OS(l, z, x, h, pix)
        NA = sp.sin(sp.arctan(x/z))
        axial_res = 2*l/NA**2.
        lateral_res = l/(2.*NA)
        CXP.log.info('Fresnel number: {:2.2e}'.format(nNF))
        CXP.log.info('Oversampling: {:3.2f}'.format(nOS))
        CXP.log.info('Detector pixel size: {:3.2f} [micron]'.format(1e6*dx_d))
        CXP.log.info('Detector width: {:3.2f} [mm]'.format(1e3*pix*dx_d))
        CXP.log.info('Sample pixel size: {:3.2f} [nm]'.format(1e9*del_x_s(l, z, x)))
        CXP.log.info('Sample FOV: {:3.2f} [micron]'.format(1e6*del_x_s(l, z, x)*pix))
        CXP.log.info('Numerical aperture: {:3.2f}'.format(NA))
        CXP.log.info('Axial resolution: {:3.2f} [micron]'.format(1e6*axial_res))
        CXP.log.info('Lateral resolution: {:3.2f} [nm]'.format(1e9*lateral_res))

        self.slow_db_queue['fresnel_number'] = (nNF,)
        self.slow_db_queue['oversampling'] = (nOS,)
        self.slow_db_queue['dx_s'] = (del_x_s(l, z, x),)
        self.slow_db_queue['sample_fov'] = (del_x_s(l, z, x)*pix,)
        self.slow_db_queue['numerical_aperture'] = (NA,)
        self.slow_db_queue['axial_resolution'] = (axial_res,)

 def __init__(self, yaml):
     self._tf_listener = tf.TransformListener()
     self._grid = SearchGrid(10, 10, 2.0, 2.0)
     camera = yaml.sensors[0].camera
     self._fov_h = camera.horizontal_fov
     self._fov_v = 2.0 * scipy.arctan(scipy.tan(self._fov_h / 2.0) * (camera.image_height / camera.image_width))
     self._fov_vectors = fov_vectors(self._fov_h, self._fov_v)

    def TB_U_exceso(self, T, P):
        """Método de cálculo de la energía interna de exceso mediante la ecuación de estado de Trebble-Bishnoi"""
        a, b, c, d, q1, q2 = self.TB_lib(T, P)
        v = self.TB_V(T, P)
        z = P * v / R_atml / T
        A = a * P / R_atml ** 2 / T ** 2
        B = b * P / R_atml / T
        u = 1 + c / b
        t = 1 + 6 * c / b + c ** 2 / b ** 2 + 4 * d ** 2 / b ** 2
        tita = abs(t) ** 0.5
        if t >= 0:
            lamda = log((2 * z + B * (u - tita)) / (2 * z + B * (u + tita)))
        else:
            lamda = 2 * arctan((2 * z + u * B) / B / tita) - pi

        delta = v ** 2 + (b + c) * v - b * c - d ** 2
        beta = 1.0 + q2 * (1 - self.tr(T) + log(self.tr(T)))
        da = -q1 * a / self.Tc
        if self.tr(T) <= 1.0:
            db = b / beta * (1 / T - 1 / self.Tc)
        else:
            db = 0
        U = lamda / b / tita * (a - da * T) + db * T * (
            -R_atml * T / (v - b)
            + a
            / b ** 2
            / t
            * ((v * (3 * c + b) - b * c + c ** 2 - 2 * d ** 2) / delta + (3 * c + b) * lamda / b / tita)
        )  # atm*l/mol
        return unidades.Enthalpy(U * 101325 / 1000 / self.peso_molecular, "Jkg")

    def TB_Cv_exceso(self, T, P):
        """Método de cálculo de la capacidad calorífica a volumen constante de exceso mediante la ecuación de estado de Trebble-Bishnoi"""
        a, b, c, d, q1, q2=self.TB_lib(T, P)
        v=self.TB_V(T, P)
        z=P*v/R_atml/T
        t=1+6*c/b+c**2/b**2+4*d**2/b**2
        tita=abs(t)**0.5
        A=a*P/R_atml**2/T**2
        B=b*P/R_atml/T
        u=1+c/b
        delta=v**2+(b+c)*v-b*c-d**2
        beta=1.+q2*(1-self.tr(T)+log(self.tr(T)))
        da=-q1*a/self.Tc
        dda=q1**2*a/self.Tc**2
        if self.tr(T)<=1.0:
            db=b/beta*(1/T-1/self.Tc)
            ddb=-q2*b/beta/T**2
        else:
            db=0
            ddb=0

        dt=-db/b**2*(6*c+2*c**2/b+8*d**2/b)
        dtita=abs(dt)/20
        if t>=0:
            lamda=log((2*z+B*(u-tita))/(2*z+B*(u+tita)))
            dlamda=(db-db*tita-b*dtita)/(2*v+b+c-b*tita)-(db+db*tita+b*dtita)/((2*v+b+c+b*tita))
        else:
            lamda=2*arctan((2*z+u*B)/B/tita)-pi
            dlamda=2/(1+((2*v+b+c)/b/tita)**2)*(db/b/tita-(2*v+b+c)*(db/b**2/tita+dtita/b/tita**2))

        Cv=1/b/tita*(dlamda*(a-da*T)-lamda*dda*T-lamda*(a-da*T)*(db/b+dtita/tita))+(ddb*T+db)*(-R_atml*T/(v-b)+a/b**2/t*((v*(3*c+b)-b*c+c**2-2*d**2)/delta+(3*c+b)*lamda/b/tita))+db*T*(-R_atml/(v-b)-R_atml*T*db/(v-b)**2+1/b**2/t*(da-2*a*db/b-a*dt/t)*((v*(3*c+b)-b*c+c**2-2*d**2)/delta+(3*c+b)*lamda/b/tita)+a/b**2/t*(db*(v-c)*(v**2-2*c*v-c**2+d**2)/delta**2+db*lamda/b/tita+(3*c+b)/b/tita*(dlamda-lamda*(db/b+dtita/tita))))
        return unidades.SpecificHeat(Cv*101325/1000/self.peso_molecular, "JkgK")

def polarZ(z):
	if(z == 0):
		return (0,0)
	else :
		a = z.real
		b = z.imag
		return( sp.hypot(a,b), sp.arctan(b/a))

    def joinT(yb,ya,xb,xa):
        dya=yb-ya+0.
        dxa=xb-xa+0.
         
        if (dxa==0 and dya>0):
            tAn=math.pi/4.
            return tAn
        elif (dxa==0 and dya<=0):
            tAn=(3./2.)*math.pi
            return tAn
        elif (dya==0 and dxa>=0):
            tAn = 0.
            return tAn
        elif (dya==0 and dxa<0):
            tAn = math.pi
            return tAn
        
        else :   
            tAn= arctan(((dya)/dxa))
           
        
#         get correct quadrant
            if(dya<0 and dxa>0):
                tAn= tAn + 2*math.pi
            elif(dya<0 and dxa<0):
                tAn= tAn + math.pi
            elif(dya>0 and dxa<0):
                tAn= tAn + math.pi
            
        return tAn #TBN

    def drawlabel(self, name, Preferences, t, W, label, unit):
        """
        Draw annotation for isolines
            name: name of isoline
            Preferences: Configparse instance of pychemqt preferences
            t: x array of line
            W: y array of line
            label: text value to draw
            unit: text units to draw
        """
        if Preferences.getboolean("Psychr", name+"label"):
            tmin = unidades.Temperature(Preferences.getfloat("Psychr", "isotdbStart")).config()
            tmax = unidades.Temperature(Preferences.getfloat("Psychr", "isotdbEnd")).config()
            x = tmax-tmin
            wmin = Preferences.getfloat("Psychr", "isowStart")
            wmax = Preferences.getfloat("Psychr", "isowEnd")
            y = wmax-wmin

            i = 0
            for ti, wi in zip(t, W):
                if tmin <= ti <= tmax and wmin <= wi <= wmax:
                    i += 1
            label = str(label)
            if Preferences.getboolean("Psychr", name+"units"):
                label += unit
            pos = Preferences.getfloat("Psychr", name+"position")
            p = int(i*pos/100-1)
            rot = arctan((W[p]-W[p-1])/y/(t[p]-t[p-1])*x)*360/2/pi
            self.diagrama2D.axes2D.annotate(label, (t[p], W[p]),
                rotation=rot, size="small", ha="center", va="center")

def pix2sky(header,x,y):
	hdr_info = parse_header(header)
	x0 = x-hdr_info[1][0]+1.	# Plus 1 python->image
	y0 = y-hdr_info[1][1]+1.
	x0 = x0.astype(scipy.float64)
	y0 = y0.astype(scipy.float64)
	x = hdr_info[2][0,0]*x0 + hdr_info[2][0,1]*y0
	y = hdr_info[2][1,0]*x0 + hdr_info[2][1,1]*y0
	if hdr_info[3]=="DEC":
		a = x.copy()
		x = y.copy()
		y = a.copy()
		ra0 = hdr_info[0][1]
		dec0 = hdr_info[0][0]/raddeg
	else:
		ra0 = hdr_info[0][0]
		dec0 = hdr_info[0][1]/raddeg
	if hdr_info[5]=="TAN":
		r_theta = scipy.sqrt(x*x+y*y)/raddeg
		theta = arctan(1./r_theta)
		phi = arctan2(x,-1.*y)
	elif hdr_info[5]=="SIN":
		r_theta = scipy.sqrt(x*x+y*y)/raddeg
		theta = arccos(r_theta)
		phi = artan2(x,-1.*y)
	ra = ra0 + raddeg*arctan2(-1.*cos(theta)*sin(phi-pi),
				   sin(theta)*cos(dec0)-cos(theta)*sin(dec0)*cos(phi-pi))
	dec = raddeg*arcsin(sin(theta)*sin(dec0)+cos(theta)*cos(dec0)*cos(phi-pi))

	return ra,dec

def myArctan(x,y):
	alpha = sp.arctan(y/x)
	if x < 0:
		alpha += sp.pi
	elif y < 0:
		alpha += 2*sp.pi
	# print 'myArctan: ',x,y,alpha
	return alpha

def grassmann_logmap(A,p, tol=1e-13, skip_orthog_check=False):
    '''
    Computes the manifold log-map of (nxk) orthogonal matrix A,
    centered at the point p (i.e. the "pole"), which is also an
    (nxk) orthogonal matrix.
    The log-map takes a point on the manifold and maps it to the
    tangent space which is centered at a given pole.
    The dimension of the tangent space is k(n-k), 
    and points A,p are on Gr(n,k).
    @param A: The orthogonal matrix A, representing a point on
    the grassmann manifold.
    @param p: An orthogonal matrix p, representing a point on
    the grassmann manifold where the tangent space will be formed.
    Also called the "pole".
    @param tol: Numerical tolerance used to set singular values
    to exactly zero when within this tolerance of zero.
    @param skip_orthog_check: Set to True if you can guarantee
    that the inputs are already orthogonal matrices. Otherwise,
    this function will check, and if A and/or p are not orthogonal,
    the closest orthogonal matrix to A (or p) will be used.
    @return: A tuple (log_p(A), ||log_p(A)|| ), representing
    the tangent-space mapping of A, and the distance from the
    mapping of A to the pole in the tangent space.
    '''
    
    #check that A and p are orthogonal, if
    # not, then compute orthogonal representations and
    # send back a warning message.
    if not skip_orthog_check:
        if not isOrthogonal(A):
            print "WARNING: You are calling grassmann_logmap function on non-orthogonal input matrix A"
            print "(This function will compute an orthogonal representation for A using an SVD.)"
            A = closestOrthogonal(A)
        if not isOrthogonal(p):
            print "WARNING: You are calling grassmann_logmap function on non-orthogonal pole p."
            print "(This function will compute an orthogonal representation for p using an SVD.)"
            p = closestOrthogonal(p)
    
    #p_perp is the orthogonal complement to p, = null(p.T)
    p_perp = nullspace(p.T)
    
    #compute p_perp * p_perp.T * A * inv(p.T * A)
    T = sp.dot(p.T,A)
    try:
        Tinv = LA.inv(T)
    except(LA.LinAlgError):
        Tinv = LA.pinv(T)
        
    X = sp.dot( sp.dot( sp.dot(p_perp,p_perp.T), A), Tinv )
    
    u, s, vh = LA.svd(X, full_matrices=False)
    s[ s < tol ]= 0   #set extremely small values to zero
    theta = sp.diag( sp.arctan(s) )
    
    logA = sp.dot(u, sp.dot( theta,vh))    
    normA = sp.trace( sp.dot(logA.T, logA) )
    
    return logA, normA

 def g(self, x):
     if x[1] == 0.0:
         A = pi/2.0
     else:
         A = arctan(x[0]/x[1])
     g1 = x[1]**2 + x[0]**2 - 1.0 -0.1*cos(16.0*A)
     g2 = 0.5 -(x[0]-0.5)**2 -(x[1]-0.5)**2 
     if g1 >= 0 and g2 >= 0:
         return True,array([0.,0.])
     return False,array([g1,g2])

 def find_tang(self, pt):
     x = pt[0]
     y = pt[1]
     d = self.param
     k = sp.sqrt((x+d)**2 + y**2 - (1+d)**2)
     theta = sp.arctan(k/(1+d))
     # translation by d, rotation by 2*theta, then translate back by d
     x += d
     x, y = rotation((x,y), 2*theta)
     x -= d
     return sp.array((x,y))

 def tosph(self):
     x, y, z = self.coord
     rho = self.norm()
     if sp.absolute(x) < 1e-8:
         if y >= 0:
             theta = sp.pi/2
         else:
             theta = 3*sp.pi/2
     else:
         theta = sp.arctan(y/x)
     phi = sp.arccos(z/rho)
     return rho, theta, phi

def RiemannSurface4():
    """Riemann surface for real part of arctan(z)"""
    
    fig = plt.figure()
    ax = Axes3D(fig)
    Xres, Yres = .01, .2
    ax.view_init(elev=11., azim=-56)
    X = sp.arange(-4, -.0001, Xres)
    Y = sp.arange(-4, 4, Yres)
    X, Y = sp.meshgrid(X, Y)
    Z = sp.real(sp.arctan(X+1j*Y))
    ax.plot_surface(X, Y, Z, rstride=1, cstride=1, linewidth=0, cmap=cmap)
    ax.plot_surface(X, Y, Z+sp.pi, rstride=1, cstride=1, linewidth=0, cmap=cmap)
    ax.plot_surface(X, Y, Z-sp.pi, rstride=1, cstride=1, linewidth=0, cmap=cmap)
    X = sp.arange(.0001, 4, Xres)
    Y = sp.arange(-4,4, Yres)
    X, Y = sp.meshgrid(X, Y)
    Z = sp.real(sp.arctan(X+1j*Y))
    ax.plot_surface(X, Y, Z, rstride=1, cstride=1, linewidth=0, cmap=cmap)
    ax.plot_surface(X, Y, Z+sp.pi, rstride=1, cstride=1, linewidth=0,cmap=cmap)
    ax.plot_surface(X, Y, Z-sp.pi, rstride=1, cstride=1, linewidth=0, cmap=cmap)
    plt.savefig('RiemannSurface4.pdf', bbox_inches='tight', pad_inches=0)