def calculate_initial_compass_bearing(self, pointA, pointB):
        """
        Calculates direction between two points.
        Code based on compassbearing.py module
        https://gist.github.com/jeromer/2005586

        pointA: latitude/longitude for first point (decimal degrees)
        pointB: latitude/longitude for second point (decimal degrees)
    
        Return: direction heading in degrees (0-360 degrees, with 90 = North)
        """

        if (type(pointA) != tuple) or (type(pointB) != tuple):
            raise TypeError("Only tuples are supported as arguments")
    
        lat1 = math.radians(pointA[0])
        lat2 = math.radians(pointB[0])
    
        diffLong = math.radians(pointB[1] - pointA[1])
    
        # Direction angle (-180 to +180 degrees):
        # θ = atan2(sin(Δlong).cos(lat2),cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))

        x = math.sin(diffLong) * math.cos(lat2)
        y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) * math.cos(lat2) * math.cos(diffLong))
    
        initial_bearing = math.atan2(x, y)
    
        # Direction calculation requires to normalize direction angle (0 - 360)
        initial_bearing = math.degrees(initial_bearing)
        compass_bearing = (initial_bearing + 360) % 360
    
        return compass_bearing

def show_visual(sec = False, radius = 39):
    c = Canvas(2*radius+1, 2*radius+1)
    x = 0
    while True:
        t = time.localtime()

        c.draw_circle(radius,radius,radius,1)
        c.draw_circle(radius,radius,radius-1,1)
        for i in xrange(12):
            dx = math.sin(2.0*math.pi*i/12)
            dy = math.cos(2.0*math.pi*i/12)
            if i%3 == 0:
                c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.8*dx*radius), radius-int(0.8*dy*radius),1)
            else:
                c.draw_line(radius+int(dx*radius), radius-int(dy*radius), radius+int(0.9*dx*radius), radius-int(0.9*dy*radius),1)

        G = [((t[3]%12), 12, 0.4, 2), (t[4], 60, 0.6, 3), (t[5], 60, 0.8, 4)]
        for gnomon in G:
            dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1])
            dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1])

            c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), gnomon[3])
            c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), gnomon[3])

        c.render(width, height)
        time.sleep(1)

        for gnomon in G:
            dx = math.sin(2.0*math.pi*gnomon[0]/gnomon[1])
            dy = math.cos(2.0*math.pi*gnomon[0]/gnomon[1])

            c.draw_line(radius, radius, radius+int(dx*gnomon[2]*radius), radius-int(dy*gnomon[2]*radius), 0)
            c.draw_line(radius, radius, radius-int(dx*gnomon[2]*radius/4), radius+int(dy*gnomon[2]*radius/4), 0)

def simplesnr(f,h,i=None,years=1,noisemodel=None,includewd=None):
    if i == None:
        h0 = h * math.sqrt(16.0/5.0)    # rms average over inclinations
    else:
        h0 = h * math.sqrt((1 + math.cos(i)**2)**2 + (2*math.cos(i))**2)        
    
    return h0 * math.sqrt(years * 365.25*24*3600) / math.sqrt(lisanoise(f,noisemodel,includewd))

def gps_distance_between(point_a, point_b):
    """
    Calculate the orthodromic distance between two GPS readings.

    point_a and point_b can be either of the two:
    - tuples in the form (latitude, longitude).
    - instances of the class logwork.Signal
    The result is in metres.

    ATTENTION: since latitude is given before longitude, if we are using the
    X and Y representation, then we must pass in (Y, X) and *not* (X, Y)

    Computed with the Haversine formula
    (http://en.wikipedia.org/wiki/Haversine_formula)
    """
    if hasattr(point_a, "latitude"):
        a_lat, a_lon = math.radians(point_a.latitude), math.radians(point_a.longitude)
    else:
        a_lat, a_lon = math.radians(point_a[0]), math.radians(point_a[1])
    if hasattr(point_b, "latitude"):
        b_lat, b_lon = math.radians(point_b.latitude), math.radians(point_b.longitude)
    else:
        b_lat, b_lon = math.radians(point_b[0]), math.radians(point_b[1])
    d_lat = b_lat - a_lat
    d_lon = b_lon - a_lon
    a = math.sin(d_lat / 2.0) ** 2 + math.cos(a_lat) * math.cos(b_lat) * math.sin(d_lon / 2.0) ** 2
    c = 2 * math.asin(math.sqrt(a))
    return EARTH_RADIUS * c * 1000

def rotate_point(point, center, angle):
	""" Rotate a point around another point
	"""
	angle = math.radians(angle)
	x = center[0] + (point[0] - center[0]) * math.cos(angle) - (point[1] - center[1]) * math.sin(angle);
	y = center[1] - (point[0] - center[0]) * math.sin(angle) + (point[1] - center[1]) * math.cos(angle);
	return (x, y)

	def cart2tether_actual(sz,xyz):
		#convert a cartesian goal to tether length goals. assumes the tethers go to points on the outside edge of the end effector.
		#returns a more precise estimate of tether length, but one that is inadmissible to the get_xyz_pos function
		#goal : 1x3 array [x,y,z]
			# L = 1x4 array [L0,L1,L2,L3] spiral zipper and each tether length
		#finds the axis-angle rotation matrix from the column's vertical pose.
		theta = sz.angle_between([0,0,sz.L[0]], xyz)
		k = np.cross([0,0,sz.L[0]],xyz)

		if np.linalg.norm(k) != 0: #ensures k is a unit vector where its norm == 1
			k = k/np.linalg.norm(k)

		Xk = k[0]
		Yk = k[1]
		Zk = k[2]
		v = 1 - m.cos(theta)

		R = np.array( [[m.cos(theta) + (Xk**2*v)   , (Xk*Yk*v) - (Zk*m.sin(theta)), (Xk*Zk*v) + Yk*m.sin(theta)],\
					   [((Yk*Xk*v) + Zk*m.sin(theta)), m.cos(theta) + (Yk**2*v)     , (Yk*Zk*v) - Xk*m.sin(theta)],\
					   [(Zk*Xk*v) - Yk*m.sin(theta), (Zk*Yk*v) + Xk*m.sin(theta)  , m.cos(theta) + (Zk**2*v)   ]])


		#calculates position vector of the column tether attachment points in the world frame
		OB1 = xyz + np.dot(R,sz.ef[0])  
		OB2 = xyz + np.dot(R,sz.ef[1])		
		OB3 = xyz + np.dot(R,sz.ef[2])

		L0 = m.sqrt((xyz[0]**2+xyz[1]**2+xyz[2]**2)) # should just be sz.L[0] if not there is a math mistake
		L1 = np.linalg.norm(OB1 - sz.p[0])
		L2 = np.linalg.norm(OB2 - sz.p[1])
		L3 = np.linalg.norm(OB3 - sz.p[2])
		L = [L0,L1,L2,L3]
		return L

def distance_on_unit_sphere(lat1, long1, lat2, long2):

    # Convert latitude and longitude to
    # spherical coordinates in radians.
    degrees_to_radians = math.pi/180.0

    # phi = 90 - latitude
    phi1 = (90.0 - lat1)*degrees_to_radians
    phi2 = (90.0 - lat2)*degrees_to_radians

    # theta = longitude
    theta1 = long1*degrees_to_radians
    theta2 = long2*degrees_to_radians

    # Compute spherical distance from spherical coordinates.

    # For two locations in spherical coordinates
    # (1, theta, phi) and (1, theta, phi)
    # cosine( arc length ) =
    #    sin phi sin phi' cos(theta-theta') + cos phi cos phi'
    # distance = rho * arc length

    cos = (math.sin(phi1)*math.sin(phi2)*math.cos(theta1 - theta2) +
           math.cos(phi1)*math.cos(phi2))
    arc = math.acos(cos)

    # Remember to multiply arc by the radius of the earth
    # in your favorite set of units to get length.
    return arc * EARTH_RADIUS_MILES

    def refresh(self, matrix):
        matrix.fade(0.995)

        y0 = matrix.height/2
        x0 = matrix.width/2

        if self.angle >= pi:
            x0 -= 1

        if self.angle > (0.5*pi) and self.angle < (1.5*pi):
            y0 -= 1

        x1 = int(self.x0 + self.radius * sin(self.angle-self.astep))
        y1 = int(self.y0 + self.radius * cos(self.angle+self.astep))

        x2 = int(self.x0 + self.radius * sin(self.angle))
        y2 = int(self.y0 + self.radius * cos(self.angle))

        matrix.drawPoly(
            [(self.x0, self.y0), (x1, y1), (x2, y2)],
            hsvToRgb(self.hue)
        )

        self.hue = fmod(self.hue+self.hstep, 1.0)
        self.angle += self.astep

    def update(self):
        if self.turning_right:
            self.rot -= 5
        if self.turning_left:
            self.rot += 5

        a = [0.0,0.0]
        if self.boost_endtime > rabbyt.get_time():
            f = 3*(self.boost_endtime - rabbyt.get_time())/self.boost_length
            a[0] += cos(radians(self.boost_rot))*f
            a[1] += sin(radians(self.boost_rot))*f
            self.create_boost_particle()

        if self.accelerating:
            a[0] += cos(radians(self.rot))*.9
            a[1] += sin(radians(self.rot))*.9
            self.create_dust_particle(self.dust_r)
            self.create_dust_particle(self.dust_l)

        ff = .9 # Friction Factor

        self.velocity[0] *= ff
        self.velocity[1] *= ff

        self.velocity[0] += a[0]
        self.velocity[1] += a[1]

        self.x += self.velocity[0]
        self.y += self.velocity[1]

    def sumVectors(self, vectors):
        """ sum all vectors (including targetvector)"""
        endObstacleVector = (0,0)

        ##generate endvector of obstacles
        #sum obstaclevectors
        for vector in vectors:
            vectorX = math.sin(math.radians(vector[1])) * vector[0] # x-position
            vectorY = math.cos(math.radians(vector[1])) * vector[0] # y-position
            endObstacleVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
        #mean obstaclevectors
        if len(vectors) > 0:
            endObstacleVector = (endObstacleVector[0]/len(vectors), endObstacleVector[1]/len(vectors))

        #add targetvector
        targetVector = self.target
        if targetVector != 0 and targetVector != None:
            vectorX = math.sin(math.radians(targetVector[1])) * targetVector[0] # x-position
            vectorY = math.cos(math.radians(targetVector[1])) * targetVector[0] # y-position
            endVector = (endObstacleVector[0]+vectorX,endObstacleVector[1]+vectorY)
            #endVector = (endVector[0]/2, endVector[1]/2)
        else:
            endVector = endObstacleVector


        return endVector

    def __init__(self, scale=1.0):
        self.translation = Vector3()
        self.rotation = Vector3()
        self.initialHeight = Vector3(0, 0, scale*StewartPlatformMath.SCALE_INITIAL_HEIGHT)
        self.baseJoint = []
        self.platformJoint = []
        self.q = []
        self.l = []
        self.alpha = []
        self.baseRadius = scale*StewartPlatformMath.SCALE_BASE_RADIUS
        self.platformRadius = scale*StewartPlatformMath.SCALE_PLATFORM_RADIUS
        self.hornLength = scale*StewartPlatformMath.SCALE_HORN_LENGTH
        self.legLength = scale*StewartPlatformMath.SCALE_LEG_LENGTH;

        for angle in self.baseAngles:
            mx = self.baseRadius*cos(radians(angle))
            my = self.baseRadius*sin(radians(angle))
            self.baseJoint.append(Vector3(mx, my))

        for angle in self.platformAngles:
            mx = self.platformRadius*cos(radians(angle))
            my = self.platformRadius*sin(radians(angle))
            self.platformJoint.append(Vector3(mx, my))

        self.q = [Vector3()]*len(self.platformAngles)
        self.l = [Vector3()]*len(self.platformAngles)
        self.alpha = [0]*len(self.beta)

def distance(origin, destination):
    """ 
    Calculates both distance and bearing
    """
    lat1, lon1 = origin
    lat2, lon2 = destination
    if lat1>1000:
        (lat1,lon1)=dm2dd(lat1,lon1)
        (lat2,lon2)=dm2dd(lat2,lon2)
        print('converted to from ddmm to dd.ddd')
    radius = 6371 # km
    

    dlat = math.radians(lat2-lat1)
    dlon = math.radians(lon2-lon1)
    a = math.sin(dlat/2) * math.sin(dlat/2) + math.cos(math.radians(lat1)) \
        * math.cos(math.radians(lat2)) * math.sin(dlon/2) * math.sin(dlon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    d = radius * c
    
    def calcBearing(lat1, lon1, lat2, lon2):
       dLon = lon2 - lon1
       y = math.sin(dLon) * math.cos(lat2)
       x = math.cos(lat1) * math.sin(lat2) \
           - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
       return math.atan2(y, x)
       
    bear= math.degrees(calcBearing(lat1, lon1, lat2, lon2))  
    return d,bear

    def update_location(self, delta_encoder_count_1, delta_encoder_count_2):
        """
        Update the robot's location

        @rtype : DifferentialDriveRobotLocation
        @return: Updated location
        @param delta_encoder_count_1: Count of wheel 1's encoder since last update
        @param delta_encoder_count_2: Count of wheel 2's encoder since last update
        @type delta_encoder_count_1: int
        @type delta_encoder_count_2: int
        """
        dfr = delta_encoder_count_2 * 2 * math.pi / self.robot_parameters.steps_per_revolution
        dfl = delta_encoder_count_1 * 2 * math.pi / self.robot_parameters.steps_per_revolution

        ds = (dfr + dfl) * self.robot_parameters.wheel_radius / 2
        dz = (dfr - dfl) * self.robot_parameters.wheel_radius / self.robot_parameters.wheel_distance

        self.location.x_position += ds * math.cos(self.location.z_position + dz / 2)
        self.location.y_position += ds * math.sin(self.location.z_position + dz / 2)
        self.location.z_position += dz

        self.globalLocation.x_position += ds * math.cos(self.globalLocation.z_position + dz / 2)
        self.globalLocation.y_position += ds * math.sin(self.globalLocation.z_position + dz / 2)
        self.globalLocation.z_position += dz

        return self.location, self.globalLocation

def distance(xlat, xlon, ylat, ylon):
    dlon = ylon - xlon
    dlat = ylat - xlat
    a = sin(dlat / 2) ** 2 + cos(xlat) * cos(ylat) * sin(dlon / 2) ** 2
    c = 2 * atan2(sqrt(a), sqrt(1 - a))
    distance = R * c
    return distance

 def __init__(self,pos,direction,d_range):
     self.surface = pygame.surface.Surface((50,50))
     self.surface.fill((0,250,200))
     self.rect = pygame.rect.Rect(pos[0],pos[1],50,50)
     self.dir = direction
     self.dist = 0
     self.range = d_range
     speed = 2
     dx = 0
     dy = 0
     if self.dir>0:
         if self.dir>90:
             dy = speed*math.sin(180-self.dir)
             dx = speed*math.cos(180-self.dir)
         else:
             dy = speed*math.sin(self.dir)
             dx = speed*math.cos(self.dir)
     else:
         if self.dir<-90:
             dy = speed*math.sin(180+self.dir)
             dx = speed*math.cos(180+self.dir)
         else:
             dy = speed*math.sin(self.dir)
             dx = speed*math.cos(self.dir)
     self.dx = dx
     self.dy = dy

    def __init__(self, matrix=None, scale=None, rotation=None,
                 translation=None):
        params = any(param is not None
                     for param in (scale, rotation, translation))

        if params and matrix is not None:
            raise ValueError("You cannot specify the transformation matrix and"
                             " the implicit parameters at the same time.")
        elif matrix is not None:
            if matrix.shape != (3, 3):
                raise ValueError("Invalid shape of transformation matrix.")
            self._matrix = matrix
        elif params:
            if scale is None:
                scale = 1
            if rotation is None:
                rotation = 0
            if translation is None:
                translation = (0, 0)

            self._matrix = np.array([
                [math.cos(rotation), - math.sin(rotation), 0],
                [math.sin(rotation),   math.cos(rotation), 0],
                [                 0,                    0, 1]
            ])
            self._matrix[0:2, 0:2] *= scale
            self._matrix[0:2, 2] = translation
        else:
            # default to an identity transform
            self._matrix = np.eye(3)

 def __init__(self, lb, lb_length, up_angle, dn_angle, hb, hb_length):
     # define the name
     # (there is only one launchbar element) --> isn't it ?
     name = 'YASim_Launchbar'
     
     # Calculate points for the mesh
     
     # here in the original script hb = hb - lb
     # --> seems to be tuple - vector, that is not working
     # assuming: (this step is necessary to get from global to local coordinates !!)
     hb = hb - Vector(lb)
     
     lb_tip = ORIGIN + lb_length * math.cos(dn_angle * DEG2RAD) * X - lb_length * math.sin(dn_angle * DEG2RAD) * Z
     hb_tip = hb - hb_length * math.cos(dn_angle * DEG2RAD) * X - hb_length * math.sin(dn_angle * DEG2RAD) * Z
     
     # create the mesh: launchbar and holdback extended position
     lb_obj = mesh_create(name, lb, [ORIGIN, lb_tip, hb, hb_tip, lb_tip+0.05*Y, lb_tip-0.05*Y, hb_tip+0.05*Y, hb_tip-0.05*Y],
                                 [(0,1),(0,2),(2,3),(4,5),(6,7)], [])
     
     # set the created object active !!!!!!!
     bpy.context.scene.objects.active = lb_obj
     
     # draw dashed lines for the retracted position
     # get the active mesh
     mesh = bpy.context.object.data
     
     lb_up = lb_length * math.cos(up_angle * DEG2RAD) * X - lb_length * math.sin(up_angle * DEG2RAD) * Z
     hb_up = hb - hb_length * math.cos(up_angle * DEG2RAD) * X - hb_length * math.sin(up_angle * DEG2RAD) * Z
     
     draw_dashed_line(mesh, ORIGIN, lb_up)
     draw_dashed_line(mesh, hb, hb_up)
     
     # set material
     Item.set_material('grey2', (0.3,0.3,0.3), 1)

 def scale(self):
     if math.cos(self.rotation) == 0:
         # sin(self.rotation) == 1
         scale = self._matrix[0, 1]
     else:
         scale = self._matrix[0, 0] / math.cos(self.rotation)
     return scale

    def execute(self, context):

        A = 6.283185307179586476925286766559 / 3

        verts = [(sin(A * 1), 0.0, cos(A * 1)),
                 (sin(A * 2), 0.0, cos(A * 2)),
                 (sin(A * 3), 0.0, cos(A * 3)),
                 ]

        faces = [(0, 1, 2)]

        mesh = bpy.data.meshes.new("Cube")

        bm = bmesh.new()

        for v_co in verts:
            bm.verts.new(v_co)

        for f_idx in faces:
            bm.faces.new([bm.verts[i] for i in f_idx])

        bm.to_mesh(mesh)
        mesh.update()

        object_utils.object_data_add(context, mesh)

        return{'FINISHED'}

    def paint(self, painter, option, widget):
        if not self.source or not self.dest:
            return

        # Draw the line itself.
        line = QtCore.QLineF(self.sourcePoint, self.destPoint)

        if line.length() == 0.0:
            return

        painter.setPen(QtGui.QPen(QtCore.Qt.black, 1, QtCore.Qt.SolidLine, QtCore.Qt.RoundCap, QtCore.Qt.RoundJoin))
        painter.drawLine(line)

        # Draw the arrows if there's enough room.
        angle = math.acos(line.dx() / line.length())
        if line.dy() >= 0:
            angle = Edge.TwoPi - angle

        sourceArrowP1 = self.sourcePoint + QtCore.QPointF(math.sin(angle + Edge.Pi / 3) * self.arrowSize,
                                                          math.cos(angle + Edge.Pi / 3) * self.arrowSize)
        sourceArrowP2 = self.sourcePoint + QtCore.QPointF(math.sin(angle + Edge.Pi - Edge.Pi / 3) * self.arrowSize,
                                                          math.cos(angle + Edge.Pi - Edge.Pi / 3) * self.arrowSize);
        destArrowP1 = self.destPoint + QtCore.QPointF(math.sin(angle - Edge.Pi / 3) * self.arrowSize,
                                                      math.cos(angle - Edge.Pi / 3) * self.arrowSize)
        destArrowP2 = self.destPoint + QtCore.QPointF(math.sin(angle - Edge.Pi + Edge.Pi / 3) * self.arrowSize,
                                                      math.cos(angle - Edge.Pi + Edge.Pi / 3) * self.arrowSize)

        painter.setBrush(QtCore.Qt.black)
        painter.drawPolygon(QtGui.QPolygonF([line.p1(), sourceArrowP1, sourceArrowP2]))
        painter.drawPolygon(QtGui.QPolygonF([line.p2(), destArrowP1, destArrowP2]))