def draw_car(x,y,th,canv):    
    r = math.sqrt(math.pow(ROBOT_LENGTH,2) + math.pow(ROBOT_WIDTH,2))/2    
    x = x + 300.0/cm_to_pixels
    y = y + 300.0/cm_to_pixels
    
    # top left
    phi = th + math.pi/2+ math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    topleft  = (x + r*math.cos(phi),y+r*math.sin(phi))
    
    #top right
    phi = th + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH) 
    topright = (x + r*math.cos(phi),y+r*math.sin(phi))

    # bottom left
    phi = th + math.pi + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH)
    bottomleft =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    # bottom right
    phi = th + 3*math.pi/2 + math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
    bottomright =  (x + r*math.cos(phi),y+r*math.sin(phi))
    
    canv.create_polygon(topleft[0]*cm_to_pixels,600 - topleft[1]*cm_to_pixels,
                        bottomleft[0]*cm_to_pixels,600 - bottomleft[1]*cm_to_pixels,
                        bottomright[0]*cm_to_pixels,600 - bottomright[1]*cm_to_pixels,
                        topright[0]*cm_to_pixels,600 - topright[1]*cm_to_pixels,
                        width = 1, outline = 'blue',fill = '')
    x1 = x*cm_to_pixels
    y1 = y*cm_to_pixels
    canv.create_oval(x1-1,600-(y1-1),x1+1,600-(y1+1),outline = 'green',fill = 'green')

    def compute_euc_distance(self, point1, point2):
        x_distance = point1[0] - point2[0]
        x_distance = math.pow(x_distance, 2)
        y_distance = point1[1] - point2[1]
        y_distance = math.pow(y_distance, 2)

        return math.sqrt(x_distance + y_distance)

def GetSpindownStuff(tc, age, l0, b0, emax0, n0):

  t = np.logspace(0,math.log10(1000.*age),300)
  lumt = []
  emaxt = []
  bt = []
  nt = []
  n = 0
  for i in t:
    l = l0/math.pow(1.+i/tc,2.)
    btt = b0*math.sqrt(l/l0)*(1.+0.5*math.sin(0.1*n*3.14))
    lumtt = l*(1.+0.5*math.cos(0.1*n*3.14))
    emaxtt = emax0*math.pow(l/l0,0.25)*(1.+0.5*math.sin(0.05*n*3.14))
    ntt = n0*math.pow(l/l0,0.25)*(1.+0.5*math.cos(0.05*n*3.14))

    bt.append([])
    bt[n].append(i)
    bt[n].append(btt)
    lumt.append([])
    lumt[n].append(i)
    lumt[n].append(lumtt)
    emaxt.append([])
    emaxt[n].append(i)
    emaxt[n].append(emaxtt)
    nt.append([])
    nt[n].append(i)
    nt[n].append(ntt)

    n = n+1
  return lumt,bt,emaxt,nt

def evaluateLogLikelihood(params, D, N, M_min, M_max):
      alpha = params[0]  # extract alpha
      # Compute normalisation constant.
      c = (1.0 - alpha)/(math.pow(M_max, 1.0-alpha) 
                          - math.pow(M_min, 1.0-alpha))
      # return log likelihood.
      return N*math.log(c) - alpha*D

 async def fetch_currencies(self, params={}):
     response = await self.publicGetCurrencyList(params)
     currencies = response['response']['entities']
     precision = self.precision['amount']
     result = {}
     for i in range(0, len(currencies)):
         currency = currencies[i]
         id = currency['currency_id']
         code = self.common_currency_code(currency['iso'].upper())
         result[code] = {
             'id': id,
             'code': code,
             'name': currency['name'],
             'active': True,
             'status': 'ok',
             'precision': precision,
             'limits': {
                 'amount': {
                     'min': None,
                     'max': math.pow(10, precision),
                 },
                 'price': {
                     'min': math.pow(10, -precision),
                     'max': math.pow(10, precision),
                 },
                 'cost': {
                     'min': None,
                     'max': None,
                 },
             },
             'info': currency,
         }
     return result

def costFunctionGaussianResponses(datapoints, params, nClasses):
	responses = np.zeros((nClasses,len(datapoints)))

	var = []
	for j in range(nClasses):
		var.append(getGaussianPdf(params[j][0], params[j][1]))

	for j in range(len(datapoints)):
		p = [datapoints[j][0], datapoints[j][1]]
		for i in range(nClasses):
			responses[i,j] = (1/float(nClasses)) * var[i].pdf(p)

	responses = responses / np.sum(responses,axis=0)

	sum = 0
	for i in range(nClasses):
		withinVariance = 0
		for j in range(0, len(datapoints)-1):
			difference = responses[i,j+1] - responses[i,j]
			differenceSquared = math.pow(difference, 2)
			withinVariance += differenceSquared

		v = math.pow(np.std(responses[i]), 2)
		betweenVariance = math.pow(v,2)

		sum += (withinVariance/float(betweenVariance))

	return sum

    def gridToGeodetic(self, north, east):
        """
        Transformation from grid coordinates to geodetic coordinates.
        @param north (corresponds to X in RT 90 and N in SWEREF 99.)
        @param east (corresponds to Y in RT 90 and E in SWEREF 99.)
        @return (latitude, longitude)
        """
        if (self._initialized == False):
            return None

        deg_to_rad = math.pi / 180
        lambda_zero = self._central_meridian * deg_to_rad
        xi = (north - self._false_northing) / (self._scale * self._a_roof)        
        eta = (east - self._false_easting) / (self._scale * self._a_roof)
        xi_prim = xi - \
                self._delta1*math.sin(2.0*xi) * math.cosh(2.0*eta) - \
                self._delta2*math.sin(4.0*xi) * math.cosh(4.0*eta) - \
                self._delta3*math.sin(6.0*xi) * math.cosh(6.0*eta) - \
                self._delta4*math.sin(8.0*xi) * math.cosh(8.0*eta)
        eta_prim = eta - \
                self._delta1*math.cos(2.0*xi) * math.sinh(2.0*eta) - \
                self._delta2*math.cos(4.0*xi) * math.sinh(4.0*eta) - \
                self._delta3*math.cos(6.0*xi) * math.sinh(6.0*eta) - \
                self._delta4*math.cos(8.0*xi) * math.sinh(8.0*eta)
        phi_star = math.asin(math.sin(xi_prim) / math.cosh(eta_prim))
        delta_lambda = math.atan(math.sinh(eta_prim) / math.cos(xi_prim))
        lon_radian = lambda_zero + delta_lambda
        lat_radian = phi_star + math.sin(phi_star) * math.cos(phi_star) * \
                (self._Astar + \
                 self._Bstar*math.pow(math.sin(phi_star), 2) + \
                 self._Cstar*math.pow(math.sin(phi_star), 4) + \
                 self._Dstar*math.pow(math.sin(phi_star), 6))      
        lat = lat_radian * 180.0 / math.pi
        lon = lon_radian * 180.0 / math.pi
        return (lat, lon)

    def test_math_functions(self):
        df = self.sc.parallelize([Row(a=i, b=2 * i) for i in range(10)]).toDF()
        from pyspark.sql import functions
        import math

        def get_values(l):
            return [j[0] for j in l]

        def assert_close(a, b):
            c = get_values(b)
            diff = [abs(v - c[k]) < 1e-6 for k, v in enumerate(a)]
            return sum(diff) == len(a)
        assert_close([math.cos(i) for i in range(10)],
                     df.select(functions.cos(df.a)).collect())
        assert_close([math.cos(i) for i in range(10)],
                     df.select(functions.cos("a")).collect())
        assert_close([math.sin(i) for i in range(10)],
                     df.select(functions.sin(df.a)).collect())
        assert_close([math.sin(i) for i in range(10)],
                     df.select(functions.sin(df['a'])).collect())
        assert_close([math.pow(i, 2 * i) for i in range(10)],
                     df.select(functions.pow(df.a, df.b)).collect())
        assert_close([math.pow(i, 2) for i in range(10)],
                     df.select(functions.pow(df.a, 2)).collect())
        assert_close([math.pow(i, 2) for i in range(10)],
                     df.select(functions.pow(df.a, 2.0)).collect())
        assert_close([math.hypot(i, 2 * i) for i in range(10)],
                     df.select(functions.hypot(df.a, df.b)).collect())

def pythag_pos_diff(objPosArray1,objPosArray2):
    
    #calculates the pythagorian position difference between two n-dimensional position arrays
    pythagSum=0
    for l in range(len(objPosArray1)):
        pythagSum+=math.pow(objPosArray1[l]-objPosArray2[l],2)
    return math.pow(pythagSum,1.0/2.0)

  def testFixedNonUniform(self):
    """Sets up the quantile summary op test as follows.

    Creates array dividing range [0, 1] to 1<<16 elements equally spaced
    with weight same as the value.
    """
    dense_float_tensor_0 = constant_op.constant(
        [(1.0 * i) / math.pow(2.0, 16)
         for i in range(0, int(math.pow(2, 16)) + 1)])
    example_weights = constant_op.constant(
        [(1.0 * i) / math.pow(2.0, 16)
         for i in range(0, int(math.pow(2, 16)) + 1)])

    config = self._gen_config(0.1, 10)

    with self.test_session():
      dense_buckets, _ = quantile_ops.quantile_buckets(
          [dense_float_tensor_0], [], [], [],
          example_weights=example_weights,
          dense_config=[config],
          sparse_config=[])
      self.assertAllClose(
          [0] + [math.sqrt((i + 1.0) / 10) for i in range(0, 10)],
          dense_buckets[0].eval(),
          atol=0.1)

 def getPointDist(self, pt):
     
     assert(self.face is not None)
     
     # fist, get my position
     p = PoseStamped()
     
     p.header.frame_id = "base_link"
     p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
     
     p.pose.position.x = 0
     p.pose.position.y = 0
     p.pose.position.z = 0
     p.pose.orientation.x = 0
     p.pose.orientation.y = 0
     p.pose.orientation.z = 0
     p.pose.orientation.w = 1
     
     try:
         
         self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
         p = self._tf.transformPose(self._robot_frame, p)
         
     except:
         
         rospy.logerr("TF error!")
         return None
     
     return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))

def get_close_matches(term, fields, fuzziness=0.4, key=None):
    import math
    import difflib

    def _ratio(a, b):
        return difflib.SequenceMatcher(None, a, b).ratio()

    term = term.lower()
    matches = []

    for field in fields:
        fld = field if key is None else key(field)
        if term == fld:
            matches.append((field, 1.0))
        else:
            name = fld.lower()
            r = _ratio(term, name)
            if name.startswith(term):
                r = math.pow(r, 0.3)
            elif term in name:
                r = math.pow(r, 0.5)
            if r >= (1.0 - fuzziness):
                matches.append((field, min(r, 0.99)))

    return sorted(matches, key=lambda x: -x[1])

def checkio(strdata):
	strdata = strdata.lower()
	alpha = [ chr(x) for x in range( ord('a'), ord('z')+1) ]
	
	for i in range(2, 36):
		radix = i
		bPassCheck1 = True
		for j in range(0, len(strdata)):
			if strdata[j].isdigit() and int( strdata[j] ) < radix:
				continue
			elif strdata[j].isalpha() and (10 + ord(strdata[j]) -ord('a') )< radix:
				continue
			else:
				bPassCheck1 = False
				continue
		if bPassCheck1 == True:
			mysum = 0
			for k in range(0,len(strdata)):
				if strdata[k].isdigit():
					mysum = mysum + math.pow( radix,  len(strdata)-k-1)*int(strdata[k])
				elif strdata[k].isalpha():
					mysum = mysum + math.pow( radix,  len(strdata)-k-1)*int(ord(strdata[k])-ord('a')+10)
			if mysum%(radix-1) == 0:
				print(radix)
				return radix
	print(0)
	return 0

def DeltaVz(z):
    """Calculates the density contrast of a virialised region S{Delta}V(z),
    assuming a S{Lambda}CDM-type flat cosmology. See, e.g., Bryan & Norman
    1998 (ApJ, 495, 80).

    @type z: float
    @param z: redshift
    @rtype: float
    @return: density contrast of a virialised region at redshift z

    @note: If OMEGA_M0+OMEGA_L0 is not equal to 1, this routine exits and
    prints an error
    message to the console.

    """

    OMEGA_K = 1.0 - OMEGA_M0 - OMEGA_L0

    if OMEGA_K == 0.0:
        Omega_Mz = OmegaMz(z)
        deltaVz = (18.0 * math.pow(math.pi, 2) + 82.0 *
                   (Omega_Mz - 1.0) - 39.0 * math.pow(Omega_Mz - 1, 2))
        return deltaVz
    else:
        raise Exception("cosmology is NOT flat.")

def gradient(image, mask_type='sobel'):
    indice_mascara = {
        "sobelx":[[-1.0, 0.0, 1.0], [-2.0, 0.0, 2.0], [-1.0, 0.0, 1.0]],
        "sobely":[[1.0, 2.0, 1.0], [0.0, 0.0, 0.0], [-1.0, -2.0, -1.0]],
        "prewittx":[[-1.0, 0.0, 1.0], [-1.0, 0.0, 1.0], [-1.0, 0.0, 1.0]],
        "prewitty":[[1.0, 1.0, 1.0], [0.0, 0.0, 0.0], [-1.0, -1.0, -1.0]]
        }

    pic_copy = (image.copy()).load()
    pic = image.load()
    kernelx = indice_mascara[mask_type+'x']
    kernely = indice_mascara[mask_type+'y']
    max_value = 0
    for i in range(image.size[0]):
        for j in range(image.size[1]):

            gx, gy = (0.0, 0.0)
            kernel_len = len(kernelx[0])
            kernel_pos = 0
            for h in range(i-1, i+2):
                for l in range(j-1, j+2):
                    if h >= 0 and l >= 0 and h < image.size[0] and l < image.size[1]:
                        pixel = pic_copy[h, l]
                        pixel = max(pixel)/len(pixel)
                        gx += pixel*kernelx[int(kernel_pos/3)][kernel_pos%3]
                        gy += pixel*kernely[int(kernel_pos/3)][kernel_pos%3]
                        kernel_pos += 1

            gradiente = int(math.sqrt(math.pow(gx, 2) + math.pow(gy, 2)))
            pic[i, j] = tuple([gradiente]*3)
            if gradiente > max_value:
                max_value = gradiente
    return max_value

 def get_nearest(eye, at, up, phis, thetas):
     """ returns phi and theta settings that most closely match current view """
     #todo: derive it instead of this brute force search
     best_phi = None
     best_theta = None
     best_dist = None
     best_up = None
     dist1 = math.sqrt(sum(math.pow(eye[x]-at[x],2) for x in [0,1,2]))
     for t,p in ((x,y) for x in thetas for y in phis):
         theta_rad = (float(t)) / 180.0 * math.pi
         phi_rad = float(p) / 180.0 * math.pi
         pos = [
             float(at[0]) - math.cos(phi_rad)   * dist1 * math.cos(theta_rad),
             float(at[1]) + math.sin(phi_rad)   * dist1 * math.cos(theta_rad),
             float(at[2]) + math.sin(theta_rad) * dist1
         ]
         nup = [
             + math.cos(phi_rad) * math.sin(theta_rad),
             - math.sin(phi_rad) * math.sin(theta_rad),
             + math.cos(theta_rad)
         ]
         dist = math.sqrt(sum(math.pow(eye[x]-pos[x],2) for x in [0,1,2]))
         updiff = math.sqrt(sum(math.pow(up[x]-nup[x],2) for x in [0,1,2]))
         if best_dist == None or (dist<best_dist and updiff<1.0):
             best_phi = p
             best_theta = t
             best_dist = dist
             best_up = updiff
     return best_phi, best_theta

 def update(self,tNow,lon_int,lat_int,t,vel,h):
     # Compute how long it's been since the system updated
     dtReal = tNow - self.tLast
     # rejection criteria
     if self.gpsState.ready and (abs( 1.0e-7*float(lon_int)-self.gpsState.lon ) > 0.01 or abs( 1.0e-7*float(lat_int)-self.gpsState.lat ) > 0.01):
         self.tLast = tNow
         return
     if self.gpsState.ready==False:
         self.gpsState.update(lon_int,lat_int,t,vel,h)
         # initialize the filter
         xk0 = np.array([self.gpsState.x,self.gpsState.y,0.0,0.0,0.0,0.0]) # initial state
         Pk0 = np.diag([math.pow(filter_dynamics.sigma_gps,2.0),math.pow(filter_dynamics.sigma_gps,2.0),1.0,1.0,1.0,1.0]) # initial covariance
         self.EKF.init_P(xk0,Pk0,t)
     else:
         # update the raw GPS object
         self.gpsState.update(lon_int,lat_int,t,vel,h)
         # test that dt is not negative
         dt = t-self.EKF.t
         if dt>0 and dt<10.0*max([dtReal,1.0]):
             # propagate the filter to the current time
             self.EKF.propagateOde(dt)
             # update the filter
             self.EKF.update(t,np.array([self.gpsState.x,self.gpsState.y]),filter_dynamics.measurement,filter_dynamics.measurementGradient,filter_dynamics.Rkin)
         else:
             print("Reject for back in time: dt = %g, dtReal=%g" % (dt,dtReal))
             pass
     # if the filter state matches the reading well enough, use it
     '''
     if math.sqrt( np.sum(np.power(self.EKF.xhat[0:2]-np.array([self.gpsState.x,self.gpsState.y]),2.0)) ) < 10.0:
         # copy the filter state to local
         self.filterState[0:2] = self.EKF.xhat[0:2].copy()
         self.filterState[2] = np.sqrt( np.sum(np.power(self.EKF.xhat[2:4],2.0)) )
         # If we're moving, use the velocity to approximate the heading; else, use the GPS heading
         if self.filterState[2] > 1.0:
             self.filterState[3] = np.arctan2( self.EKF.xhat[3],self.EKF.xhat[2] )
         else:
             self.filterState[3] = self.gpsState.hdg
     else:
         self.filterState[0] = self.gpsState.x
         self.filterState[1] = self.gpsState.y
         self.filterState[2] = self.gpsState.v
         self.filterState[3] = self.gpsState.hdg
     '''
     self.filterState[0] = self.gpsState.x
     self.filterState[1] = self.gpsState.y
     self.filterState[2] = self.gpsState.v
     self.filterState[3] = self.gpsState.hdg
     # Debug test print of state
     #print("%12.7g,%8.4g,%8.4g" % (tNow,self.filterState[2],self.filterState[3]))
     # reset the filter if things look bad
     # are the covariance diagonals zero or nan?
     if (self.EKF.Pk[0,0]==0.0) or (self.EKF.Pk[1,1]==0.0) or (self.EKF.Pk[2,2]==0.0) or (self.EKF.Pk[3,3]==0.0) or (self.EKF.Pk[4,4]==0.0) or (self.EKF.Pk[5,5]==0.0) or (np.any(np.isnan(np.diag(self.EKF.Pk)))):
         # initialize the filter
         xk0 = np.array([self.gpsState.x,self.gpsState.y,0.0,0.0,0.0,0.0]) # initial state
         Pk0 = np.diag([math.pow(filter_dynamics.sigma_gps,2.0),math.pow(filter_dynamics.sigma_gps,2.0),1.0,1.0,1.0,1.0]) # initial covariance
         self.EKF.init_P(xk0,Pk0,t)
     # call the log
     self.logFun(t,tNow)
     # update the time tracker
     self.tLast = tNow

def convert_to_polar(mag):
    hmag = mag[0]
    vmag = mag[1]
    angle = math.atan2(float(vmag), float(hmag))
    distance = math.sqrt(math.pow(mag[0], 2) + math.pow(mag[1], 2))
    angle = 180 - math.degrees(angle)
    return (angle, distance)

def RMSE(mat_predict, mat_true):
	
	sha_predict = mat_predict.shape
	sha_true = mat_true.shape

	if sha_true != sha_predict:
		print('error! yay!')
		return 0

	summy = float(0.0)
	count = float(0.0)

	# set up the data frame for outputting
	predict_out = np.matrix([[0,0,0]])

	# you only care about the non-null values of mat_true
	for i in xrange(0,numusers):
		for j in xrange(0,nummovies):
			if mat_true[i,j] != 0:
				count = count + 1
				summy = summy + math.pow((mat_true[i,j] - mat_predict[i,j]),2)

				# add to the output matrix
				predict_out = np.vstack((predict_out,np.matrix([i+1,j+1,mat_predict.item(i,j)])))

	# complete the equation
	RSME_value = math.pow(summy/count,0.5)

	# return it after deleting the first rwo etc
	predict_out = np.delete(predict_out,(0),axis = 0)
	
	return RSME_value, predict_out

def isInCircle(x,y, circle):
	dist = math.sqrt((math.pow(x-circle[0],2))+(math.pow(y-circle[1],2)))
	if dist <= int(100):
		return True
	else:
		cv2.circle(original,(x,y),1,(255,255,255))
		return False