def setLatBase(self, base):
        """Set matrix of unit cell base vectors and calculate corresponding
        lattice parameters and stdbase, baserot and metrics tensors.

        No return value.
        """
        self.base = numpy.array(base)
        detbase = numalg.det(self.base)
        if abs(detbase) < 1.0e-8:
            emsg = "base vectors are degenerate"
            raise LatticeError(emsg)
        elif detbase < 0.0:
            emsg = "base is not right-handed"
            raise LatticeError(emsg)
        self._a = a = math.sqrt(numpy.dot(self.base[0,:], self.base[0,:]))
        self._b = b = math.sqrt(numpy.dot(self.base[1,:], self.base[1,:]))
        self._c = c = math.sqrt(numpy.dot(self.base[2,:], self.base[2,:]))
        self._ca = ca = numpy.dot(self.base[1,:], self.base[2,:]) / (b*c)
        self._cb = cb = numpy.dot(self.base[0,:], self.base[2,:]) / (a*c)
        self._cg = cg = numpy.dot(self.base[0,:], self.base[1,:]) / (a*b)
        self._sa = sa = math.sqrt(1.0 - ca**2)
        self._sb = sb = math.sqrt(1.0 - cb**2)
        self._sg = sg = math.sqrt(1.0 - cg**2)
        self._alpha = math.degrees(math.acos(ca))
        self._beta = math.degrees(math.acos(cb))
        self._gamma = math.degrees(math.acos(cg))
        # cache the unit volume value
        Vunit = self.unitvolume
        # reciprocal lattice
        self._ar = ar = sa/(self.a*Vunit)
        self._br = br = sb/(self.b*Vunit)
        self._cr = cr = sg/(self.c*Vunit)
        self._car = car = (cb*cg - ca)/(sb*sg)
        self._cbr = cbr = (ca*cg - cb)/(sa*sg)
        self._cgr = cgr = (ca*cb - cg)/(sa*sb)
        self._sar = sar = math.sqrt(1.0 - car**2)
        self._sbr = sbr = math.sqrt(1.0 - cbr**2)
        self._sgr = sgr = math.sqrt(1.0 - cgr**2)
        self._alphar = math.degrees(math.acos(car))
        self._betar = math.degrees(math.acos(cbr))
        self._gammar = math.degrees(math.acos(cgr))
        # standard orientation of lattice vectors
        self.stdbase = numpy.array([
                [ 1.0/ar,   -cgr/sgr/ar,    cb*a ],
                [ 0.0,       b*sa,          b*ca ],
                [ 0.0,       0.0,           c    ]],
                dtype=float)
        # calculate unit cell rotation matrix,  base = stdbase*baserot
        self.baserot = numpy.dot(numalg.inv(self.stdbase), self.base)
        self.recbase = numalg.inv(self.base)
        # bases normalized to unit reciprocal vectors
        self.normbase = self.base * [[ar], [br], [cr]]
        self.recnormbase = self.recbase / [ar, br, cr]
        # update metrics tensor
        self.metrics = numpy.array([
                [ a*a,     a*b*cg,  a*c*cb ],
                [ b*a*cg,  b*b,     b*c*ca ],
                [ c*a*cb,  c*b*ca,  c*c    ]],
                dtype=float)
        return

 def find_position(self, mirror=False):
     n_v = self.p0 - self.apex
     a, b, c = n_v[0], n_v[1], n_v[2]
     x0, y0, z0 = self.apex[0], self.apex[1], self.apex[2]
     ref_p = 0
     if c != 0.0:
         ref_p = np.array([1.0, 1.0, (a * (x0 - 1.0) + b * (y0 - 1.0)) / c + z0])
     elif b != 0.0:
         ref_p = np.array([1.0, (a * (x0 - 1.0) + c * (z0 - 1.0)) / b + y0, 1.0])
     else:
         ref_p = np.array([(b * (y0 - 1.0) + c * (z0 - 1.0)) / a + x0, 1.0, 1.0])
     z_v = f3(np.zeros(3), (self.p0 - self.apex))
     x_v = f3(np.zeros(3), (ref_p - self.apex))
     y_v = np.cross(z_v, x_v)
     T = f1(x0_v, y0_v, z0_v, x_v, y_v, z_v)
     theta = self.top_angle
     phi_p1 = self.phi
     phi_p2 = self.phi
     if mirror == True:
         phi_p2 = phi_p2 + self.bottom_angle
     else:
         phi_p2 = phi_p2 - self.bottom_angle
     r0 = self.edge
     p1_new = np.array(
         [r0 * np.cos(phi_p1) * np.sin(theta), r0 * np.sin(phi_p1) * np.sin(theta), r0 * np.cos(theta)]
     )
     p2_new = np.array(
         [r0 * np.cos(phi_p2) * np.sin(theta), r0 * np.sin(phi_p2) * np.sin(theta), r0 * np.cos(theta)]
     )
     self.p1 = np.dot(inv(T), p1_new) + self.apex
     self.p2 = np.dot(inv(T), p2_new) + self.apex

def test_cl():
    ctx = cl.create_some_context()  # (interactive=False)

    # print 'ctx', ctx
    queue = cl.CommandQueue(ctx, properties=cl.command_queue_properties.PROFILING_ENABLE)
    f = open("part1.cl", "r")
    fstr = "".join(f.readlines())
    program = cl.Program(ctx, fstr).build()
    mf = cl.mem_flags

    cameraPos = np.array([0, 6, -1, 0])
    invView = la.inv(look_at((0, 6, -1), (0, 1, 1), (0, 1, 0)))
    invProj = la.inv(perspective(60, 1, 1, 1000))
    print "view", invView
    print "proj", invProj
    viewParamsData = (
        cameraPos.flatten().tolist()
        + np.transpose(invView).flatten().tolist()
        + np.transpose(invProj).flatten().tolist()
    )
    # print 'vpd', viewParamsData
    viewParams = struct.pack("4f16f16f", *viewParamsData)
    viewParams_buf = cl.Buffer(ctx, mf.READ_ONLY | mf.COPY_HOST_PTR, hostbuf=viewParams)
    num_pixels = 1000 * 1000
    # setup opencl
    dest = np.ndarray((1000, 1000, 4), dtype=np.float32)
    dest_buf = cl.Buffer(ctx, mf.WRITE_ONLY, dest.nbytes)
    local_shape = (8, 8)
    # run kernel
    evt = program.part1(queue, (dest.shape[0], dest.shape[1]), None, viewParams_buf, dest_buf)
    # evt = program.part1(queue, dest.shape, None, dest_buf)
    cl.enqueue_read_buffer(queue, dest_buf, dest).wait()
    print "time", (evt.profile.end - evt.profile.start) * 0.000001, "ms"
    return dest

def test_gruber():
  from numpy import dot
  from numpy.linalg import inv
  from pylada.math import gruber, is_integer
  from pylada.error import internal, input
  
  cell = [[0, 0.5, 0.5], [0.5, 0, 0.5], [0.5, 0.5, 0]]
  lim = 5
   
  for a00 in [-1, 1]: 
    for a10 in xrange(-lim, lim+1): 
      for a11 in [-1, 1]:
        for a20 in xrange(-lim, lim+1): 
          for a21 in xrange(-lim, lim+1): 
            for a22 in [-1, 1]:
              a = [[a00, 0, 0], [a10, a11, 0], [a20, a21, a22]]
              g = gruber(dot(cell, a))
              assert is_integer(dot(inv(cell), g))
              assert is_integer(dot(inv(g), cell))

  try: gruber([[0, 0, 0], [1, 2, 0], [4, 5, 6]])
  except input: pass
  else: raise Exception()

  try: gruber([[1, 0, 0], [1, 1, 0], [4, 5, 1]], itermax=2)
  except internal: pass
  else: raise Exception()

    def get_original(self, model, view, projection, screen_size, point2d):
        #print(point2d)
        point2d = 2 * point2d / screen_size - 1
        point = np.array([point2d[0], point2d[1], 1, 1])

        # transform model and view to matrices
        model = transformation_matrix(model.translation, model.orientation)
        view = transformation_matrix(view.translation, view.orientation)

        # full matrix os transformation from point in object's coordinates
        # to camera's coordinates
        transformation = projection.dot(view).dot(model)

        inverse_transformation = lin.inv(transformation)

        # some point on the same ray as original point
        point = inverse_transformation.dot(point).A1

        # position of camera in object's coordinates
        camera_point = lin.inv(view.dot(model)).dot(ORIGIN).A1

        # convert points to useful coordinates
        point = _to_cartesian_coordinates(point)
        camera_point = _to_cartesian_coordinates(camera_point)

        ray = Ray(camera_point, point)
        # original point in object's coordinates
        point3d = self._intersect(ray)

        if point3d is None:
            return None
        # original point in world's coordinates
        world_point = _to_homogeneous_coordinates(point3d)
        world_point = model.dot(world_point).A1
        return world_point

def part_b(run_count, a, b):
    """
    Solve using LU decomposition
    """
    _, l, u = lu(a)
    for run in xrange(run_count):
        inv(u).dot(inv(l).dot(b))

def initRTI(nodeLocs, delta_p, sigmax2, delta, excessPathLen):

    # Set up pixel locations as a grid.
    personLL        = nodeLocs.min(axis=0)
    personUR        = nodeLocs.max(axis=0)
    pixelCoords, xVals, yVals = calcGridPixelCoords(personLL, personUR, delta_p)
    pixels          = pixelCoords.shape[0]
    #plt.figure(3)
    #plotLocs(pixelCoords)
    

    # Find distances between pixels and transceivers
    DistPixels  = dist.squareform(dist.pdist(pixelCoords))
    DistPixelAndNode = dist.cdist(pixelCoords, nodeLocs)
    DistNodes   = dist.squareform(dist.pdist(nodeLocs))

    # Find the (inverse of) the Covariance matrix between pixels
    CovPixelsInv       = linalg.inv(sigmax2*np.exp(-DistPixels/delta))

    # Calculate weight matrix for each link.
    nodes = len(nodeLocs)
    links = nodes*(nodes-1)
    W = np.zeros((links, pixels))
    for ln in range(links):
        txNum, rxNum  = txRxForLinkNum(ln, nodes)
        ePL           = DistPixelAndNode[:,txNum] + DistPixelAndNode[:,rxNum] - DistNodes[txNum,rxNum]  
        inEllipseInd  = np.argwhere(ePL < excessPathLen)
        pixelsIn      = len(inEllipseInd)
        if pixelsIn > 0:
            W[ln, inEllipseInd] = 1.0 / float(pixelsIn)

    # Compute the projection matrix
    inversion       = np.dot(linalg.inv(np.dot(W.T, W) + CovPixelsInv), W.T)

    return (inversion, xVals, yVals)

def doTheMath(file1, file2):  
    matrixAleft = numpy.loadtxt(file1, dtype=numpy.float32, usecols=[1, 2])
    matrixAxtion = numpy.loadtxt(file2, dtype=numpy.float32)

    matrixA_x = numpy.array([[matrixAleft[0][0], (dest_size_x-matrixAleft[0][0])]])
    matrixb_x = numpy.array([ matrixAxtion[0][0] * dest_size_x ])
    for i in range(1, 54):
        matrixAi = numpy.array([[ matrixAleft[i][0], (dest_size_x-matrixAleft[i][0]) ]])
        matrixA_x = numpy.append(matrixA_x, matrixAi, axis=0)
        matrixb_x = numpy.append(matrixb_x, [ matrixAxtion[i][0] * dest_size_x ])

    ATb_x = numpy.dot (numpy.transpose(matrixA_x), matrixb_x)
    ATAinverse_x = linalg.inv( numpy.dot( numpy.transpose(matrixA_x), matrixA_x ) )
    x = numpy.dot (ATAinverse_x, ATb_x)

    print '[horizon_x, start_x] is', x

    matrixA_y = numpy.array([[matrixAleft[0][1], (dest_size_y-matrixAleft[0][1])]])
    matrixb_y = numpy.array([ matrixAxtion[0][1] * dest_size_y ])
    for i in range(1, 54):
        matrixAi = numpy.array([[ matrixAleft[i][1], (dest_size_y-matrixAleft[i][1]) ]])
        matrixA_y = numpy.append(matrixA_y, matrixAi, axis=0)
        matrixb_y = numpy.append(matrixb_y, [ matrixAxtion[i][1] * dest_size_y ])

    ATb_y = numpy.dot (numpy.transpose(matrixA_y), matrixb_y)
    ATAinverse_y = linalg.inv( numpy.dot( numpy.transpose(matrixA_y), matrixA_y ) )
    y = numpy.dot (ATAinverse_y, ATb_y)

    print '[horizon_y, start_y] is', y

    def InitializeElectrons(self):
        if self.N_up == 0 and self.N_down == 0:
            print 'Error: no electrons to initialize'
            return []
        else:
            # generate array of electron positions, normally distributed from the origin with Bohr radius
            n = self.N_up+self.N_down
            self.e_positions = np.random.randn(n,3) * GSF.a_B # generate array of electron positions
            print 'init e_pos',self.e_positions
            # Store displacements and distances
            self.e_disp = np.zeros((n,n,3)) # store the displacements in a 3D matrix to make indexing easier
            self.e_dist = np.zeros((n,n)) # the electron matrices should only be upper diagonal
            self.atom_disp = np.zeros((n,len(self.atom_list),3))
            self.atom_dist = np.zeros((n,len(self.atom_list)))
            index = 0
            for i in range(n):
                self.e_disp[i,i+1:] = self.e_positions[i] - self.e_positions[i+1:]
                self.e_dist[i,i+1:] = np.sqrt(np.sum(self.e_disp[i,i+1:]**2,1))
                self.atom_disp[i] = self.e_positions[i] - self.ion_positions
                self.atom_dist[i,:] = np.sqrt(np.sum(self.atom_disp[i,:]**2,1))
 	      #Once the e_position is initialize, the slater matrix and its deteriminant and inverse are all initialized. 
        self.slater_matrix_up = SlaterMatrix(self.e_positions[0:self.N_up],self.psi_up)
        self.slater_matrix_down = SlaterMatrix(self.e_positions[self.N_up:],self.psi_down)
        print 'slater_matrix',self.slater_matrix_up
        if self.N_up>0: 
            self.inverse_SD_up = LA.inv(self.slater_matrix_up)
            self.slater_det_up = LA.det(self.slater_matrix_up)			
        if self.N_down>0: 
            self.inverse_SD_down = LA.inv(self.slater_matrix_down) 
            self.slater_det_down = LA.det(self.slater_matrix_down)
        self.J = self.Jastrow()
        print 'slater_inv',self.inverse_SD_up
        return self.e_positions

def get_sdrefb_upd(amat, t, fbtype=None, wnrm=2,
                   B=None, R=None, Q=None, maxeps=None,
                   baseA=None, baseZ=None, baseP=None, maxfac=None, **kwargs):
    if fbtype == 'sylvupdfb' or fbtype == 'singsylvupd':
        if baseP is not None:
            deltaA = amat - baseA
            epsP = spla.solve_sylvester(amat, -baseZ, -deltaA)
            eps = npla.norm(epsP, ord=wnrm)
            print('|amat - baseA|: {0} -- |E|: {1}'.
                  format(npla.norm(deltaA, ord=wnrm), eps))
            if maxeps is not None:
                if eps < maxeps:
                    opepsPinv = npla.inv(epsP+np.eye(epsP.shape[0]))
                    return baseP.dot(opepsPinv), True
            elif maxfac is not None:
                if (1+eps)/(1-eps) < maxfac and eps < 1:
                    opepsPinv = npla.inv(epsP+np.eye(epsP.shape[0]))
                    return baseP.dot(opepsPinv), True

    # otherwise: (SDRE feedback or `eps` too large already)
    # curX = spla.solve_continuous_are(amat, B, Q, R)
    # if fbtype == 'sylvupdfb' or fbtype == 'singsylvupd':
    #     logger.debug('in `get_fb_dict`: t={0}: eps={1} too large, switch!'.
    #                  format(t, eps))
    # else:
    #     logger.debug('t={0}: computed the SDRE feedback')
    return None, False

    def de_embed(self, pad_in, s2p, pad_out):

        left_mat = pad_in["data"][0]["body"]
        right_mat = pad_out["data"][0]["body"]

        for sec_idx in range(len(s2p["data"])):

            section = s2p["data"][sec_idx]

            for i in range(len(section["body"])):

                left = map(float, left_mat[i][1:])
                l_abcd = self.get_ABCD(left)

                center = map(float, section["body"][i][1:])
                c_abcd = self.get_ABCD(center)

                right = map(float, right_mat[i][1:])
                r_abcd = self.get_ABCD(right)

                [[A, B], [C, D]] = inv(l_abcd).dot(c_abcd).dot(inv(r_abcd))
                [s11, s21, s12, s22] = self.abcd2s(A, B, C, D)
                section["body"][i][1:] = \
                    [s11.real, s11.imag, s21.real, s21.imag, s12.real, s12.imag, s22.real, s22.imag]

        return s2p

 def iterate_filter(self, dyn_model, dyn_noise_model, dyn_noise_cov,
         iters = 1, epsilon = 1, verbose=False):
     A = dyn_model
     B = dyn_noise_model
     Q = dyn_noise_cov
     x_prev = deepcopy(self.x_est)
     P_prev = deepcopy(self.P_est)
     for i in self.network.keys():
         yi = self.sensors[i].u[:,0]
         Si = self.sensors[i].U[:,:]
         for nbr in self.network[i]:
             logging.debug("Reading sensor locals {}".format(nbr))
             # Do NOT use +=, it will modify the objects
             # within the elements
             yi = yi + self.sensors[nbr].u[:,0]
             Si = Si + self.sensors[nbr].U[:,:]
             logging.debug(self.sensors[nbr].U[:,:])
         for it in xrange(iters):
             Mi = LA.inv(LA.inv(P_prev[i]) + Si)
             x_hat = self.x_est[i] + \
                     np.dot(Mi, (yi - np.dot(Si, self.x_est[i]) ) )
             for nbr in self.network[i]:
                 logging.debug(self.x_est[nbr].shape)
                 logging.debug(self.x_est[i].shape)
                 x_hat += np.dot(Mi, (self.x_est[nbr] - self.x_est[i])) * epsilon
             # Update the state of the filter
             self.P_est[i] = np.dot(A, np.dot(Mi, A.T)) + np.dot(B, np.dot(Q, B.T))
             self.x_est[i] = np.dot(A, x_hat)
         if verbose:
             print "Current estimate from {}: {}".format(i, self.x_est[i].T)

def fcvrot(f,theta):
    if asarray(f.shape).prod() == 1:
    	return f
    if len(f.shape) == 1:
        f = asarray([f])
    x,y = numpy.indices(f.shape)
    theta = radians(theta)
    Trot = asarray([
                [cos(theta),    sin(theta), 0],
                [-sin(theta),   cos(theta), 0],
                [0,             0,          1]
            ])

    Tx   = asarray([
                [1, 0,  f.shape[0]//2],
                [0, 1,  f.shape[0]//2],
                [0, 0,  1]
            ])
    T = dot(dot(Tx,Trot),inv(Tx))
    
    pt = array([ x.ravel(), y.ravel(), ones(len(x.ravel()))])
    npt= dot(inv(T), pt)
    npt= numpy.round(npt).astype(int)
    
    g = f[npt[0],npt[1]].reshape(f.shape)
    return g

def marginal_log_likelihood(y, A,Q,C,R, _x,_P):
    """
    compute the marginal log likelihood
    given the learned model parameters
    and the learned expected sufficient statistics

    C must be invertible (:= square and non-singular)
    eg aint no inverse to a fucking projection

    p(y|params) = ...
    """

    if True: return 0

    T,p = y.shape
    _,d = _x.shape
    Cy = mul(inv(C), tp(y))
    CRC = inv(mul(tp(C),inv(R),C))

    Z_swap = (1/2) * (log(det(2*pi*CRC)) - log(det(2*pi*R)))

    covar = CRC+identity(d)
    Z_merge = log(2*pi*det(covar))**(-d/2) - (1/2)*mul(tp(Cy), covar, Cy)

    for t in r(1,T-1):
        mean  = _x[t+1]
        covar = CRC + _P[t+1]
        Z_merge += log(2*pi*det(covar))**(-d/2) - (1/2)*mul(tp(Cy-mean), covar, (Cy-mean))

    return sum(T*Z_swap + Z_merge)

    def objective(self, A, r, c):
        """
        Log-likelihood function and gradient for MLLT::

          L(A) = N|A| - \\sum_j \\frac{N_j}{2} \\log |diag(A \\Sigma_j A^T)|
          \\nabla L(A) = N(A^T)^{-1} - \\sum_j N_j diag(A \\Sigma_j A^T)^{-1}A\\Sigma_j

        @param A: Flattened MLLT transformation matrix
        @type A: numpy.ndarray
        @param r: Actual number of rows in MLLT transformation
        @type r: int
        @param c: Actual number of columns in MLLT transformation
        @type c: int
        @return: negated log-likelihood and (flattened) gradient
        @rtype: (float, numpy.ndarray)
        """
        # Note: A has been flattened to make it acceptable to scipy.optimize
        A = A.reshape((r,c))
        detA = det(A)
        ll = self.totalcount * log(detA)
        lg = self.totalcount * inv(A.T)
        for j, nj in enumerate(self.count):
            C = self.cov[j]
            cl = diag(dot(dot(A, C), A.T))
            ll = ll - (float(nj) / 2) * sum(log(cl))
            lg = lg - float(nj) * dot(dot(inv(diag(cl)), A), C)
        print "likelihood: %f" % ll
        # Flatten out the gradient
        lg = lg.ravel()
        print "gradient L2: %f" % sqrt(sum(lg*lg))
        # Note: we negate these to maximize likelihood
        return -ll, -lg

def adjacent_open_to_shut_range_mean(u1, u2, QAA, QAF, QFF, QFA, phiA):
    """
    Calculate mean (ideal- no missed events) open times adjacent to a 
    specified shut time range.

    Parameters
    ----------
    u1, u2 : floats
        Shut time range.
    QAA, QAF, QFF, QFA : array_like
        Submatrices of Q.
    phiA : array_like, shape (1, kA)
        Initial vector for openings

    Returns
    -------
    m : float
        Mean open time.
    """
    
    kA = QAA.shape[0]
    uA = np.ones((kA))[:,np.newaxis]
    invQAA, invQFF = -nplin.inv(QAA), nplin.inv(QFF)
    expQFFr = qml.expQt(QFF, u2) - qml.expQt(QFF, u1)
    col = np.dot(np.dot(np.dot(np.dot(QAF, invQFF), expQFFr), QFA), uA)
    row1 = np.dot(phiA, qml.Qpow(invQAA, 2))
    row2 = np.dot(phiA, invQAA)
    m = np.dot(row1, col)[0, 0] / np.dot(row2, col)[0, 0]
    return m

def lda(class1,class2,N):
	N1 = len(class1)/float(N)
	N2 = len(class2)/float(N) 
	mu1 = np.matrix([np.sum(class1[:,0])/len(class1),np.sum(class1[:,1])/len(class1)])
	mu2 = np.matrix([np.sum(class2[:,0])/len(class2),np.sum(class2[:,1])/len(class2)])
	sigma1 = np.zeros(shape=(2,2))
	for i in range(0,len(class1)):
		a = class1[i][0]-mu1[0,0]
		b = class1[i][1]-mu1[0,1]
		X = np.matrix([a,b])
		z = np.dot(X.T,X)
		sigma1 = sigma1 + z
	sigma1 = sigma1/(N-2)
	sigma2 = np.zeros(shape=(2,2))
	for i in range(0,len(class2)):
		a = class2[i][0]-mu2[0,0]
		b = class2[i][1]-mu2[0,1]
		X = np.matrix([a,b])
		z = np.dot(X.T,X)
		sigma2 = sigma2 + z
	sigma2 = sigma2/(N-2)
	sigma = (sigma1 + sigma2)
	a0 = log(N1/N2)-0.5*((mu1+mu2)*inv(sigma)*((mu1-mu2).T))
	A0 = np.squeeze(np.asarray(a0))
	a = inv(sigma)*((mu1-mu2).T)
	A = np.squeeze(np.asarray(a))
	return A0, A, sigma1, sigma2, mu1, mu2

    def update(self, x, x_upper, mu, sigma, M, V, theta, eps):
        # lambda from equation 7
        foo = (V - x_upper * x.T * np.sum(sigma, axis=1)) / M**2 + V * theta**2 / 2.
        a = foo**2 - V**2 * theta**4 / 4
        b = 2 * (eps - np.log(M)) * foo
        c = (eps - np.log(M))**2 - V * theta**2

        a,b,c = a[0,0], b[0,0], c[0,0]

        lam = np.amax([0,
                  (-b + sqrt(b**2 - 4 * a * c)) / (2. * a),
                  (-b - sqrt(b**2 - 4 * a * c)) / (2. * a)])
        # bound it due to numerical problems
        lam = np.minimum(lam, 1E+7)

        # update mu and sigma
        U_sqroot = 0.5 * (-lam * theta * V + sqrt(lam**2 * theta**2 * V**2 + 4*V))
        mu = mu - lam * sigma * (x - x_upper) / M
        sigma = inv(inv(sigma) + theta * lam / U_sqroot * diag(x)**2)
        """
        tmp_sigma = inv(inv(sigma) + theta*lam/U_sqroot*diag(xt)^2);
        % Don't update sigma if results are badly scaled.
        if all(~isnan(tmp_sigma(:)) & ~isinf(tmp_sigma(:)))
            sigma = tmp_sigma;
        end
        """
        return mu, sigma

 def cal_apex_coor(self,switch=False,phi=0.,mirror=False):
 #basis idea: set a new coordinate frame with p0p1 as the z vector (start from p1)
 #set a arbitrary y vector on the normal plane, and cross product to solve the x vector
 #then use phi and theta (sharp angle) to sove the cross_point(CP on file) and apex (A on file)
 #note phi is in range of [0,2pi]
 
     p0,p1=self.p0,self.p1
     if switch==True:
         p0,p1=self.p1,self.p0
     n_v=p0-p1
     origin=p1
     a,b,c=n_v[0],n_v[1],n_v[2]
     x0,y0,z0=p1[0],p1[1],p1[2]
     ref_p=0
     if c!=0.:
         ref_p=np.array([1.,1.,(a*(x0-1.)+b*(y0-1.))/c+z0])
     elif b!=0.:
         ref_p=np.array([1.,(a*(x0-1.)+c*(z0-1.))/b+y0,1.])
     else:
         ref_p=np.array([(b*(y0-1.)+c*(z0-1.))/a+x0,1.,1.])
     y_v=f3(np.zeros(3),(ref_p-origin))
     z_v=f3(np.zeros(3),(p0-origin))
     x_v=np.cross(y_v,z_v)
     T=f1(x0_v,y0_v,z0_v,x_v,y_v,z_v)
     r1=self.len_offset[0]
     r2=self.len_offset[0]+self.edge_len
     theta=self.sharp_angle
     cross_pt_new = np.array([r1*np.cos(phi)*np.sin(theta),r1*np.sin(phi)*np.sin(theta),r1*np.cos(theta)])
     apex_new = np.array([r2*np.cos(phi)*np.sin(theta),r2*np.sin(phi)*np.sin(theta),r2*np.cos(theta)])
     self.cross_pt = np.dot(inv(T),cross_pt_new)+origin
     self.apex = np.dot(inv(T),apex_new)+origin
     self.cal_p2(p0,p1,mirror)

 def polar(self, i):
     """Return the polar angle for the specified peak"""
     Oimat = inv(self.Omat)
     Mat = self.pixel_size * inv(self.Dmat) * Oimat
     peak = Oimat * (vec(self.xp[i], self.yp[i]) - self.Cvec)
     v = norm(Mat * peak)
     return np.arctan(v / self.distance)