Still not perfect but workable optimizer

To fix: project the movement outside the boundary back into the boundary