Implemented basic route finding between coords