def distance(p1, p2):
    p1lat, p1lon = float(p1[0]), float(p1[1])
    p2lat, p2lon = float(p2[0]), float(p2[1])
    latdiff = (p2lat + p1lat) / 2 * 0.01745
    dlat = 111.3 * np.fabs(p1lat - p2lat)
    dlon = 111.3 * np.cos(latdiff) * np.fabs(p1lon - p2lon)
    return np.sqrt(np.pow(dlat, 2) + np.pow(dlon, 2))

def matrix_max_norm(matrix):
    maxNorm = math.fabs(matrix[0, 0])
    for rowIndex in range(0, matrix.shape[0]):
        for colIndex in range(0, matrix.shape[1]):
            if math.fabs(matrix[rowIndex, colIndex]) > math.fabs(maxNorm):
                maxNorm = matrix[rowIndex, colIndex]
    return math.fabs(maxNorm)

    def _check(self, statsE, stats):
        """
    Check earthquake stats.
    """
        attrs = ["timestamp", "ruparea", "potency", "moment", "avgslip", "mommag"]

        statsE.avgslip = statsE.potency / (statsE.ruparea + 1.0e-30)
        statsE.mommag = 2.0 / 3.0 * (numpy.log10(statsE.moment) - 9.05)

        for attr in attrs:
            valuesE = statsE.__getattribute__(attr)
            values = stats.__getattribute__(attr)
            msg = "Mismatch in number of snapshots for attribute '%s', %d != %d." % (attr, len(valuesE), len(values))
            self.assertEqual(len(valuesE), len(values), msg=msg)

            for (valueE, value) in zip(valuesE, values):
                msg = "Mismatch in value for attribute '%s', %g != %g." % (attr, valueE, value)
                if valueE != 0.0:
                    if math.isinf(math.fabs(valueE)):
                        self.assertAlmostEqual(1.0, math.fabs(value) / 1.0e30, places=6, msg=msg)
                    else:
                        self.assertAlmostEqual(1.0, value / valueE, places=6, msg=msg)
                else:
                    self.assertAlmostEqual(valueE, value, places=6, msg=msg)

        return

def colormap(pixel):
    white=(221,221,221)
    orange=(219,125,62)
    magenta=(179,80,188)
    lightblue=(107,138,201)
    yellow=(177,166,39)
    lime=(65,174,56)
    pink=(208,132,153)
    gray=(64,64,64)
    lightgray=(154,161,161)
    cyan=(46,110,137)
    purple=(126,61,181)
    blue=(46,56,141)
    brown=(79,50,31)
    green=(53,70,27)
    red=(150,52,48)
    black=(25,22,22)
 
    colors=(white,orange,magenta,lightblue,yellow,lime,pink,gray,lightgray,cyan,purple,blue,brown,green,red,black)
 
    thecolor=0
    finalresult=256*256*256
    for idx,color in enumerate(colors):
       result=math.fabs(color[0]-pixel[0])+math.fabs(color[1]-pixel[1])+math.fabs(color[2]-pixel[2])
       if result < finalresult:
          finalresult=result
          thecolor=idx
    return thecolor

    def getStepsize(self, mix_den, ht):
        mix_den_fil = np.fabs(mix_den) > 1.E-7
        a = ht[mix_den_fil] / mix_den[mix_den_fil]
        b = 1.0 + a
        b_fil = np.fabs(b) > 1.E-7
        w = self.w
        sl = w * ht[b_fil] / b[b_fil]
        s11 = sl.sum()
        s0 = (w * ht).sum()

        step, oldstep = 0., 0.
        for i in range(50):
            grad1, grad2 = 0., 0.
            for j in range(self.n):
                a = mix_den[j] + step * ht[j]
            if math.fabs(a) > 1.E-7:
                b = ht[j] / a
                grad1 = grad1 + w * b
                grad2 = grad2 - w * b * b
            if math.fabs(grad2) > 1.E-10:
                step = step - grad1 / grad2
            if oldstep > 1.0 and step > oldstep:
                step = 1.
                break
            if grad1 < 1.E-7:
                break
            oldstep = step
        if step > 1.0:
            return 1.0
        return step

def getMotionArrayRGBOLD(image1, image2, treshold = 10):
    i1 = image1
    i2 = image2
    #both images need to be the same size in pixels
    if (i1.size[0] != i2.size[0]) or (i1.size[1] != i2.size[1]):
        return 0 #if not, we return 0
    size = i1.size

    t = treshold
    motionarray = [] #2D array to store motion areas

    i=0
    while i in range(size[1]): #scan through the images
        j=0
        while j in range(size[0]):
            p1 = i1.getpixel((j,i))
            p2 = i2.getpixel((j,i))

            if (fabs(p1[0]-p2[0]) > t) or (fabs(p1[1]-p2[1]) > t) or (fabs(p1[2]-p2[2]) > t): #compare each pixel in R,G,B channel
                    motionarray.append((j,i,p2))#by deducting these values we mirror the coordinates to mimick a mirror-image
                                          #also return p2; the RGB value for the pixel from the second (newer) image
            j = j+1
        i = i+1
 
    return motionarray

def get_rectangle(bounds):
    # This converts a latitude delta into an image delta. For USA, at zoom
    # level 19, we know that we have 0.21 meters/pixel. So, an image is showing
    # about 1280 pixels * 0.21 meters/pixel = 268.8 meters.
    # On the other hand we know that at the same angle, a degress in latlon is:
    # https://en.wikipedia.org/wiki/Latitude
    # latitude = 111,132 m
    # longitude = 78,847 m
    latitude_factor = 111132.0 / 0.21
    longitude_factor = 78847.0 / 0.21

    # Feature size
    feature_width = longitude_factor * math.fabs(bounds[1] - bounds[3])
    feature_height = latitude_factor * math.fabs(bounds[0] - bounds[2])
    if feature_width > image_width or feature_height > image_height:
        print "** Warning ** The feature is bigger than the image."

    # CV params (int required)
    x = int((image_width / 2) - (feature_width / 2))
    y = int((image_height / 2) - (feature_height / 2))
    w = int(feature_width)
    h = int(feature_height)
    if w <= 25 or h <= 25:
        print "** Warning ** This image has very narrow bounds."
        print bounds
        print x, y, w, h
    if x <= 0 or y <= 0 or w <= 0 or h <= 0:
        print "** Warning ** There is something wrong with this image bounds."
        print bounds
        print x, y, w, h
    return x, y, w, h

 def moveByPlatform(self):
     for platform in map.platforms:
         for point in self.boxPoints:
                 if self.x+platform.fX+point.x>platform.x and self.x+platform.fX+point.x<platform.x+platform.sizeX and self.y+platform.fY+1+point.y>platform.y and self.y+platform.fY+1+point.y<platform.y+platform.sizeY or \
                 self.x+point.x>platform.x and self.x+point.x<platform.x+platform.sizeX and self.y+point.y>platform.y and self.y+point.y<platform.y+platform.sizeY:
                     for i in range(int(math.fabs(platform.fX)+1)):
                         if not self.touch():
                             if platform.fX>0:
                                 self.x += 1
                             if platform.fX<0:
                                 self.x -= 1
                     if platform.fX>0:
                         self.x -= 1
                     elif platform.fX<0:
                         self.x += 1
                     for i in range(int(math.fabs(platform.fY)+1)):
                         if not self.touch():
                             if platform.fY>0:
                                 self.y += 1
                             if platform.fY<0:
                                 self.y -= 1
                     if platform.fY>0:
                         self.y -= 1
                     elif platform.fY<0:
                         self.y += 1
                     break

def is_square(contour):
    """
    Squareness checker

    Square contours should:
        -have 4 vertices after approximation, 
        -have relatively large area (to filter out noisy contours)
        -be convex.
        -have angles between sides close to 90deg (cos(ang) ~0 )
    Note: absolute value of an area is used because area may be
    positive or negative - in accordance with the contour orientation
    """

    area = math.fabs( cv.ContourArea(contour) )
    isconvex = cv.CheckContourConvexity(contour)
    s = 0
    if len(contour) == 4 and area > 1000 and isconvex:
        for i in range(1, 4):
            # find minimum angle between joint edges (maximum of cosine)
            pt1 = contour[i]
            pt2 = contour[i-1]
            pt0 = contour[i-2]

            t = math.fabs(angle(pt0, pt1, pt2))
            if s <= t:s = t

        # if cosines of all angles are small (all angles are ~90 degree) 
        # then its a square
        if s < 0.3:return True

    return False       

    def updatePWM(self):
        vl = self.leftSpeed*self.left_sgn
        vr = self.rightSpeed*self.right_sgn

        pwml = self.PWMvalue(vl, self.LEFT_MOTOR_MIN_PWM, self.LEFT_MOTOR_MAX_PWM)
        pwmr = self.PWMvalue(vr, self.RIGHT_MOTOR_MIN_PWM, self.RIGHT_MOTOR_MAX_PWM)

        if self.debug:
            print "v = %5.3f, u = %5.3f, vl = %5.3f, vr = %5.3f, pwml = %3d, pwmr = %3d" % (v, u, vl, vr, pwml, pwmr)

        if fabs(vl) < self.SPEED_TOLERANCE:
            leftMotorMode = Adafruit_MotorHAT.RELEASE
        elif vl > 0:
            leftMotorMode = Adafruit_MotorHAT.FORWARD
        elif vl < 0: 
            leftMotorMode = Adafruit_MotorHAT.BACKWARD

        if fabs(vr) < self.SPEED_TOLERANCE:
            rightMotorMode = Adafruit_MotorHAT.RELEASE
            pwmr = 0;
        elif vr > 0:
            rightMotorMode = Adafruit_MotorHAT.FORWARD
        elif vr < 0: 
            rightMotorMode = Adafruit_MotorHAT.BACKWARD

        self.leftMotor.setSpeed(pwml)
        self.leftMotor.run(leftMotorMode);
        self.rightMotor.setSpeed(pwmr)
        self.rightMotor.run(rightMotorMode);

   def wmplugin_exec(self, m):
	
	axes = [None, None, None, None]


	self.acc = [self.NEW_AMOUNT*(new-zero)/(one-zero) + self.OLD_AMOUNT*old
	       for old,new,zero,one in zip(self.acc,m,self.acc_zero,self.acc_one)]
	a = math.sqrt(sum(map(lambda x: x**2, self.acc)))

	roll = math.atan(self.acc[cwiid.X]/self.acc[cwiid.Z])
	if self.acc[cwiid.Z] <= 0:
		if self.acc[cwiid.X] > 0: roll += math.pi
		else: roll -= math.pi

	pitch = math.atan(self.acc[cwiid.Y]/self.acc[cwiid.Z]*math.cos(roll))

	axes[0] = int(roll  * 1000 * self.Roll_Scale)
	axes[1] = int(pitch * 1000 * self.Pitch_Scale)

	if (a > 0.85) and (a < 1.15):
		if (math.fabs(roll)*(180/math.pi) > 10) and \
		   (math.fabs(pitch)*(180/math.pi) < 80):
			axes[2] = int(roll * 5 * self.X_Scale)

		if (math.fabs(pitch)*(180/math.pi) > 10):
			axes[3] = int(pitch * 10 * self.Y_Scale)

	return math.degrees(pitch), math.degrees(roll)

    def process_markers(self, msg):
        for marker in msg.markers:
            # do some filtering basd on prior knowledge
            # we know the approximate z coordinate and that all angles but yaw should be close to zero
            euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
                                                  marker.pose.pose.orientation.y,
                                                  marker.pose.pose.orientation.z,
                                                  marker.pose.pose.orientation.w))
            angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0)
            print angle_diffs, marker.pose.pose.position.z
            if (marker.id in self.marker_locators and
                3.0 <= marker.pose.pose.position.z <= 3.6 and
                fabs(angle_diffs[0]) <= .4 and
                fabs(angle_diffs[1]) <= .4):
                print "FOUND IT!"
                locator = self.marker_locators[marker.id]
                xy_yaw = list(locator.get_camera_position(marker))
                if self.is_flipped:
                    print "WE ARE FLIPPED!!!"
                    xy_yaw[2] += pi
                print self.pose_correction
                print self.phase_offset
                xy_yaw[0] += self.pose_correction*cos(xy_yaw[2]+self.phase_offset)
                xy_yaw[1] += self.pose_correction*sin(xy_yaw[2]+self.phase_offset)

                orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2])
                pose = Pose(position=Point(x=-xy_yaw[0],y=-xy_yaw[1],z=0),
                            orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
                # TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
                pose_stamped = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="STAR"),pose=pose)
                # TODO: use frame timestamp instead of now()
                self.star_pose_pub.publish(pose_stamped)
                self.fix_STAR_to_odom_transform(pose_stamped)

    def rank_it(self,scores):
        sorted_S = sorted(scores.items(), key=operator.itemgetter(1), reverse=True)

        if (math.fabs(sorted_S[0][1])+1) < math.fabs(sorted_S[1][1]) :
            return (sorted_S[0][0])
        else:
            return ('neutral')

    def naive_setup(self,type):
        self.naive_classes_prior = {'neutral':0,'positive':0,'negative':0}
        self.naive_classes_count = {'neutral':0,'positive':0,'negative':0}
        self.naive_classes_words = {'neutral':[],'positive':[],'negative':[]}
        total = 0
        unique = []

        for some in self.all:
            status = some[type].split(",")
            if int(some[1]) == 0:
                typeC = 'neutral'
                self.naive_classes_prior['neutral'] += 1
                self.naive_classes_count['neutral'] += len(status)
                total += 1
            elif int(some[1]) > 0:
                typeC = 'positive'
                self.naive_classes_prior['positive'] += (math.fabs(int(some[1])))
                self.naive_classes_count['positive'] += len(status)
            else:
                typeC = 'negative'
                self.naive_classes_prior['negative'] += (math.fabs(int(some[1])))
                self.naive_classes_count['negative'] += len(status)

            total += math.fabs(int(some[1]))

            for word in status:
                self.naive_classes_words[typeC].append(word)
                if word not in unique:
                    unique.append(word)

        for i in self.naive_classes_prior:
            self.naive_classes_prior[i] = self.naive_classes_prior[i]/total

        self.unique = len(unique)

 def add_level(self):
     """ 
         Splits a tree node into four child nodes
     """
     # don't split if the current node is smaller than the minimum size
     if self.min_size:
         if(math.fabs(self.bounds.north - self.bounds.south) < self.min_size
            and math.fabs(self.bounds.west - self.bounds.east) < self.min_size):
             return None
     
     
     # get the boundary points for the new child nodes 
     ns_half = self.bounds.north - (self.bounds.north - self.bounds.south) / 2
     ew_half = self.bounds.east - (self.bounds.east - self.bounds.west) / 2 
        
     self.children = []
     self.children.append(QuadTreeNode(Point(self.bounds.west, self.bounds.north), Point(ew_half, ns_half)))
     self.children.append(QuadTreeNode(Point(ew_half, self.bounds.north), Point(self.bounds.east, ns_half)))
     self.children.append(QuadTreeNode(Point(self.bounds.west, ns_half), Point(ew_half, self.bounds.south)))
     self.children.append(QuadTreeNode(Point(ew_half, ns_half), Point(self.bounds.east, self.bounds.south)))
 
     # move items to children
     for item in self.items:
         self.add(item.point(), item.object())
     self.items = []

def _scatter(actual, prediction, args):
    plt.figure()
    plt.plot(actual, prediction, 'b'+args['plot_scatter_marker'])
    xmin=min(actual)
    xmax=max(actual)
    ymin=min(prediction)
    ymax=max(prediction)
    diagxmin=min(math.fabs(x) for x in actual)
    diagymin=min(math.fabs(y) for y in prediction)
    diagpmin=min(diagxmin,diagymin)
    pmin=min(xmin,ymin)
    pmax=max(xmax,ymax)
    plt.plot([diagpmin,pmax],[diagpmin,pmax],'k-')
    if args['plot_identifier'] != 'NoName':
        plt.title(args['plot_identifier'])
    plt.xlabel('Observed')
    plt.ylabel('Modeled')
    if args['plot_performance_log'] == True:
        plt.yscale('log')
        plt.xscale('log')
    if args['plot_scatter_free'] != True:
        plt.axes().set_aspect('equal')
    if args['plot_dump'] == True:
        pfname=os.path.join(args['plot_dir'],args['plot_identifier']+'_eiger_scatter.pdf')
        plt.savefig(pfname,format="pdf")
    else:
        plt.show()

def getMotionArrayOLD(image1, image2, threshold = 10):
    i1 = image1
    i2 = image2
    #both images need to be the same size in pixels
    if (i1.size[0] != i2.size[0]) or (i1.size[1] != i2.size[1]):
        return 0 #if not, we return 0
    size = i1.size

    t = threshold
    motionarray = [] #2D array to store motion areas

    i=0
    while i in range(size[1]): #scan through the images
        j=0
        while j in range(size[0]):
            p1 = i1.getpixel((j,i))
            p2 = i2.getpixel((j,i))

            if (fabs(p1[0]-p2[0]) > t) or (fabs(p1[1]-p2[1]) > t) or (fabs(p1[2]-p2[2]) > t): #compare each pixel in R,G,B channel
                    motionarray.append((j,i))

            j = j+1
        i = i+1
 
    return motionarray

def get_interval(data, numFrames):
    data = np.absolute(data)
    data = smooth_plus(data, 3000) #smooth(data, 300)
    std = 0.30*np.std(data) #+ np.mean(data) 
    interval = []
    coeff = data.shape[0] / numFrames
    i = 0
    while i < len(data):
        if math.fabs(data[i]) > std:
            beg, end = i, 0
            count = 0
            while (math.fabs(data[i]) > std or count < 3000) and ((i+1) <= (len(data)-1)):
                i += 1
                if (math.fabs(data[i]) < std):
                    count += 1
                else:
                    count = 0
                    end = i
            if(end - beg > 3000):
                if end + 10000 < len(data):
                    end += 10000
                    i += 6500
                else:
                    end = len(data) - 1
                interval.append(['', (1+(beg/coeff), 1+(end/coeff))])
        i += 1
    return interval

def getMotionArrayRGB(image1, image2, threshold = 10):
    i1 = image1
    i2 = image2
    #both images need to be the same size in pixels
    if (i1.size[0] != i2.size[0]) or (i1.size[1] != i2.size[1]):
        return 0 #if not, we return 0
    size = i1.size

    imgArr1 = list(i1.getdata())
    imgArr2 = list(i2.getdata())

    t = threshold
    motionarray = [] #2D array to store motion areas

    i=0
    while i < len(imgArr1): #scan through the images
            p1 = imgArr1[i]
            p2 = imgArr2[i]

            if (fabs(p1[0]-p2[0]) > t) or (fabs(p1[1]-p2[1]) > t) or (fabs(p1[2]-p2[2]) > t): #compare each pixel in R,G,B channel
                    y = i/size[0]
                    x = i - y*size[0]
                    motionarray.append((x,y,p2))

            i = i+1
 
    return motionarray

def full_width_half_max (sigma_l, sigma_r=0):
    if (sigma_r == 0):
        sigma_r = sigma_l
    width_l = (2.35482/2.0)*m.fabs(sigma_l)
    width_r = (2.35482/2.0)*m.fabs(sigma_r)
    FWHM = width_l + width_r
    return FWHM