def circle(latlng_origin=None, pid_origin=None, radius=15):
    """
    Returns a validator function for a Panorama. which Returns True if
    the Panorama is inside a circle of the radius r around the center
    point latlng_0.
    :param latlng_0: tuple (lat, lng) - center GPS
    :param r: float - radius in meters
    :return: validator(panorama) - given Panorama it returns True
             if the Panorama is inside the circle
    """
    if latlng_origin:
        (easting, northing, z_number, z_letter) = utm.from_latlon(latlng_origin[0], latlng_origin[1])
    elif pid_origin:
        latlng_origin = Panorama(pid_origin).getGPS()
        (easting, northing, z_number, z_letter) = utm.from_latlon(latlng_origin[0], latlng_origin[1])
    else:
        raise ValueError("One of the arguments 'latlang_origin' or 'pid_origin' must be given.")

    def isClose(p):
        ll = p.getGPS()
        (est, nth, zn, zl) = utm.from_latlon(ll[0], ll[1], force_zone_number=z_number)
        x, y = est-easting, nth-northing
        d = sqrt(x**2+y**2)
        return d < radius
    return isClose

 def test_force_zone(self):
     # test forcing zone ranges
     # NYC should be zone 18T
     self.assert_zone_equal(UTM.from_latlon(40.71435, -74.00597, 19, 'T'), 19, 'T')
     self.assert_zone_equal(UTM.from_latlon(40.71435, -74.00597, 17, 'T'), 17, 'T')
     self.assert_zone_equal(UTM.from_latlon(40.71435, -74.00597, 18, 'u'), 18, 'U')
     self.assert_zone_equal(UTM.from_latlon(40.71435, -74.00597, 18, 'S'), 18, 'S')

def global_to_local(origin, lon, lat, heading=0.0):
    """
    Converts from the WGS84 lon/lat global frame into the local
    frame in meters.  Note that heading is in degrees!

    @param origin (lon, lat, heading)
    @return (x, y, theta) in WGS84
    """
    # Create local transformation frame.
    import utm
    ox, oy, zone, hemi = utm.from_latlon(origin[1], origin[0])
    o_theta = (90 - origin[2]) * (math.pi/180.0)

    # Convert global point into local frame.
    px, py, zone, hemi = utm.from_latlon(lat, lon)
    p_theta = math.pi/2.0 - heading

    # Translate and rotate point to origin.
    point = shapely.geometry.Point(px - ox, py - oy)
    point = shapely.affinity.rotate(point, -o_theta,
                                    origin=ORIGIN, use_radians=True)
    theta = p_theta - o_theta

    # Return transformed point.
    return point.x, point.y, theta

    def test_from_latlon_range_checks(self):
        '''from_latlon should fail with out-of-bounds input'''
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, -100, 0)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, -80.1, 0)
        for i in range(-8000, 8400):
            UTM.from_latlon(i / 100.0, 0)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 84.1, 0)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 100, 0)

        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 0, -300)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 0, -180.1)
        for i in range(-18000, 18000):
            UTM.from_latlon(0, i / 100.0)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 0, 180.1)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 0, 300)

        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, -100, -300)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 100, -300)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, -100, 300)
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 100, 300)

        # test forcing zone ranges
        # NYC should be zone 18T
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 40.71435, -74.00597, 70, 'T')
        self.assertRaises(UTM.OutOfRangeError, UTM.from_latlon, 40.71435, -74.00597, 18, 'A')

def optimize_rotate_UTMconvert(angle,df,base):
	import numpy as np
	import math
	import utm
	from HCF_functions import rotate_xy
	import matplotlib.pyplot as plt

	sum = 0
	pick = np.isfinite(df['gps_lat'])
	k = utm.from_latlon(df.loc[base]['y_working'],df.loc[base]['x_working'])
	x0 = k[0]
	y0 = k[1]
	for index, row in df[pick].iterrows():
		i = utm.from_latlon(row['y_working'], row['x_working'])
		j = utm.from_latlon(row['gps_lat'], row['gps_lon'])
		gpsX, gpsY = j[0], j[1]
		newX, newY = rotate_xy(i[0],i[1],x0,y0, angle * math.pi / 180)

		a = newX - gpsX
		b = newY - gpsY
		c = np.square(a) + np.square(b)
		misfit = np.sqrt(c)
		misfit = np.sqrt(c) / row['gps_horiz_acc']
		sum = sum + misfit
		# print angle, index, misfit
	plt.scatter(df['x_working'], df['y_working'], color='orange')
	print angle, sum
	return sum

def box(latlng_origin=None, pid_origin=None, width=15, height=15):
    """
    Returns a validator function of Panorama that returns True is
    the Panorama is inside a box of the size w,h around center
    point latlng_0.
    :param latlng_origin: tuple (lat, lng) - center GPS
    :param w: float - width in meters
    :param h: float - height in meters
    :return: validator(Panorama) - given a Panorama it returns True
             if the panorama is inside the box
    """
    if latlng_origin:
        (easting, northing, z_number, z_letter) = utm.from_latlon(latlng_origin[0], latlng_origin[1])
    elif pid_origin:
        latlng_origin = Panorama(pid_origin).getGPS()
        (easting, northing, z_number, z_letter) = utm.from_latlon(latlng_origin[0], latlng_origin[1])
    else:
        raise ValueError("One of the arguments 'latlang_origin' or 'pid_origin' must be given.")


    def isClose(p):
        ll = p.getGPS()
        (est, nth, zn, zl) = utm.from_latlon(ll[0], ll[1], force_zone_number=z_number)
        x, y = est-easting, nth-northing
        return abs(x) < w/2 and abs(y) < h/2
    return isClose

def distance_2d(lon1, lat1, lon2, lat2):
    ym1, xm1, _, _ = utm.from_latlon(lat1, lon1)
    ym2, xm2, _, _ = utm.from_latlon(lat2, lon2)

    arr1 = numpy.array((xm1, ym1))
    arr2 = numpy.array((xm2, ym2))

    dist = numpy.linalg.norm(arr1-arr2)
    return dist

def latlon_to_meters(coord_list, origin):
    # Convert coord_list to UTM
    coord_list[:] = [utm.from_latlon(*coord) for coord in coord_list]

    # Convert origin to UTM
    origin = utm.from_latlon(*origin)

    # Convert coord_list to meters relative to origin
    coord_list[:] = [(coord[0] - origin[0], coord[1] - origin[1])
                     for coord in coord_list]

def calc_utm_distance(lat1, lon1, lat2, lon2):
	"""
	Returns distance in millimeters
	"""
	point1 = utm.from_latlon(lat1, lon1)
	point2 = utm.from_latlon(lat2, lon2)
	
	x = point2[1]-point1[1]
	y = point2[2]-point1[2]
	
	return 1000*math.sqrt(x*x+y*y)

def getPolygon(row,coordinateDir,coordinateFileName):
    polygonData = openCSVFile(coordinateDir+coordinateFileName)   
    UTM1 = utm.from_latlon(float(polygonData[row][0]),float(polygonData[row][1]))
    UTM1 = [UTM1[0],UTM1[1]]
    UTM2 = utm.from_latlon(float(polygonData[row][2]),float(polygonData[row][3]))
    UTM2 = [UTM2[0],UTM2[1]]
    UTM3 = utm.from_latlon(float(polygonData[row][4]),float(polygonData[row][5]))
    UTM3 = [UTM3[0],UTM3[1]]
    UTM4 = utm.from_latlon(float(polygonData[row][6]),float(polygonData[row][7]))
    UTM4 = [UTM4[0],UTM4[1]]
    return Polygon([(UTM1[0],UTM1[1]),(UTM2[0],UTM2[1]),(UTM3[0],UTM3[1]),(UTM4[0],UTM4[1])])

def events():
  """Gets isc data."""
  missing_params = []
  for p in ['lat', 'lon', 'distance', 'start', 'end', 'mag', 'catalog']:
    if p not in request.args:
      missing_params.append(p)
  if missing_params:
    return 'Missing parameters: {}'.format(','.join(missing_params))

  start = datetime.datetime.strptime(request.args.get('start'),
                                     DATE_FORMAT)
  end = datetime.datetime.strptime(request.args.get('end'),
                                   DATE_FORMAT)
  time_delta = end - start
  logging.info('Start Time {}'.format(start))
  logging.info('End Time {}'.format(end))
  logging.info('Time Delta {}'.format(time_delta))

  # Read the isc data.
  isc_data = isc.ReadISCData('gs://clouddfe-cfs/isc',
                             request.args.get('catalog'),
                             start, abs(time_delta.days),
                             (long(request.args.get('lat')),
                              long(request.args.get('lon'))),
                             int(request.args.get('distance')))

  # Prepare the ISC data by:
  #   1) Filtering out magnitude events smaller than specified.
  #   2) Projecting the lat/lon to UTM.
  ret = []
  _, _, num, let = utm.from_latlon(float(request.args.get('lat')),
                                   float(request.args.get('lon')))
  proj = lambda lat, lon: utm.from_latlon(lat, lon, num)[0:2]
  mag = float(request.args.get('mag'))
  num_filtered = 0
  for data in isc_data:
    if data['magnitude'] < mag:
      num_filtered += 1
      continue
    x, y = proj(data['lat'], data['lon'])
    ret.append({
        'lat': data['lat'],
        'lon': data['lon'],
        'utm_x': x,
        'utm_y': y,
        'depth': data['depth'],
        'mag': data['magnitude'],
        'date': data['date_time'].isoformat(' '),  # YYYY-MM-DD HH:MM:SS
    })
  logging.info('Filtered (%d) ISC events due to magnitude', num_filtered)

  if 'callback' in request.args:
    return '{}({})'.format(request.args.get('callback'), json.dumps(ret))
  return json.dumps(ret)

def get_distance(origin_lat, origin_lng, dest_lat, dest_lng):	
	xy = utm.from_latlon(origin_lat, origin_lng) # convert origin from lat-lng to xy
	originXY = point(float(xy[0]), float(xy[1]))
	
	xy = utm.from_latlon(dest_lat, dest_lng) # convert destination from lat-lng to xy
	destinationXY = point(float(xy[0]), float(xy[1]))

	distance = math.sqrt((originXY.x - destinationXY.x) ** 2 + (originXY.y - destinationXY.y) ** 2)
	res = [(destinationXY.x - originXY.x) * 4, (destinationXY.y - originXY.y) * 4]
	# print res
	return res # return the distance vector

def calc_utm_distance(lat1, lon1, lat2, lon2):
	"""
	Returns distance in millimeters
	This is not currently used in any of the code. Left for now in case needed
	"""
	point1 = utm.from_latlon(lat1, lon1)
	point2 = utm.from_latlon(lat2, lon2)

	x = point2[1]-point1[1]
	y = point2[2]-point1[2]

	return 1000*math.sqrt(x*x+y*y)

def calc_list_core( l_points, EP, func_fretch ):
    npos1 = 0
    npos2 = 0
    npos3 = 0
    list_count = len(l_points)

    ret_list = {}
    ret_list[3]=[]
    ret_list[5]=[]

    while npos1 < list_count - 2:
        try:
            la1, lo1, dis1 = func_fretch(l_points[npos1])
            npos1 = npos1 + 1
            if not la1 or not lo1 or not dis1:
                continue
            u1 = utm_point(*utm.from_latlon(la1,lo1), r=int(dis1))
        except Exception as e:
            continue

        npos2 = npos1
        while npos2 < list_count - 1:
            try:
                la2, lo2, dis2 = func_fretch(l_points[npos2])
                npos2 = npos2 + 1
                if not la2 or not lo2 or not dis2:
                    continue
                u2 = utm_point( *utm.from_latlon(la2,lo2), r=int(dis2))
            except Exception as e:
                continue

            s, p1, p2 = calc_cross_point(u1, u2)
            if not p1 or not p2:
                continue



            npos3 = npos2
            while npos3 < list_count:
                try:
                    la3, lo3, dis3 = func_fretch(l_points[npos3])
                    npos3 = npos3 + 1
                    if not la3 or not lo3 or not dis3:
                        continue
                    u3 = utm_point( *utm.from_latlon(la3,lo3), r=int(dis3))
                except Exception as e:
                    continue
                
                ret, level = calc_list_core_cross_point(s, p1, p2, u3, EP)
                if ret:
                    return ret, level
    return None, 0

def calc( latitude1, longitude1, r1,
              latitude2, longitude2, r2,
              latitude3, longitude3, r3, EP = global_EP):
    try:
        u1 = utm_point( *utm.from_latlon(latitude1,longitude1), r=r1)
        u2 = utm_point( *utm.from_latlon(latitude2,longitude2), r=r2)
        u3 = utm_point( *utm.from_latlon(latitude3,longitude3), r=r3)
    except Exception as e:
        return None

    pr = calc_distance_utm(u1,u2,u3, EP)
    if pr:
        latitude, longitude = utm.to_latlon(pr.x, pr.y, pr.zone, pr.mark)
        return latitude, longitude, pr.r
    return None

 def new_allotment(self, address, application, disabled_access, external_link, guidence, location, name, plot_size, rent, Easting, Northing):
     self.new_address(address)
     print utm.from_latlon(float(location.split(',')[0]), float(location.split(',')[1]))[1]
     allotment = al[name.replace(" ", "-").lower()] # @@ humanize the identifier (something like #rev-$date)
     self.graph.add((allotment, RDF.type, URIRef('http://data.gmdsp.org.uk/def/council/allotment/Allotment')))
     self.graph.add((allotment, COUNCIL['application'], URIRef(application)))
     self.graph.add((allotment, COUNCIL['information'], URIRef(external_link)))
     self.graph.add((allotment, COUNCIL['guidance'], URIRef("http://www.manchester.gov.uk"+guidence)))
     self.graph.add((allotment, GEO["lat"], Literal(location.split(',')[0])))
     self.graph.add((allotment, GEO["long"], Literal(location.split(',')[1])))
     self.graph.add((allotment, OS["northing"], Literal(str(utm.from_latlon(float(location.split(',')[0]), float(location.split(',')[1]))[1]))))
     self.graph.add((allotment, OS["easting"], Literal(str(utm.from_latlon(float(location.split(',')[0]), float(location.split(',')[1]))[0]))))
     self.graph.add((allotment, RDFS['label'], Literal(name)))
     self.graph.add((allotment,VCARD['hasAddress'], URIRef("http://data.gmdsp.org.uk/id/manchester/allotments/address/"+address.replace(" ", "_").replace(",","-").lower())))
     self.save()

 def readFile(self, filename):
     """
     function that reads from netcdf file
     """
     nc = Dataset(filename, 'r')
     print "#### Reading ROMS output file !!!! ####\n"
     #print nc
     self.data = dict()
     x1 = 0; x2 = nc.variables['lon_psi'].shape[1]        
     y1 = 0; y2 = nc.variables['lon_psi'].shape[0]
     lon = nc.variables['lon_psi'][:]
     lat = nc.variables['lat_psi'][:]
     x = np.zeros_like(lon)
     y = np.zeros_like(lat)
     nx, ny = lon.shape    
     for i in range(nx):
         for j in range(ny):
             (y[i,j], x[i,j]) = utm.from_latlon(lat[i,j], lon[i,j])[0:2]
     
     self.data['lon'] = x
     self.data['lat'] = y
     self.data['lon_psi'] = lon
     self.data['lat_psi'] = lat
     u = nc.variables['u'][:,0,:,:]
     v = nc.variables['v'][:,0,:,:]
     self.data['mask'] = nc.variables['mask_psi'][:]
     
     ftime = nc.variables['ocean_time']
     time = num2date(ftime[:],ftime.units)
     self.ind0 = self.findNearest(self.starttime,time)
     self.ind1 = self.findNearest(self.endtime,time)
     self.data['time'] = time[self.ind0:self.ind1+1]
     
     self.data['u'] = u[self.ind0:self.ind1+1,y1:y2+1,x1:x2]  
     self.data['v'] = v[self.ind0:self.ind1+1,y1:y2,x1:x2+1] 

def kml_roi_process(rpc, kml):
    """
    """
    # extract lon lat from kml
    with open(kml, 'r') as f:
        a = bs4.BeautifulSoup(f, "lxml").find_all('coordinates')[0].text.split()
    ll_bbx = np.array([list(map(float, x.split(','))) for x in a])[:4, :2]

    # save lon lat bounding box to cfg dictionary
    lon_min = min(ll_bbx[:, 0])
    lon_max = max(ll_bbx[:, 0])
    lat_min = min(ll_bbx[:, 1])
    lat_max = max(ll_bbx[:, 1])
    cfg['ll_bbx'] = (lon_min, lon_max, lat_min, lat_max)

    # convert lon lat bbox to utm
    z = utm.conversion.latlon_to_zone_number((lat_min + lat_max) * .5,
                                             (lon_min + lon_max) * .5)
    utm_bbx = np.array([utm.from_latlon(p[1], p[0], force_zone_number=z)[:2] for
                        p in ll_bbx])
    east_min = min(utm_bbx[:, 0])
    east_max = max(utm_bbx[:, 0])
    nort_min = min(utm_bbx[:, 1])
    nort_max = max(utm_bbx[:, 1])
    cfg['utm_bbx'] = (east_min, east_max, nort_min, nort_max)

    # project lon lat vertices into the image
    if not isinstance(rpc, rpc_model.RPCModel):
        rpc = rpc_model.RPCModel(rpc)
    img_pts = [rpc.inverse_estimate(p[0], p[1], rpc.altOff)[:2] for p in ll_bbx]

    # return image roi
    x, y, w, h = common.bounding_box2D(img_pts)
    return {'x': x, 'y': y, 'w': w, 'h': h}

 def ExtractWaypoints(self):
     
     WPKeys = list(self.logdata.channels['CMD']['CNum'].dictData.keys())
     WPKeys.sort();
     
     WPnum = 1;
     WPtot = 35;
     for k in WPKeys:
         if(self.logdata.channels['CMD']['CNum'].dictData[k]==WPnum or WPnum == WPtot):
             
             tempLine = np.matrix('0.0 0.0 0.0 0.0 0.0 0.0');
             tempLine[0,0] = self.logdata.channels['CMD']['Lat'].dictData[k];
             tempLine[0,1] = self.logdata.channels['CMD']['Lng'].dictData[k];
             tempLine[0,2] = self.logdata.channels['CMD']['Alt'].dictData[k];
             tempLine[0,3] = self.logdata.channels['CMD']['TimeMS'].dictData[k];
             
             tempLine2 = np.matrix('0.0 0.0 0.0 0.0 0.0 0.0');
             (easting,northing,_,_) = utm.from_latlon(tempLine[0,0],tempLine[0,1]);
             tempLine2[0,0] = easting - self.LakeLag0[0,0];
             tempLine2[0,1] = northing - self.LakeLag0[0,1];
             tempLine2[0,2] = tempLine[0,2];
             tempLine2[0,3] = tempLine[0,3];
             if(WPnum == 1):
                 self.WPgeo = tempLine;
                 self.WPenu = tempLine2;
             else:
                 self.WPgeo = np.append(self.WPgeo,tempLine,axis=0);
                 self.WPenu = np.append(self.WPenu,tempLine2,axis=0);
                 
             
             WPnum = WPnum + 1;

    def __init__(self):

        # initialise message structure
        self._gpsOut = gps()

        self.nmeaID = {'GGA' : self.gga,
                       'RMC' : self.rmc,
                       'GSA' : self.gsa,
                       'GSV' : self.gsv,
                       'VTG' : self.vtg
        }
        
        # get parameters # TODO Sophia will have to loop at this section
        self._controlRate = 1. # Hz
        lat_ori = rospy.get_param('lat_orig', 50.9567)
        lon_ori = rospy.get_param('long_orig', -1.36735)
        self.dt_status = rospy.get_param('status_timing', 2.)
        
        self.x_UTM_ori, self.y_UTM_ori, _, _ = utm.from_latlon(lat_ori, lon_ori)

        self._r = rospy.Rate(self._controlRate*2)  # Hz

        self._getAll = False

        #Define Publishers
        self.pubStatus = rospy.Publisher('status', status)
        self.pub = rospy.Publisher('gps_out', gps)
        self.pubMissionLog = rospy.Publisher('MissionStrings', String)
        
        self.serialPort = self.openSerialPort()
        rospy.on_shutdown(self.onShutdownEvents)

        self.repeatedlyPublishNodeStatus(nodeID=4, interv=2)

        self.mainloop()