Add functionality to found the shortes way, but is not ok :(
Fix algorithm