def _costFunction(self,ep1,ep2,method):
        # Endpoint Atribute (1)
        x1 = numpy.array(ep1.Position,dtype=numpy.float16)
        o1 = numpy.array(ep1.Orientation,dtype=numpy.float16)
        t1 = numpy.float16(ep1.Thickness)

        # Endpoint Atribute (2)
        x2 = numpy.array(ep2.Position,dtype=numpy.float16)
        o2 = numpy.array(ep2.Orientation,dtype=numpy.float16)
        t2 = numpy.float16(ep2.Thickness)

        # Pair Features
        c = (x1+x2)/2                           # centre
        d = numpy.linalg.norm(x1-x2)            # distance
        k1= (x1-c) / numpy.linalg.norm(x1-c)    # vector pointing to center (1)
        k2= (x2-c) / numpy.linalg.norm(x2-c)    # vector pointing to center (2)

        # Meassure if orientation of endpoints is alligned [0: <- -> , 1: -> <-]
        A=1-(2+numpy.dot(k1,o1)+numpy.dot(k2,o2))/4   #[0,1]

        if d>=self.maxDist:return -1
        if A<self.minOrie:return -1

        if method == "dist":
            return d

  def testReprWorksCorrectlyScalar(self):
    normal = tfd.Normal(loc=np.float16(0), scale=np.float16(1))
    self.assertEqual(
        ("<tf.distributions.Normal"
         " 'Normal'"
         " batch_shape=()"
         " event_shape=()"
         " dtype=float16>"),  # Got the dtype right.
        repr(normal))

    chi2 = tfd.Chi2(df=np.float32([1., 2.]), name="silly")
    self.assertEqual(
        ("<tf.distributions.Chi2"
         " 'silly'"  # What a silly name that is!
         " batch_shape=(2,)"
         " event_shape=()"
         " dtype=float32>"),
        repr(chi2))

    exp = tfd.Exponential(rate=array_ops.placeholder(dtype=dtypes.float32))
    self.assertEqual(
        ("<tf.distributions.Exponential"
         " 'Exponential'"
         " batch_shape=<unknown>"
         " event_shape=()"
         " dtype=float32>"),
        repr(exp))

 def testBrokenTypes(self):
   with self.assertRaisesWithPredicateMatch(TypeError, "Categorical"):
     distributions_py.Mixture(None, [])
   cat = distributions_py.Categorical([0.3, 0.2])
   # components must be a list of distributions
   with self.assertRaisesWithPredicateMatch(
       TypeError, "all .* must be Distribution instances"):
     distributions_py.Mixture(cat, [None])
   with self.assertRaisesWithPredicateMatch(TypeError, "same dtype"):
     distributions_py.Mixture(
         cat, [
             distributions_py.Normal(
                 mu=[1.0], sigma=[2.0]), distributions_py.Normal(
                     mu=[np.float16(1.0)], sigma=[np.float16(2.0)])
         ])
   with self.assertRaisesWithPredicateMatch(ValueError, "non-empty list"):
     distributions_py.Mixture(distributions_py.Categorical([0.3, 0.2]), None)
   with self.assertRaisesWithPredicateMatch(TypeError,
                                            "either be continuous or not"):
     distributions_py.Mixture(
         cat, [
             distributions_py.Normal(
                 mu=[1.0], sigma=[2.0]), distributions_py.Bernoulli(
                     dtype=dtypes.float32, logits=[1.0])
         ])

  def testStrWorksCorrectlyScalar(self):
    normal = tfd.Normal(loc=np.float16(0), scale=np.float16(1))
    self.assertEqual(
        ("tf.distributions.Normal("
         "\"Normal\", "
         "batch_shape=(), "
         "event_shape=(), "
         "dtype=float16)"),  # Got the dtype right.
        str(normal))

    chi2 = tfd.Chi2(df=np.float32([1., 2.]), name="silly")
    self.assertEqual(
        ("tf.distributions.Chi2("
         "\"silly\", "  # What a silly name that is!
         "batch_shape=(2,), "
         "event_shape=(), "
         "dtype=float32)"),
        str(chi2))

    exp = tfd.Exponential(rate=array_ops.placeholder(dtype=dtypes.float32))
    self.assertEqual(
        ("tf.distributions.Exponential(\"Exponential\", "
         # No batch shape.
         "event_shape=(), "
         "dtype=float32)"),
        str(exp))

def dist_vec(training, test):
    
    n1, d = training.shape
    n2, d1 = test.shape
       
    assert n1 != 0, 'Training set is empty'
    assert n2 != 0, 'Test set is empty'
    assert d==d1, 'Images in training and test sets have different size'
     
    tstart = time.time()
   
    train_squared = np.sum(np.square(training), axis = 1)
    test_squared = np.sum(np.square(test), axis = 1)
    
    A = np.tile(train_squared, (n2,1)) # n2xn1 matrix
    A = A.transpose((1,0))    # n1xn2 matrix    
    B = np.tile(test_squared, (n1,1) ) # n2xn2 matrix

    a = np.tile(training, (1,1,1)) # 1xn1x64 matrix
    a = a.transpose((1,0,2))    # n1x1x64 matrix
    b = np.tile(test, (1,1,1) ) # 1xn2x64 matrix
    
    C = np.tensordot(a,b, [[1,2],[0,2]])
    
    dist = A + B - C - C
    
    dist = np.sqrt(dist)
    np.float16(dist)
    
    tstop = time.time()
    
    return dist, tstop-tstart    

def save(data, outputdir, outputfile, outputformat):
    """
    Save data to a variety of formats
    Automatically determines whether data is an array
    or an RDD and handles appropriately
    For RDDs, data are sorted and reshaped based on the keys

    :param data: RDD of key value pairs or array
    :param outputdir: Location to save data to
    :param outputfile: file name to save data to
    :param outputformat: format for data ("matlab", "text", or "image")
    """

    filename = os.path.join(outputdir, outputfile)

    if (outputformat == "matlab") | (outputformat == "text"):
        if isrdd(data):
            dims = getdims(data)
            data = subtoind(data, dims.max)
            keys = data.map(lambda (k, _): int(k)).collect()
            nout = size(data.first()[1])
            if nout > 1:
                for iout in range(0, nout):
                    result = data.map(lambda (_, v): float16(v[iout])).collect()
                    result = array([v for (k, v) in sorted(zip(keys, result), key=lambda (k, v): k)])
                    if outputformat == "matlab":
                        savemat(filename+"-"+str(iout)+".mat",
                                mdict={outputfile+str(iout): squeeze(transpose(reshape(result, dims.num[::-1])))},
                                oned_as='column', do_compression='true')
                    if outputformat == "text":
                        savetxt(filename+"-"+str(iout)+".txt", result, fmt="%.6f")
            else:
                result = data.map(lambda (_, v): float16(v)).collect()
                result = array([v for (k, v) in sorted(zip(keys, result), key=lambda (k, v): k)])

    def callback_1(self, data):
        # print "difference_cal receives position_1 update. Processing...."
        self._position_1_x = data.x
        self._position_1_y = data.y
        self._position_1_ID = data.ID
        if not self._position_0_x:
            self._position_0_x = [-1]*len(self._position_1_x)
            self._position_0_y = [-1]*len(self._position_1_x)
        w1 = list(np.float16(np.array(self._position_0_x)) - np.float16(np.array(self._position_1_x)))
        w2 = list(np.float16(np.array(self._position_0_y)) - np.float16(np.array(self._position_1_y)))
        if self._position_1_ID < self._horizon:
            if abs(w1[self._position_1_ID])<0.3 and abs(w2[self._position_1_ID])<0.3:
                rospy.loginfo("The two car are getting close.")
        else:
            if abs(w1[self._horizon])<0.3 and abs(w2[self._horizon])<0.3:
                rospy.loginfo("The two car are getting close.")
        w3 = list()
        for i in range(len(w1)):
            if abs(w1[i])<1 and abs(w2[i])<1:
                w3.append(1)
            else:
                w3.append(0)
        # print w3
        # w1 = [0]*len(w1)
        # w2 = [0]*len(w1)
        # w4 = [0]*len(w1)
        # print list(self._position_0_x)
        # print list(self._position_0_y)

        self.pub_2.publish(w3, list(self._position_0_x), list(self._position_0_y), self._position_1_ID)
        # print "disturbance_signal_1 sent with w3 = %s, and ID = %s."%(str(w3), str(self._position_1_ID))
        if self._position_1_ID < self._horizon:
            print "disturbance_signal_1 ID = %s. pos is %s, %s."%(str(self._position_1_ID),str(self._position_1_x[self._position_1_ID]), str(self._position_1_y[self._position_1_ID]))
        else:
            print "disturbance_signal_1 ID = %s. pos is %s, %s."%(str(self._position_1_ID),str(self._position_1_x[self._horizon]), str(self._position_1_y[self._horizon]))

    def __init__(self, 
                 Agent, 
                 alpha, 
                 gamma, 
                 epsilon):
        '''
        Fill all values of Q based on a given optimistic value.
        '''

        # Set the agent for this QLearning session
        self.Agent = Agent
        self.alpha = alpha
        self.gamma = gamma
        self.epsilon = epsilon
        self.policy = dict()
        self.Q = dict()
        self.V = dict()
        S = set( [ (i,j) for i in range(-5,6) for j in range(-5,6)] )
        for s in S:
            self.V[s] = numpy.float16( 0 )
            self.Q[s] = dict()
            self.policy[s] = dict()
            
            for a in self.Agent.actions:
                self.policy[s][a] = numpy.float16( 1.0 / len( self.Agent.actions ) )
                
                for o in self.Agent.actions:
                    self.Q[s][(a,o)] = numpy.float16( 0.0 )

    def callback_1(self, data):
        # print "difference_cal receives position_1 update. Processing...."
        self._position_1_x = data.x
        self._position_1_y = data.y
        self._position_1_ID = data.ID
        # xmin = self._position_1_x + 0.1
        # xmax = self._position_1_x + 0.6
        # ymin = self._position_1_y - 0.3
        # ymax = self._position_1_y + 0.3
        # if self._position_0_x > xmin and self._position_0_x < xmax and self._position_0_y > ymin and self._position_0_y < ymax:
        #     w1 = 1
        # else:
        #     w1 = 0
        # xmin = self._position_1_x - 0.3
        # xmax = self._position_1_x + 0.3
        # ymin = self._position_1_y + 0.1
        # ymax = self._position_1_y + 0.6
        # if self._position_0_x > xmin and self._position_0_x < xmax and self._position_0_y > ymin and self._position_0_y < ymax:
        #     w2 = 1
        # else:
        #     w2 = 0
        # xmin = self._position_1_x - 0.6
        # xmax = self._position_1_x - 0.1
        # ymin = self._position_1_y - 0.3
        # ymax = self._position_1_y + 0.3
        # if self._position_0_x > xmin and self._position_0_x < xmax and self._position_0_y > ymin and self._position_0_y < ymax:
        #     w3 = 1
        # else:
        #     w3 = 0
        # xmin = self._position_1_x - 0.3
        # xmax = self._position_1_x + 0.3
        # ymin = self._position_1_y - 0.6
        # ymax = self._position_1_y - 0.1
        # if self._position_0_x > xmin and self._position_0_x < xmax and self._position_0_y > ymin and self._position_0_y < ymax:
        #     w4 = 1
        # else:
        #     w4 = 0
        if not self._position_0_x:
            self._position_0_x = [-1]*len(self._position_1_x)
            self._position_0_y = [-1]*len(self._position_1_x)
        w1 = list(np.float16(np.array(self._position_0_x)) - np.float16(np.array(self._position_1_x)))
        w2 = list(np.float16(np.array(self._position_0_y)) - np.float16(np.array(self._position_1_y)))
        if abs(w1[0])<0.3 and abs(w2[0])<0.3:
            rospy.loginfo("The two car are too close with distance of x and y: %s, %s", str(w1[0]), str(w1[2]))
        w3 = list()
        for i in range(len(w1)):
            if abs(w1[i])<1 and abs(w2[i])<1:
                w3.append(1)
            else:
                w3.append(0)
        # print w3
        w1 = [0]*len(w1)
        w2 = [0]*len(w1)
        w4 = [0]*len(w1)
        # print list(self._position_0_x)
        # print list(self._position_0_y)

        self.pub_2.publish(w1, w2, w3, w4, list(self._position_0_x), list(self._position_0_y), self._position_1_ID)
        # print "disturbance_signal_1 sent with w3 = %s, and ID = %s."%(str(w3), str(self._position_1_ID))
        print "disturbance_signal_1 ID = %s."%(str(self._position_1_ID))

    def estimate(self, iterations=100, tolerance=1e-5):
        last_rmse = None

        # the algorithm will converge, but really slow
        # use MF's initialize latent parameter will be better
        for iteration in xrange(iterations):
            # update item & user parameter
            self._update_item_params()
            self._update_user_params()

            # update item & user_features
            self._udpate_item_features()
            self._update_user_features()

            # compute RMSE
            # train errors
            train_preds = self.predict(self.train)
            train_rmse = RMSE(train_preds, np.float16(self.train[:, 2]))

            # validation errors
            validation_preds = self.predict(self.validation)
            validation_rmse = RMSE(
                validation_preds, np.float16(self.validation[:, 2]))
            self.train_errors.append(train_rmse)
            self.validation_erros.append(validation_rmse)
            print "iterations: %3d, train RMSE: %.6f, validation RMSE: %.6f " % (iteration + 1, train_rmse, validation_rmse)

            # stop if converge
            if last_rmse:
                if abs(train_rmse - last_rmse) < tolerance:
                    break

            last_rmse = train_rmse

    def test_invalid(self):
        prop = bcpp.Int()

        assert not prop.is_valid(0.0)
        assert not prop.is_valid(1.0)
        assert not prop.is_valid(1.0+1.0j)
        assert not prop.is_valid("")
        assert not prop.is_valid(())
        assert not prop.is_valid([])
        assert not prop.is_valid({})
        assert not prop.is_valid(_TestHasProps())
        assert not prop.is_valid(_TestModel())

        assert not prop.is_valid(np.bool8(False))
        assert not prop.is_valid(np.bool8(True))
        assert not prop.is_valid(np.float16(0))
        assert not prop.is_valid(np.float16(1))
        assert not prop.is_valid(np.float32(0))
        assert not prop.is_valid(np.float32(1))
        assert not prop.is_valid(np.float64(0))
        assert not prop.is_valid(np.float64(1))
        assert not prop.is_valid(np.complex64(1.0+1.0j))
        assert not prop.is_valid(np.complex128(1.0+1.0j))
        if hasattr(np, "complex256"):
            assert not prop.is_valid(np.complex256(1.0+1.0j))

    def test_nans_infs(self):
        oldsettings = np.seterr(all='ignore')
        try:
            # Check some of the ufuncs
            assert_equal(np.isnan(self.all_f16), np.isnan(self.all_f32))
            assert_equal(np.isinf(self.all_f16), np.isinf(self.all_f32))
            assert_equal(np.isfinite(self.all_f16), np.isfinite(self.all_f32))
            assert_equal(np.signbit(self.all_f16), np.signbit(self.all_f32))
            assert_equal(np.spacing(float16(65504)), np.inf)

            # Check comparisons of all values with NaN
            nan = float16(np.nan)

            assert_(not (self.all_f16 == nan).any())
            assert_(not (nan == self.all_f16).any())

            assert_((self.all_f16 != nan).all())
            assert_((nan != self.all_f16).all())

            assert_(not (self.all_f16 < nan).any())
            assert_(not (nan < self.all_f16).any())

            assert_(not (self.all_f16 <= nan).any())
            assert_(not (nan <= self.all_f16).any())

            assert_(not (self.all_f16 > nan).any())
            assert_(not (nan > self.all_f16).any())

            assert_(not (self.all_f16 >= nan).any())
            assert_(not (nan >= self.all_f16).any())
        finally:
            np.seterr(**oldsettings)

def npHalfArrayToOIIOFloatPixels(width, height, channels, npPixels):
    if oiio.VERSION < 10800:
        # Read half-float pixels into a numpy float pixel array
        oiioFloatsArray = np.frombuffer(np.getbuffer(np.float16(npPixels)), dtype=np.float32)
    else:
        # Read half-float pixels into a numpy float pixel array
        oiioFloatsArray = np.frombuffer(np.getbuffer(np.float16(npPixels)), dtype=np.uint16)

    return oiioFloatsArray

def Scale_Image(image): #Scales the input image to the range:(0,255)
	min = np.amin(image)
	max = np.amax(image)
	
	image -= min
	image = np.float16(image) * (np.float16(255) / np.float16(max-min))
	image = np.uint8(image)

	return image

    def __init__(self,timeSeries=None,
                 lenSeries=2**18,
                 numChannels=1,
                 fMin=400,fMax=800,
                 sampTime=None,
                 noiseRMS=0.1):
        """ Initializes the AmplitudeTimeSeries instance. 
        If a array is not passed, then a random whitenoise dataset is generated.
        Inputs: 
        Len -- Number of time data points (usually a power of 2) 2^38 gives about 65 seconds 
        of 400 MHz sampled data
        The time binning is decided by the bandwidth
        fMin -- lowest frequency (MHz)
        fMax -- highest frequency (MHz)
        noiseRMS -- RMS value of noise (TBD)
        noiseAlpha -- spectral slope (default is white noise) (TBD)
        ONLY GENERATES WHITE NOISE RIGHT NOW!
        """
        self.shape = (np.uint(numChannels),np.uint(lenSeries))
        self.fMax = fMax
        self.fMin = fMin        
        
        if sampTime is None:
            self.sampTime = np.uint(numChannels)*1E-6/(fMax-fMin)
        else:
            self.sampTime = sampTime

        if timeSeries is None:
            # then use the rest of the data to generate a random timeseries
            if VERBOSE:
                print "AmplitudeTimeSeries __init__ did not get new data, generating white noise data"

            self.timeSeries = np.complex64(noiseRMS*(np.float16(random.standard_normal(self.shape))
                                                     +np.float16(random.standard_normal(self.shape))*1j)/np.sqrt(2))
            
        else:
            if VERBOSE:
                print "AmplitudeTimeSeries __init__ got new data, making sure it is reasonable."

            if len(timeSeries.shape) == 1:
                self.shape = (1,timeSeries.shape[0])
                
            else:
                self.shape = timeSeries.shape

            self.timeSeries = np.reshape(np.complex64(timeSeries),self.shape)
            
            self.fMin = fMin
            self.fMax = fMax

            if sampTime is None:
                self.sampTime = numChannels*1E-6/(fMax-fMin)
            else:
                self.sampTime = sampTime

        return None

def compute_grad_img(image, height, width):
    image_Sobel_x = Sobel(image, CV_64F, 1, 0, ksize=5)
    image_Sobel_y = Sobel(image, CV_64F, 0, 1, ksize=5)
    img_grad = zeros((height, width), dtype=tuple)
    for row in range(height):
        for col in range(width):
            img_grad[row, col] = (float16(sqrt(image_Sobel_x[row, col][0] ** 2 + image_Sobel_y[row, col][0] ** 2)), \
                                    float16(sqrt(image_Sobel_x[row, col][1] ** 2 + image_Sobel_y[row, col][1] ** 2)), \
                                    float16(sqrt(image_Sobel_x[row, col][2] ** 2 + image_Sobel_y[row, col][2] ** 2)))
    return img_grad

    def testFloat(self):
        num = np.float(256.2013)
        self.assertEqual(np.float(ujson.decode(ujson.encode(num))), num)

        num = np.float16(256.2013)
        self.assertEqual(np.float16(ujson.decode(ujson.encode(num))), num)

        num = np.float32(256.2013)
        self.assertEqual(np.float32(ujson.decode(ujson.encode(num))), num)

        num = np.float64(256.2013)
        self.assertEqual(np.float64(ujson.decode(ujson.encode(num))), num)

    def testFloatMax(self):
        num = np.float(np.finfo(np.float).max / 10)
        assert_approx_equal(np.float(ujson.decode(ujson.encode(num))), num, 15)

        num = np.float16(np.finfo(np.float16).max / 10)
        assert_approx_equal(np.float16(ujson.decode(ujson.encode(num))), num, 15)

        num = np.float32(np.finfo(np.float32).max / 10)
        assert_approx_equal(np.float32(ujson.decode(ujson.encode(num))), num, 15)

        num = np.float64(np.finfo(np.float64).max / 10)
        assert_approx_equal(np.float64(ujson.decode(ujson.encode(num))), num, 15)

def get_resource_data(group_serv, DFR_RAM, DFR_CPU):
    D=pd.DataFrame()
    for G in DFR_RAM.Group.unique():
        if group_serv in G:
            print(G)
            df=pd.DataFrame()
            fig, axes = plt.subplots(nrows=1, ncols=2, figsize=(20, 5))
            ax0, ax1= axes.flat

            d=DFR_RAM[DFR_RAM.Group==G]
            #print(d)
            D=D.append(d)
            X, Y=d.shape
            ind = np.arange(X)
            width = 0.5
            #d=d[['server','Used', 'Available']]
            p1 = ax0.bar(ind, d.Used.values,  color='red', label='Used_memory_Gb')
            p2 = ax0.bar(ind, d.Available.values, color='green', bottom=d.Used.values, label='Available')
            ax0.set_xticks(range(d.shape[0]))
            ax0.set_xticklabels([str(x) for x in d.server.values], rotation=90)
            ax0.set_title(G+" Memory usage Gb")
            ax0.get_legend()
            ax0.set_xlabel("KB")
            df=df.append(pd.DataFrame({"Group":G, "Number":d.shape[0],"Resource":"Memory","Sum_used":d.Used.sum(),"Sum_capacity":d.Capacity.sum() }, index=['RAM']))


            d=DFR_CPU[DFR_CPU.Group==G]
            D=D.append(d)
            X, Y=d.shape
            ind = np.arange(X)
            width = 0.5
            #d=d[['server','Used', 'Available']]
            d['Used']=np.float16(d['Used'].str.replace(" %", ""))
            p1 = ax1.bar(ind, d.Used.values,  color='red', label='Used_CPU_%')
            p2 = ax1.bar(ind, d.Available.values, color='green', bottom=d.Used.values, label='Available')
            ax1.set_xticks(range(d.shape[0]))
            ax1.set_xticklabels([str(x) for x in d.server.values], rotation=90)
            ax1.set_title(G+" CPU usage %")
            ax1.set_xlabel("%")


            #fig.text(G)
            fig.set_label(G)
            fig.show

            df=df.append(pd.DataFrame({"Group":G, "Number":d.shape[0], "Resource":"CPU","Sum_used":d.Used.sum(),"Sum_capacity":100*d.Capacity.sum() }, index=['CPU']), ignore_index=True)
            display(df)
            print("Summary CPU ")
            print("Used "+str(d.Used.sum()/d.Capacity.sum())+" %")
            print("Summary RAM ")
            print("Used "+str(100*np.float16(df['Sum_used'][df.Resource=='Memory'].values/df.Sum_capacity[df.Resource=='Memory'].values)[0])+" %")
    return D

def Load_images(file_dir, file_name):
    
    if (os.path.exists(file_dir + file_name + '/' + file_name + '_images.npy')==False):

        img = Image.open(file_dir + file_name + '/' + file_name+'.tif')

        counter=0
        while True:
            try:
                img.seek(counter)
            except EOFError:
                break
            counter+=1

        #Default pic sizes
        n_pixels = 256

        #Initialize 3D image array
        n_frames = counter
        images_raw = np.zeros((n_frames, n_pixels, n_pixels), dtype = np.float16)

        print "n_frames: ", n_frames
        for i in range(n_frames): 
            try:
                img.seek(i)
                print "Loading frame: ", i
                images_raw [i] = np.float16(img)
                if i>9640:# and i%10==0: # 
                    im = plt.imshow(images_raw[i])
                    plt.title("Frame: " +str(i))
                    plt.show()

            except EOFError:
                break

        images_start= 0
        images_end = 9642
        images_raw=images_raw[images_start:images_end]

        print "Re-saving imaging array..."

        np.save(file_dir + file_name + '/' + file_name + '_images', images_raw)
        np.savetxt(file_dir + file_name + '/' + file_name + '_images_start_'+str(images_start)+'_end_'+
        str(images_end), [images_start, images_end])

        quit()

    else:
        images_raw = np.load(file_dir + file_name + '/' + file_name + '_images.npy')
        images_raw = np.float16(images_raw)
        return images_raw