def generateMesh(n_points):
    points = [] # Simple 2D for initial testing

    for i in range(n_points):
        r = random.gauss(10,0.05)
        theta = random.vonmisesvariate(math.pi,0)
        phi = random.vonmisesvariate(math.pi, 0)

        x= r * math.sin(theta) * math.cos(phi)
        y= r * math.sin(theta) * math.sin(phi)
        z= r * math.cos(theta)
        points.append([x,y,z])

    # Now just to check we have the target number of unique points
    nppoints = np.asarray(points)

    meshgen = pymesh.tetgen();
    meshgen.points = nppoints;
    meshgen.merge_coplanar = True
    meshgen.coarsening = False
    meshgen.verbosity = 0
    meshgen.run()

    mesh = pymesh.subdivide(meshgen.mesh, order = 5, method="loop")
    mesh = meshgen.mesh
    return(mesh)

def randlog(eps):
	"""Return a random complex number, clustered near 0.
	
	The value returned has modulus in [eps, 1], with uniformly distributed logarithm.  Its argument is uniformly distributed.
	"""
	
	return rect(math.exp(uniform(math.log(eps), 0)), vonmisesvariate(0, 0))

def drop_needle(l):
    x0 = random.random()
    a = random.vonmisesvariate(0,0)
    xeind = x0+l*math.cos(a)
    if xeind <0 or xeind >1:
        return True
    else:
        return False

def randline():
    x0 = random.random()
    a = random.vonmisesvariate(0,0)
    x1 = x0+l*math.sin(a)
    
    if math.floor(x0) != math.floor(x1):
        return 1
    return 0

def naald(L):
    x=random.random()
    a=random.vonmisesvariate(0,0)
    eind=x+L*math.cos(a)
    if eind <= 1 and eind >= 0:
        return False
    else:
        return True

def drop_needle(L):
    x0 = random.random()
    theta = random.vonmisesvariate(0,0)
    x1 = x0 + L*math.cos(theta)
    if x1 >= 1 or x1 <= 0:
        return True
    else:
        return False

def drop_needle(L):
    x1 = random.random()
    hoek = random.vonmisesvariate(0,0)
    x2 = x1 + L * math.sin(hoek)
    if x2 > 1 or x2 < 0:
        return True
    else:
        return False

def drop_needle(L): 
        x = random.random()
        a = random.vonmisesvariate(0,0)
        x2 = (x + L*math.cos(a))
        if x2 > 1 or x2 < 0: 
            return True
        else:
            return False

 def drop_needle(L):
     m=random.random()
     #a=2*3.141592653589793*random.random()
     a=random.vonmisesvariate(0,0)
     if abs(math.ceil(m+L*math.sin(a))-math.ceil(m))>0.1:
         return True
     else: 
         return False

def drop_needle(L):
        x0 = random.random()
        a = random.vonmisesvariate(0,0)
        x1 = x0 + L*math.cos(a)
        if x1>0:
            if x1<1:
                return False
        return True

	def update_non_amcl(self, scan, pf):

		self.weight_particles(scan, pf)
		
		resampledPoses = []
		notAccepted = True
		numParticles = len(pf.particlecloud.poses)

		#Resamples the poses
		for i in range(0,numParticles):
			notAccepted = True
			while (notAccepted):
				index = random.randint(0,numParticles-1)
				posX = pf.particlecloud.poses[index].position.x
				posY = pf.particlecloud.poses[index].position.y
				if (random.uniform(0,1) < particleWeights[index]/totalWeight):
					notAccepted = False
			resampledPoses.append(pf.particlecloud.poses[index])

		cont = True
		pArray = PoseArray()
		temp = []
		val = Pose()
		count = 0

		#Smudges the poses
		while cont:
			temp = []
			val = resampledPoses[0]
			count = 0

			#Removes the duplicate poses from the list
			for i in range(0, len(resampledPoses)):
				if (resampledPoses[i] == val):
					count = count + 1
				else:
					temp.append(resampledPoses[i])

			resampledPoses = temp

			#Checks that we have allocated all particles to be smudged
			if (len(resampledPoses) == 0):
				cont = False
				
			#Apply smuding to all but one of the same resampled particle
			for i in range(0, count):
				if i > 0:
					newPose = Pose()
					newPose.position.x = random.gauss(val.position.x, 0.35) #TEST THIS
					newPose.position.y = random.gauss(val.position.y, 0.35)
					newPose.orientation = rotateQuaternion(val.orientation, random.vonmisesvariate(0, 7))
					 #MAKE SURE TO TEST
					pArray.poses.append(newPose)
					
				else:
					pArray.poses.append(val)
			
		return pArray

def drop_needle(L) :
    x = random.random()
    a = random.vonmisesvariate(0,0)
    x1 = x + L * math.cos(a)
  
    if x1 <= 0 or x1 >= 1:
        return(True)
    else:
        return(False)

def drop_needle(integer):
    x = random.random()
    a = random.vonmisesvariate(0,0)
    verschil = math.cos(a)
    eindpunt = x + (integer * math.cos(a))
    if 0 < eindpunt and eindpunt < 1:
        return False
    else:
        return True

def drop_needle(L):
    x_1 = random.random()
    hoek = random.vonmisesvariate(0,0)

    x_2 = x_1 + l*math.cos(hoek)

    if x_2 > 1 or x_2 < 0:
        return True
    else:
        return False

def drop_neelde(L):
    x = random.random()
    y = random.random()
    a = random.vonmisesvariate(0,0)
    xEnd = x+L*math.cos(a)
    yEnd = y+L*math.sin(a)
    if xEnd<0 or xEnd>1:
          return True
    else:
          return False

def drop_needle(L):
    startX = random.random() + L 
    #startY = random.random()
    angle = random.vonmisesvariate(0,0)
    
    endX = startX + L*math.cos(angle)
    #endY = startY + L*math.sin(angle)
    diffX = abs(int(startX) - int(endX))
    #print("xb:", startX, "xe:", endX, "=", diffX)
    return diffX > 0

def drop_needle(L):
    needleX = random.random()
    angle = random.vonmisesvariate(0,0)
    needleX2 = needleX + L* math.cos(angle)
    
    if needleX2 > 1 or needleX2 < 0:
        #print(needleX, angle, needleX2)
        return True
    else:
        return False

def drop_needle(L):
    x1=random.random()
    phi=random.vonmisesvariate(0,0)
    x2=x1+L*math.cos(phi)
    if x1==1 or x1==0:
        return True
    elif x2<=0 or x2>=1:
        return True
    else:
        return False

def drop_needle(L):
    # uniform in [0,1]
    x1 = random.random()
    # uniform in [0,2pi]
    a = random.vonmisesvariate(0,0)
    x2 = x1+L*math.sin(a)
    if x2>=1 or x2<=0:
        return True
    else:
        return False

def drop_needle(L):
    x0 = random.random()
    y0 = random.random()
    angle = random.vonmisesvariate(0,0)
    punt2 = (x0+L*math.cos(angle),y0+L*math.sin(angle))
    if punt2[0]<=0 or punt2[0]>=1:
        hit = True
    else:
        hit = False
    return hit