fixed problems with velocity calculations, refactored code