Significant bug fix, some improvements and refactoring