def haversine(location1, location2=None):  # calculates great circle distance
    __doc__ = """Returns the great circle distance of the given
    coordinates.
    
    INPUT:  location1 = ((lat1, lon1), ..., n(lat1, lon1))
           *location2 = ((lat2, lon2), ..., n(lat2, lon2))
           *if location2 is not given a square matrix of distances
             for location1 will be put out
    OUTPUT: distance in km
            (dist1  ...  ndist
              :            : 
             ndist1 ...  ndist)
            shape will depend on the input
    METHOD: a = sin(dLat / 2) * sin(dLat / 2) + 
                sin(dLon / 2) * sin(dLon / 2) * 
                cos(lat1) * cos(lat2)
            c = 2 * arctan2(sqrt(a), sqrt(1 - a))
            d = R * c
            
            where R is the earth's radius (6371 km)
            and d is the distance in km"""
    
    from itertools import product, combinations
    from pylab import   deg2rad, sin, cos, arctan2, \
                        meshgrid, sqrt, array, arange
    
    if location2: 
        location1 = array(location1, ndmin=2)
        location2 = array(location2, ndmin=2)
    elif location2 is None:
        location1 = array(location1, ndmin=2)
        location2 = location1.copy()
    
    # get all combinations using indicies
    ind1 = arange(location1.shape[0])
    ind2 = arange(location2.shape[0])
    ind  = array(list(product(ind1, ind2)))
    
    # using combination inds to get lats and lons
    lat1, lon1 = location1[ind[:,0]].T
    lat2, lon2 = location2[ind[:,1]].T
    
    # setting up variables for haversine
    R = 6371.
    dLat = deg2rad(lat2 - lat1)
    dLon = deg2rad(lon2 - lon1)
    lat1 = deg2rad(lat1)
    lat2 = deg2rad(lat2)
    
    # haversine formula
    a = sin(dLat / 2) * sin(dLat / 2) + \
        sin(dLon / 2) * sin(dLon / 2) * \
        cos(lat1) * cos(lat2)
    c = 2 * arctan2(sqrt(a), sqrt(1 - a))
    d = R * c
    
    # reshape accodring to the input
    D = d.reshape(location1.shape[0], location2.shape[0])
    
    return D

    def get_touchdown(self, t, y, params):
        """
        Compute the touchdown position of the leg w.r.t. CoM velocity
        
        :args:
            t (float): time
            y (6x float): state of the CoM
            params (4x float): leg parameter: stiffness, l0, alpha, beta
        
        :returns:
            [xFoot, yFoot, zFoot] the position of the leg tip
        """
        k, l0, alpha, beta = params
        vx, vz = y[3], y[5]
        
        a_v_com = -arctan2(vz, vx) # correct with our coordinate system
        #for debugging
        #print "v_com_angle:", a_v_com * 180. / pi
                
        xf = y[0] + l0 * cos(alpha) * cos(beta + a_v_com)
        yf = y[1] - l0 * sin(alpha)
        zf = y[2] - l0 * cos(alpha) * sin(beta + a_v_com)

        #for debugging
        #print "foot: %2.3f,%2.3f,%2.3f," % ( xf,yf, zf)
        
        return array([xf, yf, zf])

def query_nvss(options, ra0, dec0, s=">0.0", proj='SIN'):
	'''
	query_nvss: module which queries the NVSS using the Vizier protocol. 
	inputs: ra0, dec0, s="<20"
	ra0 = the central ra in degrees
	dec0 = the central dec in degrees
	s = the flux cutoff
	returns L, M (relative coordinates in degrees), N (number of sources), S (1.4GHz Flux
	Density in mJy)
	'''
	v = Vizier(column_filters={"S1.4":s})
	v.ROW_LIMIT = 10000
	result = v.query_region(coord.SkyCoord(ra=ra0, dec=dec0, unit=(u.deg, u.deg), frame='icrs'), 
	    radius=Angle(1, "deg"), catalog='NVSS')
	ra = result[0]['_RAJ2000']
	dec = result[0]['_DEJ2000']
	N = len(result[0])
	if proj.upper()=='SIN':
		L = (ra-ra0)*pl.cos(dec*deg2rad)
		M = dec-dec0
	if proj.upper()=='NCP':
		L = 57.2957795*pl.cos(deg2rad*dec)*pl.sin(deg2rad*(ra-ra0))
		M = 57.2957795*(pl.cos(deg2rad*dec0) - pl.cos(deg2rad*dec)*pl.cos(deg2rad*(ra-ra0)))/pl.sin(deg2rad*dec0) 
	S = result[0]['S1.4']
	ascii.write(result[0], options.outfile+'.dat', format='tab') 
	ann_writer(options, result[0])
	return L, M, N, S

def angleDistributionControlData(bin_count):
  bins = [180.0*x/float(bin_count) for x in range(bin_count + 1)]
  bin_pairs = zip(bins[:-1],bins[1:])
  weights = [pl.cos(b*pl.pi/180) - pl.cos(a*pl.pi/180) for (a,b) in bin_pairs]
  total = sum(weights)
  weights = [w/total for w in weights]
  return bins,weights

def sortAnglesCW(t1,t2):

    """
    Sort angles so that t2>t1 in a clockwise sense
    idea from `StackOverflow <http://stackoverflow.com/questions/242404/sort-four-points-in-clockwise-order>`_
    more description: `SoftSurfer <http://softsurfer.com/Archive/algorithm_0101/algorithm_0101.htm>`_

    If the signed area of the triangle formed between the points on a unit circle with angles t1 and t2
    and the origin is positive, the angles are sorted counterclockwise. Otherwise, the angles
    are sorted in a counter-clockwise manner.  Here we want the angles to be sorted CCW, so
    if area is negative, swap angles
    
    Area obtained from the cross product of a vector from origin 
    to 1 and a vector to point 2, so use right hand rule to get 
    sign of cross product with unit length
    """

    while (cos(t1)*sin(t2)-sin(t1)*cos(t2)>0):
        ##Swap angles
        temp=t1;
        t1=t2;
        t2=temp;
    #Make t1 between 0 and 2pi
    while (t1<0 or t1> 2.0*pi):
        if t1>2.0*pi:
            t1=t1-2*pi
        else:
            t1=t1+2*pi
    #Want t2 to be less than t1, but no more than 2*pi less
    while (t2<t1 and t1-t2>2*pi):
        t2=t2+2*pi
    while (t2>t1):
        t2=t2-2*pi
    return (t1,t2)

def haversine (latlong1, latlong2, r):

    deltaLatlong = latlong1 - latlong2
    
    dLat = deltaLatlong[0]
    dLon = deltaLatlong[1]

    lat1 = latlong1[0]
    lat2 = latlong2[0]

    a = (sin (dLat/2) * sin (dLat/2) +
         sin (dLon/2) * sin (dLon/2) * cos (lat1) * cos (lat2))
    c = 2 * arctan2 (sqrt (a), sqrt (1-a))
    d = r * c

    # initial bearing
    y = sin (dLon) * cos (lat2)
    x = (cos (lat1)*sin (lat2) -
         sin (lat1)*cos (lat2)*cos (dLon))
    b1 = arctan2 (y, x);

    # final bearing
    dLon = -dLon
    dLat = -dLat
    tmp = lat1
    lat1 = lat2
    lat2 = tmp
    y = sin (dLon) * cos (lat2)
    x = (cos (lat1) * sin (lat2) - 
         sin (lat1) * cos (lat2) * cos (dLon))
    b2 = arctan2 (y, x)
    b2 = mod ((b2 + pi), 2*pi)

    return (d, b1, b2)

    def J(self,t):
        # the -1 in the lines below is for the right rounding with int()
        # x1 is position of p1 (particle 1) 
        x1 = self.sol[int(t/self.dt)-1,2]
        # x2 is velocity of p1
        x2 = self.sol[int(t/self.dt)-1,0]
        # x3 is position of p2
        x3 = self.sol[int(t/self.dt)-1,3]
        # x4 is velocity of p2
        x4 = self.sol[int(t/self.dt)-1,1]

        # These are the differentials of the forces of the particles. Writen like this to make the
        # matrix below easier to read f14 is force of p2 on p1 _dx1 is derivitive with respect to x1
        # Note on 1/r2 part -> goes to cubic so it will always retain its sign.
        df13_dx1 = -2.0/(x1-x3)**3 + (polygamma(2,1.0+(x1-x3)/self.d)+polygamma(2,1.0-(x1-x3)/self.d))/self.d**3
        # the final deriviative of -x3 just gives you the negative of everything above
        df13_dx3 = -df13_dx1
        df31_dx1 = 2.0/(x3-x1)**3 - (polygamma(2,1.0-(x3-x1)/self.d)+polygamma(2,1.0+(x3-x1)/self.d))/self.d**3
        df31_dx3 = -df31_dx1


        # define the matrix elements of the time dependent jacobian
        jacobian = pl.array([ \
        [0.0                                   , 1.0       , 0.0                                   , 0.0],
        [self.A*pl.cos(x1)*pl.cos(t)+df13_dx1, -self.beta, df13_dx3                              , 0.0],
        [0.0                                   , 0.0       , 0.0                                   , 1.0],
        [df31_dx1                              , 0.0       , self.A*pl.cos(x3)*pl.cos(t)+df31_dx3, -self.beta]\
        ])

        return jacobian

def hillshade(data, scale=10.0, azdeg=165.0, altdeg=45.0):
    '''
    Convert data to hillshade based on matplotlib.colors.LightSource class.

    Args:
        data - a 2-d array of data
        scale - scaling value of the data. higher number = lower gradient
        azdeg - where the light comes from: 0 south ; 90 east ; 180 north ;
                        270 west
        altdeg - where the light comes from: 0 horison ; 90 zenith

    Returns:
        a 2-d array of normalized hilshade
    '''
    # convert alt, az to radians
    az = azdeg*pi/180.0
    alt = altdeg*pi/180.0
    # gradient in x and y directions
    dx, dy = gradient(data/float(scale))
    slope = 0.5*pi - arctan(hypot(dx, dy))
    aspect = arctan2(dx, dy)
    az = -az - aspect - 0.5*pi
    intensity = sin(alt)*sin(slope) + cos(alt)*cos(slope)*cos(az)
    mi, ma = intensity.min(), intensity.max()
    intensity = (intensity - mi)/(ma - mi)
    return intensity

def convert(polar,R):
    cartesian=pl.zeros((polar.shape[0],4))
    cartesian[:,0]=R * pl.sin(polar[:,1]) * pl.cos(polar[:,0]);
    cartesian[:,1]=R * pl.sin(polar[:,1]) * pl.sin(polar[:,0]) * pl.cos(polar[:,2]);
    cartesian[:,2]=R * pl.cos(polar[:,1]);
    cartesian[:,3]=R * pl.sin(polar[:,1]) * pl.sin(polar[:,0]) * pl.sin(polar[:,2]);
    return cartesian

def main():
    # Create the grid
    x = arange(-100, 101)
    y = arange(-100, 101)

    # Create the meshgrid
    Y, X = meshgrid(x, y)
    A = 1
    B = 2
    V = 6*pi / 201
    W = 4*pi / 201
    F = A*sin(V*X) + B*cos(W*Y)
    Fx = V*A*cos(V*X)
    Fy = W*B*-sin(W*Y)

    # Show the images
    show_image(F)
    show_image(Fx)
    show_image(Fy)

    # Create the grid for the quivers
    xs = arange(-100, 101, 10)
    ys = arange(-100, 101, 10)

    # Here we determine the direction of the quivers
    Ys, Xs = meshgrid(ys, xs)
    FFx = V*A*cos(V*Xs)
    FFy = W*B*-sin(W*Ys)

    # Draw the quivers and the image
    clf()
    imshow(F, cmap=cm.gray, extent=(-100, 100, -100, 100))
    quiver(ys, xs, -FFy, FFx, color='red')
    show()

def hillshade(data,scale=10.0,azdeg=165.0,altdeg=45.0):
  ''' 
    This code thanks to Ran Novitsky Nof
  http://rnovitsky.blogspot.co.uk/2010/04/using-hillshade-image-as-intensity.html
  Repeated here to make my cyclopean uk_map code prettier.

  convert data to hillshade based on matplotlib.colors.LightSource class.
    input:
         data - a 2-d array of data
         scale - scaling value of the data. higher number = lower gradient
         azdeg - where the light comes from: 0 south ; 90 east ; 180 north ;
                      270 west
         altdeg - where the light comes from: 0 horison ; 90 zenith
    output: a 2-d array of normalized hilshade
  '''
  
  from pylab import pi, gradient, arctan, hypot, arctan2, sin, cos
  # convert alt, az to radians
  az = azdeg*pi/180.0
  alt = altdeg*pi/180.0
  # gradient in x and y directions
  dx, dy = gradient(data/float(scale))
  slope = 0.5*pi - arctan(hypot(dx, dy))
  aspect = arctan2(dx, dy)
  intensity = sin(alt)*sin(slope) + cos(alt)*cos(slope)*cos(-az - aspect - 0.5*pi)
  intensity = (intensity - intensity.min())/(intensity.max() - intensity.min())
  return intensity

 def plot2D(self,ta):
     r = (self.h**2/self.ref.mu)*(1.0/(1.0+self.e*cos(ta)))
     rv = r * array([cos(ta),sin(ta),0])
     
     #v = self.ref.mu / self.h
     #vv = v * array([-sin(ta),self.e+cos(ta),0])
     
     return rv#,vv)

def measureDistance(lat1, lon1, lat2, lon2):
    R = 6383.137 # Radius of earth at Chajnantor aprox. in KM
    dLat = (lat2 - lat1) * np.pi / 180.
    dLon = (lon2 - lon1) * np.pi / 180.
    a = pl.sin(dLat/2.) * pl.sin(dLat/2.) + pl.cos(lat1 * np.pi / 180.) * pl.cos(lat2 * np.pi / 180.) * pl.sin(dLon/2.) * pl.sin(dLon/2.)
    c = 2. * atan2(pl.sqrt(a), pl.sqrt(1-a))
    d = R * c
    return d * 1000. # meters

 def rotated_rectangle(x0,y0,w,h,rot):
     
     x = np.array([-w/2,w/2,w/2,-w/2,-w/2])
     y = np.array([-h/2,-h/2,h/2,h/2,-h/2])
     
     xrot = x*cos(rot)-y*sin(rot)
     yrot = x*sin(rot)+y*cos(rot) 
     
     return xrot+x0, yrot+y0

def random_euler_angles():
    r1,r2,r3 = pylab.random(3)
    q1 = pylab.sqrt(1.0-r1)*pylab.sin(2.0*pylab.pi*r2)
    q2 = pylab.sqrt(1.0-r1)*pylab.cos(2.0*pylab.pi*r2)
    q3 = pylab.sqrt(r1)*pylab.sin(2.0*pylab.pi*r3)
    q4 = pylab.sqrt(r1)*pylab.cos(2.0*pylab.pi*r3)
    phi = math.atan2(2.0*(q1*q2+q3*q4), 1.0-2.0*(q2**2+q3**2))
    theta = math.asin(2.0*(q1*q3-q4*q2))
    psi = math.atan2(2.0*(q1*q4+q2*q3), 1.0-2.0*(q3**2+q4**2))
    return [phi,theta,psi]

def my_hor_to_eq(az, el, lat, lsts):
    dec = arcsin(sin(el) * sin(lat) + cos(el) * cos(lat) * cos(az))
    argument = (sin(el) - sin(lat) * sin(dec)) / (cos(lat) * cos(dec))
    argument = clip(argument, -1.0, 1.0)
    H = arccos(argument)
    flag = sin(az) > 0
    H[flag] = 2.0*pi - H[flag]
    ra = lsts - H
    ra %= 2*pi
    return ra,dec

def plot_trace(X, scale=1., angle=0.):
    fig = pl.figure(figsize=(12,4.75))
    
    ax1 = fig.add_subplot(1, 2, 1)
    # plot boundary
    t = pl.arange(0,2*pl.pi,.01)
    ax1.plot(pl.cos(angle)*pl.cos(t) - pl.sin(angle)*pl.sin(t)/scale, pl.cos(angle)*pl.sin(t)/scale + pl.sin(angle)*pl.cos(t), 'k:')
    
    # plot samples
    if isinstance(X, mc.Stochastic):
        tr = [X.trace()[:,0], X.trace()[:,1]]
    else:
        tr = [X[0].trace(), X[1].trace()]

    ax1.plot(tr[0], tr[1], 'ko-')
        
    # decorate plot
    book_graphics.set_font()
    pl.xlabel('$X_1$')
    pl.ylabel('$X_2$', rotation=0)
    pl.axis([-1.1,1.1,-1.1,1.1])
    pl.text(-1,1,'(a)', fontsize=16, va='top', ha='left')

    
    for i in range(2):
        if i == 0:
            ax2 = fig.add_subplot(2, 4, 3+4*i)
            ax2.plot(tr[i], 'k', drawstyle='steps-mid')
        else:
            ax2a = fig.add_subplot(2, 4, 3+4*i, sharex=ax2)
            ax2a.plot(tr[i], 'k', drawstyle='steps-mid')
            pl.xlabel('Sample')
        
        pl.xticks([25,50,75])
        pl.yticks([-.5,0,.5])
        pl.ylabel('$X_%d$'%(i+1), rotation=0)
        pl.axis([-5,105,-1.5,1.5])
        pl.text(-1,1.25,'(%s)'%'bc'[i], fontsize=16, va='top', ha='left')
        
        if i == 0:
            ax3 = fig.add_subplot(2, 4, 4+4*i)
            ax3.acorr(tr[i].reshape(100), color='k')
        else:
            ax3a = fig.add_subplot(2, 4, 4+4*i, sharex=ax3)
            ax3a.acorr(tr[i].reshape(100), color='k')
            pl.xlabel('Autocorrelation')
        
        pl.xticks([-5,0,5])
        pl.yticks([0., .5, 1])
        pl.axis([-12,12,-.1,1.1])
        pl.text(-10,1,'(%s)'%'de'[i], fontsize=16, va='top', ha='left')
        
    pl.setp(ax2.get_xticklabels(), visible=False)
    pl.setp(ax3.get_xticklabels(), visible=False)
    pl.subplots_adjust(wspace=.55, hspace=.1, bottom=.14,left=.13)

def CoordsOrbScroll(theta,geo,shaveOn=True, just_involutes = False, Ndict = {}):
    
    from PDSim.scroll import scroll_geo
    
    shaveDelta=None
    if shaveOn==True:
        shaveDelta = pi/2
    else:
        shaveDelta = 1e-16
    
    (xshave, yshave) = Shave(geo, theta, shaveDelta)
    
    Nphi = Ndict.get('phi',500)
    Narc1 = Ndict.get('arc1',100)
    Nline = Ndict.get('line',100)
    Narc2 = Ndict.get('arc2',100)
    
    phi = np.linspace(geo.phi_ois, geo.phi_oie, Nphi)
    (x_oi,y_oi) = scroll_geo.coords_inv(phi,geo,theta,flag="oi")
    phi = np.linspace(geo.phi_oos, geo.phi_ooe - shaveDelta, Nphi)
    (x_oo,y_oo) = scroll_geo.coords_inv(phi,geo,theta,flag="oo")
    
    xarc1=geo.xa_arc1+geo.ra_arc1*cos(np.linspace(geo.t2_arc1,geo.t1_arc1,Narc1))
    yarc1=geo.ya_arc1+geo.ra_arc1*sin(np.linspace(geo.t2_arc1,geo.t1_arc1,Narc1))
    xline=np.linspace(geo.t1_line,geo.t2_line,Nline)
    yline=geo.m_line*xline+geo.b_line
    xarc2=geo.xa_arc2+geo.ra_arc2*cos(np.linspace(geo.t1_arc2,geo.t2_arc2,Narc2))
    yarc2=geo.ya_arc2+geo.ra_arc2*sin(np.linspace(geo.t1_arc2,geo.t2_arc2,Narc2))
    
    ro = geo.rb*(pi-geo.phi_fi0+geo.phi_fo0)
    om = geo.phi_fie-theta+3.0*pi/2.0
    xarc1_o=-xarc1+ro*cos(om)
    yarc1_o=-yarc1+ro*sin(om)
    xline_o=-xline+ro*cos(om)
    yline_o=-yline+ro*sin(om)
    xarc2_o=-xarc2+ro*cos(om)
    yarc2_o=-yarc2+ro*sin(om)
    
    if just_involutes:
        x=np.r_[x_oo,x_oi[::-1]]
        y=np.r_[y_oo,y_oi[::-1]]
    else:
        if shaveOn:
            x=np.r_[x_oo,xshave,x_oi[::-1],xarc1_o,xline_o,xarc2_o]
            y=np.r_[y_oo,yshave,y_oi[::-1],yarc1_o,yline_o,yarc2_o]
        else:
            x=np.r_[x_oo,x_oi[::-1],xarc1_o,xline_o,xarc2_o]
            y=np.r_[y_oo,y_oi[::-1],yarc1_o,yline_o,yarc2_o]
            
    
    #Output as a column vector
    x=x.reshape(len(x),1)
    y=y.reshape(len(y),1)
    return x,y

    def Rot_x(self,x):
        """ Returns rotation matrix in x direction

        Parameters
        ----------
        x: float
           Angle to rotate the matrix with
        """
        return matrix([[1,      0,      0],
                       [0, cos(x), sin(x)],
                       [0,-sin(x), cos(x)]])

    def Rot_y(self,y):
        """ Returns rotation matrix in y direction

        Parameters
        ----------
        y: float
           Angle to rotate the matrix with
        """
        return matrix([[cos(y), 0, -sin(y)],
                       [     0, 1,       0],
                       [sin(y), 0,  cos(y)]])