Fixed bug in finding distance

Y displacement is now calculated showing the minimum displacement
version
Added comments