def test_main():
    from numpy import mat

    ## Test FITELLIPSE - run through all possibilities
    # Example
    ## 1) Linear fit, bookstein constraint
    # Data points
    x = mat("1 2 5 7 9 6 3 8; 7 6 8 7 5 7 2 4")

    z, a, b, alpha = fitellipse(x, "linear")

    ## 2) Linear fit, Trace constraint
    # Data points
    x = mat("1 2 5 7 9 6 3 8; 7 6 8 7 5 7 2 4")

    z, a, b, alpha = fitellipse(x, "linear", constraint="trace")

    ## 3) Nonlinear fit
    # Data points
    x = mat("1 2 5 7 9 6 3 8; 7 6 8 7 5 7 2 4")

    z, a, b, alpha = fitellipse(x)

    # Changing the tolerance, maxits
    z, a, b, alpha = fitellipse(x, tol=1e-8, maxits=100)

    """

    def test_rotate_inertia(self):
        """Are we obtaining the global inertia properly?"""

        # Create parameters.
        label = "seg1"
        pos = np.array([[1], [2], [3]])
        rot = inertia.rotate_space_123([pi / 2, pi / 2, pi / 2])
        solids = [self.solidAB, self.solidBC, self.solidCD]
        color = (1, 0, 0)

        # Create the segment.
        seg1 = seg.Segment(label, pos, rot, solids, color)

        # This inertia matrix describes two 1kg point masses at (0, 2, 1) and
        # (0, -2, -1) in the global reference frame, A.
        seg1._rel_inertia = mat([[10.0, 0.0, 0.0], [0.0, 2.0, -4.0], [0.0, -4.0, 8.0]])

        # If we want the inertia about a new reference frame, B, such that the
        # two masses lie on the yb axis we can rotate about xa through the angle
        # arctan(1/2). Note that this function returns R from va = R * vb.
        seg1._rot_mat = inertia.rotate_space_123((arctan(1.0 / 2.0), 0.0, 0.0))

        seg1.calc_properties()

        I_b = seg1.inertia

        expected_I_b = mat([[10.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 10.0]])

        testing.assert_allclose(I_b, expected_I_b)

def _test_matrix():
    #  A = numpy.mat([[1,2,3],[4,5,6]],numpy.float32)
    #  B = numpy.mat([[2,3],[4,5],[6,7]],numpy.float32)
    #  A = numpy.mat( numpy.array( numpy.random.random_sample((256,8192)), numpy.float32) )
    #  B = numpy.mat( numpy.array( numpy.random.random_sample((8192,256)), numpy.float32) )
    #  A = numpy.mat( numpy.array( numpy.random.random_sample((257,8191)), numpy.float32) )
    #  B = numpy.mat( numpy.array( numpy.random.random_sample((8191,257)), numpy.float32) )
    A = numpy.mat(numpy.array(numpy.random.random_sample((256, 65536)), numpy.float32))
    B = numpy.mat(numpy.array(numpy.random.random_sample((65536, 256)), numpy.float32))
    #  A = numpy.mat( numpy.array( numpy.random.random_sample((200,3000)), numpy.float32) )
    #  B = numpy.mat( numpy.array( numpy.random.random_sample((3000,3000,)), numpy.float32) )
    #  A = numpy.mat( numpy.array( numpy.random.random_sample((2048,2048)), numpy.float32) )
    #  B = numpy.mat( numpy.array( numpy.random.random_sample((2048,2048)), numpy.float32) )
    #  i = 3000
    #  A = numpy.mat( numpy.array( numpy.random.random_sample((i,i)), numpy.float32) )
    #  B = numpy.mat( numpy.array( numpy.random.random_sample((i,i)), numpy.float32) )
    #  A = numpy.mat([[1,2,3,4],[5,6,7,8],[9,10,11,12],[13,14,15,16]],numpy.float32)
    #  B = numpy.mat([[1,2,3,4],[5,6,7,8],[9,10,11,12],[13,14,15,16]],numpy.float32)
    #  print A
    #  print A*B
    print "[pycublas] shapes: ", A.shape, "*", B.shape, "=", (A.shape[0], B.shape[1])
    start = time.time()
    C1 = (CUBLASMatrix(A) * CUBLASMatrix(B)).np_matrix()
    t1 = time.time() - start
    print "[pycublas] time (CUBLAS): %fs" % t1
    start = time.time()
    C2 = A * B
    t2 = time.time() - start
    print "[pycublas] time (numpy): %fs" % t2
    print "[pycublas] speedup: %.1fX" % (t2 / t1)
    print "[pycublas] error (average per cell):", numpy.abs(C1 - C2).sum() / C2.size

    def test_lsim_double_integrator(self):
        # Note: scipy.signal.lsim fails if A is not invertible
        A = np.mat("0. 1.;0. 0.")
        B = np.mat("0.; 1.")
        C = np.mat("1. 0.")
        D = 0.
        sys = StateSpace(A, B, C, D)

        def check(u, x0, xtrue):
            _t, yout, xout = forced_response(sys, t, u, x0)
            np.testing.assert_array_almost_equal(xout, xtrue, decimal=6)
            ytrue = np.squeeze(np.asarray(C.dot(xtrue)))
            np.testing.assert_array_almost_equal(yout, ytrue, decimal=6)

        # test with zero input
        npts = 10
        t = np.linspace(0, 1, npts)
        u = np.zeros_like(t)
        x0 = np.array([2., 3.])
        xtrue = np.zeros((2, npts))
        xtrue[0, :] = x0[0] + t * x0[1]
        xtrue[1, :] = x0[1]
        check(u, x0, xtrue)

        # test with step input
        u = np.ones_like(t)
        xtrue = np.array([0.5 * t**2, t])
        x0 = np.array([0., 0.])
        check(u, x0, xtrue)

        # test with linear input
        u = t
        xtrue = np.array([1./6. * t**3, 0.5 * t**2])
        check(u, x0, xtrue)

def custom_convergence_check(x, dx, residuum, er, ea, eresiduum, vector_norm=lambda v: abs(v), debug=False):
    all_check_results = []
    if not hasattr(x, 'shape'):
        x = numpy.mat(numpy.array(x))
        dx = numpy.mat(numpy.array(dx))
        residuum = numpy.mat(numpy.array(residuum))
    if x.shape[0]:
        if not debug:
            ret = numpy.allclose(x, x + dx, rtol=er, atol=ea) and \
                numpy.allclose(residuum, numpy.zeros(
                               residuum.shape), atol=eresiduum, rtol=0)
        else:
            for i in range(x.shape[0]):
                if vector_norm(dx[i, 0]) < er * vector_norm(x[i, 0]) + ea and vector_norm(residuum[i, 0]) < eresiduum:
                    all_check_results.append(True)
                else:
                    all_check_results.append(False)
                if not all_check_results[-1]:
                    break

            ret = not (False in all_check_results)
    else:
        # We get here when there's no variable to be checked. This is because there aren't variables
        # of this type.
        # Eg. the circuit has no voltage sources nor voltage defined elements. In this case, the actual check is done
        # only by current_convergence_check, voltage_convergence_check always
        # returns True.
        ret = True

    return ret, all_check_results

def testDigits(kTup=('rbf', 10)):
    data, labels = loadImages('trainingDigits')
    b, alphas = smo(data, labels, 200, 0.0001, 10000, kTup)
    dataMat = np.mat(data)
    labelMat = np.mat(labels).transpose()
    svInd = np.nonzero(alphas.A > 0)[0]
    sVs = dataMat[svInd]
    labelSV = labelMat[svInd]
    print "There are %d Support Vectors" % np.shape(sVs)[0]
    m, n = np.shape(dataMat)
    errorCount = 0
    for i in xrange(m):
        kernelEval = kernelTransform(sVs, dataMat[i, :], kTup)
        predict = kernelEval.T * np.multiply(labelSV, alphas[svInd]) + b
        if np.sign(predict) != np.sign(labels[i]):
            errorCount += 1
    print "The training error rate is %f " % (float(errorCount) / m)
    data, labels = loadImages('testDigits')
    dataMat = np.mat(data)
    labelMat = np.mat(labels).transpose()
    m, n = np.shape(dataMat)
    errorCount = 0
    for i in xrange(m):
        kernelEval = kernelTransform(sVs, dataMat[i, :], kTup)
        predict = kernelEval.T * np.multiply(labelSV, alphas[svInd]) + b
        if np.sign(predict) != np.sign(labels[i]):
            errorCount += 1
    print "The test error rate is %f " % (float(errorCount) / m)

    def CA(self):
#        return NPortZ(self).CA
        z0 = self.z0
        A = np.mat(self.A)
        T = np.matrix([[np.sqrt(z0), -(A[0,1]+A[0,0]*z0)/np.sqrt(z0)],
                        [-1/np.sqrt(z0), -(A[1,1]+A[1,0]*z0)/np.sqrt(z0)]])
        return np.array(T * np.mat(self.CS) * T.H)

    def __init__(self,name,updateRateHz,messagingbus,sendmessagesto):

        print "Instantiating Force & Moment Test Model ",name

        # Call superclass constructor
        Model.__init__(self,name,updateRateHz,messagingbus,sendmessagesto)


        # Member variables ----------------------------------------------------
        #   Inputs

        self.timeOn                 = 0.0
        self.timeOff                = 0.0
        self.forceStationInput      = mat('0.0;0.0;0.0')
        self.momentStationInput     = mat('0.0;0.0;0.0')

        self.forceStation           = mat('0.0;0.0;0.0')
        self.momentStation          = mat('0.0;0.0;0.0')

        # Register Input Parameters -------------------------------------------
        #                       Input Parameter Name,   Member Variable Name,       Example of Type)
        self.registerInputParam('forceStation',        'forceStationInput',              self.forceStationInput )
        self.registerInputParam('momentStation',       'momentStationInput',             self.momentStationInput)
        self.registerInputParam('timeOn',             'timeOn',                     self.timeOn)
        self.registerInputParam('timeOff',     'timeOff',                           self.timeOff)

    def releaseK(self,Kl,rel):
        """Return a modified stiffness matrix to account for a moment release
        at one of the ends.  Kl is the original matrix, dx, dy are projections of the
        member, and 'rel' is 2 or 5 to identify the local dof # of the released dof.
        Both KL and KG are returned if the transformation matrix, T, is provided"""
        L = self.L
        if rel == 2:
            if Kl[5,5] == 0.:   # is other end also pinned?
                em = np.mat([1.,0.]).T    # corrective end moments, far end pinned
            else:
                em = np.mat([1.,0.5]).T   # corrective end moments, far end fixed
        elif rel == 5:
            if Kl[2,2] == 0.:
                em = np.mat([0.,1.]).T
            else:
                em = np.mat([0.5,1.]).T
        else:
            raise ValueError("Invalid release #: {}".format(rel))
        Tf = np.mat([[0.,0.],[1./L,1./L],[1.,0.],[0.,0.],[-1./L,-1./L],[0.,1.]])
        M = Tf*em

        K = Kl.copy()    
        K[:,1] -= M*K[rel,1]  # col 1 - forces for unit vertical displacment at j-end
        K[:,2] -= M*K[rel,2]  # col 2 - forces for unit rotation at j-end
        K[:,4] -= M*K[rel,4]  # col 4 - forces for unit vertical displacment at k-end
        K[:,5] -= M*K[rel,5]  # col 5 - forces for unit rotation at k-end
        return K

def ball_filter6(dt,R=1., Q = 0.1):
    f1 = KalmanFilter(dim=6)
    g = 10

    f1.F = np.mat ([[1., dt, dt**2,  0,       0,  0],
                    [0,  1., dt,     0,       0,  0],
                    [0,  0,  1.,     0,       0,  0],
                    [0,  0,  0.,    1., dt, -0.5*dt*dt*g],
                    [0,  0,  0,      0, 1.,      -g*dt],
                    [0,  0,  0,      0, 0.,      1.]])

    f1.H = np.mat([[1,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,1,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0]])


    f1.R = np.mat(np.eye(6)) * R

    f1.Q = np.zeros((6,6))
    f1.Q[2,2] = Q
    f1.Q[5,5] = Q
    f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
    f1.P = np.eye(6) * 50.
    f1.B = 0.
    f1.u = 0

    return f1

def main():
    image0 = cv.LoadImage('pic1.jpg')
    image1 = cv.LoadImage('pic2.jpg')
    buff = cv.LoadImage('buff.jpg')
    buff2 = cv.LoadImage('buff.jpg')
    #image[ y, x , rgb ]
    features0 = numpy.mat([[1771,1111],[2073.5,1056],[1963.5,1259.5],[1732.5,1435.5],[2095.5,1347.5],
                    [1908.5,1468.5],[1941.5,1666.5],[1210,1705],[2156,1551],[1534.5,2040.5],
                    [1952.5,1941.5],[1837,418],[1930.5,1100],[1611.5,1133],[2194.5,1039.5],
                    [1848,797.5],[2101,775.5],[1545.5,1408],[2167,1303.5]])
    features1 = numpy.mat([[1738,1111],[2117.5,1094.5],[1936,1309],[1710.5,1457.5],[2161.5,1430],
                    [1919.5,1512.5],[1925,1732.5],[1342,1633.5],[2420,1650],[1644.5,2029.5],
                    [2128.5,2035],[1941.5,374],[1936,1122],[1578.5,1111],[2288,1089],[1798.5,786.5],
                    [2095.5,803],[1540,1391.5],[2293.5,1364]])
    fund = fundamental(features0, features1)
    H1, H2 = H1H2Calc(fund)
    prewarp1, prewarp2 = WarpImages(image0, image1, H1, H2, buff, buff2)
    features0, features1 = formatForFund(features0, features1)
    warpFeatures0 = WarpFeatures(features0.T, H1)
    warpFeatures1 = WarpFeatures(features1.T, H2)
    #Transition(prewarp1, prewarp2, warpFeatures0, warpFeatures1, features0, features1)
    cv.NamedWindow('display')
##    cv.NamedWindow('prewarp1')
##    cv.NamedWindow('prewarp2')
##    cv.ShowImage('prewarp1', prewarp1)
##    cv.WaitKey(0)
##    cv.ShowImage('prewarp2', prewarp2)
##    cv.WaitKey(0)
    Linear(prewarp1, prewarp2, H1, H2, buff)#, writer)

 def adaBoostTrainDecisionStump(self,dataArr,classLabels,numInt=40):
     weakDecisionStumpArr = []
     m = np.shape(dataArr)[0]
     weight = np.mat(np.ones((m,1))/m)     # init the weight of the data.Normally, we set the initial weight is 1/n
     aggressionClassEst = np.mat(np.zeros((m,1)))
     for i in range(numInt): # classEst == class estimation
         bestStump,error,classEst = self.buildStump(dataArr,classLabels,weight) # D is a vector of the data's weight
         # print("D: ",weight.T)
         alpha = float(0.5 * np.log((1.0 - error)/max(error , 1e-16)))   # alpha is the weighted of the weak classifier
         bestStump['alpha'] = alpha
         weakDecisionStumpArr.append(bestStump)
         exponent = np.multiply(-1* alpha * np.mat(classLabels).T , classEst) # calculte the exponent [- alpha * Y * Gm(X)]
         print("classEst ：",classEst.T)
         weight = np.multiply(weight,np.exp(exponent)) # update the weight of the data, w_m = e^[- alpha * Y * Gm(X)]
         weight = weight/weight.sum()  # D.sum() == Z_m (Normalized Factor) which makes sure the D_(m+1) can be a probability distribution
         # give every estimated class vector (the classified result of the weak classifier) a weight
         aggressionClassEst += alpha*classEst
         print("aggression classEst: ",aggressionClassEst.T)
         # aggressionClassError = np.multiply(np.sign(aggressionClassEst) != np.mat(classLabels).T, np.ones((m,1)))
         # errorRate = aggressionClassError.sum()/m
         errorRate = (np.sign(aggressionClassEst) != np.mat(classLabels).T).sum()/m # calculate the error classification
         # errorRate = np.dot((np.sign(aggressionClassEst) != np.mat(classLabels).T).T,np.ones((m,1)))/m
         print("total error: ",errorRate,"\n")
         if errorRate == 0:
             break
     return weakDecisionStumpArr

 def buildStump(self,dataArr,classLabels,D):  # D is a vector of the data's weight
     dataMatrix = np.mat(dataArr)
     labelMat = np.mat(classLabels).T
     m,n = np.shape(dataMatrix)
     numSteps = 10.0
     bestStump = {}
     bestClassEst = np.mat(np.zeros((m,1)))
     minError = np.inf
     for i in range(n):
         rangeMin = dataMatrix[:,i].min()
         rangeMax = dataMatrix[:,i].max()
         stepSize = (rangeMax - rangeMin)/numSteps
         for j in range(-1,int(numSteps) + 1):
             for inequal in ['lt','gt']:
                 thresholdVal = (rangeMin + float(j) * stepSize)
                 predictedVals = self.stumpDecisionTree(dataMatrix,i,thresholdVal,inequal)
                 # print("Predict value:" , predictedVals.T)
                 errArr = np.mat(np.ones((m,1)))
                 errArr[predictedVals == labelMat] = 0   # set 0 to the vector which is classified correctly
                 # print(predictedVals.T," ",labelMat.T)
                 weightedError = D.T * errArr
                 # print("split: dim %d, threshold value %.2f ,threshold inequal: %s, the weighted error is %.3f" %(i,thresholdVal,inequal,weightedError))
                 if weightedError < minError:
                     minError = weightedError
                     bestClassEst = predictedVals.copy()
                     bestStump['dimension'] = i
                     bestStump['inequal'] = inequal
                     bestStump['threshold'] = thresholdVal
     return bestStump,minError,bestClassEst

def run(V = None, V1 = None):
    """
    Run examples.
    
    :param V: Target matrix to estimate.
    :type V: :class:`numpy.matrix`
    :param V1: (Second) Target matrix to estimate used in multiple NMF (e. g. SNMNMF).
    :type V1: :class:`numpy.matrix`
    """
    if V == None or V1 == None:
        prng = np.random.RandomState(42)
        # construct target matrix 
        V = abs(np.mat(prng.normal(loc = 0.0, scale = 1.0, size = (20, 30))))
        V1 = abs(np.mat(prng.normal(loc = 0.0, scale = 1.0, size = (20, 25))))
    run_snmnmf(V, V1)
    run_bd(V)
    run_bmf(V)
    run_icm(V)
    run_lfnmf(V)
    run_lsnmf(V)
    run_nmf(V)
    run_nsnmf(V)
    run_pmf(V)
    run_psmf(V)
    run_snmf(V)

def run_bd(V):
    """
    Run Bayesian decomposition.
    
    :param V: Target matrix to estimate.
    :type V: :class:`numpy.matrix`
    """
    rank = 10
    model = nimfa.mf(V, 
                  seed = "random_c", 
                  rank = rank, 
                  method = "bd", 
                  max_iter = 12, 
                  initialize_only = True,
                  alpha = np.mat(np.zeros((V.shape[0], rank))),
                  beta = np.mat(np.zeros((rank, V.shape[1]))),
                  theta = .0,
                  k = .0,
                  sigma = 1., 
                  skip = 100,
                  stride = 1,
                  n_w = np.mat(np.zeros((rank, 1))),
                  n_h = np.mat(np.zeros((rank, 1))),
                  n_sigma = False)
    fit = nimfa.mf_run(model)
    print_info(fit)

def test_rotate_inertia():
    """Are we obtaining the global inertia properly?"""

    density = 1.5
    height = 4
    height_vec = array([[0], [0], [height]])

    # One thickness is 0.
    r0 = 5; t0 = 0; r1 = 2; t1 = 2;

    stad1 = Stadium('Ls1: umbilicus', 'thicknessradius', t0, r0)
    stad2 = Stadium('Lb1: mid-arm', 'thicknessradius', t1, r1)

    solidA = StadiumSolid('solid', density, stad1, stad2, height)

    # This inertia matrix describes two 1kg point masses at (0, 2, 1) and
    # (0, -2, -1) in the global reference frame, A.
    solidA._rel_inertia = mat([[10.0, 0.0, 0.0],
                             [0.0, 2.0, -4.0],
                             [0.0, -4.0, 8.0]])

    # If we want the inertia about a new reference frame, B, such that the
    # two masses lie on the yb axis we can rotate about xa through the angle
    # arctan(1/2). Note that this function returns R from va = R * vb.
    solidA._rot_mat = inertia.rotate_space_123((arctan(1.0 / 2.0), 0.0, 0.0))

    solidA.calc_properties()

    I_b = solidA.inertia

    expected_I_b = mat([[10.0, 0.0, 0.0],
                        [0.0, 0.0, 0.0],
                        [0.0, 0.0, 10.0]])

    testing.assert_allclose(I_b, expected_I_b)

def cv_show( yEv, yEv_calc, disp = True, graph = True, grid_std = None):

	# if the output is a vector and the original is a metrix, 
	# the output is translated to a matrix. 
	if len( np.shape(yEv_calc)) == 1:	
		yEv_calc = np.mat( yEv_calc).T
	if len( np.shape(yEv)) == 1:
		yEv = np.mat( yEv).T

	r_sqr, RMSE = jchem.estimate_accuracy( yEv, yEv_calc, disp = disp)
	if graph:
		#plt.scatter( yEv.tolist(), yEv_calc.tolist())	
		plt.figure()	
		ms_sz = max(min( 4000 / yEv.shape[0], 8), 1)
		plt.plot( yEv.tolist(), yEv_calc.tolist(), '.', ms = ms_sz) # Change ms 
		ax = plt.gca()
		lims = [
			np.min([ax.get_xlim(), ax.get_ylim()]),  # min of both axes
			np.max([ax.get_xlim(), ax.get_ylim()]),  # max of both axes
		]
		# now plot both limits against eachother
		#ax.plot(lims, lims, 'k-', alpha=0.75, zorder=0)
		ax.plot(lims, lims, '-', color = 'pink')
		plt.xlabel('Experiment')
		plt.ylabel('Prediction')
		if grid_std:
			plt.title( '($r^2$, std) = ({0:.2e}, {1:.2e}), RMSE = {2:.2e}'.format( r_sqr, grid_std, RMSE))
		else:
			plt.title( '$r^2$ = {0:.2e}, RMSE = {1:.2e}'.format( r_sqr, RMSE))
		plt.show()
	return r_sqr, RMSE

    def _determine_karray(equivalent_sizes, appended_sizes,
                          max_equivalent_size,
                          total_appended_size):

        n = len(equivalent_sizes)
        import numpy as np
        A = np.mat(np.zeros((n+1, n+1), dtype="d"))
        B = np.zeros((n+1), dtype="d")
        # AxK = B

        # populated A
        for i, (r, a) in enumerate(equivalent_sizes):
            A[i, i] = r
            A[i, -1] = -1
            B[i] = -a
        A[-1, :-1] = [r for r, a in appended_sizes]
        B[-1] = total_appended_size - sum([a for rs, a in appended_sizes])

        karray_H = (A.I*np.mat(B).T).A1
        karray = karray_H[:-1]
        H = karray_H[-1]

        if H > max_equivalent_size:
            karray = ((max_equivalent_size -
                      np.array([a for r, a in equivalent_sizes]))
                      / np.array([r for r, a in equivalent_sizes]))
        return karray

 def Z(self):
     """Return Z-parameter matrix"""
     S = np.mat(self.S).astype(float)
     E = np.mat(np.eye(self.n, self.n))
     Zref = self.z0 * E
     Gref = 1 / np.sqrt(np.real(self.z0)) * E
     return np.array(Gref.I * (E - S).I * (S + E) * Zref * Gref)

    def setUp(self):
        self.a = np.array([[1,2,3],[4,0,5]])
        self.space_s = Space(SparseMatrix(np.mat(self.a)),
                                         ["a", "b"], ["f1","f2", "f3"])

        self.space_d = Space(DenseMatrix(np.mat(self.a)),
                                         ["a", "b"], ["f1","f2", "f3"])