Couple of bugs fixed, cleanup code
Moved some functions from mapping to helper, prepared motion bel update