Decision Maker: Refactor stopline waypoint detection/assignment

Discovered two bugs today, one is related to inserting two stopline waypoints for a single stopline.

The other bug is forgetting to assign the stop_line_id to the waypoint.