Better insertion + route finding