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 detect_skew(img, min_angle=-20, max_angle=20, quality='low'):
    img = sp.atleast_2d(img)
    rows, cols = img.shape
    min_min_angle = min_angle
    max_max_angle = max_angle

    if quality == 'low':
        resolution = sp.arctan2(2.0, cols) * 180.0 / sp.pi
        min_target_size = 100
        resize_order = 1
    elif quality == 'high':
        resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
        min_target_size = 300
        resize_order = 3
    else:
        resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
        min_target_size = 200
        resize_order = 2

    # resize the image so it's faster to work with
    min_size = min(rows, cols)
    target_size = min_target_size if min_size > min_target_size else min_size
    resize_ratio = float(target_size) / min_size
    img = imresize(img, resize_ratio)
    rows, cols = img.shape

    # pad the image and invert the colors
    img *= -1
    img += 255
    padded_img = sp.zeros((rows*2, cols*2))
    padded_img[rows//2:rows//2+rows, cols//2:cols//2+cols] = img
    img = padded_img

    # keep dividing the interval in half to achieve O(log(n))
    while True:
        current_resolution = (max_angle - min_angle) / 30.0
        best_angle = None
        best_variance = 0.0

        # rotate the image, sum the pixel values in each row for each rotation
        # then find the variance of all the sums, pick the highest variance
        for i in xrange(31):
            angle = min_angle + i * current_resolution
            rotated_img = rotate(img, angle, reshape=False, order=resize_order)
            num_black_pixels = sp.sum(rotated_img, axis=1)
            variance = sp.var(num_black_pixels)
            if variance > best_variance:
                best_angle = angle
                best_variance = variance

        if current_resolution < resolution:
            break

        # update the angle range
        min_angle = max(best_angle - current_resolution, min_min_angle)
        max_angle = min(best_angle + current_resolution, max_max_angle)

    return best_angle

 def update(self, order, dt=0.1):
   angle = self.X[0]
   V = 150*speed(self.X)
   order_stick = order + sp.arctan2(self.X[2], self.X[1])/sp.pi/2
   dx0 = self.k*(windForce(self.X, 400) +0.)*(order_stick-0.*sp.sin(sp.arctan2(self.X[2], self.X[1])-sp.pi/2))
   dx1 = -V*sp.sin(angle) + self.kdrift*order*sp.cos(angle) 
   dx2 = V*sp.cos(angle) + self.kdrift*order*sp.sin(angle)-30
   dX = dt*np.array([dx0[0], dx1[0], dx2[0]])
   self.X = self.X + dX
   self.X = np.array([self.X[0], self.X[1], np.max([0, self.X[2]])])

def matrixToEuler(m,order='Aerospace',inDegrees=True):
    if order == 'Aerospace' or order == 'ZYX':
        sp = -m[2,0]
        if sp < (1-EPS):
            if sp > (-1+EPS):
                p = arcsin(sp)
                r = arctan2(m[2,1],m[2,2])
                y = arctan2(m[1,0],m[0,0])
            else:
                p = -pi/2.
                r = 0
                y = pi-arctan2(-m[0,1],m[0,2])
        else:
            p = pi/2.
            y = arctan2(-m[0,1],m[0,2])
            r = 0
        
        if inDegrees:
            return degrees((y,p,r))
        else:
            return (y,p,r)
    elif order == 'BVH' or order == 'ZXY':
        sx = m[2,1]
        if sx < (1-EPS):
            if sx > (-1+EPS):
                x = arcsin(sx)
                z = arctan2(-m[0,1],m[1,1])
                y = arctan2(-m[2,0],m[2,2])
            else:
                x = -pi/2
                y = 0
                z = -arctan2(m[0,2],m[0,0])
        else:
            x = pi/2
            y = 0
            z = arctan2(m[0,2],m[0,0])
        if inDegrees:
            return degrees((z,x,y))
        else:
            return (z,x,y)

    elif order == "ZXZ":
        x = arccos(m[2,2])
        z2 = arctan2(m[2,0],m[2,1])
        z1 = arctan2(m[0,2],-m[1,2])
        if inDegrees:
            return degrees((z1,x,z2))
        else:
            return (z1,x,z2)

def detect_skew(img, min_angle=-20, max_angle=20, quality='low'):
    img = sp.atleast_2d(img)
    rows, cols = img.shape
    min_min_angle = min_angle
    max_max_angle = max_angle

    if quality == 'low':
        resolution = sp.arctan2(2.0, cols) * 180.0 / sp.pi
        min_target_size = 100
        resize_order = 1
    elif quality == 'high':
        resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
        min_target_size = 300
        resize_order = 3
    else:
        resolution = sp.arctan2(1.0, cols) * 180.0 / sp.pi
        min_target_size = 200
        resize_order = 2

    # resize the image so it's faster to work with
    min_size = min(rows, cols)
    target_size = min_target_size if min_size > min_target_size else min_size
    resize_ratio = float(target_size) / min_size
    img = imresize(img, resize_ratio)
    rows, cols = img.shape

    img *= -1
    img += 255

    while True:
        current_resolution = (max_angle - min_angle) / 30.0
        angles = sp.linspace(min_angle, max_angle, 31)

        # do the hough transfer
        hough_out = _hough_transform(img, angles * sp.pi / 180)
        
        # determine which angle gives max variance
        variances = sp.var(hough_out, axis=0)
        max_variance_index = sp.argmax(variances)
        best_angle = min_angle + max_variance_index * current_resolution

        if current_resolution < resolution:
            break

        # update the angle range
        min_angle = max(best_angle - current_resolution, min_min_angle)
        max_angle = min(best_angle + current_resolution, max_max_angle)

    return best_angle

def xyz2lbr (x, y, z, d0=dsun):
    """ convert galactic xyz into sun-centered lbr coordinates; derived from stCoords.c"""
    #if len(xyz.shape) > 1:  x, y, z = xyz[:,0], xyz[:,1], xyz[:,2]
    #else:                   x, y, z = xyz[0], xyz[1], xyz[2]
    xsun = x + d0
    temp = (xsun*xsun) + (y*y)
    l = sc.arctan2(y, xsun) * deg
    b = sc.arctan2(z, sc.sqrt(temp)) * deg
    r = sc.sqrt(temp + (z*z))
    if type(l) == type(arr):
        for i in range(len(l)):
            if l[i] < 0.0:  l[i] = l[i] + 360.0
    else:
        if l < 0.0:  l = l + 360.0
    return l,b,r

def rect_to_cyl(X,Y,Z):
    """
    NAME:

       rect_to_cyl

    PURPOSE:

       convert from rectangular to cylindrical coordinates

    INPUT:

       X, Y, Z - rectangular coordinates

    OUTPUT:

       R,phi,z

    HISTORY:

       2010-09-24 - Written - Bovy (NYU)

    """
    R= sc.sqrt(X**2.+Y**2.)
    phi= sc.arctan2(Y,X)
    if isinstance(phi,nu.ndarray): phi[phi<0.]+= 2.*nu.pi
    elif phi < 0.: phi+= 2.*nu.pi
    return (R,phi,Z)

def rect_to_cyl(X,Y,Z):
    """
    NAME:

       rect_to_cyl

    PURPOSE:

       convert from rectangular to cylindrical coordinates

    INPUT:

       X, Y, Z - rectangular coordinates

    OUTPUT:

       R,phi,z

    HISTORY:

       2010-09-24 - Written - Bovy (NYU)

    """
    R= sc.sqrt(X**2.+Y**2.)
    phi= sc.arctan2(Y,X)
    return (R,phi,Z)

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 _update_strain(self):
     self.e = (self.exy - self.eyx) / 2
     self.k = (self.exx + self.eyy) * 1000000. / 2
     self.strain = scipy.sqrt((self.exx - self.eyy) * (self.exx - self.eyy) + (self.exy + self.eyx) * (self.exy + self.eyx)) * 1000000.
     self.k_max = self.k + self.strain / 2
     self.k_min = self.k - self.strain / 2
     self.az = scipy.degrees(2 * scipy.arctan2(self.exy + self.eyx, self.eyy - self.exx))

def north_direction(lat):
    '''get the north direction relative to image positive y coordinate'''
    dlatdx = nd.filters.sobel(lat,axis=1,mode='constant',cval=sp.nan) #gradient in x-direction
    dlatdy = nd.filters.sobel(lat,axis=0,mode='constant',cval=sp.nan)
    ydir = lat[-1,0] -lat[0,0] # check if latitude is ascending or descending in y axis
    # same step might have to be done with x direction.
    return sp.arctan2(dlatdx,dlatdy*sp.sign(ydir) )*180/sp.pi

def xyz2longlat(x,y,z):
    """ converts cartesian x,y,z coordinates into spherical longitude and latitude """
    r = sc.sqrt(x*x + y*y + z*z)
    long = sc.arctan2(y,x)
    d = sc.sqrt(x*x + y*y)
    lat = sc.arcsin(z/r)
    return long*deg, lat*deg, r

    def two_d_ellipse(self,proj_vars,p,**kwargs):
        """Return the 2d projection as a matplotlib Ellipse object for the given p values
        Parameters
        ----------
        proj_vars : array that is 1 for the projection dimension, and 0 other wise
                    i.e. array([0,0,1,0,1]) will project 5d ellipsoid onto the plane
                    span by the 3rd and 5th variable.
        p         : the percent of points contained in the ellipsoid, either a single
                     value of a list of values i.e. 0.68 or [0.68,0.955],

                     if p is a list then a list of Ellipse objects will be returned, one for each p value
        Keywords
        --------
        kwargs : keywords to be passed into the matplotlib Ellipse object

        Return
        ------
        ells : matplotlib Ellipse object
        """
        mu,u,s=self.proj(proj_vars) #get the mean, eigenvectors, and eigenvales for projected array
        try: #if a list get the length
            l=len(p)
        except: #if not then make it a list of length 1
            l=1
            p=[p]
        invp=st.chi.ppf(p,self.dim) #scale it using a chi distribution (see, now we scale it)
        angle=rad2deg(arctan2(u[0,1],u[0,0])) #angle the first eignevector makes with the x-axis
        ells=[] #list to hold the Ellipse objects
        for i in invp:
            ells.append(Ellipse(xy=mu,width=s[0]*i*2,height=s[1]*i*2,angle=angle,**kwargs))#make the Ellipse objects, the *2 is needed since Ellipse takes the full axis vector
        if l==1: #if only one p values was given return the Ellipse object (not as a list)
            return ells[0]
        else: #else return the list of Ellipse objects
            return ells

def resolve_tri(A,B,a,b,up=True):
    AB=A-B
    c=l.norm(AB)
    aa=s.arctan2(AB[1],AB[0])
    bb=s.arccos((b**2+c**2-a**2)/(2*b*c))
    if up: return B+b*s.array((s.cos(aa+bb),s.sin(aa+bb)))
    else: return B+b*s.array((s.cos(aa-bb),s.sin(aa-bb)))

 def __init__(self, output='out', input='in', \
              mag=None, phase=None, coh=None, \
              freqlim=[], maglim=[], phaselim=[], \
              averaged='not specified', \
              seedfreq=-1, seedphase=0,
              labels=[], legloc=-1, compin=[]):
     self.output = output
     self.input = input
     if len(compin) > 0:
        if mag is None:
           self.mag = squeeze(colwise(abs(compin)))
        if phase is None:
           self.phase = squeeze(colwise(arctan2(imag(compin),real(compin))*180.0/pi))
     else:
         self.mag = squeeze(mag)
         self.phase = squeeze(phase)
     self.coh = coh
     self.averaged = averaged
     self.seedfreq = seedfreq
     self.seedphase = seedphase
     self.freqlim = freqlim
     self.maglim = maglim
     self.phaselim = phaselim
     self.labels = labels
     self.legloc = legloc

    def thinningEdges(self):
        self.gradDir = scipy.arctan2(self.gradDY, self.gradDX)

        self.gradCopy = self.grad.copy();
        for currRow in range(1, self.width-1):
            for currCol in range(1, self.height-1):
                up = currCol - 1;
                down = currCol + 1;
                left = currRow - 1;
                right = currRow + 1;
                error = 22.5
                
                if self.gradDir[currRow][currCol] >= 0 and self.gradDir[currRow][currCol] < 0 + error or \
                    self.gradDir[currRow][currCol] >= 360 and self.gradDir[currRow][currCol] < 360 - error or \
                    self.gradDir[currRow][currCol] >= 180 - error and self.gradDir[currRow][currCol] < 180 + error :
	                if self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][down] or \
                            (self.gradCopy[currRow][currCol] <= self.gradCopy[currRow][up]):
                            self.grad[currRow][currCol] = 0
                elif self.gradDir[currRow][currCol] >= 45 - error and self.gradDir[currRow][currCol] < 45 + error or \
                    self.gradDir[currRow][currCol] >= 135 - error and self.gradDir[currRow][currCol] < 135 + error:
                    if (self.gradCopy[currRow][currCol] <= self.gradCopy[left][down]) or \
                        (self.gradCopy[currRow][currCol] <= self.gradCopy[right][up]):
                        self.grad[currRow][currCol] = 0
                elif self.gradDir[currRow][currCol] >= 90 - error and self.gradDir[currRow][currCol] < 90 + error or \
                    self.gradDir[currRow][currCol] >= 270 - error and self.gradDir[currRow][currCol] < 270 + error:
                    if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][currCol]) or \
                        (self.gradCopy[currRow][currCol] <= self.gradCopy[left][currCol]):
                        self.grad[currRow][currCol] = 0
 	        else: ## for angles in second and forth quadrant
                    if (self.gradCopy[currRow][currCol] <= self.gradCopy[right][down]) or \
                        (self.gradCopy[currRow][currCol] <= self.gradCopy[left][up]):
                        self.grad[currRow][currCol] = 0

def rotation_matrix_from_cross_prod(a,b):
    """
    Returns the rotation matrix which rotates the
    vector :samp:`a` onto the the vector :samp:`b`.
    
    :type a: 3 sequence of :obj:`float`
    :param a: Vector to be rotated on to :samp:`{b}`.
    :type b: 3 sequence of :obj:`float`
    :param b: Vector.
    :rtype: :obj:`numpy.array`
    :return: 3D rotation matrix.
    """

    crs = np.cross(a,b)
    dotProd = np.dot(a,b)
    crsNorm = sp.linalg.norm(crs)
    eps = sp.sqrt(sp.finfo(a.dtype).eps)
    r = sp.eye(a.size, a.size, dtype=a.dtype)
    if (crsNorm > eps):
        theta = sp.arctan2(crsNorm, dotProd)
        r = axis_angle_to_rotation_matrix(crs, theta)
    elif (dotProd < 0):
        r = -r
    
    return r

def extract_hog(img,ROI):
	#print("first")
	#cellRows = cellCols = 5
	#binCount = 4
#	BlockRowCells = 3
#	BlockColCells = 3
	orientations=8
	pixels_per_cell=(5, 5)#5,5 - 0.9844
	cells_per_block=(3, 3)#3,3
	img = resize(img,(50,50))
	image = rgb2gray(img)

	image = np.atleast_2d(image)
	#hist = hog(img,binCount,(cellCols,cellRows),(BlockRowCells,BlockColCells))
	#hist = np.divide(hog,np.linalg.norm(hog))
	gx = roll(image, 1, axis = 1) - roll(image, -1, axis = 1)
	gx[:,0],gx[:,-1] = 0,0;
	
	gy = roll(image, 1, axis = 0) - roll(image, -1, axis = 0)
	gy[-1,:],gy[0,:] = 0,0;
	matr = np.square(gx) + np.square(gy)
	matr = np.sqrt(matr)
	orientation = arctan2(gy, (gx + 1e-15)) * (180 / pi) + 90

	imx, imy = image.shape
	cx, cy = pixels_per_cell
	bx, by = cells_per_block

	n_cellsx = int(np.floor(imx // cx))  # number of cells in i
	n_cellsy = int(np.floor(imy // cy))  # number of cells in j

    
	or_hist = np.zeros((n_cellsx, n_cellsy, orientations))
	for i in range(orientations):
		
	
		condition = orientation < 180 / orientations * (i + 1)

		tmp = np.where(condition,orientation, 0)
		condition = orientation >= 180 / orientations * i
		tmp = np.where(condition,tmp, 0)
	
		cond2 = tmp > 0
		temp_mag = np.where(cond2, matr, 0)

		or_hist[:,:,i] = uniform_filter(temp_mag, size=(cx, cy))[cx/2::cx, cy/2::cy].T
	numbx = (n_cellsx - bx) + 1
	numby = (n_cellsy - by) + 1
	normb = np.zeros((numbx, numby, bx, by, orientations))

	for i in range(numbx):
		for j in range(numby):
			block = or_hist[i:i + bx, j:j + by, :]
			eps = 1e-5
			normb[i, j, :] = block / sqrt(block.sum() ** 2 + eps)

    
   
	return normb.ravel()

    def r1(self):        
        """returns cylindrical coordinate along second dimension

        Returns:
            numpy array of cylindrical coordinates in radians

        """
        return scipy.arctan2(self.unit[1],self.unit[0])

def getG(X,Y):
    """
    This function calculates the angular components of a state
    :param X:   X Points
    :param Y:   Y Points
    :return:    Arctan(Y/X)
    """
    return sp.cos(sp.arctan2(Y, X))