def numpyImplicit(self, tmin, tmax,nPlotInc):
        g = self.grid
        k = self.k         #Diffusivity
        r = self.r         #Numerical Fourier number
        theta =self.theta  #Parameter for implicitness: theta=0.5 Crank-Nicholson, theta=1.0 fully implicit
        u, x, dx  = g.u, g.x, g.dx
        xmin, xmax = g.xmin, g.xmax
        
        dt=r*dx**2/k     #Compute timestep based on Fourier number, dx and diffusivity
        print 'timestep = ',dt

        m=round((tmax-tmin)/dt) # Number of temporal intervals
        print 'm = ',m
        print 'implicit solver with r=',r
        print 'and theta = ',theta
        time=np.linspace(tmin,tmax,m)

        #Create matrix for sparse solver. Solve for interior values only (nx-1)
        diagonals=np.zeros((3,g.nx-1))   
        diagonals[0,:] = -r*theta                       #all elts in first row is set to 1
        diagonals[1,:] = 1+2.0*r*theta  
        diagonals[2,:] = -r*theta 
        As = sc.sparse.spdiags(diagonals, [-1,0,1], g.nx-1, g.nx-1,format='csc') #sparse matrix instance

        #Crete rhs array
        d=np.zeros((g.nx-1,1),'d')
        
#        nPlotInc=5 #output every nPlotInc iteration
        i = 0        #iteration counter

                #Plot initial solution
        fig = plt.figure()
        ax=fig.add_subplot(111)
        Curve, = ax.plot( x, u[:], '-')
        ax.set_xlim([xmin,xmax])
#        ax.set_ylim([umin,umax])
        plt.xlabel('x')
        plt.ylabel('Velocity')

        plt.ion()
        plt.show()

        #Advance in time an solve tridiagonal system for each t in time
        for t in time:
            i+=1
            d[:] = u[1:-1]+r*(1-theta)*(u[0:-2]-2*u[1:-1]+u[2:])  
            d[0] += r*theta*u[0]
            w = sc.sparse.linalg.spsolve(As,d) #theta=sc.linalg.solve_triangular(A,d)
            u[1:-1] = w[:,None]
           
            if (np.mod(i,nPlotInc)==0): #output every nPlotInc iteration
                Curve.set_ydata(u)
                plt.pause(.005)
                plt.title( 'step = %3d; t = %f' % (i,t ) )
        
        g.u=u
        
        plt.pause(1)
        plt.ion()
        plt.close()

def show_trajectory(target, xc, yc):  # pragma: no cover
    plt.clf()
    plot_arrow(target.x, target.y, target.yaw)
    plt.plot(xc, yc, "-r")
    plt.axis("equal")
    plt.grid(True)
    plt.pause(0.1)

def plot_sample(m):
    for seq_to_plot in range(N_experiments):
        fig = plt.figure(seq_to_plot)
        fig.clf()
        if plot == 'states':
            axs = plot_latent_compartment_state(t,
                                                true_model.data_sequences[seq_to_plot].latent,
                                                true_model.data_sequences[seq_to_plot].states,
                                                true_model.population.neurons[0].compartments[0])
            plot_latent_compartment_state(t,
                                          m.data_sequences[seq_to_plot].latent,
                                          m.data_sequences[seq_to_plot].states,
                                          m.population.neurons[0].compartments[0],
                                          axs=axs, colors=['r'])
        elif plot == 'currents':
            axs = plot_latent_compartment_V_and_I(t,
                                                  true_model.data_sequences[seq_to_plot],
                                                  true_model.population.neurons[0].compartments[0],
                                                  true_model.observation.observations[0])
            plot_latent_compartment_V_and_I(t,
                                            m.data_sequences[seq_to_plot],
                                            m.population.neurons[0].compartments[0],
                                            m.observation.observations[0],
                                          axs=axs, colors=['r'])
        fig.suptitle('Iteration: %d' % i['i'])
    i['i'] += 1
    plt.pause(0.1)

  def write(self, timestamps, actualValues, predictedValues,
            predictionStep=1):

    assert len(timestamps) == len(actualValues) == len(predictedValues)

    # We need the first timestamp to initialize the lines at the right X value,
    # so do that check first.
    if not self.linesInitialized:
      self.initializeLines(timestamps)

    for index in range(len(self.names)):
      self.dates[index].append(timestamps[index])
      self.convertedDates[index].append(date2num(timestamps[index]))
      self.actualValues[index].append(actualValues[index])
      self.predictedValues[index].append(predictedValues[index])

      # Update data
      self.actualLines[index].set_xdata(self.convertedDates[index])
      self.actualLines[index].set_ydata(self.actualValues[index])
      self.predictedLines[index].set_xdata(self.convertedDates[index])
      self.predictedLines[index].set_ydata(self.predictedValues[index])

      self.graphs[index].relim()
      self.graphs[index].autoscale_view(True, True, True)

    plt.draw()
    plt.legend(('actual','predicted'), loc=3)
    plt.pause(0.00000001)

def plot_data(x, t):
    plt.figure()
    plt.scatter(x, t, edgecolor='b', color='w', marker='o')
    plt.xlabel('x')
    plt.ylabel('t')
    plt.title('Data')
    plt.pause(.1)

def plot(model, div = 8):
    ecg, diff, filt = preprocess(div)

    # e = np.atleast_2d(eorig).T
    # sube = np.atleast_2d(eorig[0:3000]).T
    e = diff[:10000].reshape(-1,1)
    # e = np.column_stack((diff,filt))
    sube = e[:3000]

    plt.clf()
    plt.subplot(411)
    plt.imshow(model.transmat_,interpolation='nearest', shape=model.transmat_.shape)
    ax = plt.subplot(412)
    plt.plot(e[0:3000])
    plt.plot(ecg[:3000])
    # plt.imshow(model.emissions,interpolation='nearest', shape=model.emissions.shape)
    plt.subplot(413, sharex = ax)
    model.algorithm = 'viterbi'
    plt.plot(model.predict(sube))
    model.algorithm = 'map'
    plt.plot(model.predict(sube))
    plt.subplot(414, sharex = ax)
    samp = model.sample(3000)[0]
    plt.plot(samp)
    plt.plot(np.cumsum(samp[:,0]))
    plt.show()
    plt.pause(1)

def show_vector(dx, dy, arr = None, w=None, h=None, skip=6, holdon=False):
    if w is None or h is None:
        h = dx.shape[0]
        w = dx.shape[1]

    import matplotlib.pyplot as plt
    x, y = np.meshgrid(np.linspace(0, w, w), np.linspace(0, h, h))

    ax = plt.axes(xlim=(0, w), ylim=(0, h))
    line, = plt.plot(0,0,'ro')
    plt.ion()
    plt.ylim([0, h])
    if skip is None:
        ax.quiver(x, y, dx, dy)
    else:
        skip = (slice(None, None, skip), slice(None, None, skip))
        ax.quiver(x[skip], y[skip], dx[skip], dy[skip])
        if arr is not None:
            plt.imshow(arr, cmap=plt.cm.Greys_r)


    if holdon is True:
        plt.draw()
        plt.pause(0.0001)
        return line, plt
    else:
        plt.show()

def view_patches(Yr, A, C, b, f, d1, d2):
    """
    view spatial and temporal components
    """

    nr, T = C.shape
    nA2 = np.sum(np.array(A.todense()) ** 2, axis=0)
    Y_r = np.array(spdiags(1 / nA2, 0, nr, nr) * (A.T * np.matrix(Yr - b[:, np.newaxis] * f[np.newaxis])) + C)
    fig = plt.figure()

    for i in range(nr + 1):
        if i < nr:
            ax1 = fig.add_subplot(2, 1, 1)
            plt.imshow(np.reshape(np.array(A.todense())[:, i], (d1, d2), order="F"), interpolation="None")
            ax1.set_title("Spatial component " + str(i + 1))
            ax2 = fig.add_subplot(2, 1, 2)
            plt.plot(np.arange(T), np.squeeze(np.array(Y_r[i, :])))
            plt.plot(np.arange(T), np.squeeze(np.array(C[i, :])))
            ax2.set_title("Temporal component " + str(i + 1))
            ax2.legend(labels=["Filtered raw data", "Inferred trace"])
            plt.pause(4)
            # plt.waitforbuttonpress()
            fig.delaxes(ax2)
        else:
            ax1 = fig.add_subplot(2, 1, 1)
            plt.imshow(np.reshape(b, (d1, d2), order="F"), interpolation="None")
            ax1.set_title("Spatial background background")
            ax2 = fig.add_subplot(2, 1, 2)
            plt.plot(np.arange(T), np.squeeze(np.array(f)))
            ax2.set_title("Temporal background")

    def visCC(self):
        """fix me.... :/"""

        """to visualize the neighbours"""
        if isVisualize:
            fig888 = plt.figure()
            ax     = plt.subplot(1,1,1)

        """ visualization, see if connected components make sense"""
        s111,c111 = connected_components(sparsemtx) #s is the total CComponent, c is the label
        color     = np.array([np.random.randint(0,255) for _ in range(3*int(s111))]).reshape(s111,3)
        fig888    = plt.figure(888)
        ax        = plt.subplot(1,1,1)
        # im = plt.imshow(np.zeros([528,704,3]))
        for i in range(s111):
            ind = np.where(c111==i)[0]
            print ind
            for jj in range(len(ind)):
                startlimit = np.min(np.where(x[ind[jj],:]!=0))
                endlimit = np.max(np.where(x[ind[jj],:]!=0))
                # lines = ax.plot(x[ind[jj],startlimit:endlimit], y[ind[jj],startlimit:endlimit],color = (0,1,0),linewidth=2)
                lines = ax.plot(x[ind[jj],startlimit:endlimit], y[ind[jj],startlimit:endlimit],color = (color[i-1].T)/255.,linewidth=2)
                fig888.canvas.draw()
            plt.pause(0.0001) 
        plt.show()

def main():
    print("Start informed rrt star planning")

    # create obstacles
    obstacleList = [
        (5, 5, 0.5),
        (9, 6, 1),
        (7, 5, 1),
        (1, 5, 1),
        (3, 6, 1),
        (7, 9, 1)
    ]

    # Set params
    rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
                          randArea=[-2, 15], obstacleList=obstacleList)
    path = rrt.InformedRRTStarSearch(animation=show_animation)
    print("Done!!")

    # Plot path
    if show_animation:
        rrt.drawGraph()
        plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
        plt.grid(True)
        plt.pause(0.01)
        plt.show()

    def plotdata(self,new_values):
        # is  a valid message struct
        #print new_values

        self.x.append( float(new_values[0]))
        self.y.append( float(new_values[1]))
        self.z.append( float(new_values[2]))

        self.plotx.append( self.plcounter )

        self.line1.set_ydata(self.x)
        self.line2.set_ydata(self.y)
        self.line3.set_ydata(self.z)

        self.line1.set_xdata(self.plotx)
        self.line2.set_xdata(self.plotx)
        self.line3.set_xdata(self.plotx)

        self.fig.canvas.draw()
        plt.pause(0.0001)

        self.plcounter = self.plcounter+1

        if self.plcounter > self.rangeval:
          self.plcounter = 0
          self.plotx[:] = []
          self.x[:] = []
          self.y[:] = []
          self.z[:] = []

 def start_display(self):
     # improve spacing between graphs 
     self._fig.tight_layout()
     #  Do not forget this call, it displays graphs, plt.draw(),  
     #  self._fig.canvas.draw(), cannot replace it 
     plt.pause(0.001)   
     self.update_display()  

def test_mge_vcirc():
    """
    Usage example for mge_vcirc()
    It takes a fraction of a second on a 2GHz computer
    
    """    
    import matplotlib.pyplot as plt
    
    # Realistic MGE galaxy surface brightness
    # 
    surf = np.array([39483, 37158, 30646, 17759, 5955.1, 1203.5, 174.36, 21.105, 2.3599, 0.25493])
    sigma = np.array([0.153, 0.515, 1.58, 4.22, 10, 22.4, 48.8, 105, 227, 525])
    qObs = np.array([0.57, 0.57, 0.57, 0.57, 0.57, 0.57, 0.57, 0.57, 0.57, 0.57])
    
    inc = 60. # Inclination in degrees
    mbh = 1e6 # BH mass in solar masses
    distance = 10. # Mpc
    rad = np.logspace(-1,2,25) # Radii in arscec where Vcirc has to be computed
    ml = 5.0 # Adopted M/L ratio
    
    vcirc = mge_vcirc(surf*ml, sigma, qObs, inc, mbh, distance, rad)
    
    plt.clf()
    plt.plot(rad, vcirc, '-o')
    plt.xlabel('R (arcsec)')
    plt.ylabel(r'$V_{circ}$ (km/s)')
    plt.pause(0.01)

def animatepoints(t, order, theta):
    fig, (ax, ax2) = plt.subplots(1, 2, subplot_kw=dict(polar=True))
    ax2 = plt.subplot(1, 2, 2, polar=False)
    ax.set_yticklabels([])
    ax.set_title('Individual Neuron Simulation')
    ax2.set_title('Order Parameter Trajectory')
    r = [0.98]*len(theta[0])
    pausetime = (t[1]-t[0])/1000
    for i in range(0, len(t)):
        if i == 0:
            points, = ax.plot(theta[i], r, color='r', marker='.', linestyle='None')
            ax.set_rmax(1.0)
            ax.grid = True
            unpackorder = [[order[0][0]], [order[0][1]]]
            orderpoints, = ax2.plot(unpackorder[0], unpackorder[1], color='b')
            ax2.set_ylim([-1, 1])
            ax2.set_xlim([-1, 1])
        else:
            points.set_data(theta[i], r)
            unpackorder[0].append(order[i][0])
            unpackorder[1].append(order[i][1])
            orderpoints.set_data(unpackorder[0], unpackorder[1])
#        print(unpackorder)
        plt.pause(pausetime)
    plt.show()
    print('Plotting Done.')

    def pause(interval):
        """Pause for `interval` seconds, letting the GUI flush its event queue.

        @note This is a *necessary* function to be defined if these globals are
        not used!
        """
        plt.pause(interval)

def main( steps=40 ):

  data = zeros((40,2))
  data[:,0] = 3
  data[4:9,1] = pi/4
  #data[:,1] = .3

  print (data)

  sim = RatSLAM(data=data,shape=POSE_SIZE) 

  fig = plt.figure()
  ax = fig.add_subplot(111, projection='3d')
  ax.set_xlim3d([0, POSE_SIZE[0]])
  ax.set_ylim3d([0, POSE_SIZE[1]])
  ax.set_zlim3d([0, POSE_SIZE[2]])
  ax.hold(False)
  plt.ion()
  plt.show()

  for s in xrange( steps ):
    #print ("Step: ",s)
    sim.step()

    pc = sim.pcn.posecells
    pc_index = nonzero(pc>.002)
    pc_value = pc[pc_index] * 100
    ax.scatter(pc_index[0],pc_index[1],pc_index[2],s=pc_value)
    ax.set_xlim3d([0, POSE_SIZE[0]])
    ax.set_ylim3d([0, POSE_SIZE[1]])
    ax.set_zlim3d([0, POSE_SIZE[2]])
    plt.pause(.01)

    def display_data(self):
        try:
            while not self.end_sampling:
                mxyz=self.mxyz
                self.plt_xy.set_xdata(mxyz[:,0])
                self.plt_xy.set_ydata(mxyz[:,1])
                self.plt_zy.set_xdata(mxyz[:,2])
                self.plt_zy.set_ydata(mxyz[:,1])
                self.plt_xz.set_xdata(mxyz[:,0])
                self.plt_xz.set_ydata(mxyz[:,2])

                self.point_xy.set_xdata(mxyz[-1:,0])
                self.point_xy.set_ydata(mxyz[-1:,1])
                self.point_zy.set_xdata(mxyz[-1:,2])
                self.point_zy.set_ydata(mxyz[-1:,1])
                self.point_xz.set_xdata(mxyz[-1:,0])
                self.point_xz.set_ydata(mxyz[-1:,2])
                
                print len(mxyz)
                plt.pause(0.001)
        except KeyboardInterrupt:
            pass
        
        self.end_sampling=True
        self.sample_thread.join()
        return

def rotate_window(maze, pt, yaw, window_size=(64, 64)):
    wall, route = np.max(maze), 0
    h_maze, w_maze = maze.shape
    #
    x, y = pt
    h_slide, w_slide = window_size
    # expected rect
    top, bottom, left, right = y - h_slide // 2, y + h_slide // 2, x - w_slide // 2, x + w_slide // 2
    # valid rect
    v_top, v_bottom, v_left, v_right = max(top, 0), min(bottom, h_maze), max(left, 0), min(right, w_maze)
    # generate slide window
    sw = np.ones([h_slide, w_slide], dtype=np.float32) * wall
    sw[v_top - top:h_slide - bottom + v_bottom, v_left - left:w_slide - right + v_right] = \
        maze[v_top:v_bottom,v_left:v_right]
    # rotation
    rr, cc = skimage.draw.circle(31, 31, 32)
    # circle = np.zeros_like(sw, dtype=np.bool)
    # circle[rr, cc] = True
    # circle = np.bitwise_not(circle)
    # sw = np.multiply(sw, circle)
    rw = np.ones_like(sw)
    rw[rr, cc] = sw[rr, cc]
    rw = skimage.transform.rotate(rw, yaw)
    #
    plt.ioff()
    plt.imshow(rw, cmap='Greys')
    plt.draw()
    plt.pause(0.1)

def vis(i):
    s = 1.
    u = 0.
    zs = np.linspace(-1, 1, 500).astype('float32')[:, np.newaxis]
    xs = np.linspace(-5, 5, 500).astype('float32')[:, np.newaxis]
    ps = gaussian_likelihood(xs, 1.)

    gs = generator.predict(zs)
    preal = decoder.predict(xs)
    kde = gaussian_kde(gs.flatten())

    plt.clf()
    plt.plot(xs, ps, '--', lw=2)
    plt.plot(xs, kde(xs.T), lw=2)
    plt.plot(xs, preal, lw=2)
    plt.xlim([-5., 5.])
    plt.ylim([u, s])
    plt.ylabel('Prob')
    plt.xlabel('x')
    plt.legend(['P(data)', 'G(z)', 'D(x)'])
    plt.title('GAN learning gaussian')
    fig.canvas.draw()
    plt.show(block=False)
    if i % 100 == 0:
        plt.savefig('current.png')
    plt.pause(0.01)

def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
    #Get the handle of the vision sensor
    res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
    #Get the image
    res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
    #Allow the display to be refreshed
    plt.ion()
    #Initialiazation of the figure
    time.sleep(0.5)
    res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
    im = I.new("RGB", (resolution[0], resolution[1]), "white")
    #Give a title to the figure
    fig = plt.figure(1)    
    fig.canvas.set_window_title(visionSensorName)
    #inverse the picture
    plotimg = plt.imshow(im,origin='lower')
    #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
    time.sleep(1)
    while (vrep.simxGetConnectionId(clientID)!=-1): 
        #Get the image of the vision sensor
        res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
        #Transform the image so it can be displayed using pyplot
        image_byte_array = array.array('b',image)
        im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
        #Update the image
        plotimg.set_data(im)
        #Refresh the display
        plt.draw()
        #The mandatory pause ! (or it'll not work)
        plt.pause(pause)
    print 'End of Simulation'