Add the fix position from mapping

Copy past the fix position from the mapping branch, only one small change for the function is_inside_square() where I need to add two extra parameters. It looks ok now