def plotFiguresTemp(xSolid, ySolid, zSolid, xFFDDeform, yFFDDeform, zFFDDeform):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    #fig = plt.figure()
    #ax = fig.gca(projection='3d')

    # Axes3D.plot_wireframe(ax, z, x, y)
    ax.set_xlabel('Z axis')
    ax.set_ylabel('X axis')
    ax.set_zlabel('Y axis')

    #ax.plot_trisurf(zSolid, xSolid, ySolid, cmap=cm.jet, linewidth=0.2)
    ax.plot_wireframe(zSolid, xSolid, ySolid, rstride = 1, cstride = 1, color="y")

    #Axes3D.scatter(ax, zSolid, xSolid, ySolid, s=10, c='b')
    Axes3D.scatter(ax, zFFDDeform, xFFDDeform, yFFDDeform, s=30, c='r')

    # Axes3D.set_ylim(ax, [-0.5,4.5])
    # Axes3D.set_xlim(ax, [-0.5,4.5])
    Axes3D.set_zlim(ax, [-0.7, 0.7])




    plt.show(block=True)

def plotData(tuplesArray):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    
    
    x = []
    y = []
    z = []
    
    for point in tuplesArray:
        x.append(point[0])
        y.append(point[1])
        z.append(point[2])
    

    if (FILEInQuestion == "newsbp.fuselageZ.dat"):
        Axes3D.scatter(ax, x,y,z, s=30, c ='b')
        
        Axes3D.set_zlim(ax, [0, 1000])
        Axes3D.set_ylim(ax, [0, 3000])
        Axes3D.set_xlim(ax, [0, 3000])
        
        ax.set_xlabel('Z axis')
        ax.set_ylabel('X axis')
        ax.set_zlabel('Y axis')
    else:
        Axes3D.scatter(ax, z,x,y, s=30, c ='b')
        
        ax.set_xlabel('Z axis')
        ax.set_ylabel('X axis')
        ax.set_zlabel('Y axis')

    plt.show(block=True)

    def onpick(event):

        # get event indices and x y z coord for click
        ind = event.ind[0];
        x_e, y_e, z_e = event.artist._offsets3d;
        print x_e[ind], y_e[ind], z_e[ind];
        seg_coord = [x_e[ind], y_e[ind], z_e[ind]];
        seg_elecs.append(seg_coord);

        # refresh plot with red dots picked and blue dots clean
        #fig.clf();
        seg_arr = np.array(seg_elecs);
        x_f = seg_arr[:,0];
        y_f = seg_arr[:,1];
        z_f = seg_arr[:,2];
        plt.cla();
        Axes3D.scatter3D(ax,x_f,y_f,z_f,s=150,c='r', picker=5);

        # get array of only clean elecs to re-plot as blue dots
        clean_list = list(coords);
        clean_list = [list(i) for i in clean_list];
        for coordin in clean_list:
                if list(coordin) in seg_elecs:
                    clean_list.pop(clean_list.index(coordin));
        clean_coordis = np.array(clean_list);
        x_c = clean_coordis[:,0];
        y_c = clean_coordis[:,1];
        z_c = clean_coordis[:,2];
        Axes3D.scatter3D(ax,x_c, y_c, z_c, s=150,c='b', picker=5);
        time.sleep(0.5);
        plt.draw();

def clust(elect_coords, n_clusts, iters, init_clusts):
    
   # Load resultant coordinates from Hough circles transform
   #coords = scipy.io.loadmat(elect_coords);
   #dat = coords.get('elect_coords');
   dat = elect_coords;

   # Configure Kmeans
   cluster = sklearn.cluster.KMeans();
   cluster.n_clusters= n_clusts;
   cluster.init = 'k-means++';
   cluster.max_iter = iters;
   cluster.verbose = 0;
   cluster.n_init = init_clusts;
   cluster.fit(dat);

   # Grab vector for plotting each dimension
   x = list(cluster.cluster_centers_[:,0]);
   y = list(cluster.cluster_centers_[:,1]);
   z = list(cluster.cluster_centers_[:,2]);
   c = list(cluster.labels_);

   scipy.io.savemat('k_labels.mat', {'labels':cluster.labels_})
   scipy.io.savemat('k_coords.mat', {'coords':cluster.cluster_centers_})

   # plot the results of kmeans
   cmap = colors.Colormap('hot');
   norm  = colors.Normalize(vmin=1, vmax=10);
   s = 64;
   fig = plt.figure();
   ax = fig.add_subplot(111,projection='3d');
   Axes3D.scatter3D(ax,x,y,z,s=s);
   plt.show(fig);
   
   return cluster.cluster_centers_,cluster.labels_;

def plotData2files(tuplesArray1, tuplesArray2):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    
    
    x1 = []
    y1 = []
    z1 = []
    
    for point in tuplesArray1:
        x1.append(point[0])
        y1.append(point[1])
        z1.append(point[2])
    
    x2 = []
    y2 = []
    z2 = []
    
    for point in tuplesArray2:
        x2.append(point[0])
        y2.append(point[1])
        z2.append(point[2])
    
    Axes3D.scatter(ax, x1,y1,z1, s=30, c ='b')
    Axes3D.scatter(ax, x2,y2,z2, s=30, c ='r')

    Axes3D.set_xlim(ax, [0, 2000])
    Axes3D.set_ylim(ax, [0, 3000])
    Axes3D.set_zlim(ax, [0, 1000])

    plt.show(block=True)

 def volume_display(self, zstep = 3.00):
     """
     This is a daring attempt for 3-D plots of all the blobs in the brain. 
     Prerequisites: self.blobs_archive are established.
     The magnification of the microscope must be known.
     """
     fig3 = plt.figure(figsize = (10,6))
     ax_3d = fig3.add_subplot(111, projection = '3d')
     
     for n_frame in self.valid_frames:
         # below the coordinates of the cells are computed.
         blobs_list = self.c_list[n_frame] 
         ys = blobs_list[:,0]*magni_lateral
         xs = blobs_list[:,1]*magni_lateral 
         zs = np.ones(len(blobs_list))*n_frame*zstep
         ss = (np.sqrt(2)*blobs_list[:,2]*magni_lateral)**2*np.pi
         Axes3D.scatter(xs,ys, zs, zdir = 'z', s=ss, c='g')        
     
     
     
     ax_3d.set_xlabel('x (micron)', fontsize = 12)
     ax_3d.set_xlabel('y (micron)', fontsize = 12)
     
     return fig3
     

def plot_cam_location(camera_list, n, fig=None, ax=None):
	FigSz = (8, 8)
	L = np.array([list(c.location[n]) for c in camera_list])
	x = L[:, 0]
	y = L[:, 1]
	z = L[:, 2]
	if not fig:
		fig = plt.figure(figsize=FigSz)
	#view1 = np.array([-85., 60.])
	if not ax:
		ax = Axes3D(fig) #, azim=view1[0], elev=view1[1])
	Axes3D.scatter(ax, x, y, zs=z, color='k')
	zer = np.array
	Axes3D.scatter(ax, zer([0]), zer([0]), zs=zer([5]), color='r')
	ax.set_xlabel('X')
	ax.set_ylabel('Y')
	ax.set_zlabel('Z')
	ax.set_ylim3d(-50, 50)
	ax.set_xlim3d(-50, 50)
	ax.set_zlim3d(0, 50)
	#if Flag['ShowPositions']:
	fig.show()
	#else:
	#	fig.savefig(fName)
	return ax, fig

    def updatePlot(self, X, Y, Z, population=None):
        self.activePlot = (X,Y,Z)
        x, y = np.meshgrid(X,Y)

        if self.surface is not None:
            self.surface.remove()

        if self.scatter is not None:
            self.scatter.remove()

        # surface
        self.surface = Axes3D.plot_surface(
                self.axes,
                x, y, Z,
                rstride=1,
                cstride=1,
                cmap=cm.coolwarm,
                linewidth=0,
                antialiased=False,
                shade=False,
                alpha=0.5
        )

        # population
        if population is not None:
            self.activePopulation = population
            x, y, z = self.preparePopulationData(population)
            self.scatter = Axes3D.scatter(self.axes, x, y, z, c="r", marker="o")
            self.scatter.set_alpha(1.0)

        # Draw all
        self.canvas.draw()
        self.canvas.flush_events()

def plotFigures(xSolid,ySolid,zSolid,xFFDDeform,yFFDDeform,zFFDDeform, xsolidInitial,
               ysolidInitial, zsolidInitial, xFFDInitial, yFFDInitial, zFFDInitial):
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')


    # Axes3D.plot_wireframe(ax, z, x, y)
    ax.set_xlabel('Z axis')
    ax.set_ylabel('X axis')
    ax.set_zlabel('Y axis')

    #Axes3D.scatter(ax, zSolid, xSolid, ySolid, s=10, c='b')
    Axes3D.plot_wireframe(ax, zSolid, xSolid, ySolid, rstride = 1, cstride = 1, color="b")
    Axes3D.scatter(ax, zFFDDeform, xFFDDeform, yFFDDeform, s=30, c='r')

    #Axes3D.scatter(ax, zsolidInitial, xsolidInitial, ysolidInitial, s=10, c='y')
    Axes3D.plot_wireframe(ax, zsolidInitial, xsolidInitial, ysolidInitial,rstride = 1, cstride = 1, color="y")
    Axes3D.scatter(ax, zFFDInitial, xFFDInitial, yFFDInitial, s=30, c='g')

    xZCross = []
    yZCross = []
    zZCross = []

    #plot the points for the limits of each cross section
    for zCrossSect in GLOBAL_zCrossSectionObjects:
        zCrossSectionObject = GLOBAL_zCrossSectionObjects[zCrossSect]

        #add to the arrays, for a fixed z, the following combinations
        # (xmax, ymax) (xmax, ymin) (xmin, ymin) (xmin, ymax)

        # (xmax, ymax)
        xZCross.append(zCrossSectionObject.getXMax())
        yZCross.append(zCrossSectionObject.getYMax())
        zZCross.append(zCrossSect)

        #(xmax, ymin)
        xZCross.append(zCrossSectionObject.getXMax())
        yZCross.append(zCrossSectionObject.getYMin())
        zZCross.append(zCrossSect)

        #(xmin, ymin)
        xZCross.append(zCrossSectionObject.getXMin())
        yZCross.append(zCrossSectionObject.getYMin())
        zZCross.append(zCrossSect)

        #(xmin, ymax)
        xZCross.append(zCrossSectionObject.getXMin())
        yZCross.append(zCrossSectionObject.getYMax())
        zZCross.append(zCrossSect)

    #Axes3D.plot_wireframe(ax, zZCross, xZCross, yZCross)



    #Axes3D.set_ylim(ax, [-0.5,4.5])
    #Axes3D.set_xlim(ax, [-0.5,4.5])
    Axes3D.set_zlim(ax, [-0.7, 0.7])

    plt.show(block=True)

    def draw(self, axes: Axes3D):
        R = (self.orientation * (self.orientation_origin.get_inverse())).to_rot_matrix()

        xx = R[0, 0] * self.x + R[0, 1] * self.y + R[0, 2] * self.z
        yy = R[1, 0] * self.x + R[1, 1] * self.y + R[1, 2] * self.z
        zz = R[2, 0] * self.x + R[2, 1] * self.y + R[2, 2] * self.z

        axes.plot_surface(xx, yy, zz, rstride=4, cstride=4, color='b')
        return axes,

def elecDetect(CT_dir, T1_dir, img, thresh, frac, num_gridStrips, n_elects):

    # navigate to dir with CT img
    os.chdir(T1_dir)

    # Extract and apply CT brain mask
    mask_CTBrain(CT_dir, T1_dir, frac)
    masked_img = "masked_" + img

    # run CT_electhresh
    print "::: Thresholding CT at %f stdv :::" % (thresh)
    electhresh(masked_img, thresh)

    # run im_filt to gaussian smooth thresholded image
    fname = "thresh_" + masked_img
    print "::: Applying guassian smooth of 2.5mm to thresh-CT :::"
    img_filt(fname)

    # run hough transform to detect electrodes
    new_fname = "smoothed_" + fname
    print "::: Applying 2d Hough Transform to localize electrode clusts :::"
    elect_coords = CT_hough(CT_dir, new_fname)

    # set up x,y,z coords to view hough results
    x = elect_coords[:, 0]
    x = x.tolist()
    y = elect_coords[:, 1]
    y = y.tolist()
    z = elect_coords[:, 2]
    z = z.tolist()

    # plot the hough results
    s = 64
    fig = plt.figure()
    ax = fig.add_subplot(111, projection="3d")
    Axes3D.scatter3D(ax, x, y, z, s=s)
    plt.show(fig)

    # run K means to find grid and strip clusters
    n_clusts = num_gridStrips
    # number of cluster to find = number of electrodes
    iters = 1000
    init_clusts = n_clusts
    print "::: Performing K-means cluster to find %d grid-strip and noise clusters :::" % (n_clusts)
    [clust_coords, clust_labels] = clust(elect_coords, n_clusts, iters, init_clusts)

    # run K means to find electrode center coordinates
    n_clusts = n_elects
    # number of cluster to find = number of electrodes
    iters = 1000
    init_clusts = n_clusts
    print "::: Performing K-means cluster to find %d electrode centers :::" % (n_clusts)
    [center_coords, labels] = clust(elect_coords, n_clusts, iters, init_clusts)

    print "::: Finished :::"

    return elect_coords, clust_coords, clust_labels

    def water(self, *argv):  # Dimension is unit type as V(olt) W(att), etc
#        http://matplotlib.org/examples/mplot3d/polys3d_demo.html
        Axes3D.plot_surface(self.signalx, self.signaly, self.signalz)  # till better funcion
        plt.xlabel('time [s]')  # Or Sample number
        plt.ylabel('Frequency [Hz]')  # Or Freq Bins number
        plt.zlabel('voltage [mV]')  # auto add unit here
        plt.title(' ')  # set title
        plt.grid(True)
        plt.show()

 def addmpl(self, fig):
   self.fig = fig
   self.canvas = FigureCanvas(fig)
   self.mplvl.addWidget(self.canvas)
   self.canvas.draw()
   self.toolbar = NavigationToolbar(self.canvas, 
                                    self.mplwindow, coordinates=True)
   self.mplvl.addWidget(self.toolbar)
   ax = fig.get_axes()[0]
   Axes3D.mouse_init(ax)

  def __init__(self,*args,**kwargs):
    if (len(args) > 1) | kwargs.has_key('rect'):
      _Axes3D.__init__(self,*args,**kwargs)

    else:
      rect = (0.1,0.1,0.8,0.8)
      _Axes3D.__init__(self,*args,rect=rect,**kwargs)

    self.cross_section_clim_set = False
    self.pbaspect = np.array([1.0,1.0,1.0])

def addcurve( ax, path, endpoints, color ) :
  px = path[ :,0 ]
  py = path[ :,1 ]
  pz = path[ :,2 ]
  n = len(px) - 1
  q = Axes3D.plot(ax, px, py, zs=pz, c=color, linewidth=2 )
  px = endpoints[ :,0 ]
  py = endpoints[ :,1 ]
  pz = endpoints[ :,2 ]
  print px, py, pz
  q = Axes3D.scatter(ax, px,py, zs=pz, c=color, marker='o', s=60)

def main():
    # initialize ros node
    rospy.init_node('compute_calibration')

    # get path to folder containing recorded data
    filesPath = rospy.get_param('~files_path')

    # load trajectories
    kinectTraj = np.loadtxt('%skinect_trajectory' % (filesPath), delimiter=',')
    baxterTraj = np.loadtxt('%sbaxter_trajectory' % (filesPath), delimiter=',')

    # compute absolute orientation
    kinectOut, rot, trans = absOrientation(kinectTraj,baxterTraj)

    # plot both point sets together
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # plot the data
    Axes3D.scatter(ax, kinectOut[:,0], kinectOut[:,1], kinectOut[:,2], s=20, c='r')
    Axes3D.scatter(ax, baxterTraj[:,0],baxterTraj[:,1], baxterTraj[:,2], s=20, c='b')

    # add labels and title
    ax.set_xticks([])
    ax.set_yticks([])
    ax.set_zticks([])
    ax.set_xlabel('X-axis')
    ax.set_ylabel('Y-axis')
    ax.set_zlabel('Z-axis')
    plt.title('Kinect-Baxter Calibration')
    plt.legend(['Kinect','Baxter'])
    plt.show()

    # get rotation matrix as quaternion and euler angles
    euler = tf.transformations.euler_from_matrix(rot)
    quat = tf.transformations.quaternion_from_euler(*euler)

    # save results to yaml file
    f = open('%skinect_calibration.yaml' % (filesPath), 'w')
    lines = ['trans: [', 'rot: [', 'rot_euler: [']

    for elem in trans: lines[0] += str(elem) + ', '
    lines[0] += ']\n'
    for elem in quat: lines[1] += str(elem) + ', '
    lines[1] += ']\n'
    for elem in euler: lines[2] += str(elem) + ', '
    lines[2] += ']\n'

    lines.append('parent: /base\n')
    lines.append('child: /kinect2_link\n')

    f.writelines(lines)
    f.close()

def plotPCA(proj3D, X_r, PCs, ligs, colors, csvPath, save_flag):
    """
    Plot the PCA data on 2D plot
    """

    # Main figure
    fig = plt.figure(figsize=(13, 12), dpi=100)

    if proj3D:
        ax = fig.add_subplot(111, projection="3d")
        for label, col, x, y, z in zip(ligs, colors,
                                       X_r[:, 0], X_r[:, 1], X_r[:, 2]):
            newCol = makeColor(col)
            Axes3D.scatter(ax, x, y, z, label=label, color=newCol,
                           marker="o", lw=1, s=800)
        ax.set_xlabel("PC1 (" + '{0:g}'.format(PCs[0]) + " %)", fontsize=30)
        ax.set_ylabel("PC2 (" + '{0:g}'.format(PCs[1]) + " %)", fontsize=30)
        ax.set_zlabel("PC3 (" + '{0:g}'.format(PCs[2]) + " %)", fontsize=30)
        ax.tick_params(axis="both", which="major", labelsize=20)
        pngPath = csvPath.replace(".csv", "_3D.png")
    else:
        ax = fig.add_subplot(111)
        for label, col, x, y in zip(ligs, colors, X_r[:, 0], X_r[:, 1]):
            newCol = makeColor(col)
            ax.scatter(x, y, label=label, color=newCol, marker="o", lw=1, s=800)
            # ax.annotate(label, xy=(x, y - 0.05), fontsize=10,
            #             ha='center', va='top')
        ax.set_xlabel("PC1 (" + '{0:g}'.format(PCs[0]) + " %)", fontsize=30)
        ax.set_ylabel("PC2 (" + '{0:g}'.format(PCs[1]) + " %)", fontsize=30)
        ax.tick_params(axis="both", which="major", labelsize=30)
        pngPath = csvPath.replace(".csv", "_2D.png")

    # figTitle = "PCA on " + csvPath + " (PC1=" + pcVals[0] + ", PC2=" +
    # pcVals[1] + ")"
    # ax.text(0.5, 1.04, figTitle, horizontalalignment="center", fontsize=30,
    #         transform=ax.transAxes)

    # Legend figure
    fig_legend = plt.figure(figsize=(13, 12), dpi=100)
    plt.figlegend(*ax.get_legend_handles_labels(), scatterpoints=1,
                  loc="center", fancybox=True,
                  shadow=True, prop={"size": 30})

    # Save figures if save flag was used
    if save_flag:
        print("\nSAVING figures\n")
        fig.savefig(pngPath, bbox_inches="tight")
        fig_legend.savefig(pngPath.replace(".png", "_legend.png"))
    # Otherwise show the plots
    else:
        print("\nSHOWING figures\n")
        plt.show()

 def Spect(self):  #, dimension):  # Dimension is unit type as V(olt) W(att), etc
     Axes3D.plot_surface(self.signalx, self.signaly, self.signalz)
     if self.signalx.shape == self.signaly.shape == self.signalz.shape:
         pass
     elif self.signalx.shape == (len(self.signalx), ) or self.signaly.shape == (len(self.signaly), ):
     # Check for 1D array both
         if self.signalx.shape == (len(self.signalx), ) and self.signaly.shape == (len(self.signaly), ):
             self.signalx, self.signaly = np.meshgrid(self.signalx, self.signaly)
             nx, mx = np.shape(self.signalx)
             nz, mz = np.shape(self.signalz)
             if nx == nz and mx == mz:
                 pass
             elif nx == mz and mx == nz:
                 self.signalz = np.rot90(self.signalz, 1) 
                 # find out if it has to be 1 or 3
         elif self.signalx.shape == (len(self.signalx), ):
             pass
         elif self.signaly.shape == (len(self.signaly), ):
             pass
         if self.signalx.shape == self.signaly.shape != self.signalz.shape:
             nx, mx = np.shape(self.signalx)
             nz, mz = np.shape(self.signalz)
             if nx == nz and mx == mz:
                 pass
             elif nx == mz and mx == nz:
                 self.signalz = np.rot90(self.signalz, 1) 
                 # find out if it has to be 1 or 3
         elif self.signalx.shape == self.signalz.shape != self.signaly.shape:
             nx, mx = np.shape(self.signalx)
             ny, my = np.shape(self.signalz)
             if nx == ny and mx == my:
                 pass
             elif nx == my and mx == ny:
                 self.signaly = np.rot90(self.signaly, 1) 
                 # find out if it has to be 1 or 3
     elif self.signaly.shape == self.signalz.shape != self.signalx.shape:
         ny, my = np.shape(self.signaly)
         nx, mx = np.shape(self.signalx)
         if ny == nx and my == mx:
             pass
         elif ny == mx and my == nx:
             self.signalz = np.rot90(self.signalz, 1) 
             # find out if it has to be 1 or 3
     else:
         raise MeasError.DataError("No Valid data found in at least one of the axis")
     
     plt.xlabel('time [s]')  # Or Sample number
     plt.ylabel('Frequency [Hz]')  # Or Freq Bins number
     plt.zlabel('voltage [mV]')  # auto add unit here
     plt.title(' ')  # set title
     plt.grid(True)
     plt.show()

def makeequator( ax ) :
  npts = 256
  x = numpy.zeros( npts )
  y = numpy.zeros( npts )
  z = numpy.zeros( npts )
  n = 0
  for phi in numpy.arange( 0., 2*math.pi, 2*math.pi/npts ) :
    x[n] = math.cos(phi)
    y[n] = math.sin(phi)
    n = n+1
  q = Axes3D.plot(ax, x, y, zs=z, c='gray' )
  q = Axes3D.plot(ax, x, z, zs=y, c='gray' )
  q = Axes3D.plot(ax, z, y, zs=x, c='gray' )

    def __init__(self, parent):
        self.figure = Figure(tight_layout=True)
        self.canvas = FigureCanvas(self.figure)
        self.canvas.setContentsMargins(0,0,0,0)
        self.axes = self.figure.add_subplot(111, projection='3d')
        Axes3D.set_autoscale_on(self.axes, True)
        Axes3D.autoscale_view(self.axes)
        self.canvas.setParent(parent)
        self.activePlot = None
        self.activePopulation = None

        self.surface = None
        self.scatter = None