Fix and refactoring of previous commit

Change-Id: I620885fa4032d3711ec1b8e9fd281c704c659062