Added position calculation and tests