Delete the old code that was solving motion planning problems to do conflict resolution instead of just doing path tracking

When getting the nn for path tracking, take the closest one on the ref path that ocurrs the soonest and has not been visited already

Treat the optimization problem as a path tracking problem instead. Works both with and without the database as a conflict resolver