* added internal function that is able to get sorted paths from units
positions into final position from shortest.

* added wrapper that is used to sort paths and units and given final
location in a proper way

* added test internal action that is for debug only