def main(args):
    # Load parameters from yaml
    param_path = 'params.yaml' # rospy.get_param("~param_path")
    f = open(param_path,'r')
    params_raw = f.read()
    f.close()
    params = yaml.load(params_raw)
    occupancy_map = np.array(params['occupancy_map'])
    world_map = np.array(params['world_map'])
    pos_init = np.array(params['pos_init'])
    pos_goal = np.array(params['pos_goal'])
    max_vel = params['max_vel']
    max_omega = params['max_omega']
    t_cam_to_body = np.array(params['t_cam_to_body'])
    x_spacing = params['x_spacing']
    y_spacing = params['y_spacing']

    # Intialize the RobotControl object
    robotControl = RobotControl(world_map, occupancy_map, pos_init, pos_goal,
                                max_vel, max_omega, x_spacing, y_spacing,
                                t_cam_to_body)

    # TODO for student: Comment this when running on the robot 
    # Run the simulation
    while not robotControl.robot_sim.done and plt.get_fignums():
        robotControl.process_measurements()
        robotControl.robot_sim.update_frame()

    plt.ioff()
    plt.show()

    # TODO for student: Use this to run the interface on the robot
    # Call process_measurements at 60Hz
    """r = rospy.Rate(60)

    def end(self):
        """End plot"""

        # Make sure that the window does not close before we wan't it to

        plt.ioff()
        plt.show()

def oystek_start(fname):
  global num_oysters

  plt.ioff()  # interactive

  # initialize
  oystek_reset(fname)

  while (True):
    print '\nfetching oyster ', num_oysters+1, ' ...'
    if (skip_to_image() != True):
      break
    if (fetch_1_oyster() != True):
      break
    num_oysters += 1
    prepare_scatter_images()
    log_to_file()
    print_measurement()
    #plot_oyster_images()
    plot_oyster_images_AiO()
    plot_oyster_3Dimages()
  # clean up before exit
  fin.close()
  fout.close()
  print '\ntotal # oysters: ', num_oysters
  return

 def plot_samples(self):
     """ Plot the samples requested for each arm """
     plt.clf()
     plt.scatter(range(0, self.K), self.sample_size)
     plt.yscale('log')
     plt.ioff()
     plt.show()

def plot_dataframe(data, error=False, log=False):
    '''
    plot data frame columns in a long plot
    '''
    try:
        ioff()
        fig, ax = plt.subplots(
            (len(data.columns)), figsize=(10, 50), sharex=True)
        close('all')
        ion()
    except:
        print 'you may be in a non interactive environment'
    if not error:
        for i, j in zip(data.columns, ax):
            if i != 'name':
                j.plot(data.index, data[i].values)
                j.set_ylabel(str(i))
    if error:
        for i, j in zip(data.columns, ax):
            if i != 'name':
                j.errorbar(
                    data.index, data[i].values, yerr=sqrt(data[i].values))
                j.set_ylabel(str(i))
    if log:
        for i, j in zip(data.columns, ax):
            if i != 'name':
                j.set_yscale('log')
    return fig, ax

    def write(self):
        self.writePreHook()

        for cut in self.contourData.filteredCombinedData:
            # Add cut to the output filename
            (outputFilename, ext) = os.path.splitext(self.outputFilename)
            outputFilename = "{0}_{1}_{2:.0f}{3}".format(outputFilename, cut.key, cut.value, ext)

            plt.ioff()
            plt.figure(figsize=(8,6))
            #plt.yscale('log')
            plt.ylim([0,1.0])
            plt.figtext(0.05, 0.96, 'Cut: {0} = {1:.0f}'.format(cut.key, cut.value), color='grey', size='small')
            plt.figtext(0.05, 0.93, 'Plot written at {:%d/%m/%Y %H:%M}'.format(datetime.datetime.now()), color='grey', size='small')

            for f in self.contourData.contributingRegionsFiltered[cut]:
                data = self.contourData.filteredData[f][cut]
                label = filterLabel(f, self.gridConfig)

                if not cut.isSimple():
                    plt.xticks( np.arange(len(data)), ["%d_%d" % (x[self.gridConfig.x], x[self.gridConfig.y]) for x in data.values()])
                    xLabel = "Grid point"
                else:
                    var = self.gridConfig.x
                    if cut.key == var: 
                        var = self.gridConfig.y
                    xLabel = var
                    plt.xticks( np.arange(len(data)), ["%d" % (x[var]) for x in data.values()])
                print [x[self.contourData.combineOn] for x in data.values()]
                plt.plot( [x[self.contourData.combineOn] for x in data.values()], label=label )

                plt.plot( [0.05 for x in data.values()], color='r', linestyle='--', linewidth=2)

def Main():
    args=ParseArg()
    data=np.loadtxt(args.input,delimiter='\t',dtype=float)
    min_x=int(args.xlim[0])
    max_x=int(args.xlim[1])
    start=int(data[0,0])
    peak=data[:,1].argmax()
    plt.ioff()
    plt.plot(np.array(range(min_x,max_x)),data[(min_x-start):(max_x-start),1],color='r',label="real_count")
    if args.distogram:
        plt.annotate('local max: '+str(peak+start)+"bp",xy=(peak+start,data[peak,1]),xytext=(peak+start+30,0.8*data[peak,1]),)
                #   arrowprops=dict(facecolor='black', shrink=0.05))

    # smoth the plot
    xnew=np.linspace(min_x,max_x,(max_x-min_x)/5)
    smooth=spline(np.array(range(min_x,max_x)),data[(min_x-start):(max_x-start),1],xnew)
    plt.plot(xnew,smooth,color='g',label='smooth(5bp)')
    max_y=max(data[(min_x-start):(max_x-start),1])
    min_y=min(data[(min_x-start):(max_x-start),1])
    plt.xlabel("Distance")
    plt.ylabel("Counts")
    plt.xlim(min_x,max_x)
    plt.ylim(min_y*0.9,max_y*1.1)
    plt.title(os.path.basename(args.input).split("_"+str(start))[0])
    plt.legend()
    plt.savefig(os.path.basename(args.input).split("_"+str(start))[0]+"_%d~%dbp."%(min_x,max_x)+args.output)
    print >>sys.stderr,"output figure file generated!!"

def q_explore_simulation():
    plt.ion()
    time_step = 1  # Discrete approximation time step for the continuous game
    exploration_rate = 0.5  # Exploration rate
    discount_factor = 0.9  # Discounted factor
    for iteration in range(5):
        logging.info('starting iteration %d', iteration)
        x, y, angle = -4, 0, 0  # Robot state
        weight = initialize_param(3 + 1, 2)
        cumulative_reward, discount = 0, 1
        for i in range(10):
            show_state(x, y, angle)
            feature = state_to_feature(x, y, angle)
            q_value = get_q_value(feature, weight)
            max_q_value, max_action = get_max_q_action(q_value)
            action = explore_exploit(max_action, exploration_rate)
            velocity, angle_speed = action_to_physics(action)
            x, y, angle = update_state(x, y, angle, velocity, angle_speed, time_step)
            reward = get_reward(x, y, angle, velocity, time_step)
            cumulative_reward += discount * reward
            discount *= discount_factor
            logging.info('cumulative reward: %.4f', cumulative_reward)
            if state_is_end(x, y, angle):
                break
    plt.ioff()
    plt.show()

def testmotion2(u=(5, 3)):
    env = Environment([Rect(-10, -10, 20, 20)])  # for reference
    env.plot(plt)
    mot = makeMotion1()
    odom = Robot_Odometry_Model()
    meas = Robot_Measurement_Model(measure_count=0, fov=pi / 8)
    start_pose = Pose(0.5, 1.5, 0.5)

    p1 = Particle(env, mot, meas, start_pose)
    p1.plot(plt)
    P0 = []
    for i in range(10):
        p_temp = p1.sample_mov(u, 0.2)
        p_temp.plot(plt)
        P0.append(p_temp)
    P = [P0]
    for i in range(10):
        P_temp = []
        for p in P[i]:
            p_temp = p.sample_mov(u, 0.1)
            p_temp.plot(plt)
            P_temp.append(p_temp)
        P.append(P_temp)
    plt.ioff()
    plt.show()

def open_data(env=makeEnviron3(), filename="out.dat"):
    r = Robot_Sim(None, None, None, None, None)
    f = open(filename, "r")
    plt.ion()
    plt.show()
    for line in f:
        if len(line) < 2:
            break
        try:
            parse = line.split(";")
            r.x = pose_from_str(parse[0])
            r.Z = measurement_from_str(parse[1])
            # print(r.x)
            # print(r.Z)
            P = [part_from_str(p) for p in parse[2:]]
        except:
            break
        # print([str(p) for p in P])
        plt.clf()
        env.plot(plt)
        r.plot(plt)
        for p in P:
            p.plot(plt, p_rgb)
        plt.draw()

    f.close()
    plt.ioff()
    plt.show()

def testdriver2():
    env = makeEnviron2()

    P = [(0.5, 1.5), (2.5, 1.5), (2.5, 0.5), (3.5, 0.5), (3.5, 3.5), (1.5, 3.5), (1.5, 1.5), (0.5, 1.5)]
    plt.ion()
    env.plot(plt)
    plt.draw()
    mot = makeMotion1()
    odom = Robot_Odometry_Model()
    meas = Robot_Measurement_Model(measure_count=4, fov=pi / 8)
    start_pose = Pose(0.5, 1.5, 0)

    r = Robot_Sim(env, mot, odom, meas, start_pose)
    # print(r)
    driver = Robot_Driver(r, P, v_max=5, w_max=6)
    while not driver.finished:
        u = driver.next_control()
        # print(u)
        r.tick(u, driver.dt)
        r.plot(plt)
        plt.draw()
        # print(driver.count)

    plt.ioff()
    plt.show()

def VerifyCurrent(Status):
    dirdata = "CompressedData/"
    AllVectors = {}
    if os.path.exists(dirdata) is False:
        return False
    files = os.listdir(dirdata)
    # Begin loading the data    x is like "132-1"
    for x in files:
        pid, state = x.split('-')
        pid = int(pid)
        state = int(state)
        try:
            fp = open("%s%s"%(dirdata, x), 'r')
        except:
            print 'Open file error'
        sf = cPickle.load(fp)
        AllVectors[(pid,state)] = sf[115,:].tolist()
        fp.close()
    vector, SelectedVectors =GetCurrent(Status,AllVectors)
    PCAclass = calib.PCA.PCAClass()
    DeVector = PCAclass.PCA(np.matrix(vector).transpose(), Status)
    plt.ioff()
    dims = len(AllVectors[(0, 1)])
    for pid in SelectedVectors: 
        fig=plt.figure()
        plt.bar(np.arange(0,dims,1),DeVector[pid],0.3)
        plt.bar(np.arange(0.3,dims+0.3,1),SelectedVectors[pid],0.3,color='r')
        plt.show()

    def get_legend_size(self):
        """
        Determine the size of the legend by building a dummy figure and
        extracting its properties
        """

        if len(self.leg_items) > 0 and self.leg_on:
            now = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
            mplp.ioff()
            fig = mpl.pyplot.figure(dpi=self.dpi)
            ax = fig.add_subplot(111)
            lines = []
            for i in self.leg_items:
                lines += ax.plot([1,2,3])
            leg = mpl.pyplot.legend(lines,
                                    list(self.leg_items),
                                    title=self.leg_title,
                                    numpoints=self.leg_points,
                                    prop={'size':self.leg_font_size})
            fig.canvas.draw()
            mpl.pyplot.savefig('dummy_legend_%s.png' % now)
            self.leg_h = leg.get_window_extent().height + self.leg_border
            self.leg_w = leg.get_window_extent().width + self.leg_border
            mpl.pyplot.close(fig)
            os.remove('dummy_legend_%s.png' % now)

        self.leg_w = max(self.leg_w, self.ax_leg_fig_ws)

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 q_test(weight):
    plt.ion()
    time_step = 1  # Discrete approximation time step for the continuous game
    exploration_rate = 0.1  # Exploration rate
    discount_factor = 0.9  # Discounted factor
    test_iter = 5  # Number of training iterations
    for i in range(test_iter):
        # x, y, angle = -4, 0, 0  # Robot state
        x, y, angle = random_state()
        cumulative_reward, discount = 0, 1
        while True:
            show_state(x, y, angle)
            feature = state_to_feature(x, y, angle)
            q_value = get_q_value(feature, weight)
            max_q_value, max_action = get_max_q_action(q_value)
            action = explore_exploit(max_action, exploration_rate)
            velocity, angle_speed = action_to_physics(action)
            x, y, angle = update_state(x, y, angle, velocity, angle_speed, time_step)
            if state_is_end(x, y, angle):
                reward = -10
            else:
                reward = get_reward(x, y, angle, velocity, time_step)
            cumulative_reward += discount * reward
            discount *= discount_factor
            if state_is_end(x, y, angle):
                break
        logging.info('iteration: %d, exploration rate: %.4f, cumulative reward: %.4f',
                     i, exploration_rate, cumulative_reward)
    plt.ioff()
    plt.show()

    def generate_image(self, filepath, smoothness=7, size=400):
        x = self.average_hist[:, 1]
        y = self.average_hist[:, 0]
        x -= 50
        y = np.concatenate((y[-50:], y[:-50]))

        #The following is for plotting xticks at proper peak locations
        locs = np.arange(0, 1200, 100)
        labels = ["Sa", "Ri1", "Ri2/Ga1", "Ri3/Ga2", "Ga3", "Ma1", "Ma2", "Pa", "Da1", "Da2/Ni1", "Da3/Ni2", "Ni3"]
        label_map = {}
        for i in xrange(len(locs)):
            label_map[locs[i]] = labels[i]

        dobj = pypeaks.Data(x, y, smoothness=7)
        dobj.get_peaks(peak_amp_thresh=6e-04)
        actual_locs = np.sort(dobj.peaks["peaks"][0])
        for i in xrange(len(actual_locs)):
            actual_locs[i] = locs[self.find_nearest_index(locs, actual_locs[i])]
        actual_labels = [label_map[i] for i in actual_locs]

        y = gaussian_filter(y, smoothness)
        plt.ioff()
        fig = plt.figure()
        fig.set_size_inches(2, 2)
        fig.set_dpi(300)

        plt.plot(x, y, "k-")
        plt.xlim(-50, 1150)

        plt.xticks(actual_locs, actual_labels, fontsize=6)
        plt.yticks([])

        plt.savefig(filepath, bbox_inches="tight")
        plt.close(fig)

def show_q_value(weight):
    plt.ion()
    time_step = 1
    discount_factor = 0.9
    # x, y, angle = -4, 0, 0  # Robot state
    x, y, angle = random_state()
    cumulative_reward, discount = 0, 1
    while True:
        show_state(x, y, angle)
        feature = state_to_feature(x, y, angle)
        q_value = get_q_value(feature, weight)
        logging.info('q_value: [%.4f, %.4f]', q_value[0], q_value[1])
        input("Press Enter to continue...\n")
        max_q_value, max_action = get_max_q_action(q_value)
        action = explore_exploit(max_action, 0.1)
        velocity, angle_speed = action_to_physics(action)
        x, y, angle = update_state(x, y, angle, velocity, angle_speed, time_step)
        if state_is_end(x, y, angle):
            reward = -10
        else:
            reward = get_reward(x, y, angle, velocity, time_step)
        cumulative_reward += discount * reward
        discount *= discount_factor
        if state_is_end(x, y, angle):
            break
        logging.info('cumulative reward: %.4f', cumulative_reward)
    plt.ioff()
    plt.show()

def hovmoellerPlot(varlist, clevs=None,cbls=None,title='',subplot=(),slices={},figargs={'figsize':(8,8)},**plotargs):
  import matplotlib.pylab as pyl    
  from pygeode.axis import XAxis, YAxis, TAxis
  from plotting.misc import multiPlot, sharedColorbar
  if not isinstance(varlist,list): varlist = [varlist]
  if not isinstance(clevs,list): clevs = [clevs]
  if not isinstance(cbls,list): cbls = [cbls]
  pyl.ioff() # non-interactive mode
  # construct figure and axes    
  f = pyl.figure(**figargs)    
  f.clf()
  # create zonal-mean variables
  titles = [var.name for var in varlist]
  plotlist = [var(**slices).mean(XAxis).transpose(YAxis,TAxis) for var in varlist] # latitude sliced
  # organize colorbar (cleanup arguments) 
  colorbar = plotargs.pop('colorbar',{})
  manualCbar = colorbar.pop('manual',False)
  if manualCbar: cbar = False
  else: cbar = colorbar 
  # set default margins  
  defaultMargins = {'left':0.065,'right':0.975,'bottom':0.05,'top':0.95,'wspace':0.05,'hspace':0.1}
  defaultMargins.update(plotargs.pop('margins',{}))
  ## make subplots
  (f,cf,subplot) = multiPlot(f=f,varlist=plotlist,titles=titles,clevs=clevs,cbls=cbls,subplot=subplot, #
                     colorbar=cbar,margins=defaultMargins,**plotargs)
  if title: f.suptitle(title,fontsize=14) 
  ## add common colorbar
  if manualCbar:
    f = sharedColorbar(f, cf, clevs, colorbar, cbls, subplot, defaultMargins)        
  # finalize
  pyl.draw(); # pyl.ion();
  return f

    def go(self, start, end):

        start = (start / self.batchSize) * self.batchSize
        end = (end / self.batchSize + 1) * self.batchSize
        cur = 0

        # skip to the start
        for _ in range(start / self.batchSize):
            cur += self.batchSize
            self.parser.getNextBatch()

        for _ in range((end - start) / self.batchSize):
            dens, _, _ = self.parser.getNextBatch()

            r, theta = np.meshgrid(self.params['radialIntervals'], self.params['thetaIntervals'])
            plt.ioff()
            #-- Plot... ------------------------------------------------
            for i in range(len(dens)):
                fig = plt.figure()
                ax = plt.subplot(111, polar=True)
                ax.contourf(theta, r, np.log(dens[i]).transpose(), cmap=plt.cm.afmhot)
                ax.scatter([self.secondaryTheta[cur]], [self.secondaryRadius[cur]], s=150)
                ax.set_rmax(1.5)
                #ax.set_title(r"$\theta_{sec}=" + "{0:.2f}$ rad".format(self.secondaryTheta[cur] % 6.283), va='bottom')
                plt.savefig(self.outputDir + "/figs/dens" + str(cur) + ".png")
                plt.close(fig)

                cur += 1

 def run (self, gamma, ELA, dt, dx):
     for t in range(len(self.timestep)):
         self.dqdx=glacier_model.dqdx_func(dx)
         self.dhdt=glacier_model.b_func(gamma, ELA)-self.dqdx
         self.h+=self.dhdt*dt
         for i in range(0, len(self.h)): #make sure bottom limit of z does not go below zb
             if self.h[i]<0:
                 self.h[i]=0
         self.z=self.h+self.zb
         if self.timestep[t] % 10 ==0:
             self.figure.clear()
             plt.title('Glacier Accumulation & Ablation Over Time')
             plt.xlabel('Distance (m)')
             plt.ylabel('Elevation (m)')
             self.figure.set_ylim(2000, 4000) #make sure y axis doesn't change
             self.figure.set_xlim(0, 15000) #make sure x axis doesn't change
             plt.text(12000, 3500, 'Time [yrs]: %d\nELA=%d m' % (self.timestep[t], ELA))
             self.figure.plot(self.spacestep, self.z, label='Glacier Height', color='b')  
             self.figure.plot(self.spacestep, self.zb, label='Bedrock Height', color='k')
             self.figure.plot(self.spacestep, (np.zeros(len(self.spacestep))+ELA),  '--r', label='ELA')
             self.figure.fill_between(self.spacestep, self.zb, self.z, color ='b', interpolate=True)
             self.figure.fill_between(self.spacestep, 0, self.zb, color='k', interpolate=True)
             self.figure.legend()
             plt.pause(0.00001)
     plt.ioff()