def compute_density(lon, lat, xx, yy, use_hsa):
    min_lon = np.min(xx)
    max_lon = np.max(xx)

    min_lat = np.min(yy)
    max_lat = np.max(yy)

    selected = filter_array(lon, lat, min_lon, min_lat, max_lon, max_lat)

    lon = lon[selected]
    lat = lat[selected]

    samples = np.dstack([lon, lat])
    assert samples.shape[0] == 1
    samples = samples.reshape(samples.shape[1:])
    support = np.squeeze(np.dstack([xx, yy]))

    bwlist = np.array([cpu_ref.approx_bandwidth(support[:, k])
                       for k in range(support.shape[1])])

    pdf = np.zeros(support.shape[0], dtype=np.float64)

    if samples.size:
        print(samples.shape, samples.dtype)

        start_time = timer()
        if use_hsa:
            print("HSA".center(80, '-'))
            hsa_imp.hsa_multi_kde(support, samples, bwlist, pdf)
        else:
            print("CPU".center(80, '-'))
            cpu_ref.multi_kde_seq(support, samples, bwlist, pdf)
        end_time = timer()
        print("duration", "{0:0.2f} seconds".format(end_time - start_time))
    return pdf, samples.size

def visualize_depth_image(data):

    data[data == 0.0] = np.nan

    maxdepth = np.nanmax(data)
    mindepth = np.nanmin(data)
    data = data.copy()
    data -= mindepth
    data /= (maxdepth - mindepth)

    gray = np.zeros(list(data.shape) + [3], dtype=data.dtype)
    data = (1.0 - data)
    gray[..., :3] = np.dstack((data, data, data))

    # use a greenish color to visualize missing depth
    gray[np.isnan(data), :] = (97, 160, 123)
    gray[np.isnan(data), :] /= 255

    gray = exposure.equalize_hist(gray)

    # set alpha channel
    gray = np.dstack((gray, np.ones(data.shape[:2])))
    gray[np.isnan(data), -1] = 0.5

    return gray * 255

def interp(pic,flow):
    ys=np.arange(pic.shape[0]*pic.shape[1])/pic.shape[1]
    ud=(flow[:,:,0].reshape(-1)+ys)%pic.shape[0]
    xs=np.arange(pic.shape[0]*pic.shape[1])%pic.shape[1]
    lr=(flow[:,:,1].reshape(-1)+xs)%pic.shape[1]

    u=np.int32(np.floor(ud))
    d=np.int32(np.ceil(ud))%pic.shape[0]
    udiffs=ud-u
    udiffs=np.dstack((udiffs,udiffs,udiffs))
    l=np.int32(np.floor(lr))
    r=np.int32(np.ceil(lr))%pic.shape[1]
    ldiffs=lr-l
    ldiffs=np.dstack((ldiffs,ldiffs,ldiffs))

    ul=pic[u,l,:]
    ur=pic[u,r,:]
    dl=pic[d,l,:]
    dr=pic[d,r,:]


    udl=ul*(1-udiffs)+dl*udiffs
    udr=ur*(1-udiffs)+dr*udiffs
    ans=np.zeros(pic.shape)
    ans[ys,xs,:]=udl*(1-ldiffs)+udr*ldiffs
    return ans

def _array_from_bitmap(bitmap):
    """Convert a FreeImage bitmap pointer to a numpy array.

    """
    dtype, shape = FI_TYPES.get_type_and_shape(bitmap)
    array = _wrap_bitmap_bits_in_array(bitmap, shape, dtype)
    # swizzle the color components and flip the scanlines to go from
    # FreeImage's BGR[A] and upside-down internal memory format to something
    # more normal

    def n(arr):
        return arr[..., ::-1].T
    if len(shape) == 3 and _FI.FreeImage_IsLittleEndian() and \
       dtype.type == numpy.uint8:
        b = n(array[0])
        g = n(array[1])
        r = n(array[2])
        if shape[0] == 3:
            return numpy.dstack((r, g, b))
        elif shape[0] == 4:
            a = n(array[3])
            return numpy.dstack((r, g, b, a))
        else:
            raise ValueError('Cannot handle images of shape %s' % shape)

    # We need to copy because array does *not* own its memory
    # after bitmap is freed.
    return n(array).copy()

    def getParallelPatches(self, parallels, **kwargs):
        """Get parallel lines in matplotlib format.

        Parallel lines in conics are straight, appropriate
        matplotlib.patches will be returned.

        Args:
            meridians: list of rectascensions
            **kwargs: matplotlib.collection.LineCollection parameters

        Returns:
            matplotlib.LineCollection
        """

        # remove duplicates
        parallels_ = np.unique(parallels % 360)

        # the outer boundaries need to be duplicated because the same
        # parallel appear on the left and the right side of the map
        if self.ra_0 < 180:
            outer = self.ra_0 - 180
        else:
            outer = self.ra_0 + 180
        parallels_ = np.array(list(parallels_) + [outer])

        from matplotlib.collections import LineCollection
        top = self.__call__(parallels_, 90)
        bottom = self.__call__(parallels_, -90)
        x_ = np.dstack((top[0], bottom[0]))[0]
        y_ = np.dstack((top[1], bottom[1]))[0]
        return LineCollection(np.dstack((x_, y_)), color='k', **kwargs)

  def testReturnPaddedImageWithNonZeroPadValue(self):
    for dtype in [np.int32, np.int64, np.float32, np.float64]:
      image = np.dstack([[[5, 6],
                          [9, 0]],
                         [[4, 3],
                          [3, 5]]]).astype(dtype)
      expected_image = np.dstack([[[255, 255, 255, 255, 255],
                                   [255, 255, 255, 255, 255],
                                   [255, 5, 6, 255, 255],
                                   [255, 9, 0, 255, 255],
                                   [255, 255, 255, 255, 255]],
                                  [[255, 255, 255, 255, 255],
                                   [255, 255, 255, 255, 255],
                                   [255, 4, 3, 255, 255],
                                   [255, 3, 5, 255, 255],
                                   [255, 255, 255, 255, 255]]]).astype(dtype)

      with self.session() as sess:
        padded_image = preprocess_utils.pad_to_bounding_box(
            image, 2, 1, 5, 5, 255)
        padded_image = sess.run(padded_image)
        self.assertAllClose(padded_image, expected_image)
        # Add batch size = 1 to image.
        padded_image = preprocess_utils.pad_to_bounding_box(
            np.expand_dims(image, 0), 2, 1, 5, 5, 255)
        padded_image = sess.run(padded_image)
        self.assertAllClose(padded_image, np.expand_dims(expected_image, 0))

    def load_from_filestore(self,directory,tablename="Untitled",limit=None,dtype=numpy.float32):
        """
        Not intended to be used often - loads individual files from a directory
        and stores them in a named table in the datastore - unfinished
        """
        hh=None
        for dirname, dirnames, filenames in os.walk(directory):
            num=0
            # print path to all subdirectories first.
            for subdirname in dirnames:
                print os.path.join(dirname, subdirname)    
            # import data and append to an image stack.
            imgs=[]
            names=[]
            for filename in filenames:
                print os.path.join(dirname, filename)
                try: 
                    imgs.append(numpy.loadtxt(os.path.join(dirname, filename),dtype=dtype))
                    names.append(os.path.join(dirname, filename))
                    num+=1
                    if num>=limit:
                        return numpy.dstack(imgs)        
                    #del(imgs[0])
                except: 
                    print 'failed'
                if hh==None:
                    hh=self.get_handle("/"+tablename,imgs[0])

                try: hh.append(imgs[-1],num,0)
                except: pass
                
            return numpy.dstack(imgs)

def _zigzag(xmin, xmax, ymin, ymax, nx, ny):
    # Create the vertices.
    x_range = numpy.linspace(xmin, xmax, nx)
    y_range = numpy.linspace(ymin, ymax, ny)
    nodes = numpy.dstack(numpy.meshgrid(x_range, y_range, numpy.array([0.0]))).reshape(
        -1, 3
    )

    # Create the elements (cells).
    # a = [i + j*nx]
    a = numpy.add.outer(numpy.array(range(nx - 1)), nx * numpy.array(range(ny - 1)))

    # [i + j*nx, i+1 + j*nx, i+1 + (j+1)*nx]
    elems0 = numpy.dstack([a, a + 1, a + nx + 1])
    # [i+1 + j*nx, i+1 + (j+1)*nx, i + (j+1)*nx] for "every other" element
    elems0[0::2, 1::2, 0] += 1
    elems0[1::2, 0::2, 0] += 1
    elems0[0::2, 1::2, 1] += nx
    elems0[1::2, 0::2, 1] += nx
    elems0[0::2, 1::2, 2] -= 1
    elems0[1::2, 0::2, 2] -= 1

    # [i + j*nx, i+1 + (j+1)*nx,  i + (j+1)*nx]
    elems1 = numpy.dstack([a, a + 1 + nx, a + nx])
    # [i + j*nx, i+1 + j*nx, i + (j+1)*nx] for "every other" element
    elems1[0::2, 1::2, 1] -= nx
    elems1[1::2, 0::2, 1] -= nx

    elems = numpy.vstack([elems0.reshape(-1, 3), elems1.reshape(-1, 3)])

    return nodes, elems

def raw_deinterleaver(input_queue, output_queue, plot_queue):
    '''
    Process for deinterleaving raw FFT data and plotting.
    '''
    signal.signal(signal.SIGINT, signal.SIG_IGN) # Ignore keyboard interrupt signal, parent process will handle.
    time.sleep(1)
    while 1:
        LCP = []
        RCP = []

        interleavedWindow = np.array(input_queue.get())
        if interleavedWindow == None:
            break

        index = 0

        even1 = (interleavedWindow[0::8] + interleavedWindow[1::8]*1j)
        odd1  = (interleavedWindow[2::8] + interleavedWindow[3::8]*1j)
        LCP   = np.reshape(np.dstack((even1, odd1)), (1,-1))

        even2 = (interleavedWindow[4::8] + interleavedWindow[5::8]*1j)
        odd2  = (interleavedWindow[6::8] + interleavedWindow[7::8]*1j)
        RCP   = np.reshape(np.dstack((even2, odd2)), (1, -1))

        #need to figure out here how to write out to the plotting function.

        output_queue.put((LCP,RCP))
        plot_queue.put((LCP,RCP))
    print 'raw_deinterleaver found poison pill'
    output_queue.put(None)

def findedges(inputtiles, parsenames):


    tiles = sutils.tile_parser(inputtiles, parsenames)

    xmin, xmax, ymin, ymax = sutils.get_range(tiles)

    zoom = sutils.get_zoom(tiles)
    # zoom = inputtiles[0, -1]

    # make an array of shape (xrange + 3, yrange + 3)
    burn = sutils.burnXYZs(tiles, xmin, xmax, ymin, ymax)

    # Create the indixes for rolling
    idxs = sutils.get_idx()

    # Using the indices to roll + stack the array, find the minimum along the rolled / stacked axis
    xys_edge = (np.min(np.dstack((
        np.roll(np.roll(burn, i[0], 0), i[1], 1) for i in idxs
        )), axis=2) - burn)

    # Set missed non-tiles to False
    xys_edge[burn == False] = False

    # Recreate the tile xyzs, and add the min vals
    xys_edge = np.dstack(np.where(xys_edge))[0]
    xys_edge[:, 0] += xmin - 1
    xys_edge[:, 1] += ymin - 1

    # Return the edge array

    return np.append(xys_edge, np.zeros((xys_edge.shape[0], 1), dtype=np.uint8) + zoom, axis=1)

    def applyFilter5(self):
        # Run the functions
        img = np.copy(self.curRoadRGB).astype(np.uint8)
        gradx = self.abs_sobel_thresh(img, orient='x', thresh=(25, 100))
        grady = self.abs_sobel_thresh(img, orient='y', thresh=(50, 150))
        magch = self.mag_thresh(img, sobel_kernel=9, mag_thresh=(30, 150))
        dirch = self.dir_threshold(img, sobel_kernel=15, thresh=(0.5, 1.3))
        sch = self.hls_s(img, thresh=(20, 80))
        hch = self.hls_h(img, thresh=(130, 175))

        # create the Red filter
        rEdgeDetect = img[:, :, 0] / 4
        rEdgeDetect = 255 - rEdgeDetect
        rEdgeDetect[(rEdgeDetect > 220)] = 0

        # Output "masked_lines" is a single channel mask
        shadow = np.zeros_like(dirch).astype(np.uint8)
        shadow[(sch > 0) & (hch > 0)] = 128

        # build the combination
        combined = np.zeros_like(dirch).astype(np.uint8)
        combined[(rEdgeDetect > 192) & (rEdgeDetect < 205) & (sch > 0)] = 35
        self.curRoadEdge = combined

        # build diag screen if in debug mode
        if self.debug:
            # create diagnostic screen 1-3
            # creating a blank color channel for combining
            ignore_color = np.copy(gradx) * 0
            self.diag1 = np.dstack((rEdgeDetect, gradx, grady))
            self.diag2 = np.dstack((ignore_color, magch, dirch))
            self.diag3 = np.dstack((sch, shadow, hch))
            self.diag4 = np.dstack((combined, combined, combined)) * 4

    def soc(self, count):
        """Get a SoC bus trace"""

        self.write_reg(0x231, 0)
        self.write_reg(0x231, 1)
        time.sleep(0.1)

        # Single Data Rate (SDR) signals
        sdr0 = self.read_regs(0x2000, count)
        sdr1 = sdr0
        sdr = numpy.dstack((sdr1, sdr0))[0].reshape(len(sdr0)+len(sdr1))

        print sdr0
        print sdr1
        print sdr

        # Registered copies of SDR signals
        reg0 = self.read_regs(0x2000, count)
        reg1 = reg0
        reg = numpy.dstack((reg1, reg0))[0].reshape(len(reg0)+len(reg1))

        print reg0
        print reg1
        print reg

        # Double Data Rate DDR signals
        ddr0 = self.read_regs(0x3000, count)
        ddr1 = self.read_regs(0x3800, count)
        ddr = numpy.dstack((ddr1, ddr0))[0].reshape(len(ddr0)+len(ddr1))

        print ddr0
        print ddr1
        print ddr

        return sdr, reg, ddr

    def convert(self, components):
        if self.components == components:
            return self

        hasAlpha = self.components in (2,4)
        needAlpha = components in (2,4)

        if hasAlpha:
            alpha = self._data[...,-1]
            color = self._data[...,:-1]
        else:
            alpha = None
            color = self._data

        isMono = self.components in (1,2)
        toMono = components in (1,2)

        if isMono and not toMono:
            color = np.dstack((color, color, color))
        elif toMono and not isMono:
            color = np.sum(color.astype(np.uint16), axis=-1) / 3
            color = color.astype(np.uint8)[...,None]

        if needAlpha and alpha is None:
            alpha = np.zeros_like(color[...,:1]) + 255

        if needAlpha:
            data = np.dstack((color, alpha))
        else:
            data = color

        return type(self)(data = data)

def compute_maximum_ts_map(ts_map_results):
    """
    Compute maximum TS map across a list of given `TSMapResult` objects.

    Parameters
    ----------
    ts_map_results : list
        List of `TSMapResult` objects.

    Returns
    -------
    TS : `TSMapResult`
        `TSMapResult` object.
    """

    # Get data
    ts = np.dstack([result.ts for result in ts_map_results])
    niter = np.dstack([result.niter for result in ts_map_results])
    amplitude = np.dstack([result.amplitude for result in ts_map_results])
    scales = [result.scale for result in ts_map_results]

    # Set up max arrays
    ts_max = np.max(ts, axis=2)
    scale_max = np.zeros(ts.shape[:-1])
    niter_max = np.zeros(ts.shape[:-1])
    amplitude_max = np.zeros(ts.shape[:-1])

    for i, scale in enumerate(scales):
        index = np.where(ts[:, :, i] == ts_max)
        scale_max[index] = scale
        niter_max[index] = niter[:, :, i][index]
        amplitude_max[index] = amplitude[:, :, i][index]

    return TSMapResult(ts=ts_max, niter=niter_max, amplitude=amplitude_max,
                       morphology=ts_map_results[0].morphology, scale=scale_max)

def objects_walls_algorithm(cmd, world, k1=4.2, k2=4.4):
	x, y = np.mgrid[0:world.xdim:.1, 0:world.ydim:.1]

	# Calculate naive distribution
	naive_dist = naive_algorithm(cmd, world)
	naive_vals = naive_dist.pdf(np.dstack((x, y)))

	# Find Distance to closest object
	ref_dists = {ref : np.sqrt((x - ref.center[0])**2 + (y - ref.center[1])**2) for ref in world.references}
	min_ref_dists = np.min(np.dstack(ref_dists[ref] for ref in ref_dists), axis=2)

	# Difference between distance to closest object and object reference in command
	ref_distance_diff = ref_dists[cmd.reference] - min_ref_dists

	ref_distance_vals = expon.pdf(ref_distance_diff, scale=k1)

	# Find distance to nearest wall
	min_wall_dists = np.min(np.dstack((x, y, world.xdim - x, world.ydim - y)), axis=2)

	# Difference between distance to closest wall and object reference in command
	wall_distance_diff = ref_dists[cmd.reference] - min_wall_dists
	wall_distance_diff[wall_distance_diff < 0] = 0

	wall_distance_vals = expon.pdf(wall_distance_diff, scale=k2)

	mean_prob = naive_vals*ref_distance_vals*wall_distance_vals
	loc = np.where(mean_prob == mean_prob.max())
	mean = 0.1*np.array([loc[0][0], loc[1][0]])

	mv_dist = multivariate_normal(mean, naive_dist.cov)

	return mv_dist

def loadWeather(m):
    h5f = h5py.File('c:\\tmp\\data.h5','r')
    #randlist = np.random.randint(0,h5f['weather_data'].shape[0],2920)
    randlist = np.arange(161686,164606)
    randlist_usort = list(np.sort(np.array(list(set(randlist)))))
    weather_points = h5f['weather_points'][:]
    [coord] = np.dstack(m(weather_points['lon'],weather_points['lat']))
    weather_data = h5f['weather_data'][randlist_usort]
    h5f.close()

    minx, maxx, miny, maxy = (27686.0,848650.0,56061.0,645608.0)
    grid_x, grid_y = np.mgrid[minx:maxx:10000, miny:maxy:10000]
    [vluPoints]=np.dstack(([grid_x.reshape(grid_x.shape[0]*grid_x.shape[1],1)],[grid_y.reshape(grid_y.shape[0]*grid_y.shape[1],1)]))

    tree = cKDTree(coord)

    d, inds = tree.query(vluPoints, k = 10)
    _,indsNN = tree.query(vluPoints, k = 1)
    w = 1.0 / d**2
    su = np.sum(w, axis=1)
    values_idw = np.empty((weather_data.shape[0],grid_x.shape[0]*grid_x.shape[1]),
                           dtype=[('wsp', np.float32),('wdir', np.float32),('hs', np.float32),('light', np.uint32)])
    for d in range(0,values_idw.shape[0]):
        values_idw[d,:]['wsp'] = weather_data[d,:]['wsp'][indsNN] / 10.
        values_idw[d,:]['wdir'] = np.deg2rad(weather_data[d,:]['wdir'][indsNN])
        values_idw[d,:]['hs'] = (np.sum(w * (weather_data[d,:]['hs'][inds]), axis=1) / su)/10.
        values_idw[d,:]['light'] = weather_data[d,:]['light'][indsNN]

    values_idw.shape =((values_idw.shape[0],grid_x.shape[0],grid_x.shape[1]))

    return values_idw

def mfoldX(I, L, m, maxk):
    # I is the trainset
    # L is the Training Labels
    # m is the number of folds
    # maxk is the largest value of k we wish to test
    # first thing to acomplish is to randomly divide the data into m parts
    indices = np.random.permutation(I.shape[0]) # Creates a randomized index vector
    jump = round(len(L) / m) # Calculates the number of rows to jump for each fold
    # The following code cuts up our indices vector into m parts
    # I intended it to handle cases were m % I != 0 but it doesn't so rows(I) needs to be divisible by m
    I_index = indices[:jump]
    L_index = indices[:jump]
    for n in range(1, m - 1): # Iterats through the folds
        # stacks fold into a third diminsion
        I_index = np.dstack((I_index, indices[n * jump:(n + 1) * jump])) # a random index for the images
        L_index= np.dstack((L_index, indices[n * jump:(n + 1) * jump])) # a random index for the labels
    I_index = np.dstack((I_index, indices[(m-1) * jump:]))
    L_index = np.dstack((L_index, indices[(m-1) * jump:]))
    # Yea I'm pretty sure that wasn't necessary. I could have just used jump and the indices
    # but I'm not changing it now
    #
    # now data should be all nice and divided up we need to do something else
    error = np.zeros(maxk) # Creates a array to store our error rates
    for n in range(0, m): # Loop through each fold
        mask = np.ones(m,dtype=bool)
        mask[n]=0
        notn = np.arange(0,m)[mask] # Creates a series of number except for the m we are currently on
        # Creates a Ipt variable that has all
        Ipt = I[I_index[:,:,notn].reshape(((m-1)*I_index.shape[1]))]
        Lpt = L[I_index[:,:,notn].reshape(((m-1)*I_index.shape[1]))]
        label,near = KNN(Ipt,Lpt ,I[I_index[:,:,n].reshape(I_index.shape[1])],10)
        for k in range(10):
            error[k] = error[k] + sum((label[k] != L[L_index[:,:,n]])[0])
    error = error / (len(L))
    return error

def upsample(arr, n):
    z = numpy.zeros(len(arr)) # upsample with values
    for i in range(int(int(n-1)/2)): #TODO 
        arr = numpy.dstack((z,arr))
    for i in range(int(int( n )/2)):#TODO 
        arr = numpy.dstack((arr,z))
    return arr.reshape((1,-1))[0]

def test_optimize():
    lena = scipy.misc.lena()
    blurred1 = scipy.ndimage.gaussian_filter(lena, 3.0)
    blurred2 = scipy.ndimage.gaussian_filter(blurred1, 1.5)
    fp = af.FocusPoint(0, 0, 10, 10)
    stack = np.dstack((blurred2, np.dstack((blurred1, lena))))
    assert af.optimize(stack, fp, af.cost_sobel) == 2

  def testFlipWhenProbIsOne(self):
    numpy_image = np.dstack([[[5., 6.],
                              [9., 0.]],
                             [[4., 3.],
                              [3., 5.]]])
    dim0_flipped = np.dstack([[[9., 0.],
                               [5., 6.]],
                              [[3., 5.],
                               [4., 3.]]])
    dim1_flipped = np.dstack([[[6., 5.],
                               [0., 9.]],
                              [[3., 4.],
                               [5., 3.]]])
    dim2_flipped = np.dstack([[[4., 3.],
                               [3., 5.]],
                              [[5., 6.],
                               [9., 0.]]])
    image = tf.convert_to_tensor(numpy_image)

    with self.test_session():
      actual, is_flipped = preprocess_utils.flip_dim([image], prob=1, dim=0)
      self.assertAllEqual(dim0_flipped, actual.eval())
      self.assertAllEqual(True, is_flipped.eval())
      actual, is_flipped = preprocess_utils.flip_dim([image], prob=1, dim=1)
      self.assertAllEqual(dim1_flipped, actual.eval())
      self.assertAllEqual(True, is_flipped.eval())
      actual, is_flipped = preprocess_utils.flip_dim([image], prob=1, dim=2)
      self.assertAllEqual(dim2_flipped, actual.eval())
      self.assertAllEqual(True, is_flipped.eval())