Still working on mode 2 -doesn't calculate distance properly