def main():
    # create centers
    means = [[1.0, 1.0], [2.5, 2.5]]
    data = []
    rect_start_x = 2.0
    rect_start_y = 2.0
    rect_width = 1.0
    rec_height = 1.0
    for i, mean in enumerate(means):
        class_centers = mvn(mean, np.eye(2), 10)
        class_data = [np.hstack((mvn(class_center, np.eye(2), 10), np.zeros((10, 1)) + i))
                               for class_center in class_centers]
        data.append(class_data)
    data = np.array(data).reshape(200, 3)
    rectangle = np.array([[rect_start_x, rect_start_y],
                          [rect_start_x + rect_width, rect_start_y],
                          [rect_start_x + rect_width, rect_start_y + rec_height],
                          [rect_start_x, rect_start_y + rec_height],
                          [rect_start_x, rect_start_y]]).reshape(5, 2)

    pts = get_points_in_rectangle(rectangle, data)
    posterior = calc_posterior(means, pts)
    print posterior
    for class_data in data:
        plt.plot(class_data[:, 0], class_data[:, 1], 'o', markersize=8)
    plt.plot(rectangle[:, 0], rectangle[:, 1])
    plt.show()

def cluster(dist_mat):
  """ Cluster based on difference matrix """
  
  DM = np.ma.masked_array(dist_mat + np.eye(N) * 1*10**8)
  DM.mask = np.tril(np.eye(N))
  num_ts = dist_mat.shape[0] 
  cluster_levels = []

  ts = []
  for i in range(num_ts):
    ts.append(set(i))
  
  count = 0
  while run:
    #XXXXXX broke
    # Find min in DM and join sets
    indexes = np.ma.where(DM == DM.min())
    i_set = set(indexes[0])
    j_set = set(indexes[1])
    cluster_levels[count] = set.union(i_set, j_set)
    count += 1 
    #Exclude using mask 
    DM.mask[indexes] = 1
  
  
  
  for i in indexes[0]:
    for j in indexes[1]:      
      cluster_levels[0] = set.union(ts[i], ts[j])

def _make_eye(shape):
    if len(shape) == 2:
        n = shape[0]
        return numpy.eye(n, dtype=numpy.float32)
    m = shape[0]
    n = shape[1]
    return numpy.array([numpy.eye(n, dtype=numpy.float32)] * m)

def test_modelgen_sparse():
    tempdir = mkdtemp()
    filename1 = os.path.join(tempdir, 'test1.nii')
    filename2 = os.path.join(tempdir, 'test2.nii')
    Nifti1Image(np.random.rand(10, 10, 10, 50), np.eye(4)).to_filename(filename1)
    Nifti1Image(np.random.rand(10, 10, 10, 50), np.eye(4)).to_filename(filename2)
    s = SpecifySparseModel()
    s.inputs.input_units = 'secs'
    s.inputs.functional_runs = [filename1, filename2]
    s.inputs.time_repetition = 6
    info = [Bunch(conditions=['cond1'], onsets=[[0, 50, 100, 180]], durations=[[2]]),
            Bunch(conditions=['cond1'], onsets=[[30, 40, 100, 150]], durations=[[1]])]
    s.inputs.subject_info = info
    s.inputs.volumes_in_cluster = 1
    s.inputs.time_acquisition = 2
    s.inputs.high_pass_filter_cutoff = np.inf
    res = s.run()
    yield assert_equal, len(res.outputs.session_info), 2
    yield assert_equal, len(res.outputs.session_info[0]['regress']), 1
    yield assert_equal, len(res.outputs.session_info[0]['cond']), 0
    s.inputs.stimuli_as_impulses = False
    res = s.run()
    yield assert_equal, res.outputs.session_info[0]['regress'][0]['val'][0], 1.0
    s.inputs.model_hrf = True
    res = s.run()
    yield assert_almost_equal, res.outputs.session_info[0]['regress'][0]['val'][0], 0.016675298129743384
    yield assert_equal, len(res.outputs.session_info[0]['regress']), 1
    s.inputs.use_temporal_deriv = True
    res = s.run()
    yield assert_equal, len(res.outputs.session_info[0]['regress']), 2
    yield assert_almost_equal, res.outputs.session_info[0]['regress'][0]['val'][0], 0.016675298129743384
    yield assert_almost_equal, res.outputs.session_info[1]['regress'][1]['val'][5], 0.007671459162258378
    rmtree(tempdir)

def calc_acceleration(velocities):
    ## kalman
    
    data = velocities
    ss = 6 # state size
    os = 3 # observation size
    F = np.array([   [1,0,0,1,0,0], # process update
                     [0,1,0,0,1,0],
                     [0,0,1,0,0,1],
                     [0,0,0,1,0,0],
                     [0,0,0,0,1,0],
                     [0,0,0,0,0,1]],                  
                    dtype=np.float)
    H = np.array([   [1,0,0,0,0,0], [0,1,0,0,0,0], [0,0,1,0,0,0]], # observation matrix
                    dtype=np.float)
    Q = 0.01*np.eye(ss) # process noise
    
    R = 1*np.eye(os) # observation noise
    
    initx = np.array([velocities[0,0], velocities[0,1], velocities[0,2], 0, 0, 0], dtype=np.float)
    initv = 0*np.eye(ss)
    xsmooth,Vsmooth = kalman_math.kalman_smoother(data, F, H, Q, R, initx, initv, plot=False)

    accel_smooth = xsmooth[:,3:]*100.
    
    return accel_smooth

def test_joint_feature_continuous():
    # FIXME
    # first make perfect prediction, including pairwise part
    X, Y = generate_blocks_multinomial(noise=2, n_samples=1, seed=1)
    x, y = X[0], Y[0]
    n_states = x.shape[-1]

    pw_horz = -1 * np.eye(n_states)
    xx, yy = np.indices(pw_horz.shape)
    # linear ordering constraint horizontally
    pw_horz[xx > yy] = 1

    # high cost for unequal labels vertically
    pw_vert = -1 * np.eye(n_states)
    pw_vert[xx != yy] = 1
    pw_vert *= 10

    # create crf, assemble weight, make prediction
    for inference_method in get_installed(["lp", "ad3"]):
        crf = DirectionalGridCRF(inference_method=inference_method)
        crf.initialize(X, Y)
        w = np.hstack([np.eye(3).ravel(), -pw_horz.ravel(), -pw_vert.ravel()])
        y_pred = crf.inference(x, w, relaxed=True)

        # compute joint_feature for prediction
        joint_feature_y = crf.joint_feature(x, y_pred)
        assert_equal(joint_feature_y.shape, (crf.size_joint_feature,))

    def update(self, r, g, r0, g0, p0):
        self.I = eye(len(self.atoms) * 3, dtype=int)
        if self.H is None:
            self.H = eye(3 * len(self.atoms))
            #self.B = np.linalg.inv(self.H)
            return
        else:
            dr = r - r0
            dg = g - g0 
            if not ((self.alpha_k > 0 and abs(np.dot(g,p0))-abs(np.dot(g0,p0)) < 0) \
                or self.replay):
                return
            if self.no_update == True:
                print 'skip update'
                return

            try: # this was handled in numeric, let it remaines for more safety
                rhok = 1.0 / (np.dot(dg,dr))
            except ZeroDivisionError:
                rhok = 1000.0
                print "Divide-by-zero encountered: rhok assumed large"
            if isinf(rhok): # this is patch for np
                rhok = 1000.0
                print "Divide-by-zero encountered: rhok assumed large"
            A1 = self.I - dr[:, np.newaxis] * dg[np.newaxis, :] * rhok
            A2 = self.I - dg[:, np.newaxis] * dr[np.newaxis, :] * rhok
            H0 = self.H
            self.H = np.dot(A1, np.dot(self.H, A2)) + rhok * dr[:, np.newaxis] \
                     * dr[np.newaxis, :]

def test_dkln3():
    dim, scale = 3, 4
    m1, m2 = np.zeros(dim), np.zeros(dim)
    P1, P2 = np.eye(dim), scale * np.eye(dim)
    test1 = .5 * (dim * np.log(scale) + dim * (1. / scale - 1))
    test2 = .5 * (-dim * np.log(scale) + dim * (scale - 1)) 
    assert dkl_gaussian(m1, P1, m2, P2) == test2

    def test_rigid_body_api(self):
        # Tests as much of the RigidBody API as is possible in isolation.
        # Adding collision geometry is *not* tested here, as it needs to
        # be done in the context of the RigidBodyTree.
        body = RigidBody()
        name = "body"
        body.set_name(name)
        self.assertEqual(body.get_name(), name)
        inertia = np.eye(6)
        body.set_spatial_inertia(inertia)
        self.assertTrue(np.allclose(inertia, body.get_spatial_inertia()))

        # Try adding a joint to a dummy body.
        body_joint = PrismaticJoint("z", np.eye(4),
                                    np.array([0., 0., 1.]))
        self.assertFalse(body.has_joint())
        dummy_body = RigidBody()
        body.add_joint(dummy_body, body_joint)
        self.assertEqual(body.getJoint(), body_joint)
        self.assertTrue(body.has_joint())

        # Try adding visual geometry.
        box_element = shapes.Box([1.0, 1.0, 1.0])
        box_visual_element = shapes.VisualElement(
            box_element, np.eye(4), [1., 0., 0., 1.])
        body.AddVisualElement(box_visual_element)
        body_visual_elements = body.get_visual_elements()
        self.assertEqual(len(body_visual_elements), 1)
        self.assertEqual(body_visual_elements[0].getGeometry().getShape(),
                         box_visual_element.getGeometry().getShape())

        # Test collision-related methods.
        self.assertEqual(body.get_num_collision_elements(), 0)
        self.assertEqual(len(body.get_collision_element_ids()), 0)

def LQR_control(A, B, x):

    Kopt, X, ev = dlqr(A, B, np.eye(2), np.eye(1))

    u = -Kopt * x

    return u

def test_dkln2():
    dim, offset = 3, 4.
    m1 = np.zeros(dim)
    P1 = np.eye(dim)
    m2 = offset * np.ones(dim)
    P2 = np.eye(dim)
    assert dkl_gaussian(m1, P1, m2, P2) == .5 * dim * offset ** 2

 def test_f_test(self):
     m = self.m
     kvars = self.kvars
     f_unreg = self.res_unreg.f_test(np.eye(m))
     f_reg = self.res_reg.f_test(np.eye(kvars)[:m])
     assert_almost_equal(f_unreg.fvalue, f_reg.fvalue, DECIMAL_2)
     assert_almost_equal(f_unreg.pvalue, f_reg.pvalue, DECIMAL_3)

    def __init__(self, dim_x, dim_z, plant, kappa=0):

        self.plant = plant
        self.fx = plant.fx
        self.hx = plant.hx

        self._dim_x = dim_x
        self._dim_z = dim_z

        self.x = zeros(dim_x)
        self.P = eye(dim_x)

        self.R = 1.0e-03 * array([
            [0.000003549299086,-0.000002442814972,-0.000004480024840,0.000267707847733,-0.000144518246735,-0.000212282673978],
            [-0.000002442814972,0.000005899512446,0.000006498387107,-0.000138622536892,0.000440883366233,0.000388550687603],
            [-0.000004480024840,0.000006498387107,0.000014749347917,-0.000218834499062,0.000402004146826,0.000932091499876],
            [0.000267707847733,-0.000138622536892,-0.000218834499062,0.042452413803684,-0.022718840083072,-0.034590131072346],
            [-0.000144518246735,0.000440883366233,0.000402004146826,-0.022718840083072,0.071342980281184,0.064549199777213],
            [-0.000212282673978,0.000388550687603,0.000932091499876,-0.034590131072346,0.064549199777213,0.149298685351403],
            ])

        self.Q = 1.0e-09*eye(dim_x) 

        self._num_sigmas = 2*dim_x + 1
        self.kappa = kappa

        # weights for the sigma points
        self.W = self.weights(dim_x, kappa)
        
        # sigma points transformed through f(x) and h(x)
        # variables for efficiency so we don't recreate every update
        self.sigmas_f = zeros((self._num_sigmas, self._dim_x))

def bench_load_save():
    rng = np.random.RandomState(20111001)
    repeat = 4
    img_shape = (128, 128, 64)
    arr = rng.normal(size=img_shape)
    img = Nifti1Image(arr, np.eye(4))
    sio = BytesIO()
    img.file_map['image'].fileobj = sio
    hdr = img.get_header()
    sys.stdout.flush()
    print("\nImage load save")
    print("----------------")
    hdr.set_data_dtype(np.float32)
    mtime = measure('img.to_file_map()', repeat)
    print('%30s %6.2f' % ('Save float64 to float32', mtime))
    mtime = measure('img.from_file_map(img.file_map)', repeat)
    print('%30s %6.2f' % ('Load from float32', mtime))
    hdr.set_data_dtype(np.int16)
    mtime = measure('img.to_file_map()', repeat)
    print('%30s %6.2f' % ('Save float64 to int16', mtime))
    mtime = measure('img.from_file_map(img.file_map)', repeat)
    print('%30s %6.2f' % ('Load from int16', mtime))
    arr = np.random.random_integers(low=-1000,high=-1000, size=img_shape)
    arr = arr.astype(np.int16)
    img = Nifti1Image(arr, np.eye(4))
    sio = BytesIO()
    img.file_map['image'].fileobj = sio
    hdr = img.get_header()
    hdr.set_data_dtype(np.float32)
    mtime = measure('img.to_file_map()', repeat)
    print('%30s %6.2f' % ('Save Int16 to float32', mtime))
    sys.stdout.flush()

    def initialize_cov(self, cov=None, scales=None, scaling=20):
        """Define C_0, the initial jump distributioin covariance matrix.

        Return:
            - cov,  if cov != None
            - covariance matrix built from the scales dictionary if scales!=None
            - covariance matrix estimated from the stochastics trace
            - covariance matrix estimated from the stochastics value, scaled by
                scaling parameter.
        """
        if cov:
            return cov
        elif scales:
            ord_sc = self.order_scales(scales)
            return np.eye(self.dim) * ord_sc
        else:
            try:
                a = self.trace2array(-2000, -1)
                nz = a[:, 0] != 0
                return np.cov(a[nz, :], rowvar=0)
            except:
                ord_sc = []
                for s in self.stochastics:
                    this_value = abs(np.ravel(s.value))
                    for elem in this_value:
                        ord_sc.append(elem)
                # print len(ord_sc), self.dim
                return np.eye(self.dim) * ord_sc / scaling

    def test_joints_api(self):
        # Verify construction from both Isometry3d and 4x4 arrays,
        # and sanity-check that the accessors function.
        name = "z"
        prismatic_joint_np = PrismaticJoint(name, np.eye(4),
                                            np.array([0., 0., 1.]))
        prismatic_joint_isom = PrismaticJoint(name, Isometry3.Identity(),
                                              np.array([0., 0., 1.]))
        self.assertEqual(prismatic_joint_isom.get_num_positions(), 1)
        self.assertEqual(prismatic_joint_isom.get_name(), name)

        name = "theta"
        revolute_joint_np = RevoluteJoint(name, np.eye(4),
                                          np.array([0., 0., 1.]))
        revolute_joint_isom = RevoluteJoint(name, Isometry3.Identity(),
                                            np.array([0., 0., 1.]))
        self.assertEqual(revolute_joint_isom.get_num_positions(), 1)
        self.assertEqual(revolute_joint_isom.get_name(), name)

        name = "fixed"
        fixed_joint_np = FixedJoint(name, np.eye(4))
        fixed_joint_isom = FixedJoint(name, Isometry3.Identity())
        self.assertEqual(fixed_joint_isom.get_num_positions(), 0)
        self.assertEqual(fixed_joint_isom.get_name(), name)

        name = "rpy"
        rpy_floating_joint_np = RollPitchYawFloatingJoint(
            name, np.eye(4))
        rpy_floating_joint_isom = RollPitchYawFloatingJoint(
            name, Isometry3.Identity())
        self.assertEqual(rpy_floating_joint_isom.get_num_positions(), 6)
        self.assertEqual(rpy_floating_joint_isom.get_name(), name)

    def __init__(self, driver, simulation=False):

        self.driver = driver

        self.width = int(self.driver.canvas['width'])
        self.height = int(self.driver.canvas['height'])

        self.driver.root.bind("<Key>", self._check_quit)

        self.driver.root.title('IMU Telemetry')

        # Vehicle dimensions
        W = VEHICLE_SCALE
        D = VEHICLE_SCALE / 2
        L = VEHICLE_SCALE * 2

        #Let these be in World-coordinates (worldview-matrix already applied)
        ####In right-handed, counter-clockwise order
        self.vehicle_points, self.vehicle_faces, self.vehicle_face_colors = self._get_vehicle(W, D, L)

        # Assume no angles to start
        self.yaw_pitch_roll = None

        # Rotation matrices
        self.pitchrot = np.eye(3)
        self.yawrot = np.eye(3)
        self.rollrot = np.eye(3)

        self.simulation = simulation
        self.running = False

    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        scale = [1,1,1]
        if self.parent:
            scale = self.cumul_scale()
        scale_translation = [self.translation[i]*scale[i] for i in range(3)]

        self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
            numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3,0:4]=[0,0,0,1]

 def testImplicitLargeDiag(self):
   mu = np.array([[1., 2, 3],
                  [11, 22, 33]])      # shape: [b, k] = [2, 3]
   u = np.array([[[1., 2],
                  [3, 4],
                  [5, 6]],
                 [[0.5, 0.75],
                  [1, 0.25],
                  [1.5, 1.25]]])      # shape: [b, k, r] = [2, 3, 2]
   m = np.array([[0.1, 0.2],
                 [0.4, 0.5]])         # shape: [b, r] = [2, 2]
   scale = np.stack([
       np.eye(3) + np.matmul(np.matmul(u[0], np.diag(m[0])),
                             np.transpose(u[0])),
       np.eye(3) + np.matmul(np.matmul(u[1], np.diag(m[1])),
                             np.transpose(u[1])),
   ])
   cov = np.stack([np.matmul(scale[0], scale[0].T),
                   np.matmul(scale[1], scale[1].T)])
   logging.vlog(2, "expected_cov:\n{}".format(cov))
   with self.test_session():
     mvn = ds.MultivariateNormalDiagPlusLowRank(
         loc=mu,
         scale_perturb_factor=u,
         scale_perturb_diag=m)
     self.assertAllClose(cov, mvn.covariance().eval(), atol=0., rtol=1e-6)

    def test_comparisons(self):
        f, g, h = self.f, self.g, self.h
        self.fake_raw_data = {
            f: arange(25).reshape(5, 5),
            g: arange(25).reshape(5, 5) - eye(5),
            h: full((5, 5), 5),
        }
        f_data = self.fake_raw_data[f]
        g_data = self.fake_raw_data[g]

        cases = [
            # Sanity Check with hand-computed values.
            (f, g, eye(5), zeros((5, 5))),
            (f, 10, f_data, 10),
            (10, f, 10, f_data),
            (f, f, f_data, f_data),
            (f + 1, f, f_data + 1, f_data),
            (1 + f, f, 1 + f_data, f_data),
            (f, g, f_data, g_data),
            (f + 1, g, f_data + 1, g_data),
            (f, g + 1, f_data, g_data + 1),
            (f + 1, g + 1, f_data + 1, g_data + 1),
            ((f + g) / 2, f ** 2, (f_data + g_data) / 2, f_data ** 2),
        ]
        for op in (gt, ge, lt, le, ne):
            for expr_lhs, expr_rhs, expected_lhs, expected_rhs in cases:
                self.check_output(
                    op(expr_lhs, expr_rhs),
                    op(expected_lhs, expected_rhs),
                )