def __init__(self, scanList, stateArray):
     if len(scanList)!=stateArray.shape[0]:
         raise Exception('number of scan and state should be the same')
     times = [scan.timestamp for scan in scanList]
     self.avgTime = times[int(pl.floor(len(times)/2))]
     #self.avgTime = pl.mean([scan.timestamp for scan in scanList])
     
     #transform the 3d coordinates of each scan
     #and numpy.vstack all the output m*3 array together
     
     #find average Lidar frame
     avgBodyState = stateArray[int(pl.floor(len(stateArray)/2))]
     #avgBodyState=pl.mean(stateArray, 0)
     
     
     w_R_avg_L, w_T_avg_L = self._bodyState2LidarState(avgBodyState)
     self.avgLidarState = self._matrix2State(w_R_avg_L, w_T_avg_L)
     
     transform = self._transformPointsFromBodyToAvgLidar                
     #from data points with transformation to avgState
     self.dataPoints = pl.vstack([transform(scan.dataArray, state, w_R_avg_L, w_T_avg_L)  for scan, state in zip(scanList, stateArray) if scan.hasValidData()])
     self.intensity = pl.vstack([scan.intensityArray for scan in scanList if scan.hasValidData()]).flatten()
     
     if self.dataPoints.shape[0]!=self.intensity.shape[0]:
         raise Exception('dist and intensity have different size')

def stackSimRes(simRes):
    """
    input: a *list* of single steps
    returns: an array that contains the complete gait (consecutive time & way)
    """
    resDat = []
    res_t = []
    for part in simRes:
        if len(resDat) == 0:
            res_t.append(part['t'])
            resDat.append(vstack( [ part['x'],
                                    part['y'],
                                    part['z'],
                                    part['vx'],
                                    part['vy'],
                                    part['vz'],
                                    ]).T)
        else:
            res_t.append(part['t'][1:] + res_t[-1][-1])
            # compensate x and z translation
            resDat.append(vstack( [ part['x'][1:] + resDat[-1][-1,0],
                                    part['y'][1:],
                                    part['z'][1:] + resDat[-1][-1,2],
                                    part['vx'][1:],
                                    part['vy'][1:],
                                    part['vz'][1:],
                                    ]).T)
    return hstack(res_t), vstack(resDat)

  def extrude_mesh(self,l,z_offset):
    # accepts the number of layers and the length of extrusion

    # Extrude vertices
    all_coords = []
    for i in linspace(0,z_offset,l):
      all_coords.append(hstack((mesh.coordinates(),i*ones((self.n_v2,1)))))
    self.global_vertices = vstack(all_coords)

    # Extrude cells (tris to tetrahedra)
    for i in range(l-1):
      for c in self.mesh.cells():
        # Make a prism out of 2 stacked triangles
        vertices = hstack((c+i*self.n_v2,c+(i+1)*self.n_v2))

        # Determine prism orientation
        smallest_vertex_index = argmin(vertices)

        # Map to I-ordering of Dompierre et al.
        mapping = self.indirection_table[smallest_vertex_index]

        # Determine which subdivision scheme to use.
        if min(vertices[mapping][[1,5]]) < min(vertices[mapping][[2,4]]):
          local_tets = vstack((vertices[mapping][[0,1,2,5]],\
                               vertices[mapping][[0,1,5,4]],\
                               vertices[mapping][[0,4,5,3]]))
        else:
          local_tets = vstack((vertices[mapping][[0,1,2,4]],\
                               vertices[mapping][[0,4,2,5]],\
                               vertices[mapping][[0,4,5,3]]))
        # Concatenate local tet to cell array
        self.global_tets = vstack((self.global_tets,local_tets))

    # Eliminate phantom initialization tet
    self.global_tets = self.global_tets[1:,:]

    # Query number of vertices and tets in new mesh
    self.n_verts = self.global_vertices.shape[0]
    self.n_tets = self.global_tets.shape[0]

    # Initialize new dolfin mesh of dimension 3
    self.new_mesh = Mesh()
    m = MeshEditor()
    m.open(self.new_mesh,3,3)
    m.init_vertices(self.n_verts,self.n_verts)
    m.init_cells(self.n_tets,self.n_tets)

    # Copy vertex data into new mesh
    for i,v in enumerate(self.global_vertices):
      m.add_vertex(i,Point(*v))

    # Copy cell data into new mesh
    for j,c in enumerate(self.global_tets):
      m.add_cell(j,*c)

    m.close()

def getPeriodicOrbit(statesL, T_L, ymin_L,
                     statesR, T_R, ymin_R,
                     baseParams ,
                     startParams=[14000, 1.16, 1, 0.] ):
    """
    returns a tuple of SLIP parameters, that result in the two-step periodic
    solution defined by <statesL> -> <statesR> -> >statesL>,
    with step time left (right) = <T_L> (<T_R>)
    minimal vertical position left (right) = <ymin_L> (<ymin_R>)
    statesL/R: a list of (left/right) apex states y, vx, vz
    baseParams: dict of base SLIP parameters: g, m (gravity acceleration, mass)
    
    returns: [SL, paramsL, dEL], [SR, paramsR, dER] 
             two tuples of initial apex states and corresponding SLIP
             parameters that yield the two-step periodic solution
             (dE: energy fluctuation)
        
    """    
    SL = mean(vstack(statesL), axis=0) if len(statesL) > 1 else statesL
    SR = mean(vstack(statesR), axis=0) if len(statesR) > 1 else statesR
    tr = mean(hstack(T_R))
    tl = mean(hstack(T_L))
    yminl = mean(hstack(ymin_L))
    yminr = mean(hstack(ymin_R))
    m = baseParams['m']
    g = baseParams['g']
    # energy input right (left) step
    dER = (SL[0]-SR[0])*m*abs(g) + .5*m*(SL[1]**2 + SL[2]**2 
                                       - SR[1]**2 - SR[2]**2)
    dEL = -dER

    # initialize parameters
    PR = copy.deepcopy( baseParams )
    PL = copy.deepcopy( baseParams )
    PL['IC'] = SL    
    PL['dE'] = dEL
    PR['IC'] = SR
    PR['dE'] = dER
    
    # define step params: (y_apex2, T, y_min, vz_apex2)
    spL = (SR[0], tl, yminl, SR[2])
    spR = (SL[0], tr, yminr, SL[2])
    
    # compute necessary model parameters
    paramsL = fl.calcSlipParams3D(spL, PL, startParams)
    paramsR = fl.calcSlipParams3D(spR, PR, startParams)
    
    
    return ([SL, paramsL, dEL],[SR, paramsR, dER])

def calcJacobian(fun, x0, h=.0001):
    """
    calculates the jacobian of a given function fun with respect to its
    parameters at the point (array or list) x0.

    :args:
        fun (function): the function to calcualte the jacobian from
        x0 (iterable, e.g. array): position to evaluate the jacobian at
        h (float): step size 

    :returns:
        J (n-by-n array): the jacobian of f at x0
    """
    J = []
    x = array(x0)
    for elem, val in enumerate(x0):
        
        ICp = x.copy()
        ICp[elem] += h
        resp = fun(ICp)
        ICn = x.copy()
        ICn[elem] -= h
        resn = fun(ICn)
        J.append((resp - resn)  / (2. * h))
        
    J = vstack(J).T    
    return J

def plot_matches(im1, im2, locs1, locs2, matchscores, show_below=True):
    """
    Show a figure with lines joining the accepted matches
    input: im1, im2 (images as arrays), locs1, locs2 (feature locations),
    matchscores (as output from the match() method)
    show_below (if images should be shown below matches).
    :param im1:
    :param im2:
    :param locs1:
    :param locs2:
    :param matchscores:
    :param show_below:
    :return:
    """

    im3 = appendImages(im1, im2)
    if show_below:
        im3 = vstack((im3, im3))
        imshow(im3)

        cols1 = im1.shape[1]
        for i,m in enumerate(matchscores):
            if m>0:
                plot([locs1[i][1], locs2[m][1]+cols1], [locs1[i][0],locs2[m][0]],'c')
        axis('off')

def main():
	shifts = [
		[-1,  1], [0,  1], [1,  1],
		[-1,  0],          [1,  0],
		[-1, -1], [0, -1], [1, -1]
	]

	num_atoms = 100
	num_dims = 2 # dimensions
	coords = pl.random((num_atoms, num_dims))
	chosen = pl.random_integers(num_atoms) # from 1 to num_atoms
	chosen -= 1 # from 0 to num_atoms - 1

	for i in range(len(shifts)):
		coords = pl.vstack((coords, coords[:num_atoms] + shifts[i]))
	num_atoms *= 9 # after 8 shifts added

	max_distance = 0.9
	for i in range(num_atoms):
		if i != chosen:
			dx = coords[chosen, 0] - coords[i, 0]
			dy = coords[chosen, 1] - coords[i, 1]
			distance = pl.sqrt(dx*dx + dy*dy)
			if distance < max_distance:
				pl.plot([coords[i, 0]], [coords[i, 1]], "bo")
			else:
				pl.plot([coords[i, 0]], [coords[i, 1]], "ko")

	# plot last for visibility
	pl.plot([coords[chosen, 0]], [coords[chosen, 1]], "ro")
	pl.grid(True)
	pl.show()

def homog2D(xPrime, x):
    """
    
    Compute the 3x3 homography matrix mapping a set of N 2D homogeneous 
    points (3xN) to another set (3xN)

    """

    numPoints = xPrime.shape[1]
    assert numPoints >= 4

    A = None
    for i in range(0, numPoints):
        xiPrime = xPrime[:, i]
        xi = x[:, i]

        Ai_row0 = pl.concatenate((pl.zeros(3), -xiPrime[2] * xi, xiPrime[1] * xi))
        Ai_row1 = pl.concatenate((xiPrime[2] * xi, pl.zeros(3), -xiPrime[0] * xi))
        Ai = pl.row_stack((Ai_row0, Ai_row1))

        if A is None:
            A = Ai
        else:
            A = pl.vstack((A, Ai))

    U, S, V = pl.svd(A)
    V = V.T
    h = V[:, -1]
    H = pl.reshape(h, (3, 3))
    return H

def homog3D(points2d, points3d):
    """
    
    Compute a matrix relating homogeneous 3D points (4xN) to homogeneous
    2D points (3xN)

    Not sure why anyone would do this.  Note that the returned transformation 
    *NOT* an isometry.  But it's here... so deal with it.

    """

    numPoints = points2d.shape[1]
    assert numPoints >= 4

    A = None
    for i in range(0, numPoints):
        xiPrime = points2d[:, i]
        xi = points3d[:, i]

        Ai_row0 = pl.concatenate((pl.zeros(4), -xiPrime[2] * xi, xiPrime[1] * xi))
        Ai_row1 = pl.concatenate((xiPrime[2] * xi, pl.zeros(4), -xiPrime[0] * xi))
        Ai = pl.row_stack((Ai_row0, Ai_row1))

        if A is None:
            A = Ai
        else:
            A = pl.vstack((A, Ai))

    U, S, V = pl.svd(A)
    V = V.T
    h = V[:, -1]
    P = pl.reshape(h, (3, 4))
    return P

def dS_dP(x0, PR, keys = [('k',750.),('alpha',0.05),('L0',0.05),
                                ('beta',0.05), ('dE', 7.5) ], r_mag = .005):
    """
    calculates the SLIP derivative with respect to 'keys'
    keys is a list of tuples with the keys of PR that should be changed,
    and the order of magnitude of deviation (i.e. something like std(x))
    
    -- only for a single step --
    """
    df = []
    # r_mag = .005 # here: relative magnitude of disturbance in standrad dev's
    
    for elem,mag in keys:
        h = r_mag*mag
        # positive direction
        PRp = copy.deepcopy(PR)
        PRp[elem] += h
        resR = sl.SLIP_step3D(x0, PRp)
        SRp = array([resR['y'][-1], resR['vx'][-1], resR['vz'][-1]])
        #fhp = array(SR2 - x0)
        # positive direction
        PRn = copy.deepcopy(PR)            
        PRn[elem] -= h
        resR = sl.SLIP_step3D(x0, PRn)
        SRn = array([resR['y'][-1], resR['vx'][-1], resR['vz'][-1]])
        #fhn = array(SR2 - x0)
        # derivative: difference quotient
        df.append( (SRp - SRn)/(2.*h) )
    
    return vstack(df).T

def dS_dX(x0, PR, h_mag = .0005):
    """
    calculates the Jacobian of the SLIP at the given point x0,
    with PR beeing the parameters for that step
    coordinates under consideration are:
        y
        vx
        vz
    only for a single step!
    """
    df = []
    for dim in range(len(x0)):
        delta = zeros_like(x0)
        delta[dim] = 1.            
        h = h_mag * delta      
        # in positive direction           
        resRp = sl.SLIP_step3D(x0 + h, PR)
        SRp = array([resRp['y'][-1], resRp['vx'][-1], resRp['vz'][-1]])
        #fhp = array(SR2 - x0)
        # in negative direction
        resRn = sl.SLIP_step3D(x0 - h, PR)
        SRn = array([resRn['y'][-1], resRn['vx'][-1], resRn['vz'][-1]])
        #fhn = array(SR2 - x0)
        # derivative: difference quotient
        df.append( (SRp - SRn)/(2.*h_mag) )
    
    return vstack(df).T

 def get_kin(self, fps=250.):
     """
     returns a list of the selected kinematics (one list item for each repetition)
     
     :args:
         self: kin object
         fps (float, default 250): sampling frequency. Required to correctly compute the velocities.
 
     :returns:
         a list. Each element contains the selected (-> self.selection) data with corresponding 
            velocities (i.e. 2d x n elements per item)
     """
     # walk through each element of "selection"
     all_pos = []
     all_vel = []
     for raw in self.raw_dat:
         curr_pos = []
         curr_vel = []
         for elem in self.selection:
             items = [x.strip() for x in elem.split('-')] # 1 item if no "-" present
             dims = []
             markers = []
             for item in items:                
                 if item.endswith('_x'):
                     dims.append(0)
                 elif item.endswith('_y'):
                     dims.append(1)
                 elif item.endswith('_z'):
                     dims.append(2)
                 else:
                     print "invalid marker suffix: ", item
                     continue
                 markers.append(item[:-2])
                         
             if len(items) == 1: # single marker
                 curr_pos.append(raw[markers[0]][:, dims[0]])
                 curr_vel.append(gradient(raw[markers[0]][:, dims[0]]) * fps)
             else: # difference between two markers
                 curr_pos.append(raw[markers[0]][:, dims[0]] - raw[markers[1]][:, dims[1]])
                 curr_vel.append(gradient(raw[markers[0]][:, dims[0]] - raw[markers[1]][:, dims[1]]) * fps)
 
         all_pos.append(vstack(curr_pos + curr_vel))
         all_vel.append(vstack(curr_vel))  
         
     return all_pos        

 def _generate_labeled_correlation_matrix(self, label):
     """ Concatenates the feature names to the actual correlation matrices.
         This is for better overview in stored txt files later on."""
     labeled_corr_matrix = pylab.array([])
     for i in pylab.array(self.corr_important_feats[label]):
         if len(labeled_corr_matrix) == 0:
             labeled_corr_matrix = [[('% .2f' % j).rjust(10) for j in i]]
         else:
             labeled_corr_matrix = pylab.vstack((labeled_corr_matrix,
                                 [[('% .2f' % j).rjust(10) for j in i]]))
     
     labeled_corr_matrix = pylab.c_[self.corr_important_feat_names,
                                    labeled_corr_matrix]
     labeled_corr_matrix = pylab.vstack((pylab.hstack(('          ',
                                        self.corr_important_feat_names)),
                                        labeled_corr_matrix))
     
     return labeled_corr_matrix

 def getAllPrecNoise(self,timePreceedingSignal=-1):
     #returns the concatenated Preceeding Noise
     precNoise=py.array([])
     for tdData in self._thzdata_raw:
         tN=self.getPreceedingNoise(tdData,timePreceedingSignal)
         if precNoise.shape[0]==0:
             precNoise=tN
         else:
             precNoise=py.vstack((precNoise,tN))
     return precNoise

    def get_kin_apex(self, phases, return_times = False):
        """
        returns the kinematic state at the apices which are close to the given phases. Apex is re-calculated.
        
        :args:
            self: kin object (-> later: "self")
            phases (list): list of lists of apex phases. must match with length of "kin.raw_data". 
               The n'th list of apex phases will be assigned to the nth "<object>.raw_data" element.
            return_times (bool): if true, return only the times at which apex occurred.
    
        :returns:
           if lr_split is True:
              [[r_apices], [l_apices]]
           else:
              [[apices], ]
              where apices is the kinematic (from <object>.selection at the apices *around* the given phases.
              *NOTE* The apices themselves are re-computed for higher accuracy.
    
        """
        
        all_kin = []
        all_kin_orig = self.get_kin()
        all_apex_times = []
        if len(self.raw_dat) != len(phases):
            raise ValueError("length of phases list does not match number of datasets")
        for raw, phase, kin_orig in zip(self.raw_dat, phases, all_kin_orig):
            kin_apex = []
            kin_time = arange(len(raw['phi2'].squeeze()), dtype=float) / 250.
            # 1st: compute apex *times*
            apex_times = []
            for phi_apex in phase:
                # 1a: get "rough" estimate
                idx_apex = argmin(abs(raw['phi2'].squeeze() - phi_apex))
                # 1b: fit quadratic function to com_y
                idx_low = max(0, idx_apex - 4)
                idx_high = min(raw['com'].shape[0] - 1, idx_apex + 4)
                com_y_pt = raw['com'][idx_low:idx_high + 1, 2]            
                tp = arange(idx_high - idx_low + 1) # improve numerical accuracy: do not take absolute time
                p = polyfit(tp, com_y_pt, 2) # p: polynomial, highest power coeff first
                t0 = -p[1] / (2.*p[0]) # "real" index of apex (offset is 2: a value of 2
                           # indicates that apex is exactly at the measured frame
                t_apex = kin_time[idx_apex] + (t0 - 4.) / 250.
                apex_times.append(t_apex)
            
            if return_times:
                all_apex_times.append(array(apex_times))		    
            else:
                # 2nd: interpolate data
                dat = vstack([interp(apex_times, kin_time, kin_orig[row, :]) for row in arange(kin_orig.shape[0])])
                all_kin.append(dat)

        if return_times:
	    return all_apex_times

        return all_kin

 def test_sim_data_2(self): 
     sims = 10000 
     return # skip for now
     test1 = pl.zeros(3, dtype='f').view(pl.recarray)
     for i in range(sims): 
         temp = data.sim_data(1, [0.1,0.1,0.8], [0.01,0.01,0.01])
         test1 = pl.vstack((test1, temp))
     test1 = test1[1:,]
     test2 = data.sim_data(sims, [0.1,0.1,0.8], [0.01, 0.01, 0.01])
     diff = (test1.mean(0) - test2.mean(0))/test1.mean(0)
     assert pl.allclose(diff, 0, atol=0.01), 'should be close to zero, (%s found)' % str(diff)

def sinDistort(data,twisting=1.):
    """
    a distortion of the data:
        x will be mapped to sin(x/max(abs(x))), for every coordinate
    this is to distort a lower dimensional system so that it is not
    restricted to a lower-dimensional subspace any longer
    data must be given in NxD - Format
    the optional twisting factor increases the twisting strength
    """
    return vstack([sin( data[:,x]*twisting/max(abs(data[:,x])))*max(abs(data[:,x]))
                   for x in range(data.shape[1])]).T

def block_hankel(data, f):
    """
    Create a block hankel matrix.
    f : number of rows
    """
    data = pl.matrix(data)
    assert len(data.shape) == 2
    n = data.shape[1] - f
    return pl.matrix(pl.hstack([
        pl.vstack([data[:, i+j] for i in range(f)])
        for j in range(n)]))

	def LS(self,X):

		"""
			estimate the connectivity kernel parameters and the time constant parameter using Least Square method
	
			Arguments
			----------
			X: list of matrix
				state vectors

			Returns
			---------
			Least Square estimation of the the connectivity kernel parameters and the time constant parameter 
		"""
		q=self.q_calc(X)
		Z=pb.vstack(X[1:])
		X_t_1=pb.vstack(X[:-1])
		q_t_1=pb.vstack(q[:-1])
		X_ls=pb.hstack((q_t_1,X_t_1))
		W=(X_ls.T*X_ls).I*X_ls.T*Z
		return [float( W[0]),float(W[1]),float(W[2]),float(W[3])]

def approximate(x,y):
    """
    Linear approximation of y=f(x) using least square estimator.
    In:
        x : ndarray
        y : ndarray
    Out:
        a, b : float, as in a*x+b=y
    """
    assert pl.shape(x) == pl.shape(y)
    A = pl.vstack([x, pl.ones(len(x))]).T
    a, b = pl.lstsq(A, y)[0]
    return a, b