route.py

from collections import namedtuple
import requests
from geopy import Point, distance
from typing import List, Tuple

API_KEY = 'AIzaSyCeo86HIGUtpWaPOpOA6pbNQhHDe3-26ko'
MAX_STEPS = 5

FROM = (40.728833, -74.000852)
TO = (40.735012, -73.979333)
MOCKED_WAYPOINTS = [((40.72223320000001, -73.9874291), 4),
                    ((40.7324849, -74.00259299999999), 7),
                    ((40.7356354, -74.006691), 9),
                    ((40.742975, -73.98927599999999), 3),
                    ((40.73139849999999, -74.0024617), 4),
                    ((40.73686, -73.991384), 2),
                    ((40.7290258, -73.98712069999999), 6),
                    ((40.7327507, -74.0029358), 5)]


class Waypoint(namedtuple('Waypoint', 'pt area')):
    def closest(self, graph: List['Waypoint']) -> 'Waypoint':
        valid_nodes = self.closer_nodes(graph)
        best_dist = None
        best = None
        for other in valid_nodes:
            dist = distance.distance(self.pt, other.pt).meters
            if not best_dist or dist < best_dist:
                best_dist = dist
                best = other
        return best

    def closer_nodes(self, graph: List['Waypoint']) -> List['Waypoint']:
        lats = sorted([self.area[0].latitude, self.area[1].latitude])
        lngs = sorted([self.area[0].longitude, self.area[1].longitude])

        def within(pt: Point) -> bool:
            return ((lats[0] <= pt.latitude <= lats[1]) and
                    (lngs[0] <= pt.longitude <= lngs[1]))

        nodes = [node for node in graph if within(node.pt)]
        return nodes


def best_path(nodes: List[Point], origin: Point, dest: Point) -> List:
    waypoints = [Waypoint(origin, (origin, dest))]
    for node in nodes:
        waypoints.append(Waypoint(node, (origin, dest)))
    to = Waypoint(dest, (origin, dest))
    waypoints.append(to)

    start, remaining = waypoints[0], waypoints[1:]
    path = []
    for i in range(MAX_STEPS):
        closer = start.closest(remaining)
        if closer is None:
            break
        path.append(closer)
        remaining = [pt for pt in remaining if pt != closer]
        start = closer

    # check to see if destination is in path
    if to in path:
        path.remove(to)
    return '|'.join(["{},{}".format(pt.pt.latitude, pt.pt.longitude) for pt in path])


def main():
    lat_longs = [Point(pt[0]) for pt in MOCKED_WAYPOINTS]
    path = best_path(lat_longs, Point(FROM), Point(TO))
    print(path)

if __name__ == '__main__':
    main()