Finished AStar algorithm. Needs more work on cost calculation + vehicle change penalty + delays.