Fixed the step calculation

Some more "lovely" code, defintely due a refactor